From feddb618c492c115933c8714ed158325af940203 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 11 Jul 2013 23:12:06 +0200 Subject: [PATCH 001/447] Added rotation matrix based vector controller --- makefiles/config_px4fmu-v1_default.mk | 1 + src/modules/mathlib/math/Vector3.cpp | 10 + src/modules/mathlib/math/Vector3.hpp | 2 + src/modules/mathlib/math/arm/Vector.hpp | 2 +- .../ecl_mc_att_control_vector.cpp | 146 ++++ .../ecl_mc_att_control_vector.h | 60 ++ .../mc_att_control_vector_main.cpp | 692 ++++++++++++++++++ src/modules/mc_att_control_vector/module.mk | 41 ++ 8 files changed, 953 insertions(+), 1 deletion(-) create mode 100644 src/modules/mc_att_control_vector/ecl_mc_att_control_vector.cpp create mode 100644 src/modules/mc_att_control_vector/ecl_mc_att_control_vector.h create mode 100644 src/modules/mc_att_control_vector/mc_att_control_vector_main.cpp create mode 100644 src/modules/mc_att_control_vector/module.mk diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk index 213eb651b0..f3015b97e9 100644 --- a/makefiles/config_px4fmu-v1_default.mk +++ b/makefiles/config_px4fmu-v1_default.mk @@ -74,6 +74,7 @@ MODULES += modules/fixedwing_backside MODULES += modules/fixedwing_att_control MODULES += modules/fixedwing_pos_control MODULES += modules/multirotor_att_control +MODULES += modules/mc_att_control_vector MODULES += modules/multirotor_pos_control MODULES += examples/flow_position_control MODULES += examples/flow_speed_control diff --git a/src/modules/mathlib/math/Vector3.cpp b/src/modules/mathlib/math/Vector3.cpp index dcb85600ef..3936650c6f 100644 --- a/src/modules/mathlib/math/Vector3.cpp +++ b/src/modules/mathlib/math/Vector3.cpp @@ -84,6 +84,16 @@ Vector3 Vector3::cross(const Vector3 &b) const return result; } +Vector3 Vector3::operator %(const Vector3 &v) const +{ + return cross(v); +} + +float Vector3::operator *(const Vector3 &v) const +{ + return dot(v); +} + int __EXPORT vector3Test() { printf("Test Vector3\t\t: "); diff --git a/src/modules/mathlib/math/Vector3.hpp b/src/modules/mathlib/math/Vector3.hpp index 568d9669ab..2bf00f26be 100644 --- a/src/modules/mathlib/math/Vector3.hpp +++ b/src/modules/mathlib/math/Vector3.hpp @@ -54,6 +54,8 @@ public: Vector3(const float *data); virtual ~Vector3(); Vector3 cross(const Vector3 &b) const; + Vector3 operator %(const Vector3 &v) const; + float operator *(const Vector3 &v) const; /** * accessors diff --git a/src/modules/mathlib/math/arm/Vector.hpp b/src/modules/mathlib/math/arm/Vector.hpp index 4155800e86..0f7722143e 100644 --- a/src/modules/mathlib/math/arm/Vector.hpp +++ b/src/modules/mathlib/math/arm/Vector.hpp @@ -141,7 +141,7 @@ public: getRows()); return result; } - inline Vector operator*(float right) const { + inline Vector operator*(const float &right) const { Vector result(getRows()); arm_scale_f32((float *)getData(), right, result.getData(), diff --git a/src/modules/mc_att_control_vector/ecl_mc_att_control_vector.cpp b/src/modules/mc_att_control_vector/ecl_mc_att_control_vector.cpp new file mode 100644 index 0000000000..b21b0cf67e --- /dev/null +++ b/src/modules/mc_att_control_vector/ecl_mc_att_control_vector.cpp @@ -0,0 +1,146 @@ +/**************************************************************************** + * + * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * Author: Tobias Naegeli + * + * 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 ecl_mc_att_control_vector.cpp + * + * Multirotor attitude controller based on concepts in: + * + * Minimum Snap Trajectory Generation and Control for Quadrotors + * http://www.seas.upenn.edu/~dmel/mellingerICRA11.pdf + * + * @author Tobias Naegeli + * @author Lorenz Meier + * + */ + +#include +#include "ecl_mc_att_control_vector.h" + +void ECL_MCAttControlVector::control(float dt, const math::Dcm &R_nb, float yaw, const math::Vector &F_des_in, + float Kp, float Kd, float Ki, const math::Vector &angular_rates, + math::Vector &rates_des, float &thrust) +{ + //function [rates_des,Thrust,ie_out] = attitudeController(R1,yaw,F_des,Kp,Kd,Ki,e_Rd_v,ie) + //% dt + //dt=1/50; + + //% shoult the controll be in the body frame or in the inertial frame: 0 is + //% body frame, 1 is inertial frame. + + // XXX + bool earth = true; + bool integral_freeze = false; + bool integral_reset = false; + + math::Matrix R_bn = R_nb.transpose(); + + float cy = cosf(yaw); + float sy = sinf(yaw); + + math::Matrix RYaw = math::Dcm(cy,-sy,0.0f,sy,cy,0.0f,0.0f,0.0f,1.0f); + //%F_des=RYaw*F_des; + math::Vector z_b = math::Vector3(R_bn(0,2), R_bn(1,2), R_bn(2,2)); + + math::Vector3 F_des = F_des_in; + + // desired thrust in body frame + // avoid division by zero + if (F_des(2) >= 0.1f) { + thrust = F_des(2) / R_bn(2, 2); + } else { + F_des(2) = 0.1f; + thrust= F_des(2) / R_bn(2, 2); + } + + math::Vector3 x_B_des; + math::Vector3 y_B_des; + math::Vector3 z_B_des; + + // desired body z axis + if (earth) { + z_B_des = (F_des / F_des.norm()); + } else { + z_B_des = RYaw * (F_des / F_des.norm()); + } + + // desired direction in world coordinates (yaw angle) + math::Vector3 x_C(cy, sy, 0.0f); + //desired body y axis + y_B_des = z_B_des.cross(x_C) / (z_B_des.cross(x_C)).norm(); + // desired body x axis + x_B_des = y_B_des.cross(z_B_des); + // desired Rotation Matrix + math::Matrix R_des = math::Dcm(x_B_des(0), x_B_des(1), x_B_des(2), + y_B_des(0), y_B_des(1), y_B_des(2), + z_B_des(0), z_B_des(1), z_B_des(2)); + + + // Attitude Controller + // P controller + + // error rotation matrix + math::Matrix e_R = (R_des.transpose() * R_bn - R_bn.transpose() * R_des) * 0.5f; + + // error rotation vector + math::Vector e_R_v(3); + e_R_v(0) = e_R(1,2); + e_R_v(1) = e_R(0,2); + e_R_v(2) = e_R(0,1); + + + // include an integral part + math::Vector intError = math::Vector3(0.0f, 0.0f, 0.0f); + if (!integral_freeze) { + if (thrust > 0.3f && !integral_reset) { + if (fabsf(_integral_error(0)) < _integral_max(0)) { + _integral_error(0) = _integral_error(0) + e_R_v(0) * dt; + } + + if (fabsf(_integral_error(1)) < _integral_max(1)) { + _integral_error(1) = _integral_error(1) + e_R_v(1) * dt; + } + + } else { + _integral_error(0) = 0.0f; + _integral_error(1) = 0.0f; + } + + intError(0) = _integral_error(0); + intError(1) = _integral_error(1); + intError(2) = 0.0f; + } + + rates_des = (e_R_v * Kp + angular_rates * Kd + intError * Ki); +} diff --git a/src/modules/mc_att_control_vector/ecl_mc_att_control_vector.h b/src/modules/mc_att_control_vector/ecl_mc_att_control_vector.h new file mode 100644 index 0000000000..98689f0bee --- /dev/null +++ b/src/modules/mc_att_control_vector/ecl_mc_att_control_vector.h @@ -0,0 +1,60 @@ +/**************************************************************************** + * + * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * Author: Tobias Naegeli + * + * 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 ecl_mc_att_control_vector.cpp + * + * Multirotor attitude controller based on concepts in: + * + * Minimum Snap Trajectory Generation and Control for Quadrotors + * http://www.seas.upenn.edu/~dmel/mellingerICRA11.pdf + * + * @author Tobias Naegeli + * @author Lorenz Meier + * + */ + +#include + +class ECL_MCAttControlVector { + +public: + void control(float dt, const math::Dcm &R_nb, float yaw, const math::Vector &F_des, + float Kp, float Kd, float Ki, const math::Vector &angular_rates, + math::Vector &rates_des, float &thrust); + +protected: + math::Vector2 _integral_error; + math::Vector2 _integral_max; +}; diff --git a/src/modules/mc_att_control_vector/mc_att_control_vector_main.cpp b/src/modules/mc_att_control_vector/mc_att_control_vector_main.cpp new file mode 100644 index 0000000000..f3f8235f0e --- /dev/null +++ b/src/modules/mc_att_control_vector/mc_att_control_vector_main.cpp @@ -0,0 +1,692 @@ +/**************************************************************************** + * + * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * Author: Lorenz Meier + * + * 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 mc_att_control_vector_main.c + * Implementation of a multicopter attitude controller based on desired thrust vector. + * + * @author Tobias Naegeli + * @author Lorenz Meier + * + * Please refer to the library files for the authors and acknowledgements of + * the used control library functions. + */ + +#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 +#include + +//#include +#include "ecl_mc_att_control_vector.h" + +/** + * Multicopter attitude control app start / stop handling function + * + * @ingroup apps + */ +extern "C" __EXPORT int mc_att_control_vector_main(int argc, char *argv[]); + +class MulticopterAttitudeControl +{ +public: + /** + * Constructor + */ + MulticopterAttitudeControl(); + + /** + * Destructor, also kills the sensors task. + */ + ~MulticopterAttitudeControl(); + + /** + * Start the sensors task. + * + * @return OK on success. + */ + int start(); + +private: + + bool _task_should_exit; /**< if true, sensor task should exit */ + int _control_task; /**< task handle for sensor task */ + + int _att_sub; /**< vehicle attitude subscription */ + int _accel_sub; /**< accelerometer subscription */ + int _att_sp_sub; /**< vehicle attitude setpoint */ + int _attitude_sub; /**< raw rc channels data subscription */ + int _airspeed_sub; /**< airspeed subscription */ + int _vstatus_sub; /**< vehicle status subscription */ + int _params_sub; /**< notification of parameter updates */ + int _manual_sub; /**< notification of manual control updates */ + int _arming_sub; /**< arming status of outputs */ + + orb_advert_t _rate_sp_pub; /**< rate setpoint publication */ + orb_advert_t _actuators_0_pub; /**< actuator control group 0 setpoint */ + + struct vehicle_attitude_s _att; /**< vehicle attitude */ + struct accel_report _accel; /**< body frame accelerations */ + struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */ + struct manual_control_setpoint_s _manual; /**< r/c channel data */ + struct airspeed_s _airspeed; /**< airspeed */ + struct vehicle_status_s _vstatus; /**< vehicle status */ + struct actuator_controls_s _actuators; /**< actuator control inputs */ + struct actuator_armed_s _arming; /**< actuator arming status */ + + perf_counter_t _loop_perf; /**< loop performance counter */ + + bool _setpoint_valid; /**< flag if the position control setpoint is valid */ + bool _airspeed_valid; /**< flag if the airspeed measurement is valid */ + + // ECL_L1_Pos_Control _att_control; + + struct { + float yaw_p; + float yaw_i; + float yaw_d; + float yaw_imax; + + float att_p; + float att_i; + float att_d; + float att_imax; + + float att_rate_p; + + float yaw_rate_p; + } _parameters; /**< local copies of interesting parameters */ + + struct { + param_t yaw_p; + param_t yaw_i; + param_t yaw_d; + param_t yaw_imax; + + param_t att_p; + param_t att_i; + param_t att_d; + param_t att_imax; + + param_t att_rate_p; + + param_t yaw_rate_p; + } _parameter_handles; /**< handles for interesting parameters */ + + + ECL_MCAttControlVector _att_control; + + /** + * Update our local parameter cache. + */ + int parameters_update(); + + /** + * Update control outputs + * + */ + void control_update(); + + /** + * Check for changes in vehicle status. + */ + void vehicle_status_poll(); + + /** + * Check for changes in manual inputs. + */ + void vehicle_manual_poll(); + + + /** + * Check for airspeed updates. + */ + bool vehicle_airspeed_poll(); + + /** + * Check for accel updates. + */ + void vehicle_accel_poll(); + + /** + * Check for set triplet updates. + */ + void vehicle_setpoint_poll(); + + /** + * Check for arming status updates. + */ + void arming_status_poll(); + + /** + * Shim for calling task_main from task_create. + */ + static void task_main_trampoline(int argc, char *argv[]); + + /** + * Main sensor collection task. + */ + void task_main() __attribute__((noreturn)); +}; + +namespace att_control +{ + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; + +MulticopterAttitudeControl *g_control; +} + +MulticopterAttitudeControl::MulticopterAttitudeControl() : + + _task_should_exit(false), + _control_task(-1), + +/* subscriptions */ + _att_sub(-1), + _accel_sub(-1), + _airspeed_sub(-1), + _vstatus_sub(-1), + _params_sub(-1), + _manual_sub(-1), + _arming_sub(-1), + +/* publications */ + _rate_sp_pub(-1), + _actuators_0_pub(-1), + +/* performance counters */ + _loop_perf(perf_alloc(PC_ELAPSED, "fw att control")), +/* states */ + _setpoint_valid(false), + _airspeed_valid(false) +{ + _parameter_handles.yaw_p = param_find("MC_YAWPOS_P"); + _parameter_handles.yaw_i = param_find("MC_YAWPOS_I"); + _parameter_handles.yaw_d = param_find("MC_YAWPOS_D"); + _parameter_handles.yaw_imax = param_find("MC_YAWPOS_IMAX"); + + _parameter_handles.att_p = param_find("MC_ATT_P"); + _parameter_handles.att_i = param_find("MC_ATT_I"); + _parameter_handles.att_d = param_find("MC_ATT_D"); + _parameter_handles.att_imax = param_find("MC_ATT_IMAX"); + + _parameter_handles.att_rate_p = param_find("MC_ATTRATE_P"); + _parameter_handles.yaw_rate_p = param_find("MC_YAWRATE_P"); + + /* fetch initial parameter values */ + parameters_update(); +} + +MulticopterAttitudeControl::~MulticopterAttitudeControl() +{ + if (_control_task != -1) { + + /* task wakes up every 100ms or so at the longest */ + _task_should_exit = true; + + /* wait for a second for the task to quit at our request */ + unsigned i = 0; + + do { + /* wait 20ms */ + usleep(20000); + + /* if we have given up, kill it */ + if (++i > 50) { + task_delete(_control_task); + break; + } + } while (_control_task != -1); + } + + att_control::g_control = nullptr; +} + +int +MulticopterAttitudeControl::parameters_update() +{ + + //param_get(_parameter_handles.tconst, &(_parameters.tconst)); + + param_get(_parameter_handles.yaw_p, &(_parameters.yaw_p)); + param_get(_parameter_handles.yaw_i, &(_parameters.yaw_i)); + param_get(_parameter_handles.yaw_d, &(_parameters.yaw_d)); + param_get(_parameter_handles.yaw_imax, &(_parameters.yaw_imax)); + + param_get(_parameter_handles.att_p, &(_parameters.att_p)); + param_get(_parameter_handles.att_i, &(_parameters.att_i)); + param_get(_parameter_handles.att_d, &(_parameters.att_d)); + param_get(_parameter_handles.att_imax, &(_parameters.att_imax)); + + param_get(_parameter_handles.yaw_rate_p, &(_parameters.yaw_rate_p)); + + param_get(_parameter_handles.att_rate_p, &(_parameters.att_rate_p)); + + /* class control parameters */ + // _att_ctrl.set_tau(_parameters.p_tconst); + // _att_ctrl.set_k_p(math::radians(_parameters.p_p)); + // _att_ctrl.set_k_i(math::radians(_parameters.p_i)); + // _att_ctrl.set_k_d(math::radians(_parameters.p_d)); + + return OK; +} + +void +MulticopterAttitudeControl::vehicle_status_poll() +{ + bool vstatus_updated; + + /* Check HIL state if vehicle status has changed */ + orb_check(_vstatus_sub, &vstatus_updated); + + if (vstatus_updated) { + + orb_copy(ORB_ID(vehicle_status), _vstatus_sub, &_vstatus); + } +} + +void +MulticopterAttitudeControl::vehicle_manual_poll() +{ + bool manual_updated; + + /* get pilots inputs */ + orb_check(_manual_sub, &manual_updated); + + if (manual_updated) { + + orb_copy(ORB_ID(manual_control_setpoint), _manual_sub, &_manual); + } +} + +bool +MulticopterAttitudeControl::vehicle_airspeed_poll() +{ + /* check if there is a new position */ + bool airspeed_updated; + orb_check(_airspeed_sub, &airspeed_updated); + + if (airspeed_updated) { + orb_copy(ORB_ID(airspeed), _airspeed_sub, &_airspeed); + return true; + } + + return false; +} + +void +MulticopterAttitudeControl::vehicle_accel_poll() +{ + /* check if there is a new position */ + bool accel_updated; + orb_check(_accel_sub, &accel_updated); + + if (accel_updated) { + orb_copy(ORB_ID(sensor_accel), _accel_sub, &_accel); + } +} + +void +MulticopterAttitudeControl::vehicle_setpoint_poll() +{ + /* check if there is a new setpoint */ + bool att_sp_updated; + orb_check(_att_sp_sub, &att_sp_updated); + + if (att_sp_updated) { + orb_copy(ORB_ID(vehicle_attitude_setpoint), _att_sp_sub, &_att_sp); + _setpoint_valid = true; + } +} + +void +MulticopterAttitudeControl::arming_status_poll() +{ + /* check if there is a new setpoint */ + bool arming_updated; + orb_check(_arming_sub, &arming_updated); + + if (arming_updated) { + orb_copy(ORB_ID(actuator_armed), _arming_sub, &_arming); + } +} + +void +MulticopterAttitudeControl::task_main_trampoline(int argc, char *argv[]) +{ + att_control::g_control->task_main(); +} + +void +MulticopterAttitudeControl::task_main() +{ + + /* inform about start */ + warnx("Initializing.."); + fflush(stdout); + + /* + * do subscriptions + */ + _att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); + _att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); + _accel_sub = orb_subscribe(ORB_ID(sensor_accel)); + _airspeed_sub = orb_subscribe(ORB_ID(airspeed)); + _vstatus_sub = orb_subscribe(ORB_ID(vehicle_status)); + _params_sub = orb_subscribe(ORB_ID(parameter_update)); + _manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); + _arming_sub = orb_subscribe(ORB_ID(actuator_armed)); + + /* rate limit vehicle status updates to 5Hz */ + orb_set_interval(_vstatus_sub, 200); + orb_set_interval(_att_sub, 100); + + parameters_update(); + + /* initialize values of critical structs until first regular update */ + _arming.armed = false; + + /* get an initial update for all sensor and status data */ + (void)vehicle_airspeed_poll(); + vehicle_setpoint_poll(); + vehicle_accel_poll(); + vehicle_status_poll(); + vehicle_manual_poll(); + arming_status_poll(); + + /* wakeup source(s) */ + struct pollfd fds[2]; + + /* Setup of loop */ + fds[0].fd = _params_sub; + fds[0].events = POLLIN; + fds[1].fd = _att_sub; + fds[1].events = POLLIN; + + while (!_task_should_exit) { + + /* wait for up to 500ms for data */ + int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100); + + /* timed out - periodic check for _task_should_exit, etc. */ + if (pret == 0) + continue; + + /* this is undesirable but not much we can do - might want to flag unhappy status */ + if (pret < 0) { + warn("poll error %d, %d", pret, errno); + continue; + } + + perf_begin(_loop_perf); + + /* only update parameters if they changed */ + if (fds[0].revents & POLLIN) { + /* read from param to clear updated flag */ + struct parameter_update_s update; + orb_copy(ORB_ID(parameter_update), _params_sub, &update); + + /* update parameters from storage */ + parameters_update(); + } + + /* only run controller if attitude changed */ + if (fds[1].revents & POLLIN) { + + + static uint64_t last_run = 0; + float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f; + last_run = hrt_absolute_time(); + + /* guard against too large deltaT's */ + if (deltaT > 1.0f) + deltaT = 0.01f; + + /* load local copies */ + orb_copy(ORB_ID(vehicle_attitude), _att_sub, &_att); + + _airspeed_valid = vehicle_airspeed_poll(); + + vehicle_setpoint_poll(); + + vehicle_accel_poll(); + + /* check vehicle status for changes to publication state */ + vehicle_status_poll(); + + /* check for arming status changes */ + arming_status_poll(); + + vehicle_manual_poll(); + + /* lock integrator until armed */ + bool lock_integrator; + if (_arming.armed) { + lock_integrator = false; + } else { + lock_integrator = true; + } + + /* decide if in auto or full manual control */ + float roll_sp, pitch_sp; + float throttle_sp = 0.0f; + float yaw_sp = 0.0f; + + if (_vstatus.state_machine == SYSTEM_STATE_MANUAL || + (_vstatus.state_machine == SYSTEM_STATE_STABILIZED)) { + + /* + * Scale down roll and pitch as the setpoints are radians + * and a typical remote can only do 45 degrees, the mapping is + * -1..+1 to -45..+45 degrees or -0.75..+0.75 radians. + * + * With this mapping the stick angle is a 1:1 representation of + * the commanded attitude. If more than 45 degrees are desired, + * a scaling parameter can be applied to the remote. + */ + roll_sp = _manual.roll * 0.75f; + pitch_sp = _manual.pitch * 0.75f; + yaw_sp = _manual.yaw; + throttle_sp = _manual.throttle; + + } else if (_vstatus.state_machine == SYSTEM_STATE_AUTO) { + + roll_sp = _att_sp.roll_body; + pitch_sp = _att_sp.pitch_body; + yaw_sp = _att_sp.yaw_body; + throttle_sp = _att_sp.thrust; + } + + // XXX take rotation matrix directly from att_sp for auto mode + math::Vector3 F_des(roll_sp, pitch_sp, yaw_sp); + + math::Vector3 rates_des; + + math::Dcm R_nb(_att.R); + math::Vector3 angular_rates(_att.rollspeed, _att.pitchspeed, _att.yawspeed); + + _att_control.control(deltaT, R_nb, _att.yaw, F_des, + _parameters.att_p, _parameters.att_d, _parameters.att_i, + angular_rates, rates_des, throttle_sp); + + float roll_out = _parameters.att_rate_p * rates_des(0); + float pitch_out = _parameters.att_rate_p * rates_des(1); + float yaw_out = _parameters.yaw_rate_p * rates_des(2); + + _actuators.control[0] = (isfinite(roll_out)) ? roll_out : 0.0f; + _actuators.control[1] = (isfinite(pitch_out)) ? pitch_out : 0.0f; + _actuators.control[2] = (isfinite(yaw_out)) ? yaw_out : 0.0f; + + /* throttle passed through */ + _actuators.control[3] = (isfinite(throttle_sp)) ? throttle_sp : 0.0f; + + /* + * Lazily publish the rate setpoint (for analysis, the actuators are published below) + * only once available + */ + vehicle_rates_setpoint_s rates_sp; + rates_sp.roll = rates_des(0); + rates_sp.pitch = rates_des(1); + rates_sp.yaw = rates_des(2); + + rates_sp.timestamp = hrt_absolute_time(); + + if (_rate_sp_pub > 0) { + /* publish the attitude setpoint */ + orb_publish(ORB_ID(vehicle_rates_setpoint), _actuators_0_pub, &rates_sp); + + } else { + /* advertise and publish */ + _rate_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &rates_sp); + } + + /* lazily publish the setpoint only once available */ + _actuators.timestamp = hrt_absolute_time(); + + if (_actuators_0_pub > 0) { + /* publish the attitude setpoint */ + orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators); + + } else { + /* advertise and publish */ + _actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators); + } + + } + + perf_end(_loop_perf); + } + + warnx("exiting.\n"); + + _control_task = -1; + _exit(0); +} + +int +MulticopterAttitudeControl::start() +{ + ASSERT(_control_task == -1); + + /* start the task */ + _control_task = task_spawn_cmd("mc_att_control_vector", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 2048, + (main_t)&MulticopterAttitudeControl::task_main_trampoline, + nullptr); + + if (_control_task < 0) { + warn("task start failed"); + return -errno; + } + + return OK; +} + +int mc_att_control_vector_main(int argc, char *argv[]) +{ + if (argc < 1) + errx(1, "usage: mc_att_control_vector {start|stop|status}"); + + if (!strcmp(argv[1], "start")) { + + if (att_control::g_control != nullptr) + errx(1, "already running"); + + att_control::g_control = new MulticopterAttitudeControl; + + if (att_control::g_control == nullptr) + errx(1, "alloc failed"); + + if (OK != att_control::g_control->start()) { + delete att_control::g_control; + att_control::g_control = nullptr; + err(1, "start failed"); + } + + exit(0); + } + + if (!strcmp(argv[1], "stop")) { + if (att_control::g_control == nullptr) + errx(1, "not running"); + + delete att_control::g_control; + att_control::g_control = nullptr; + exit(0); + } + + if (!strcmp(argv[1], "status")) { + if (att_control::g_control) { + errx(0, "running"); + + } else { + errx(1, "not running"); + } + } + + warnx("unrecognized command"); + return 1; +} diff --git a/src/modules/mc_att_control_vector/module.mk b/src/modules/mc_att_control_vector/module.mk new file mode 100644 index 0000000000..61eb3b3fdf --- /dev/null +++ b/src/modules/mc_att_control_vector/module.mk @@ -0,0 +1,41 @@ +############################################################################ +# +# Copyright (c) 2012, 2013 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. +# +############################################################################ + +# +# Multirotor attitude controller (vector based, no Euler singularities) +# + +MODULE_COMMAND = mc_att_control_vector + +SRCS = mc_att_control_vector_main.cpp \ + ecl_mc_att_control_vector.cpp From 02d918b46d20d096e54379ea30762be55f8a3da9 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 11 Jul 2013 23:22:56 +0200 Subject: [PATCH 002/447] Removed airspeed --- .../mc_att_control_vector_main.cpp | 35 +------------------ 1 file changed, 1 insertion(+), 34 deletions(-) diff --git a/src/modules/mc_att_control_vector/mc_att_control_vector_main.cpp b/src/modules/mc_att_control_vector/mc_att_control_vector_main.cpp index f3f8235f0e..5b4a3b6000 100644 --- a/src/modules/mc_att_control_vector/mc_att_control_vector_main.cpp +++ b/src/modules/mc_att_control_vector/mc_att_control_vector_main.cpp @@ -57,7 +57,6 @@ #include #include #include -#include #include #include #include @@ -112,7 +111,6 @@ private: int _accel_sub; /**< accelerometer subscription */ int _att_sp_sub; /**< vehicle attitude setpoint */ int _attitude_sub; /**< raw rc channels data subscription */ - int _airspeed_sub; /**< airspeed subscription */ int _vstatus_sub; /**< vehicle status subscription */ int _params_sub; /**< notification of parameter updates */ int _manual_sub; /**< notification of manual control updates */ @@ -125,7 +123,6 @@ private: struct accel_report _accel; /**< body frame accelerations */ struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */ struct manual_control_setpoint_s _manual; /**< r/c channel data */ - struct airspeed_s _airspeed; /**< airspeed */ struct vehicle_status_s _vstatus; /**< vehicle status */ struct actuator_controls_s _actuators; /**< actuator control inputs */ struct actuator_armed_s _arming; /**< actuator arming status */ @@ -133,9 +130,6 @@ private: perf_counter_t _loop_perf; /**< loop performance counter */ bool _setpoint_valid; /**< flag if the position control setpoint is valid */ - bool _airspeed_valid; /**< flag if the airspeed measurement is valid */ - - // ECL_L1_Pos_Control _att_control; struct { float yaw_p; @@ -193,12 +187,6 @@ private: */ void vehicle_manual_poll(); - - /** - * Check for airspeed updates. - */ - bool vehicle_airspeed_poll(); - /** * Check for accel updates. */ @@ -245,7 +233,6 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : /* subscriptions */ _att_sub(-1), _accel_sub(-1), - _airspeed_sub(-1), _vstatus_sub(-1), _params_sub(-1), _manual_sub(-1), @@ -258,8 +245,7 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "fw att control")), /* states */ - _setpoint_valid(false), - _airspeed_valid(false) + _setpoint_valid(false) { _parameter_handles.yaw_p = param_find("MC_YAWPOS_P"); _parameter_handles.yaw_i = param_find("MC_YAWPOS_I"); @@ -360,21 +346,6 @@ MulticopterAttitudeControl::vehicle_manual_poll() } } -bool -MulticopterAttitudeControl::vehicle_airspeed_poll() -{ - /* check if there is a new position */ - bool airspeed_updated; - orb_check(_airspeed_sub, &airspeed_updated); - - if (airspeed_updated) { - orb_copy(ORB_ID(airspeed), _airspeed_sub, &_airspeed); - return true; - } - - return false; -} - void MulticopterAttitudeControl::vehicle_accel_poll() { @@ -432,7 +403,6 @@ MulticopterAttitudeControl::task_main() _att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); _att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); _accel_sub = orb_subscribe(ORB_ID(sensor_accel)); - _airspeed_sub = orb_subscribe(ORB_ID(airspeed)); _vstatus_sub = orb_subscribe(ORB_ID(vehicle_status)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); _manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); @@ -448,7 +418,6 @@ MulticopterAttitudeControl::task_main() _arming.armed = false; /* get an initial update for all sensor and status data */ - (void)vehicle_airspeed_poll(); vehicle_setpoint_poll(); vehicle_accel_poll(); vehicle_status_poll(); @@ -506,8 +475,6 @@ MulticopterAttitudeControl::task_main() /* load local copies */ orb_copy(ORB_ID(vehicle_attitude), _att_sub, &_att); - _airspeed_valid = vehicle_airspeed_poll(); - vehicle_setpoint_poll(); vehicle_accel_poll(); From d999a376da9eff802831a5866b4be1890f83ca01 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 11 Jul 2013 23:23:55 +0200 Subject: [PATCH 003/447] Enabled imax params --- .../multirotor_att_control/multirotor_attitude_control.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/multirotor_att_control/multirotor_attitude_control.c b/src/modules/multirotor_att_control/multirotor_attitude_control.c index 8f19c6a4bc..af5bac88a5 100644 --- a/src/modules/multirotor_att_control/multirotor_attitude_control.c +++ b/src/modules/multirotor_att_control/multirotor_attitude_control.c @@ -65,13 +65,13 @@ PARAM_DEFINE_FLOAT(MC_YAWPOS_P, 0.3f); PARAM_DEFINE_FLOAT(MC_YAWPOS_I, 0.15f); PARAM_DEFINE_FLOAT(MC_YAWPOS_D, 0.0f); -//PARAM_DEFINE_FLOAT(MC_YAWPOS_AWU, 1.0f); +PARAM_DEFINE_FLOAT(MC_YAWPOS_IMAX, 1.0f); //PARAM_DEFINE_FLOAT(MC_YAWPOS_LIM, 3.0f); PARAM_DEFINE_FLOAT(MC_ATT_P, 0.2f); PARAM_DEFINE_FLOAT(MC_ATT_I, 0.0f); PARAM_DEFINE_FLOAT(MC_ATT_D, 0.05f); -//PARAM_DEFINE_FLOAT(MC_ATT_AWU, 0.05f); +PARAM_DEFINE_FLOAT(MC_ATT_IMAX, 0.05f); //PARAM_DEFINE_FLOAT(MC_ATT_LIM, 0.4f); //PARAM_DEFINE_FLOAT(MC_ATT_XOFF, 0.0f); From f6b3692696b0ab0cce6bf5e659e72b1d12013745 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 12 Jul 2013 09:09:11 +0200 Subject: [PATCH 004/447] Minor fixes and cleanups, ready for first bench tests --- .../ecl_mc_att_control_vector.cpp | 21 +++++++++---------- .../ecl_mc_att_control_vector.h | 16 ++++++++++++++ .../mc_att_control_vector_main.cpp | 17 ++++++++------- 3 files changed, 35 insertions(+), 19 deletions(-) diff --git a/src/modules/mc_att_control_vector/ecl_mc_att_control_vector.cpp b/src/modules/mc_att_control_vector/ecl_mc_att_control_vector.cpp index b21b0cf67e..8bee18ba49 100644 --- a/src/modules/mc_att_control_vector/ecl_mc_att_control_vector.cpp +++ b/src/modules/mc_att_control_vector/ecl_mc_att_control_vector.cpp @@ -48,20 +48,20 @@ #include #include "ecl_mc_att_control_vector.h" +ECL_MCAttControlVector::ECL_MCAttControlVector() : + _integral_error(0.0f, 0.0f), + _integral_max(1000.0f), + _integral_lock(false) + { + + } + void ECL_MCAttControlVector::control(float dt, const math::Dcm &R_nb, float yaw, const math::Vector &F_des_in, float Kp, float Kd, float Ki, const math::Vector &angular_rates, math::Vector &rates_des, float &thrust) { - //function [rates_des,Thrust,ie_out] = attitudeController(R1,yaw,F_des,Kp,Kd,Ki,e_Rd_v,ie) - //% dt - //dt=1/50; - - //% shoult the controll be in the body frame or in the inertial frame: 0 is - //% body frame, 1 is inertial frame. - // XXX bool earth = true; - bool integral_freeze = false; bool integral_reset = false; math::Matrix R_bn = R_nb.transpose(); @@ -70,7 +70,6 @@ void ECL_MCAttControlVector::control(float dt, const math::Dcm &R_nb, float yaw, float sy = sinf(yaw); math::Matrix RYaw = math::Dcm(cy,-sy,0.0f,sy,cy,0.0f,0.0f,0.0f,1.0f); - //%F_des=RYaw*F_des; math::Vector z_b = math::Vector3(R_bn(0,2), R_bn(1,2), R_bn(2,2)); math::Vector3 F_des = F_des_in; @@ -97,7 +96,7 @@ void ECL_MCAttControlVector::control(float dt, const math::Dcm &R_nb, float yaw, // desired direction in world coordinates (yaw angle) math::Vector3 x_C(cy, sy, 0.0f); - //desired body y axis + // desired body y axis y_B_des = z_B_des.cross(x_C) / (z_B_des.cross(x_C)).norm(); // desired body x axis x_B_des = y_B_des.cross(z_B_des); @@ -122,7 +121,7 @@ void ECL_MCAttControlVector::control(float dt, const math::Dcm &R_nb, float yaw, // include an integral part math::Vector intError = math::Vector3(0.0f, 0.0f, 0.0f); - if (!integral_freeze) { + if (!_integral_lock) { if (thrust > 0.3f && !integral_reset) { if (fabsf(_integral_error(0)) < _integral_max(0)) { _integral_error(0) = _integral_error(0) + e_R_v(0) * dt; diff --git a/src/modules/mc_att_control_vector/ecl_mc_att_control_vector.h b/src/modules/mc_att_control_vector/ecl_mc_att_control_vector.h index 98689f0bee..b975f2f7f5 100644 --- a/src/modules/mc_att_control_vector/ecl_mc_att_control_vector.h +++ b/src/modules/mc_att_control_vector/ecl_mc_att_control_vector.h @@ -50,11 +50,27 @@ class ECL_MCAttControlVector { public: + ECL_MCAttControlVector(float integral_max); void control(float dt, const math::Dcm &R_nb, float yaw, const math::Vector &F_des, float Kp, float Kd, float Ki, const math::Vector &angular_rates, math::Vector &rates_des, float &thrust); + void set_imax(float integral_max) { + _integral_max(0) = integral_max; + _integral_max(1) = integral_max; + } + + void reset_integral() { + _integral_error(0) = 0.0f; + _integral_error(1) = 0.0f; + } + + void lock_integral(bool lock) { + _integral_lock = lock; + } + protected: math::Vector2 _integral_error; math::Vector2 _integral_max; + bool _integral_lock; }; diff --git a/src/modules/mc_att_control_vector/mc_att_control_vector_main.cpp b/src/modules/mc_att_control_vector/mc_att_control_vector_main.cpp index 5b4a3b6000..368d343bde 100644 --- a/src/modules/mc_att_control_vector/mc_att_control_vector_main.cpp +++ b/src/modules/mc_att_control_vector/mc_att_control_vector_main.cpp @@ -309,11 +309,12 @@ MulticopterAttitudeControl::parameters_update() param_get(_parameter_handles.att_rate_p, &(_parameters.att_rate_p)); - /* class control parameters */ - // _att_ctrl.set_tau(_parameters.p_tconst); - // _att_ctrl.set_k_p(math::radians(_parameters.p_p)); - // _att_ctrl.set_k_i(math::radians(_parameters.p_i)); - // _att_ctrl.set_k_d(math::radians(_parameters.p_d)); + /* att control parameters */ + _att_control.set_imax(_parameters.att_imax); + //_att_control.set_tau(_parameters.p_tconst); + // _att_control.set_k_p(math::radians(_parameters.p_p)); + // _att_control.set_k_i(math::radians(_parameters.p_i)); + // _att_control.set_k_d(math::radians(_parameters.p_d)); return OK; } @@ -488,11 +489,11 @@ MulticopterAttitudeControl::task_main() vehicle_manual_poll(); /* lock integrator until armed */ - bool lock_integrator; if (_arming.armed) { - lock_integrator = false; + _att_control.lock_integrator(false); } else { - lock_integrator = true; + _att_control.reset_integral(); + _att_control.lock_integrator(true); } /* decide if in auto or full manual control */ From 9a6bf91b93824eb694a01e1ed3db757fec6b906d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 12 Jul 2013 09:10:58 +0200 Subject: [PATCH 005/447] More comments --- src/modules/mc_att_control_vector/ecl_mc_att_control_vector.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/modules/mc_att_control_vector/ecl_mc_att_control_vector.cpp b/src/modules/mc_att_control_vector/ecl_mc_att_control_vector.cpp index 8bee18ba49..b0e010a1c0 100644 --- a/src/modules/mc_att_control_vector/ecl_mc_att_control_vector.cpp +++ b/src/modules/mc_att_control_vector/ecl_mc_att_control_vector.cpp @@ -76,6 +76,7 @@ void ECL_MCAttControlVector::control(float dt, const math::Dcm &R_nb, float yaw, // desired thrust in body frame // avoid division by zero + // compensates for thrust loss due to roll/pitch if (F_des(2) >= 0.1f) { thrust = F_des(2) / R_bn(2, 2); } else { From 5adfdfa8f9995a74e7779d9976c096d15c4da043 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 13 Jul 2013 18:40:48 +0200 Subject: [PATCH 006/447] Extended math lib --- src/modules/mathlib/math/Dcm.cpp | 5 +++++ src/modules/mathlib/math/Dcm.hpp | 5 +++++ 2 files changed, 10 insertions(+) diff --git a/src/modules/mathlib/math/Dcm.cpp b/src/modules/mathlib/math/Dcm.cpp index f509f7081b..5c15d4d6d9 100644 --- a/src/modules/mathlib/math/Dcm.cpp +++ b/src/modules/mathlib/math/Dcm.cpp @@ -135,6 +135,11 @@ Dcm::Dcm(const Dcm &right) : { } +Dcm::Dcm(const Matrix &right) : + Matrix(right) +{ +} + Dcm::~Dcm() { } diff --git a/src/modules/mathlib/math/Dcm.hpp b/src/modules/mathlib/math/Dcm.hpp index df8970d3aa..38f697c157 100644 --- a/src/modules/mathlib/math/Dcm.hpp +++ b/src/modules/mathlib/math/Dcm.hpp @@ -96,6 +96,11 @@ public: */ Dcm(const Dcm &right); + /** + * copy ctor (deep) + */ + Dcm(const Matrix &right); + /** * dtor */ From 67fcdae30a829feefab61649955904f5fd838c76 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 13 Jul 2013 18:41:09 +0200 Subject: [PATCH 007/447] Added fixed wing vector based control --- makefiles/config_px4fmu-v1_default.mk | 1 + .../ecl_fw_att_control_vector.cpp | 163 ++++ .../ecl_fw_att_control_vector.h | 115 +++ .../fw_att_control_vector_main.cpp | 763 ++++++++++++++++++ src/modules/fw_att_control_vector/module.mk | 41 + 5 files changed, 1083 insertions(+) create mode 100644 src/modules/fw_att_control_vector/ecl_fw_att_control_vector.cpp create mode 100644 src/modules/fw_att_control_vector/ecl_fw_att_control_vector.h create mode 100644 src/modules/fw_att_control_vector/fw_att_control_vector_main.cpp create mode 100644 src/modules/fw_att_control_vector/module.mk diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk index f3015b97e9..dcdb860379 100644 --- a/makefiles/config_px4fmu-v1_default.mk +++ b/makefiles/config_px4fmu-v1_default.mk @@ -73,6 +73,7 @@ MODULES += examples/flow_position_estimator MODULES += modules/fixedwing_backside MODULES += modules/fixedwing_att_control MODULES += modules/fixedwing_pos_control +MODULES += modules/fw_att_control_vector MODULES += modules/multirotor_att_control MODULES += modules/mc_att_control_vector MODULES += modules/multirotor_pos_control diff --git a/src/modules/fw_att_control_vector/ecl_fw_att_control_vector.cpp b/src/modules/fw_att_control_vector/ecl_fw_att_control_vector.cpp new file mode 100644 index 0000000000..d689d4eef5 --- /dev/null +++ b/src/modules/fw_att_control_vector/ecl_fw_att_control_vector.cpp @@ -0,0 +1,163 @@ +/**************************************************************************** + * + * Copyright (c) 2013 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 ecl_fw_att_control_vector.cpp + * + * Fixed wing attitude controller + * + * @author Lorenz Meier + * @author Tobias Naegeli + * + */ + +#include +#include +#include "ecl_fw_att_control_vector.h" + +ECL_FWAttControlVector::ECL_FWAttControlVector() : + _integral_error(0.0f, 0.0f), + _integral_max(1000.0f, 1000.0f), + _rates_demanded(0.0f, 0.0f, 0.0f), + _k_p(1.0f, 1.0f, 1.0f), + _k_d(1.0f, 1.0f, 1.0f), + _k_i(1.0f, 1.0f, 1.0f), + _integral_lock(false), + _p_airspeed_min(12.0f), + _p_airspeed_max(24.0f), + _p_tconst(0.1f), + _p_roll_ffd(1.0f), + _airspeed_enabled(false) + { + + } + +/** + * + * @param F_des_in Desired force vector in body frame (NED). Straight flight is (0 0 -1)', + * banking hard right (1 0 -1)' and pitching down (1 0 -1)'. + */ +void ECL_FWAttControlVector::control(float dt, float airspeed, float airspeed_scaling, const math::Dcm &R_nb, float roll, float pitch, float yaw, const math::Vector &F_des_in, + const math::Vector &angular_rates, + math::Vector &moment_des, float &thrust) +{ + if (!isfinite(airspeed) || !airspeed_enabled()) { + // If airspeed is not available or Inf/NaN, use the center + // of min / max + airspeed = 0.5f * (_p_airspeed_min + _p_airspeed_max); + } + + math::Dcm R_bn(R_nb.transpose()); + math::Matrix R_yaw_bn = math::Dcm(math::EulerAngles(0.0f, 0.0f, -yaw)); + + // Establish actuator signs and lift compensation + float lift_sign = (R_bn(3, 3) >= 0) ? 1.0f : -1.0f; + + // XXX temporary workaround + float lift_comp = fabsf((CONSTANTS_ONE_G / math::max(airspeed , float(_p_airspeed_min)) * tanf(roll) * sinf(roll))) * _p_roll_ffd; + + float cy = cosf(yaw); + float sy = sinf(yaw); + + //math::Matrix RYaw = math::Dcm(cy,-sy,0.0f,sy,cy,0.0f,0.0f,0.0f,1.0f); + math::Vector z_b = math::Vector3(R_bn(0,2), R_bn(1,2), R_bn(2,2)); + + math::Vector3 F_des = R_yaw_bn * F_des_in; + + // desired thrust in body frame + // avoid division by zero + // compensates for thrust loss due to roll/pitch + if (F_des(2) >= 0.1f) { + thrust = F_des(2) / R_bn(2, 2); + } else { + F_des(2) = 0.1f; + thrust= F_des(2) / R_bn(2, 2); + } + + math::Vector3 x_B_des; + math::Vector3 y_B_des; + math::Vector3 z_B_des; + + // desired body z axis + z_B_des = (F_des / F_des.norm()); + + // desired direction in world coordinates (yaw angle) + math::Vector3 x_C(cy, sy, 0.0f); + // desired body y axis + y_B_des = z_B_des.cross(x_C) / (z_B_des.cross(x_C)).norm(); + // desired body x axis + x_B_des = y_B_des.cross(z_B_des); + // desired Rotation Matrix + math::Dcm R_des(x_B_des(0), x_B_des(1), x_B_des(2), + y_B_des(0), y_B_des(1), y_B_des(2), + z_B_des(0), z_B_des(1), z_B_des(2)); + + + // Attitude Controller + // P controller + + // error rotation matrix + // operation executed in quaternion space to allow large differences + // XXX switch between operations based on difference, + // benchmark both options + math::Quaternion e_q = math::Quaternion(R_des) - math::Quaternion(R_bn); + math::Matrix e_R = math::Dcm(e_q); + //small angles: math::Matrix e_R = (R_des.transpose() * R_bn - R_bn.transpose() * R_des) * 0.5f; + + // error rotation vector + math::Vector e_R_v(3); + e_R_v(0) = e_R(1,2); + e_R_v(1) = e_R(0,2); + e_R_v(2) = e_R(0,1); + + + // attitude integral error + math::Vector intError = math::Vector3(0.0f, 0.0f, 0.0f); + if (!_integral_lock) { + + if (fabsf(_integral_error(0)) < _integral_max(0)) { + _integral_error(0) = _integral_error(0) + e_R_v(0) * dt; + } + + if (fabsf(_integral_error(1)) < _integral_max(1)) { + _integral_error(1) = _integral_error(1) + e_R_v(1) * dt; + } + + intError(0) = _integral_error(0); + intError(1) = _integral_error(1); + intError(2) = 0.0f; + } + + _rates_demanded = (e_R_v * _k_p(0) + angular_rates * _k_d(0) + intError * _k_i(0)); + moment_des = _rates_demanded; +} diff --git a/src/modules/fw_att_control_vector/ecl_fw_att_control_vector.h b/src/modules/fw_att_control_vector/ecl_fw_att_control_vector.h new file mode 100644 index 0000000000..40e4662a3f --- /dev/null +++ b/src/modules/fw_att_control_vector/ecl_fw_att_control_vector.h @@ -0,0 +1,115 @@ +/**************************************************************************** + * + * Copyright (c) 2013 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 ecl_fw_att_control_vector.cpp + * + * Fixed wing attitude controller + * + * @author Lorenz Meier + * @author Tobias Naegeli + * + */ + +#include + +class ECL_FWAttControlVector { + +public: + ECL_FWAttControlVector(); + void control(float dt, float airspeed, float airspeed_scaling, const math::Dcm &R_nb, float roll, float pitch, float yaw, const math::Vector &F_des_in, + const math::Vector &angular_rates, + math::Vector &moment_des, float &thrust); + + void set_imax(float integral_max) { + _integral_max(0) = integral_max; + _integral_max(1) = integral_max; + } + + void set_tconst(float tconst) { + _p_tconst = tconst; + } + + void set_k_p(float roll, float pitch, float yaw) { + _k_p(0) = roll; + _k_p(1) = pitch; + _k_p(2) = yaw; + } + + void set_k_d(float roll, float pitch, float yaw) { + _k_d(0) = roll; + _k_d(1) = pitch; + _k_d(2) = yaw; + } + + void set_k_i(float roll, float pitch, float yaw) { + _k_i(0) = roll; + _k_i(1) = pitch; + _k_i(2) = yaw; + } + + void reset_integral() { + _integral_error(0) = 0.0f; + _integral_error(1) = 0.0f; + } + + void lock_integral(bool lock) { + _integral_lock = lock; + } + + bool airspeed_enabled() { + return _airspeed_enabled; + } + + void enable_airspeed(bool airspeed) { + _airspeed_enabled = airspeed; + } + + math::Vector3 get_rates_des() { + return _rates_demanded; + } + +protected: + math::Vector2f _integral_error; + math::Vector2f _integral_max; + math::Vector3 _rates_demanded; + math::Vector3 _k_p; + math::Vector3 _k_d; + math::Vector3 _k_i; + bool _integral_lock; + float _p_airspeed_min; + float _p_airspeed_max; + float _p_tconst; + float _p_roll_ffd; + bool _airspeed_enabled; +}; diff --git a/src/modules/fw_att_control_vector/fw_att_control_vector_main.cpp b/src/modules/fw_att_control_vector/fw_att_control_vector_main.cpp new file mode 100644 index 0000000000..a06bc9cb62 --- /dev/null +++ b/src/modules/fw_att_control_vector/fw_att_control_vector_main.cpp @@ -0,0 +1,763 @@ +/**************************************************************************** + * + * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * Author: Lorenz Meier + * + * 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 fw_att_control_vector_main.c + * Implementation of a generic attitude controller based on classic orthogonal PIDs. + * + * @author Lorenz Meier + * + * Please refer to the library files for the authors and acknowledgements of + * the used control library functions. + */ + +#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 + +#include "ecl_fw_att_control_vector.h" + +/** + * Fixedwing attitude control app start / stop handling function + * + * @ingroup apps + */ +extern "C" __EXPORT int fw_att_control_vector_main(int argc, char *argv[]); + +class FixedwingAttitudeControlVector +{ +public: + /** + * Constructor + */ + FixedwingAttitudeControlVector(); + + /** + * Destructor, also kills the sensors task. + */ + ~FixedwingAttitudeControlVector(); + + /** + * Start the sensors task. + * + * @return OK on success. + */ + int start(); + +private: + + bool _task_should_exit; /**< if true, sensor task should exit */ + int _control_task; /**< task handle for sensor task */ + + int _att_sub; /**< vehicle attitude subscription */ + int _accel_sub; /**< accelerometer subscription */ + int _att_sp_sub; /**< vehicle attitude setpoint */ + int _attitude_sub; /**< raw rc channels data subscription */ + int _airspeed_sub; /**< airspeed subscription */ + int _vstatus_sub; /**< vehicle status subscription */ + int _params_sub; /**< notification of parameter updates */ + int _manual_control_sub; /**< notification of manual control updates */ + int _arming_sub; /**< arming status of outputs */ + + orb_advert_t _rate_sp_pub; /**< rate setpoint publication */ + orb_advert_t _actuators_0_pub; /**< actuator control group 0 setpoint */ + + struct vehicle_attitude_s _att; /**< vehicle attitude */ + struct accel_report _accel; /**< body frame accelerations */ + struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */ + struct manual_control_setpoint_s _manual; /**< r/c channel data */ + struct airspeed_s _airspeed; /**< airspeed */ + struct vehicle_status_s _vstatus; /**< vehicle status */ + struct actuator_controls_s _actuators; /**< actuator control inputs */ + struct actuator_armed_s _arming; /**< actuator arming status */ + + perf_counter_t _loop_perf; /**< loop performance counter */ + + bool _setpoint_valid; /**< flag if the position control setpoint is valid */ + bool _airspeed_valid; /**< flag if the airspeed measurement is valid */ + + struct { + + float p_tconst; + float p_p; + float p_d; + float p_i; + float p_rmax_up; + float p_rmax_dn; + float p_imax; + float p_rll; + float r_tconst; + float r_p; + float r_d; + float r_i; + float r_imax; + float r_rmax; + float y_slip; + float y_int; + float y_damp; + float y_rll; + float y_imax; + + float airspeed_min; + float airspeed_trim; + float airspeed_max; + } _parameters; /**< local copies of interesting parameters */ + + struct { + + param_t p_tconst; + param_t p_p; + param_t p_d; + param_t p_i; + param_t p_rmax_up; + param_t p_rmax_dn; + param_t p_imax; + param_t p_rll; + param_t r_tconst; + param_t r_p; + param_t r_d; + param_t r_i; + param_t r_imax; + param_t r_rmax; + param_t y_slip; + param_t y_int; + param_t y_damp; + param_t y_rll; + param_t y_imax; + + param_t airspeed_min; + param_t airspeed_trim; + param_t airspeed_max; + } _parameter_handles; /**< handles for interesting parameters */ + + + ECL_FWAttControlVector _att_control; + + + /** + * Update our local parameter cache. + */ + int parameters_update(); + + /** + * Update control outputs + * + */ + void control_update(); + + /** + * Check for changes in vehicle status. + */ + void vehicle_status_poll(); + + /** + * Check for airspeed updates. + */ + bool vehicle_airspeed_poll(); + + /** + * Check for accel updates. + */ + void vehicle_accel_poll(); + + /** + * Check for set triplet updates. + */ + void vehicle_setpoint_poll(); + + /** + * Check for arming status updates. + */ + void arming_status_poll(); + + /** + * Shim for calling task_main from task_create. + */ + static void task_main_trampoline(int argc, char *argv[]); + + /** + * Main sensor collection task. + */ + void task_main() __attribute__((noreturn)); +}; + +namespace att_control +{ + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; + +FixedwingAttitudeControlVector *g_control; +} + +FixedwingAttitudeControlVector::FixedwingAttitudeControlVector() : + + _task_should_exit(false), + _control_task(-1), + +/* subscriptions */ + _att_sub(-1), + _accel_sub(-1), + _airspeed_sub(-1), + _vstatus_sub(-1), + _params_sub(-1), + _manual_control_sub(-1), + _arming_sub(-1), + +/* publications */ + _rate_sp_pub(-1), + _actuators_0_pub(-1), + +/* performance counters */ + _loop_perf(perf_alloc(PC_ELAPSED, "fw att control")), +/* states */ + _setpoint_valid(false), + _airspeed_valid(false) +{ + // _parameter_handles.roll_p = param_find("FW_ROLL_P"); + // _parameter_handles.pitch_p = param_find("FW_PITCH_P"); + _parameter_handles.p_tconst = param_find("FW_P_TCONST"); + _parameter_handles.p_p = param_find("FW_P_P"); + _parameter_handles.p_d = param_find("FW_P_D"); + _parameter_handles.p_i = param_find("FW_P_I"); + _parameter_handles.p_rmax_up = param_find("FW_P_RMAX_UP"); + _parameter_handles.p_rmax_dn = param_find("FW_P_RMAX_DN"); + _parameter_handles.p_imax = param_find("FW_P_IMAX"); + _parameter_handles.p_rll = param_find("FW_P_RLL"); + + _parameter_handles.r_tconst = param_find("FW_R_TCONST"); + _parameter_handles.r_p = param_find("FW_R_P"); + _parameter_handles.r_d = param_find("FW_R_D"); + _parameter_handles.r_i = param_find("FW_R_I"); + _parameter_handles.r_imax = param_find("FW_R_IMAX"); + _parameter_handles.r_rmax = param_find("FW_R_RMAX"); + + _parameter_handles.y_slip = param_find("FW_Y_SLIP"); + _parameter_handles.y_int = param_find("FW_Y_INT"); + _parameter_handles.y_damp = param_find("FW_Y_DAMP"); + _parameter_handles.y_rll = param_find("FW_Y_RLL"); + _parameter_handles.y_imax = param_find("FW_Y_IMAX"); + + _parameter_handles.airspeed_min = param_find("FW_AIRSPD_MIN"); + _parameter_handles.airspeed_trim = param_find("FW_AIRSPD_TRIM"); + _parameter_handles.airspeed_max = param_find("FW_AIRSPD_MAX"); + + /* fetch initial parameter values */ + parameters_update(); +} + +FixedwingAttitudeControlVector::~FixedwingAttitudeControlVector() +{ + if (_control_task != -1) { + + /* task wakes up every 100ms or so at the longest */ + _task_should_exit = true; + + /* wait for a second for the task to quit at our request */ + unsigned i = 0; + + do { + /* wait 20ms */ + usleep(20000); + + /* if we have given up, kill it */ + if (++i > 50) { + task_delete(_control_task); + break; + } + } while (_control_task != -1); + } + + att_control::g_control = nullptr; +} + +int +FixedwingAttitudeControlVector::parameters_update() +{ + + param_get(_parameter_handles.p_tconst, &(_parameters.p_tconst)); + param_get(_parameter_handles.p_p, &(_parameters.p_p)); + param_get(_parameter_handles.p_d, &(_parameters.p_d)); + param_get(_parameter_handles.p_i, &(_parameters.p_i)); + param_get(_parameter_handles.p_rmax_up, &(_parameters.p_rmax_up)); + param_get(_parameter_handles.p_rmax_dn, &(_parameters.p_rmax_dn)); + param_get(_parameter_handles.p_imax, &(_parameters.p_imax)); + param_get(_parameter_handles.p_rll, &(_parameters.p_rll)); + + param_get(_parameter_handles.r_tconst, &(_parameters.r_tconst)); + param_get(_parameter_handles.r_p, &(_parameters.r_p)); + param_get(_parameter_handles.r_d, &(_parameters.r_d)); + param_get(_parameter_handles.r_i, &(_parameters.r_i)); + param_get(_parameter_handles.r_imax, &(_parameters.r_imax)); + param_get(_parameter_handles.r_rmax, &(_parameters.r_rmax)); + + param_get(_parameter_handles.y_slip, &(_parameters.y_slip)); + param_get(_parameter_handles.y_int, &(_parameters.y_int)); + param_get(_parameter_handles.y_damp, &(_parameters.y_damp)); + param_get(_parameter_handles.y_rll, &(_parameters.y_rll)); + param_get(_parameter_handles.y_imax, &(_parameters.y_imax)); + + param_get(_parameter_handles.airspeed_min, &(_parameters.airspeed_min)); + param_get(_parameter_handles.airspeed_trim, &(_parameters.airspeed_trim)); + param_get(_parameter_handles.airspeed_max, &(_parameters.airspeed_max)); + + /* pitch control parameters */ + _att_control.set_tconst(_parameters.p_tconst); + // _pitch_ctrl.set_k_p(math::radians(_parameters.p_p)); + // _pitch_ctrl.set_k_i(math::radians(_parameters.p_i)); + // _pitch_ctrl.set_k_d(math::radians(_parameters.p_d)); + // _pitch_ctrl.set_imax(math::radians(_parameters.p_imax)); + // _pitch_ctrl.set_max_rate_pos(math::radians(_parameters.p_rmax_up)); + // _pitch_ctrl.set_max_rate_neg(math::radians(_parameters.p_rmax_dn)); + // _pitch_ctrl.set_roll_ff(math::radians(_parameters.p_rll)); + + // /* roll control parameters */ + // _roll_ctrl.set_tau(_parameters.r_tconst); + // _roll_ctrl.set_k_p(math::radians(_parameters.r_p)); + // _roll_ctrl.set_k_i(math::radians(_parameters.r_i)); + // _roll_ctrl.set_k_d(math::radians(_parameters.r_d)); + // _roll_ctrl.set_imax(math::radians(_parameters.r_imax)); + // _roll_ctrl.set_max_rate(math::radians(_parameters.r_rmax)); + + // /* yaw control parameters */ + // _yaw_ctrl.set_k_a(math::radians(_parameters.y_slip)); + // _yaw_ctrl.set_k_i(math::radians(_parameters.y_int)); + // _yaw_ctrl.set_k_d(math::radians(_parameters.y_damp)); + // _yaw_ctrl.set_k_ff(math::radians(_parameters.y_rll)); + // _yaw_ctrl.set_imax(math::radians(_parameters.y_imax)); + + return OK; +} + +void +FixedwingAttitudeControlVector::vehicle_status_poll() +{ + bool vstatus_updated; + + /* Check HIL state if vehicle status has changed */ + orb_check(_vstatus_sub, &vstatus_updated); + + if (vstatus_updated) { + + orb_copy(ORB_ID(vehicle_status), _vstatus_sub, &_vstatus); + } +} + +bool +FixedwingAttitudeControlVector::vehicle_airspeed_poll() +{ + /* check if there is a new position */ + bool airspeed_updated; + orb_check(_airspeed_sub, &airspeed_updated); + + if (airspeed_updated) { + orb_copy(ORB_ID(airspeed), _airspeed_sub, &_airspeed); + return true; + } + + return false; +} + +void +FixedwingAttitudeControlVector::vehicle_accel_poll() +{ + /* check if there is a new position */ + bool accel_updated; + orb_check(_accel_sub, &accel_updated); + + if (accel_updated) { + orb_copy(ORB_ID(sensor_accel), _accel_sub, &_accel); + } +} + +void +FixedwingAttitudeControlVector::vehicle_setpoint_poll() +{ + /* check if there is a new setpoint */ + bool att_sp_updated; + orb_check(_att_sp_sub, &att_sp_updated); + + if (att_sp_updated) { + orb_copy(ORB_ID(vehicle_attitude_setpoint), _att_sp_sub, &_att_sp); + _setpoint_valid = true; + } +} + +void +FixedwingAttitudeControlVector::arming_status_poll() +{ + /* check if there is a new setpoint */ + bool arming_updated; + orb_check(_arming_sub, &arming_updated); + + if (arming_updated) { + orb_copy(ORB_ID(actuator_armed), _arming_sub, &_arming); + } +} + +void +FixedwingAttitudeControlVector::task_main_trampoline(int argc, char *argv[]) +{ + att_control::g_control->task_main(); +} + +void +FixedwingAttitudeControlVector::task_main() +{ + + /* inform about start */ + warnx("Initializing.."); + fflush(stdout); + + /* + * do subscriptions + */ + _att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); + _att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); + _accel_sub = orb_subscribe(ORB_ID(sensor_accel)); + _airspeed_sub = orb_subscribe(ORB_ID(airspeed)); + _vstatus_sub = orb_subscribe(ORB_ID(vehicle_status)); + _params_sub = orb_subscribe(ORB_ID(parameter_update)); + _manual_control_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); + _arming_sub = orb_subscribe(ORB_ID(actuator_armed)); + + /* rate limit vehicle status updates to 5Hz */ + orb_set_interval(_vstatus_sub, 200); + orb_set_interval(_att_sub, 100); + + parameters_update(); + + /* initialize values of critical structs until first regular update */ + _arming.armed = false; + + /* get an initial update for all sensor and status data */ + (void)vehicle_airspeed_poll(); + vehicle_setpoint_poll(); + vehicle_accel_poll(); + vehicle_status_poll(); + arming_status_poll(); + + /* wakeup source(s) */ + struct pollfd fds[2]; + + /* Setup of loop */ + fds[0].fd = _params_sub; + fds[0].events = POLLIN; + fds[1].fd = _att_sub; + fds[1].events = POLLIN; + + while (!_task_should_exit) { + + /* wait for up to 500ms for data */ + int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100); + + /* timed out - periodic check for _task_should_exit, etc. */ + if (pret == 0) + continue; + + /* this is undesirable but not much we can do - might want to flag unhappy status */ + if (pret < 0) { + warn("poll error %d, %d", pret, errno); + continue; + } + + perf_begin(_loop_perf); + + /* only update parameters if they changed */ + if (fds[0].revents & POLLIN) { + /* read from param to clear updated flag */ + struct parameter_update_s update; + orb_copy(ORB_ID(parameter_update), _params_sub, &update); + + /* update parameters from storage */ + parameters_update(); + } + + /* only run controller if attitude changed */ + if (fds[1].revents & POLLIN) { + + + static uint64_t last_run = 0; + float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f; + last_run = hrt_absolute_time(); + + /* guard against too large deltaT's */ + if (deltaT > 1.0f) + deltaT = 0.01f; + + /* load local copies */ + orb_copy(ORB_ID(vehicle_attitude), _att_sub, &_att); + + _airspeed_valid = vehicle_airspeed_poll(); + + vehicle_setpoint_poll(); + + vehicle_accel_poll(); + + /* check vehicle status for changes to publication state */ + vehicle_status_poll(); + + /* check for arming status changes */ + arming_status_poll(); + + /* lock integrator until armed */ + bool lock_integrator; + if (_arming.armed) { + lock_integrator = false; + } else { + lock_integrator = true; + } + + /* decide if in stabilized or full manual control */ + + if (_vstatus.state_machine == SYSTEM_STATE_MANUAL && !(_vstatus.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS) + ) { + + _actuators.control[0] = _manual.roll; + _actuators.control[1] = _manual.pitch; + _actuators.control[2] = _manual.yaw; + _actuators.control[3] = _manual.throttle; + _actuators.control[4] = _manual.flaps; + + } else if (_vstatus.state_machine == SYSTEM_STATE_AUTO || + (_vstatus.state_machine == SYSTEM_STATE_STABILIZED) || + (_vstatus.state_machine == SYSTEM_STATE_MANUAL && _vstatus.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS)) { + + /* scale from radians to normalized -1 .. 1 range */ + const float actuator_scaling = 1.0f / (M_PI_F / 4.0f); + + /* scale around tuning airspeed */ + + float airspeed; + + /* if airspeed is smaller than min, the sensor is not giving good readings */ + if (!_airspeed_valid || + (_airspeed.indicated_airspeed_m_s < _parameters.airspeed_min) || + !isfinite(_airspeed.indicated_airspeed_m_s)) { + airspeed = _parameters.airspeed_min + (_parameters.airspeed_max - _parameters.airspeed_min) / 2.0f; + } else { + airspeed = _airspeed.indicated_airspeed_m_s; + } + + float airspeed_scaling = _parameters.airspeed_trim / airspeed; + + float roll_sp, pitch_sp; + float throttle_sp = 0.0f; + + if (_vstatus.state_machine == SYSTEM_STATE_AUTO) { + roll_sp = _att_sp.roll_body; + pitch_sp = _att_sp.pitch_body; + throttle_sp = _att_sp.thrust; + } else if ((_vstatus.state_machine == SYSTEM_STATE_STABILIZED) || + (_vstatus.state_machine == SYSTEM_STATE_MANUAL && _vstatus.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS)) { + + /* + * Scale down roll and pitch as the setpoints are radians + * and a typical remote can only do 45 degrees, the mapping is + * -1..+1 to -45..+45 degrees or -0.75..+0.75 radians. + * + * With this mapping the stick angle is a 1:1 representation of + * the commanded attitude. If more than 45 degrees are desired, + * a scaling parameter can be applied to the remote. + */ + roll_sp = _manual.roll * 0.75f; + pitch_sp = _manual.pitch * 0.75f; + throttle_sp = _manual.throttle; + _actuators.control[4] = _manual.flaps; + } + + math::Dcm R_nb(_att.R); + + // Transform body frame forces to + // global frame + const math::Vector3 F_des(pitch_sp, roll_sp, throttle_sp); + const math::Vector3 angular_rates(_att.rollspeed, _att.pitchspeed, _att.yawspeed); + + // Return variables + math::Vector3 moments_des; + float thrust; + + _att_control.control(deltaT, airspeed_scaling, airspeed, R_nb, _att.roll, _att.pitch, _att.yaw, + F_des, angular_rates, moments_des, thrust); + + _actuators.control[0] = (isfinite(moments_des(0))) ? moments_des(0) * actuator_scaling : 0.0f; + _actuators.control[1] = (isfinite(moments_des(1))) ? moments_des(1) * actuator_scaling : 0.0f; + _actuators.control[2] = 0.0f;//(isfinite(moments_des(0))) ? moments_des(0) * actuator_scaling : 0.0f; + + /* throttle passed through */ + _actuators.control[3] = (isfinite(thrust)) ? thrust : 0.0f; + + // warnx("aspd: %s: %6.2f, aspd scaling: %6.2f, controls: %5.2f %5.2f %5.2f %5.2f", (_airspeed_valid) ? "valid" : "unknown", + // airspeed, airspeed_scaling, _actuators.control[0], _actuators.control[1], + // _actuators.control[2], _actuators.control[3]); + + /* + * Lazily publish the rate setpoint (for analysis, the actuators are published below) + * only once available + */ + vehicle_rates_setpoint_s rates_sp; + math::Vector3 rates_des = _att_control.get_rates_des(); + rates_sp.roll = rates_des(0); + rates_sp.pitch = rates_des(1); + rates_sp.yaw = 0.0f; // XXX rates_des(2); + + rates_sp.timestamp = hrt_absolute_time(); + + if (_rate_sp_pub > 0) { + /* publish the attitude setpoint */ + orb_publish(ORB_ID(vehicle_rates_setpoint), _actuators_0_pub, &rates_sp); + + } else { + /* advertise and publish */ + _rate_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &rates_sp); + } + + /* lazily publish the setpoint only once available */ + _actuators.timestamp = hrt_absolute_time(); + + if (_actuators_0_pub > 0) { + /* publish the attitude setpoint */ + orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators); + + } else { + /* advertise and publish */ + _actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators); + } + + } + + } + + perf_end(_loop_perf); + } + + warnx("exiting.\n"); + + _control_task = -1; + _exit(0); +} + +int +FixedwingAttitudeControlVector::start() +{ + ASSERT(_control_task == -1); + + /* start the task */ + _control_task = task_spawn_cmd("fw_att_control", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 2048, + (main_t)&FixedwingAttitudeControlVector::task_main_trampoline, + nullptr); + + if (_control_task < 0) { + warn("task start failed"); + return -errno; + } + + return OK; +} + +int fw_att_control_vector_main(int argc, char *argv[]) +{ + if (argc < 1) + errx(1, "usage: fw_att_control {start|stop|status}"); + + if (!strcmp(argv[1], "start")) { + + if (att_control::g_control != nullptr) + errx(1, "already running"); + + att_control::g_control = new FixedwingAttitudeControlVector; + + if (att_control::g_control == nullptr) + errx(1, "alloc failed"); + + if (OK != att_control::g_control->start()) { + delete att_control::g_control; + att_control::g_control = nullptr; + err(1, "start failed"); + } + + exit(0); + } + + if (!strcmp(argv[1], "stop")) { + if (att_control::g_control == nullptr) + errx(1, "not running"); + + delete att_control::g_control; + att_control::g_control = nullptr; + exit(0); + } + + if (!strcmp(argv[1], "status")) { + if (att_control::g_control) { + errx(0, "running"); + + } else { + errx(1, "not running"); + } + } + + warnx("unrecognized command"); + return 1; +} diff --git a/src/modules/fw_att_control_vector/module.mk b/src/modules/fw_att_control_vector/module.mk new file mode 100644 index 0000000000..4def85cd31 --- /dev/null +++ b/src/modules/fw_att_control_vector/module.mk @@ -0,0 +1,41 @@ +############################################################################ +# +# Copyright (c) 2012, 2013 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. +# +############################################################################ + +# +# Multirotor attitude controller (vector based, no Euler singularities) +# + +MODULE_COMMAND = fw_att_control_vector + +SRCS = fw_att_control_vector_main.cpp \ + ecl_fw_att_control_vector.cpp From c4e967eb97e83605b143cb64bb1c65472af34778 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 13 Jul 2013 18:41:35 +0200 Subject: [PATCH 008/447] Fixed compile errors for multicopter --- .../mc_att_control_vector/ecl_mc_att_control_vector.cpp | 2 +- .../mc_att_control_vector/ecl_mc_att_control_vector.h | 8 ++++---- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/src/modules/mc_att_control_vector/ecl_mc_att_control_vector.cpp b/src/modules/mc_att_control_vector/ecl_mc_att_control_vector.cpp index b0e010a1c0..252cdafab5 100644 --- a/src/modules/mc_att_control_vector/ecl_mc_att_control_vector.cpp +++ b/src/modules/mc_att_control_vector/ecl_mc_att_control_vector.cpp @@ -50,7 +50,7 @@ ECL_MCAttControlVector::ECL_MCAttControlVector() : _integral_error(0.0f, 0.0f), - _integral_max(1000.0f), + _integral_max(1000.0f, 1000.0f), _integral_lock(false) { diff --git a/src/modules/mc_att_control_vector/ecl_mc_att_control_vector.h b/src/modules/mc_att_control_vector/ecl_mc_att_control_vector.h index b975f2f7f5..a3440a06b2 100644 --- a/src/modules/mc_att_control_vector/ecl_mc_att_control_vector.h +++ b/src/modules/mc_att_control_vector/ecl_mc_att_control_vector.h @@ -50,7 +50,7 @@ class ECL_MCAttControlVector { public: - ECL_MCAttControlVector(float integral_max); + ECL_MCAttControlVector(); void control(float dt, const math::Dcm &R_nb, float yaw, const math::Vector &F_des, float Kp, float Kd, float Ki, const math::Vector &angular_rates, math::Vector &rates_des, float &thrust); @@ -65,12 +65,12 @@ public: _integral_error(1) = 0.0f; } - void lock_integral(bool lock) { + void lock_integrator(bool lock) { _integral_lock = lock; } protected: - math::Vector2 _integral_error; - math::Vector2 _integral_max; + math::Vector2f _integral_error; + math::Vector2f _integral_max; bool _integral_lock; }; From cbcc124c714625d99688454bb8b7b2074bdbc12a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 13 Jul 2013 20:37:26 +0200 Subject: [PATCH 009/447] Cleanup, finished param read, ready for testing --- .../fw_att_control_vector_main.cpp | 43 ++++-------- .../fw_att_control_vector_params.c | 67 +++++++++++++++++++ src/modules/fw_att_control_vector/module.mk | 3 +- 3 files changed, 81 insertions(+), 32 deletions(-) create mode 100644 src/modules/fw_att_control_vector/fw_att_control_vector_params.c diff --git a/src/modules/fw_att_control_vector/fw_att_control_vector_main.cpp b/src/modules/fw_att_control_vector/fw_att_control_vector_main.cpp index a06bc9cb62..c64f82e544 100644 --- a/src/modules/fw_att_control_vector/fw_att_control_vector_main.cpp +++ b/src/modules/fw_att_control_vector/fw_att_control_vector_main.cpp @@ -133,7 +133,7 @@ private: struct { - float p_tconst; + float tconst; float p_p; float p_d; float p_i; @@ -141,7 +141,6 @@ private: float p_rmax_dn; float p_imax; float p_rll; - float r_tconst; float r_p; float r_d; float r_i; @@ -160,7 +159,7 @@ private: struct { - param_t p_tconst; + param_t tconst; param_t p_p; param_t p_d; param_t p_i; @@ -168,7 +167,6 @@ private: param_t p_rmax_dn; param_t p_imax; param_t p_rll; - param_t r_tconst; param_t r_p; param_t r_d; param_t r_i; @@ -274,7 +272,7 @@ FixedwingAttitudeControlVector::FixedwingAttitudeControlVector() : { // _parameter_handles.roll_p = param_find("FW_ROLL_P"); // _parameter_handles.pitch_p = param_find("FW_PITCH_P"); - _parameter_handles.p_tconst = param_find("FW_P_TCONST"); + _parameter_handles.tconst = param_find("FW_TCONST"); _parameter_handles.p_p = param_find("FW_P_P"); _parameter_handles.p_d = param_find("FW_P_D"); _parameter_handles.p_i = param_find("FW_P_I"); @@ -283,7 +281,6 @@ FixedwingAttitudeControlVector::FixedwingAttitudeControlVector() : _parameter_handles.p_imax = param_find("FW_P_IMAX"); _parameter_handles.p_rll = param_find("FW_P_RLL"); - _parameter_handles.r_tconst = param_find("FW_R_TCONST"); _parameter_handles.r_p = param_find("FW_R_P"); _parameter_handles.r_d = param_find("FW_R_D"); _parameter_handles.r_i = param_find("FW_R_I"); @@ -333,7 +330,7 @@ int FixedwingAttitudeControlVector::parameters_update() { - param_get(_parameter_handles.p_tconst, &(_parameters.p_tconst)); + param_get(_parameter_handles.tconst, &(_parameters.tconst)); param_get(_parameter_handles.p_p, &(_parameters.p_p)); param_get(_parameter_handles.p_d, &(_parameters.p_d)); param_get(_parameter_handles.p_i, &(_parameters.p_i)); @@ -342,7 +339,6 @@ FixedwingAttitudeControlVector::parameters_update() param_get(_parameter_handles.p_imax, &(_parameters.p_imax)); param_get(_parameter_handles.p_rll, &(_parameters.p_rll)); - param_get(_parameter_handles.r_tconst, &(_parameters.r_tconst)); param_get(_parameter_handles.r_p, &(_parameters.r_p)); param_get(_parameter_handles.r_d, &(_parameters.r_d)); param_get(_parameter_handles.r_i, &(_parameters.r_i)); @@ -360,29 +356,14 @@ FixedwingAttitudeControlVector::parameters_update() param_get(_parameter_handles.airspeed_max, &(_parameters.airspeed_max)); /* pitch control parameters */ - _att_control.set_tconst(_parameters.p_tconst); - // _pitch_ctrl.set_k_p(math::radians(_parameters.p_p)); - // _pitch_ctrl.set_k_i(math::radians(_parameters.p_i)); - // _pitch_ctrl.set_k_d(math::radians(_parameters.p_d)); - // _pitch_ctrl.set_imax(math::radians(_parameters.p_imax)); - // _pitch_ctrl.set_max_rate_pos(math::radians(_parameters.p_rmax_up)); - // _pitch_ctrl.set_max_rate_neg(math::radians(_parameters.p_rmax_dn)); - // _pitch_ctrl.set_roll_ff(math::radians(_parameters.p_rll)); - - // /* roll control parameters */ - // _roll_ctrl.set_tau(_parameters.r_tconst); - // _roll_ctrl.set_k_p(math::radians(_parameters.r_p)); - // _roll_ctrl.set_k_i(math::radians(_parameters.r_i)); - // _roll_ctrl.set_k_d(math::radians(_parameters.r_d)); - // _roll_ctrl.set_imax(math::radians(_parameters.r_imax)); - // _roll_ctrl.set_max_rate(math::radians(_parameters.r_rmax)); - - // /* yaw control parameters */ - // _yaw_ctrl.set_k_a(math::radians(_parameters.y_slip)); - // _yaw_ctrl.set_k_i(math::radians(_parameters.y_int)); - // _yaw_ctrl.set_k_d(math::radians(_parameters.y_damp)); - // _yaw_ctrl.set_k_ff(math::radians(_parameters.y_rll)); - // _yaw_ctrl.set_imax(math::radians(_parameters.y_imax)); + _att_control.set_tconst(_parameters.tconst); + _att_control.set_k_p(math::radians(_parameters.r_p), + math::radians(_parameters.p_p), 0.0f); + _att_control.set_k_d(math::radians(_parameters.r_p), + math::radians(_parameters.p_p), 0.0f); + _att_control.set_k_i(math::radians(_parameters.r_i), + math::radians(_parameters.p_i), + math::radians(_parameters.y_int)); return OK; } diff --git a/src/modules/fw_att_control_vector/fw_att_control_vector_params.c b/src/modules/fw_att_control_vector/fw_att_control_vector_params.c new file mode 100644 index 0000000000..7e434c164b --- /dev/null +++ b/src/modules/fw_att_control_vector/fw_att_control_vector_params.c @@ -0,0 +1,67 @@ +/**************************************************************************** + * + * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * Author: Lorenz Meier + * + * 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 fw_pos_control_l1_params.c + * + * Parameters defined by the L1 position control task + * + * @author Lorenz Meier + */ + +#include + +#include + +/* + * Controller parameters, accessible via MAVLink + * + */ + +PARAM_DEFINE_FLOAT(FW_TCONST, 0.5f); +PARAM_DEFINE_FLOAT(FW_P_P, 40.0f); +PARAM_DEFINE_FLOAT(FW_P_D, 0.0f); +PARAM_DEFINE_FLOAT(FW_P_I, 0.0f); +PARAM_DEFINE_FLOAT(FW_P_RMAX_UP, 0.0f); +PARAM_DEFINE_FLOAT(FW_P_RMAX_DN, 0.0f); +PARAM_DEFINE_FLOAT(FW_P_IMAX, 15.0f); +PARAM_DEFINE_FLOAT(FW_P_RLL, 1.0f); +PARAM_DEFINE_FLOAT(FW_R_P, 40.0f); +PARAM_DEFINE_FLOAT(FW_R_D, 0.0f); +PARAM_DEFINE_FLOAT(FW_R_I, 0.0f); +PARAM_DEFINE_FLOAT(FW_R_IMAX, 15.0f); +PARAM_DEFINE_FLOAT(FW_R_RMAX, 60); +PARAM_DEFINE_FLOAT(FW_AIRSPD_MIN, 9.0f); +PARAM_DEFINE_FLOAT(FW_AIRSPD_TRIM, 12.0f); +PARAM_DEFINE_FLOAT(FW_AIRSPD_MAX, 18.0f); diff --git a/src/modules/fw_att_control_vector/module.mk b/src/modules/fw_att_control_vector/module.mk index 4def85cd31..e78a715954 100644 --- a/src/modules/fw_att_control_vector/module.mk +++ b/src/modules/fw_att_control_vector/module.mk @@ -38,4 +38,5 @@ MODULE_COMMAND = fw_att_control_vector SRCS = fw_att_control_vector_main.cpp \ - ecl_fw_att_control_vector.cpp + ecl_fw_att_control_vector.cpp \ + fw_att_control_vector_params.c From 6b779275bc2ba02ba2e6b90df7d4698e7b3d8174 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 13 Jul 2013 20:42:13 +0200 Subject: [PATCH 010/447] Added quaternion renormalization --- src/modules/fw_att_control_vector/ecl_fw_att_control_vector.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/fw_att_control_vector/ecl_fw_att_control_vector.cpp b/src/modules/fw_att_control_vector/ecl_fw_att_control_vector.cpp index d689d4eef5..32545e0e02 100644 --- a/src/modules/fw_att_control_vector/ecl_fw_att_control_vector.cpp +++ b/src/modules/fw_att_control_vector/ecl_fw_att_control_vector.cpp @@ -131,6 +131,8 @@ void ECL_FWAttControlVector::control(float dt, float airspeed, float airspeed_sc // XXX switch between operations based on difference, // benchmark both options math::Quaternion e_q = math::Quaternion(R_des) - math::Quaternion(R_bn); + // Renormalize + e_q = e_q / e_q.norm(); math::Matrix e_R = math::Dcm(e_q); //small angles: math::Matrix e_R = (R_des.transpose() * R_bn - R_bn.transpose() * R_des) * 0.5f; From 6901402853e213f75530b7a46505da2b1c5e8d1d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 13 Jul 2013 20:44:17 +0200 Subject: [PATCH 011/447] Fixed signs --- src/modules/fw_att_control_vector/ecl_fw_att_control_vector.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/fw_att_control_vector/ecl_fw_att_control_vector.cpp b/src/modules/fw_att_control_vector/ecl_fw_att_control_vector.cpp index 32545e0e02..0f1a355d76 100644 --- a/src/modules/fw_att_control_vector/ecl_fw_att_control_vector.cpp +++ b/src/modules/fw_att_control_vector/ecl_fw_att_control_vector.cpp @@ -78,7 +78,7 @@ void ECL_FWAttControlVector::control(float dt, float airspeed, float airspeed_sc } math::Dcm R_bn(R_nb.transpose()); - math::Matrix R_yaw_bn = math::Dcm(math::EulerAngles(0.0f, 0.0f, -yaw)); + math::Matrix R_yaw_bn = math::Dcm(math::EulerAngles(0.0f, 0.0f, yaw)).transpose(); // Establish actuator signs and lift compensation float lift_sign = (R_bn(3, 3) >= 0) ? 1.0f : -1.0f; From 0b12efc8400519724a2884e20bbb44d8caf7629d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 14 Jul 2013 09:44:58 +0200 Subject: [PATCH 012/447] Checked in lift compensation based on Dcm from Paul Riseborough --- .../fw_att_control_vector/ecl_fw_att_control_vector.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/fw_att_control_vector/ecl_fw_att_control_vector.cpp b/src/modules/fw_att_control_vector/ecl_fw_att_control_vector.cpp index 0f1a355d76..b72edfefa1 100644 --- a/src/modules/fw_att_control_vector/ecl_fw_att_control_vector.cpp +++ b/src/modules/fw_att_control_vector/ecl_fw_att_control_vector.cpp @@ -83,8 +83,8 @@ void ECL_FWAttControlVector::control(float dt, float airspeed, float airspeed_sc // Establish actuator signs and lift compensation float lift_sign = (R_bn(3, 3) >= 0) ? 1.0f : -1.0f; - // XXX temporary workaround - float lift_comp = fabsf((CONSTANTS_ONE_G / math::max(airspeed , float(_p_airspeed_min)) * tanf(roll) * sinf(roll))) * _p_roll_ffd; + float lift_comp = CONSTANTS_ONE_G / math::max(airspeed , float(_p_airspeed_min) * (R_nb(2,3) * R_nb(2,3)) / (R_nb(3,3) * sqrtf((R_nb(2,3) * R_nb(2,3)) + (R_nb(3,3) * R_nb(3,3))); + //float lift_comp = fabsf((CONSTANTS_ONE_G / math::max(airspeed , float(_p_airspeed_min)) * tanf(roll) * sinf(roll))) * _p_roll_ffd; float cy = cosf(yaw); float sy = sinf(yaw); From e77dfc39c28d6ebb3609ea3e6c89feb90f39b016 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 2 Aug 2013 13:23:28 +0200 Subject: [PATCH 013/447] Fixed compile error on vector --- src/modules/fw_att_control_vector/ecl_fw_att_control_vector.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/fw_att_control_vector/ecl_fw_att_control_vector.cpp b/src/modules/fw_att_control_vector/ecl_fw_att_control_vector.cpp index b72edfefa1..16e60f3e0b 100644 --- a/src/modules/fw_att_control_vector/ecl_fw_att_control_vector.cpp +++ b/src/modules/fw_att_control_vector/ecl_fw_att_control_vector.cpp @@ -83,7 +83,7 @@ void ECL_FWAttControlVector::control(float dt, float airspeed, float airspeed_sc // Establish actuator signs and lift compensation float lift_sign = (R_bn(3, 3) >= 0) ? 1.0f : -1.0f; - float lift_comp = CONSTANTS_ONE_G / math::max(airspeed , float(_p_airspeed_min) * (R_nb(2,3) * R_nb(2,3)) / (R_nb(3,3) * sqrtf((R_nb(2,3) * R_nb(2,3)) + (R_nb(3,3) * R_nb(3,3))); + float lift_comp = CONSTANTS_ONE_G / math::max(airspeed , float(_p_airspeed_min) * (R_nb(2,3) * R_nb(2,3)) / (R_nb(3,3) * sqrtf((R_nb(2,3) * R_nb(2,3)) + (R_nb(3,3) * R_nb(3,3))))); //float lift_comp = fabsf((CONSTANTS_ONE_G / math::max(airspeed , float(_p_airspeed_min)) * tanf(roll) * sinf(roll))) * _p_roll_ffd; float cy = cosf(yaw); From 4c23b482f85efbd207c182bfea3332433537741e Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 19 Sep 2013 19:14:34 +0200 Subject: [PATCH 014/447] position_estimator_inav: GPS health detection changed, minor improvements --- .../multirotor_pos_control.c | 10 +- .../position_estimator_inav_main.c | 94 +++++++++++-------- 2 files changed, 65 insertions(+), 39 deletions(-) diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c index 1b3ddfdcda..a8add5d73b 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c @@ -133,8 +133,14 @@ int multirotor_pos_control_main(int argc, char *argv[]) } if (!strcmp(argv[1], "stop")) { - warnx("stop"); - thread_should_exit = true; + if (thread_running) { + warnx("stop"); + thread_should_exit = true; + + } else { + warnx("app not started"); + } + exit(0); } diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index 10007bf960..6d1f257494 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -95,8 +95,7 @@ static void usage(const char *reason) if (reason) fprintf(stderr, "%s\n", reason); - fprintf(stderr, - "usage: position_estimator_inav {start|stop|status} [-v]\n\n"); + fprintf(stderr, "usage: position_estimator_inav {start|stop|status} [-v]\n\n"); exit(1); } @@ -115,7 +114,7 @@ int position_estimator_inav_main(int argc, char *argv[]) if (!strcmp(argv[1], "start")) { if (thread_running) { - printf("position_estimator_inav already running\n"); + warnx("already running"); /* this is not an error */ exit(0); } @@ -135,16 +134,23 @@ int position_estimator_inav_main(int argc, char *argv[]) } if (!strcmp(argv[1], "stop")) { - thread_should_exit = true; + if (thread_running) { + warnx("stop"); + thread_should_exit = true; + + } else { + warnx("app not started"); + } + exit(0); } if (!strcmp(argv[1], "status")) { if (thread_running) { - printf("\tposition_estimator_inav is running\n"); + warnx("app is running"); } else { - printf("\tposition_estimator_inav not started\n"); + warnx("app not started"); } exit(0); @@ -159,7 +165,7 @@ int position_estimator_inav_main(int argc, char *argv[]) ****************************************************************************/ int position_estimator_inav_thread_main(int argc, char *argv[]) { - warnx("started."); + warnx("started"); int mavlink_fd; mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); mavlink_log_info(mavlink_fd, "[inav] started"); @@ -169,6 +175,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) float y_est[3] = { 0.0f, 0.0f, 0.0f }; float z_est[3] = { 0.0f, 0.0f, 0.0f }; + float acc_offs[3] = { 0.0f, 0.0f, 0.0f }; + int baro_init_cnt = 0; int baro_init_num = 200; float baro_alt0 = 0.0f; /* to determine while start up */ @@ -268,7 +276,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) bool ref_xy_inited = false; hrt_abstime ref_xy_init_start = 0; - const hrt_abstime ref_xy_init_delay = 5000000; // wait for 5s after 3D fix + const hrt_abstime ref_xy_init_delay = 1000000; // wait for 1s after 3D fix hrt_abstime t_prev = 0; @@ -299,6 +307,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) float sonar_prev = 0.0f; hrt_abstime sonar_time = 0; + bool gps_valid = false; + /* main loop */ struct pollfd fds[7] = { { .fd = parameter_update_sub, .events = POLLIN }, @@ -311,7 +321,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) }; if (!thread_should_exit) { - warnx("main loop started."); + warnx("main loop started"); } while (!thread_should_exit) { @@ -428,32 +438,29 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) /* vehicle GPS position */ if (fds[6].revents & POLLIN) { orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, &gps); + /* require EPH < 10m */ + gps_valid = gps.fix_type >= 3 && gps.eph_m < 10.0f && t < gps.timestamp_position + gps_timeout; - if (gps.fix_type >= 3 && t < gps.timestamp_position + gps_timeout) { + if (gps_valid) { /* initialize reference position if needed */ if (!ref_xy_inited) { - /* require EPH < 10m */ - if (gps.eph_m < 10.0f) { - if (ref_xy_init_start == 0) { - ref_xy_init_start = t; + if (ref_xy_init_start == 0) { + ref_xy_init_start = t; - } else if (t > ref_xy_init_start + ref_xy_init_delay) { - ref_xy_inited = true; - /* reference GPS position */ - double lat = gps.lat * 1e-7; - double lon = gps.lon * 1e-7; + } else if (t > ref_xy_init_start + ref_xy_init_delay) { + ref_xy_inited = true; + /* reference GPS position */ + double lat = gps.lat * 1e-7; + double lon = gps.lon * 1e-7; - local_pos.ref_lat = gps.lat; - local_pos.ref_lon = gps.lon; - local_pos.ref_timestamp = t; + local_pos.ref_lat = gps.lat; + local_pos.ref_lon = gps.lon; + local_pos.ref_timestamp = t; - /* initialize projection */ - map_projection_init(lat, lon); - warnx("init GPS: lat = %.10f, lon = %.10f", lat, lon); - mavlink_log_info(mavlink_fd, "[inav] init GPS: %.7f, %.7f", lat, lon); - } - } else { - ref_xy_init_start = 0; + /* initialize projection */ + map_projection_init(lat, lon); + warnx("init GPS: lat = %.10f, lon = %.10f", lat, lon); + mavlink_log_info(mavlink_fd, "[inav] init GPS: %.7f, %.7f", lat, lon); } } @@ -513,23 +520,28 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) inertial_filter_correct(sonar_corr, dt, z_est, 0, params.w_alt_sonar); inertial_filter_correct(accel_corr[2], dt, z_est, 2, params.w_alt_acc); - bool gps_valid = ref_xy_inited && gps.fix_type >= 3 && t < gps.timestamp_position + gps_timeout; - bool flow_valid = false; // TODO implement opt flow + bool use_gps = ref_xy_inited && gps_valid; + bool use_flow = false; // TODO implement opt flow /* try to estimate xy even if no absolute position source available, * if using optical flow velocity will be correct in this case */ - bool can_estimate_xy = gps_valid || flow_valid; + bool can_estimate_xy = use_gps || use_flow; if (can_estimate_xy) { /* inertial filter prediction for position */ inertial_filter_predict(dt, x_est); inertial_filter_predict(dt, y_est); + if (!isfinite(x_est[0]) || !isfinite(y_est[0])) { + warnx("BAD ESTIMATE PRED %.6f x: %.3f %.3f %.3f y: %.3f %.3f %.3f", dt, x_est[0], x_est[1], x_est[2], y_est[0], y_est[1], y_est[2]); + thread_should_exit = true; + } + /* inertial filter correction for position */ inertial_filter_correct(accel_corr[0], dt, x_est, 2, params.w_pos_acc); inertial_filter_correct(accel_corr[1], dt, y_est, 2, params.w_pos_acc); - if (gps_valid) { + if (use_gps) { inertial_filter_correct(gps_corr[0][0], dt, x_est, 0, params.w_pos_gps_p); inertial_filter_correct(gps_corr[1][0], dt, y_est, 0, params.w_pos_gps_p); @@ -538,6 +550,12 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) inertial_filter_correct(gps_corr[1][1], dt, y_est, 1, params.w_pos_gps_v); } } + + if (!isfinite(x_est[0]) || !isfinite(y_est[0])) { + warnx("BAD ESTIMATE CORR dt: %.6f x: %.3f %.3f %.3f y: %.3f %.3f %.3f", dt, x_est[0], x_est[1], x_est[2], y_est[0], y_est[1], y_est[2]); + warnx("BAD ESTIMATE CORR acc: %.3f %.3f gps x: %.3f %.3f gps y: %.3f %.3f", accel_corr[0], accel_corr[1], gps_corr[0][0], gps_corr[0][1], gps_corr[1][0], gps_corr[1][1]); + thread_should_exit = true; + } } /* detect land */ @@ -595,9 +613,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) pub_last = t; /* publish local position */ local_pos.timestamp = t; - local_pos.xy_valid = can_estimate_xy && gps_valid; + local_pos.xy_valid = can_estimate_xy && use_gps; local_pos.v_xy_valid = can_estimate_xy; - local_pos.xy_global = local_pos.xy_valid && gps_valid; // will make sense when local position sources (e.g. vicon) will be implemented + local_pos.xy_global = local_pos.xy_valid && use_gps; // will make sense when local position sources (e.g. vicon) will be implemented local_pos.x = x_est[0]; local_pos.vx = x_est[1]; local_pos.y = y_est[0]; @@ -637,17 +655,19 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (local_pos.v_z_valid) { global_pos.vz = local_pos.vz; } + global_pos.yaw = local_pos.yaw; global_pos.timestamp = t; orb_publish(ORB_ID(vehicle_global_position), vehicle_global_position_pub, &global_pos); } + flag_armed = armed.armed; } - warnx("exiting."); - mavlink_log_info(mavlink_fd, "[inav] exiting"); + warnx("stopped"); + mavlink_log_info(mavlink_fd, "[inav] stopped"); thread_running = false; return 0; } From 648fb6c52342c47b71f74e76cf8c15f71a833805 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 21 Sep 2013 15:46:15 +0200 Subject: [PATCH 015/447] position_estimator_inav: minor fixes --- .../position_estimator_inav_main.c | 79 +++++++++---------- 1 file changed, 39 insertions(+), 40 deletions(-) diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index 6d1f257494..9daa075e7e 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -170,24 +170,57 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); mavlink_log_info(mavlink_fd, "[inav] started"); - /* initialize values */ float x_est[3] = { 0.0f, 0.0f, 0.0f }; float y_est[3] = { 0.0f, 0.0f, 0.0f }; float z_est[3] = { 0.0f, 0.0f, 0.0f }; - float acc_offs[3] = { 0.0f, 0.0f, 0.0f }; - int baro_init_cnt = 0; int baro_init_num = 200; float baro_alt0 = 0.0f; /* to determine while start up */ float alt_avg = 0.0f; bool landed = true; hrt_abstime landed_time = 0; + bool flag_armed = false; uint32_t accel_counter = 0; uint32_t baro_counter = 0; + bool ref_xy_inited = false; + hrt_abstime ref_xy_init_start = 0; + const hrt_abstime ref_xy_init_delay = 1000000; // wait for 1s after 3D fix + + uint16_t accel_updates = 0; + uint16_t baro_updates = 0; + uint16_t gps_updates = 0; + uint16_t attitude_updates = 0; + uint16_t flow_updates = 0; + + hrt_abstime updates_counter_start = hrt_absolute_time(); + hrt_abstime pub_last = hrt_absolute_time(); + + hrt_abstime t_prev = 0; + + /* acceleration in NED frame */ + float accel_NED[3] = { 0.0f, 0.0f, -CONSTANTS_ONE_G }; + + /* store error when sensor updates, but correct on each time step to avoid jumps in estimated value */ + float accel_corr[] = { 0.0f, 0.0f, 0.0f }; // N E D + float accel_bias[] = { 0.0f, 0.0f, 0.0f }; // body frame + float baro_corr = 0.0f; // D + float gps_corr[2][2] = { + { 0.0f, 0.0f }, // N (pos, vel) + { 0.0f, 0.0f }, // E (pos, vel) + }; + float sonar_corr = 0.0f; + float sonar_corr_filtered = 0.0f; + float flow_corr[] = { 0.0f, 0.0f }; // X, Y + + float sonar_prev = 0.0f; + hrt_abstime sonar_time = 0; + + bool gps_valid = false; + /* declare and safely initialize all structs */ struct actuator_controls_s actuator; memset(&actuator, 0, sizeof(actuator)); @@ -274,41 +307,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } } - bool ref_xy_inited = false; - hrt_abstime ref_xy_init_start = 0; - const hrt_abstime ref_xy_init_delay = 1000000; // wait for 1s after 3D fix - - hrt_abstime t_prev = 0; - - uint16_t accel_updates = 0; - uint16_t baro_updates = 0; - uint16_t gps_updates = 0; - uint16_t attitude_updates = 0; - uint16_t flow_updates = 0; - - hrt_abstime updates_counter_start = hrt_absolute_time(); - hrt_abstime pub_last = hrt_absolute_time(); - - /* acceleration in NED frame */ - float accel_NED[3] = { 0.0f, 0.0f, -CONSTANTS_ONE_G }; - - /* store error when sensor updates, but correct on each time step to avoid jumps in estimated value */ - float accel_corr[] = { 0.0f, 0.0f, 0.0f }; // N E D - float accel_bias[] = { 0.0f, 0.0f, 0.0f }; // body frame - float baro_corr = 0.0f; // D - float gps_corr[2][2] = { - { 0.0f, 0.0f }, // N (pos, vel) - { 0.0f, 0.0f }, // E (pos, vel) - }; - float sonar_corr = 0.0f; - float sonar_corr_filtered = 0.0f; - float flow_corr[] = { 0.0f, 0.0f }; // X, Y - - float sonar_prev = 0.0f; - hrt_abstime sonar_time = 0; - - bool gps_valid = false; - /* main loop */ struct pollfd fds[7] = { { .fd = parameter_update_sub, .events = POLLIN }, @@ -524,7 +522,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) bool use_flow = false; // TODO implement opt flow /* try to estimate xy even if no absolute position source available, - * if using optical flow velocity will be correct in this case */ + * if using optical flow velocity will be valid */ bool can_estimate_xy = use_gps || use_flow; if (can_estimate_xy) { @@ -612,7 +610,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (t > pub_last + pub_interval) { pub_last = t; /* publish local position */ - local_pos.timestamp = t; local_pos.xy_valid = can_estimate_xy && use_gps; local_pos.v_xy_valid = can_estimate_xy; local_pos.xy_global = local_pos.xy_valid && use_gps; // will make sense when local position sources (e.g. vicon) will be implemented @@ -625,6 +622,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) local_pos.landed = landed; local_pos.yaw = att.yaw; + local_pos.timestamp = t; + orb_publish(ORB_ID(vehicle_local_position), vehicle_local_position_pub, &local_pos); /* publish global position */ From 0d5ead7d858293759cf412cc810c60b83cdebe09 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 22 Sep 2013 12:33:08 +0200 Subject: [PATCH 016/447] position_estimator_inav: GPS signal quality hysteresis, accel bias estimation fixed, cleanup --- .../position_estimator_inav_main.c | 42 ++++++++++++------- 1 file changed, 28 insertions(+), 14 deletions(-) diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index 9daa075e7e..8ac9af15c8 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -294,8 +294,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } else { wait_baro = false; baro_alt0 /= (float) baro_init_cnt; - warnx("init baro: alt = %.3f", baro_alt0); - mavlink_log_info(mavlink_fd, "[inav] init baro: alt = %.3f", baro_alt0); + warnx("init ref: alt = %.3f", baro_alt0); + mavlink_log_info(mavlink_fd, "[inav] init ref: alt = %.3f", baro_alt0); local_pos.ref_alt = baro_alt0; local_pos.ref_timestamp = hrt_absolute_time(); local_pos.z_valid = true; @@ -366,6 +366,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (sensor.accelerometer_counter > accel_counter) { if (att.R_valid) { /* correct accel bias, now only for Z */ + sensor.accelerometer_m_s2[0] -= accel_bias[0]; + sensor.accelerometer_m_s2[1] -= accel_bias[1]; sensor.accelerometer_m_s2[2] -= accel_bias[2]; /* transform acceleration vector from body frame to NED frame */ @@ -416,7 +418,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } else { // new ground level baro_alt0 += sonar_corr; - mavlink_log_info(mavlink_fd, "[inav] new home: alt = %.3f", baro_alt0); + mavlink_log_info(mavlink_fd, "[inav] update ref: alt = %.3f", baro_alt0); local_pos.ref_alt = baro_alt0; local_pos.ref_timestamp = hrt_absolute_time(); z_est[0] += sonar_corr; @@ -436,8 +438,16 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) /* vehicle GPS position */ if (fds[6].revents & POLLIN) { orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, &gps); - /* require EPH < 10m */ - gps_valid = gps.fix_type >= 3 && gps.eph_m < 10.0f && t < gps.timestamp_position + gps_timeout; + if (gps.fix_type >= 3 && t < gps.timestamp_position + gps_timeout) { + /* hysteresis for GPS quality */ + if (gps_valid) { + gps_valid = gps.eph_m < 10.0f; + } else { + gps_valid = gps.eph_m < 5.0f; + } + } else { + gps_valid = false; + } if (gps_valid) { /* initialize reference position if needed */ @@ -457,8 +467,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) /* initialize projection */ map_projection_init(lat, lon); - warnx("init GPS: lat = %.10f, lon = %.10f", lat, lon); - mavlink_log_info(mavlink_fd, "[inav] init GPS: %.7f, %.7f", lat, lon); + warnx("init ref: lat = %.10f, lon = %.10f", lat, lon); + mavlink_log_info(mavlink_fd, "[inav] init ref: lat = %.7f, lon = %.7f", lat, lon); } } @@ -502,12 +512,16 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) z_est[0] = 0.0f; local_pos.ref_alt = baro_alt0; local_pos.ref_timestamp = hrt_absolute_time(); - mavlink_log_info(mavlink_fd, "[inav] new home on arm: alt = %.3f", baro_alt0); } - /* accel bias correction, now only for Z - * not strictly correct, but stable and works */ - accel_bias[2] += (accel_NED[2] + CONSTANTS_ONE_G) * params.w_acc_bias * dt; + /* accelerometer bias correction, now only uses baro correction */ + /* transform error vector from NED frame to body frame */ + // TODO add sonar weight + float accel_bias_corr = -(baro_corr + baro_alt0) * params.w_acc_bias * params.w_alt_baro * params.w_alt_baro * dt; + for (int i = 0; i < 3; i++) { + accel_bias[i] += att.R[2][i] * accel_bias_corr; + // TODO add XY correction + } /* inertial filter prediction for altitude */ inertial_filter_predict(dt, z_est); @@ -531,7 +545,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) inertial_filter_predict(dt, y_est); if (!isfinite(x_est[0]) || !isfinite(y_est[0])) { - warnx("BAD ESTIMATE PRED %.6f x: %.3f %.3f %.3f y: %.3f %.3f %.3f", dt, x_est[0], x_est[1], x_est[2], y_est[0], y_est[1], y_est[2]); + warnx("BAD ESTIMATE AFTER PREDICTION %.6f x: %.3f %.3f %.3f y: %.3f %.3f %.3f", dt, x_est[0], x_est[1], x_est[2], y_est[0], y_est[1], y_est[2]); thread_should_exit = true; } @@ -550,8 +564,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } if (!isfinite(x_est[0]) || !isfinite(y_est[0])) { - warnx("BAD ESTIMATE CORR dt: %.6f x: %.3f %.3f %.3f y: %.3f %.3f %.3f", dt, x_est[0], x_est[1], x_est[2], y_est[0], y_est[1], y_est[2]); - warnx("BAD ESTIMATE CORR acc: %.3f %.3f gps x: %.3f %.3f gps y: %.3f %.3f", accel_corr[0], accel_corr[1], gps_corr[0][0], gps_corr[0][1], gps_corr[1][0], gps_corr[1][1]); + warnx("BAD ESTIMATE AFTER CORRECTION dt: %.6f x: %.3f %.3f %.3f y: %.3f %.3f %.3f", dt, x_est[0], x_est[1], x_est[2], y_est[0], y_est[1], y_est[2]); + warnx("BAD ESTIMATE AFTER CORRECTION acc: %.3f %.3f gps x: %.3f %.3f gps y: %.3f %.3f", accel_corr[0], accel_corr[1], gps_corr[0][0], gps_corr[0][1], gps_corr[1][0], gps_corr[1][1]); thread_should_exit = true; } } From 8a9a230e0db9a8348cef2ff93357fb9bb2389394 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 22 Sep 2013 12:39:51 +0200 Subject: [PATCH 017/447] position_estimator_inav: more compact messages to fit in mavlink packet --- .../position_estimator_inav_main.c | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index 8ac9af15c8..ba7a5df44b 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -294,8 +294,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } else { wait_baro = false; baro_alt0 /= (float) baro_init_cnt; - warnx("init ref: alt = %.3f", baro_alt0); - mavlink_log_info(mavlink_fd, "[inav] init ref: alt = %.3f", baro_alt0); + warnx("init ref: alt=%.3f", baro_alt0); + mavlink_log_info(mavlink_fd, "[inav] init ref: alt=%.3f", baro_alt0); local_pos.ref_alt = baro_alt0; local_pos.ref_timestamp = hrt_absolute_time(); local_pos.z_valid = true; @@ -418,7 +418,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } else { // new ground level baro_alt0 += sonar_corr; - mavlink_log_info(mavlink_fd, "[inav] update ref: alt = %.3f", baro_alt0); + mavlink_log_info(mavlink_fd, "[inav] update ref: alt=%.3f", baro_alt0); local_pos.ref_alt = baro_alt0; local_pos.ref_timestamp = hrt_absolute_time(); z_est[0] += sonar_corr; @@ -467,8 +467,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) /* initialize projection */ map_projection_init(lat, lon); - warnx("init ref: lat = %.10f, lon = %.10f", lat, lon); - mavlink_log_info(mavlink_fd, "[inav] init ref: lat = %.7f, lon = %.7f", lat, lon); + warnx("init ref: lat=%.7f, lon=%.7f", lat, lon); + mavlink_log_info(mavlink_fd, "[inav] init ref: lat=%.7f, lon=%.7f", lat, lon); } } From 4124417934932907d4663d23e44ab2f436064b58 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 22 Sep 2013 16:53:48 +0200 Subject: [PATCH 018/447] position_estimator_inav: GPS topic timeout detection fixed, messages about GPS signal state in mavlink added --- .../position_estimator_inav_main.c | 21 +++++++++++++++---- 1 file changed, 17 insertions(+), 4 deletions(-) diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index ba7a5df44b..8807020d0d 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -438,12 +438,20 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) /* vehicle GPS position */ if (fds[6].revents & POLLIN) { orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, &gps); - if (gps.fix_type >= 3 && t < gps.timestamp_position + gps_timeout) { + if (gps.fix_type >= 3) { /* hysteresis for GPS quality */ if (gps_valid) { - gps_valid = gps.eph_m < 10.0f; + if (gps.eph_m > 10.0f) { + gps_valid = false; + warnx("GPS signal lost"); + mavlink_log_info(mavlink_fd, "[inav] GPS signal lost"); + } } else { - gps_valid = gps.eph_m < 5.0f; + if (gps.eph_m < 5.0f) { + gps_valid = true; + warnx("GPS signal found"); + mavlink_log_info(mavlink_fd, "[inav] GPS signal found"); + } } } else { gps_valid = false; @@ -501,7 +509,12 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } } - /* end of poll return value check */ + /* check for timeout on GPS topic */ + if (gps_valid && t > gps.timestamp_position + gps_timeout) { + gps_valid = false; + warnx("GPS timeout"); + mavlink_log_info(mavlink_fd, "[inav] GPS timeout"); + } float dt = t_prev > 0 ? (t - t_prev) / 1000000.0f : 0.0f; t_prev = t; From defb37c43bd3f2d4de35def68a092731ed77d0d5 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 5 Oct 2013 22:59:32 +0200 Subject: [PATCH 019/447] commander: require only valid velocity for EASY mode, allows flying with FLOW and no GPS --- src/modules/commander/commander.cpp | 1 + src/modules/commander/state_machine_helper.cpp | 5 ++--- src/modules/uORB/topics/vehicle_status.h | 1 + 3 files changed, 4 insertions(+), 3 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 01b7b84d01..a9f6a23513 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -853,6 +853,7 @@ int commander_thread_main(int argc, char *argv[]) /* update condition_local_position_valid and condition_local_altitude_valid */ check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.xy_valid, &(status.condition_local_position_valid), &status_changed); check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.z_valid, &(status.condition_local_altitude_valid), &status_changed); + check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.v_xy_valid, &(status.condition_local_velocity_valid), &status_changed); if (status.condition_local_altitude_valid) { if (status.condition_landed != local_position.landed) { diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 8ce31550f9..58f7238ff3 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -238,9 +238,8 @@ main_state_transition(struct vehicle_status_s *current_state, main_state_t new_m case MAIN_STATE_EASY: - /* need at minimum local position estimate */ - if (current_state->condition_local_position_valid || - current_state->condition_global_position_valid) { + /* need at minimum local velocity estimate */ + if (current_state->condition_local_velocity_valid) { ret = TRANSITION_CHANGED; } diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index 1c184d3a7a..6bc63cbaea 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -201,6 +201,7 @@ struct vehicle_status_s bool condition_home_position_valid; /**< indicates a valid home position (a valid home position is not always a valid launch) */ bool condition_local_position_valid; bool condition_local_altitude_valid; + bool condition_local_velocity_valid; bool condition_airspeed_valid; /**< set to true by the commander app if there is a valid airspeed measurement available */ bool condition_landed; /**< true if vehicle is landed, always true if disarmed */ From d005815c686da64a21d9230150d451da96756a44 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 5 Oct 2013 23:00:25 +0200 Subject: [PATCH 020/447] position_estimator_inav: major update, using optical flow for position estimation --- .../position_estimator_inav_main.c | 134 +++++++++++++++--- .../position_estimator_inav_params.c | 9 +- .../position_estimator_inav_params.h | 2 + 3 files changed, 121 insertions(+), 24 deletions(-) diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index 8807020d0d..950a47fd9c 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -76,8 +76,10 @@ static bool thread_running = false; /**< Deamon status flag */ static int position_estimator_inav_task; /**< Handle of deamon task / thread */ static bool verbose_mode = false; -static const hrt_abstime gps_timeout = 1000000; // GPS timeout = 1s -static const hrt_abstime flow_timeout = 1000000; // optical flow timeout = 1s +static const hrt_abstime gps_timeout = 1000000; // GPS topic timeout = 1s +static const hrt_abstime sonar_timeout = 150000; // sonar timeout = 150ms +static const hrt_abstime sonar_valid_timeout = 1000000; // assume that altitude == distance to surface during this time +static const hrt_abstime flow_timeout = 1000000; // optical flow topic timeout = 1s static const uint32_t updates_counter_len = 1000000; static const uint32_t pub_interval = 4000; // limit publish rate to 250 Hz @@ -214,12 +216,17 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) }; float sonar_corr = 0.0f; float sonar_corr_filtered = 0.0f; + float flow_corr[] = { 0.0f, 0.0f }; // X, Y + float flow_w = 0.0f; float sonar_prev = 0.0f; - hrt_abstime sonar_time = 0; + hrt_abstime sonar_time = 0; // time of last sonar measurement (not filtered) + hrt_abstime sonar_valid_time = 0; // time of last sonar measurement used for correction (filtered) bool gps_valid = false; + bool sonar_valid = false; + bool flow_valid = false; /* declare and safely initialize all structs */ struct actuator_controls_s actuator; @@ -402,7 +409,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (fds[5].revents & POLLIN) { orb_copy(ORB_ID(optical_flow), optical_flow_sub, &flow); - if (flow.ground_distance_m > 0.31f && flow.ground_distance_m < 4.0f && (flow.ground_distance_m != sonar_prev || t - sonar_time < 150000)) { + if (flow.ground_distance_m > 0.31f && flow.ground_distance_m < 4.0f && att.R[2][2] > 0.7 && (flow.ground_distance_m != sonar_prev || t < sonar_time + sonar_timeout)) { if (flow.ground_distance_m != sonar_prev) { sonar_time = t; sonar_prev = flow.ground_distance_m; @@ -410,13 +417,14 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) sonar_corr_filtered += (sonar_corr - sonar_corr_filtered) * params.sonar_filt; if (fabsf(sonar_corr) > params.sonar_err) { - // correction is too large: spike or new ground level? + /* correction is too large: spike or new ground level? */ if (fabsf(sonar_corr - sonar_corr_filtered) > params.sonar_err) { - // spike detected, ignore + /* spike detected, ignore */ sonar_corr = 0.0f; + sonar_valid = false; } else { - // new ground level + /* new ground level */ baro_alt0 += sonar_corr; mavlink_log_info(mavlink_fd, "[inav] update ref: alt=%.3f", baro_alt0); local_pos.ref_alt = baro_alt0; @@ -424,12 +432,56 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) z_est[0] += sonar_corr; sonar_corr = 0.0f; sonar_corr_filtered = 0.0f; + sonar_valid_time = t; + sonar_valid = true; } + + } else { + sonar_valid_time = t; + sonar_valid = true; } } } else { sonar_corr = 0.0f; + sonar_valid = false; + } + + float flow_q = flow.quality / 255.0f; + + if (z_est[0] < - 0.31f && flow_q > params.flow_q_min && t < sonar_valid_time + sonar_valid_timeout && att.R[2][2] > 0.7) { + /* distance to surface */ + float flow_dist = -z_est[0] / att.R[2][2]; + /* convert raw flow to angular flow */ + float flow_ang[2]; + flow_ang[0] = flow.flow_raw_x * params.flow_k; + flow_ang[1] = flow.flow_raw_y * params.flow_k; + /* flow measurements vector */ + float flow_m[3]; + flow_m[0] = -flow_ang[0] * flow_dist; + flow_m[1] = -flow_ang[1] * flow_dist; + flow_m[2] = z_est[1]; + /* velocity in NED */ + float flow_v[2] = { 0.0f, 0.0f }; + + /* project measurements vector to NED basis, skip Z component */ + for (int i = 0; i < 2; i++) { + for (int j = 0; j < 3; j++) { + flow_v[i] += att.R[i][j] * flow_m[j]; + } + } + + /* velocity correction */ + flow_corr[0] = flow_v[0] - x_est[1]; + flow_corr[1] = flow_v[1] - y_est[1]; + /* adjust correction weight */ + float flow_q_weight = (flow_q - params.flow_q_min) / (1.0f - params.flow_q_min); + flow_w = att.R[2][2] * flow_q_weight; + flow_valid = true; + + } else { + flow_w = 0.0f; + flow_valid = false; } flow_updates++; @@ -438,6 +490,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) /* vehicle GPS position */ if (fds[6].revents & POLLIN) { orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, &gps); + if (gps.fix_type >= 3) { /* hysteresis for GPS quality */ if (gps_valid) { @@ -446,6 +499,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) warnx("GPS signal lost"); mavlink_log_info(mavlink_fd, "[inav] GPS signal lost"); } + } else { if (gps.eph_m < 5.0f) { gps_valid = true; @@ -453,6 +507,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) mavlink_log_info(mavlink_fd, "[inav] GPS signal found"); } } + } else { gps_valid = false; } @@ -509,6 +564,14 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } } + /* check for timeout on FLOW topic */ + if ((flow_valid || sonar_valid) && t > flow.timestamp + flow_timeout) { + flow_valid = false; + sonar_valid = false; + warnx("FLOW timeout"); + mavlink_log_info(mavlink_fd, "[inav] FLOW timeout"); + } + /* check for timeout on GPS topic */ if (gps_valid && t > gps.timestamp_position + gps_timeout) { gps_valid = false; @@ -527,31 +590,55 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) local_pos.ref_timestamp = hrt_absolute_time(); } - /* accelerometer bias correction, now only uses baro correction */ + bool use_gps = ref_xy_inited && gps_valid; + bool use_flow = flow_valid; + + /* try to estimate xy even if no absolute position source available, + * if using optical flow velocity will be valid */ + bool can_estimate_xy = use_gps || use_flow; + + /* baro offset correction if sonar available */ + if (sonar_valid) { + baro_alt0 -= (baro_corr + baro_alt0) * params.w_alt_sonar * dt; + } + + /* accelerometer bias correction */ + float accel_bias_corr[3] = { 0.0f, 0.0f, 0.0f }; + if (use_gps) { + accel_bias_corr[0] -= gps_corr[0][0] * params.w_pos_gps_p * params.w_pos_gps_p; + accel_bias_corr[0] -= gps_corr[0][1] * params.w_pos_gps_v; + accel_bias_corr[1] -= gps_corr[1][0] * params.w_pos_gps_p * params.w_pos_gps_p; + accel_bias_corr[1] -= gps_corr[1][1] * params.w_pos_gps_v; + } + if (use_flow) { + accel_bias_corr[0] -= flow_corr[0] * params.w_pos_flow; + accel_bias_corr[1] -= flow_corr[1] * params.w_pos_flow; + } + accel_bias_corr[2] -= (baro_corr + baro_alt0) * params.w_alt_baro * params.w_alt_baro; + if (sonar_valid) { + accel_bias_corr[2] -= sonar_corr * params.w_alt_sonar * params.w_alt_sonar; + } + /* transform error vector from NED frame to body frame */ - // TODO add sonar weight - float accel_bias_corr = -(baro_corr + baro_alt0) * params.w_acc_bias * params.w_alt_baro * params.w_alt_baro * dt; for (int i = 0; i < 3; i++) { - accel_bias[i] += att.R[2][i] * accel_bias_corr; - // TODO add XY correction + float c = 0.0f; + for (int j = 0; j < 3; j++) { + c += att.R[j][i] * accel_bias_corr[j]; + } + accel_bias[i] += c * params.w_acc_bias * dt; } /* inertial filter prediction for altitude */ inertial_filter_predict(dt, z_est); /* inertial filter correction for altitude */ - baro_alt0 += sonar_corr * params.w_alt_sonar * dt; + if (sonar_valid) { + inertial_filter_correct(sonar_corr, dt, z_est, 0, params.w_alt_sonar); + } + inertial_filter_correct(baro_corr + baro_alt0, dt, z_est, 0, params.w_alt_baro); - inertial_filter_correct(sonar_corr, dt, z_est, 0, params.w_alt_sonar); inertial_filter_correct(accel_corr[2], dt, z_est, 2, params.w_alt_acc); - bool use_gps = ref_xy_inited && gps_valid; - bool use_flow = false; // TODO implement opt flow - - /* try to estimate xy even if no absolute position source available, - * if using optical flow velocity will be valid */ - bool can_estimate_xy = use_gps || use_flow; - if (can_estimate_xy) { /* inertial filter prediction for position */ inertial_filter_predict(dt, x_est); @@ -566,6 +653,11 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) inertial_filter_correct(accel_corr[0], dt, x_est, 2, params.w_pos_acc); inertial_filter_correct(accel_corr[1], dt, y_est, 2, params.w_pos_acc); + if (use_flow) { + inertial_filter_correct(flow_corr[0], dt, x_est, 1, params.w_pos_flow * flow_w); + inertial_filter_correct(flow_corr[1], dt, y_est, 1, params.w_pos_flow * flow_w); + } + if (use_gps) { inertial_filter_correct(gps_corr[0][0], dt, x_est, 0, params.w_pos_gps_p); inertial_filter_correct(gps_corr[1][0], dt, y_est, 0, params.w_pos_gps_p); diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.c b/src/modules/position_estimator_inav/position_estimator_inav_params.c index 4f9ddd009d..b3c32b1805 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.c @@ -41,14 +41,15 @@ #include "position_estimator_inav_params.h" PARAM_DEFINE_FLOAT(INAV_W_ALT_BARO, 0.5f); -PARAM_DEFINE_FLOAT(INAV_W_ALT_ACC, 50.0f); +PARAM_DEFINE_FLOAT(INAV_W_ALT_ACC, 20.0f); PARAM_DEFINE_FLOAT(INAV_W_ALT_SONAR, 3.0f); PARAM_DEFINE_FLOAT(INAV_W_POS_GPS_P, 1.0f); PARAM_DEFINE_FLOAT(INAV_W_POS_GPS_V, 2.0f); PARAM_DEFINE_FLOAT(INAV_W_POS_ACC, 10.0f); -PARAM_DEFINE_FLOAT(INAV_W_POS_FLOW, 0.0f); +PARAM_DEFINE_FLOAT(INAV_W_POS_FLOW, 5.0f); PARAM_DEFINE_FLOAT(INAV_W_ACC_BIAS, 0.0f); -PARAM_DEFINE_FLOAT(INAV_FLOW_K, 1.0f); +PARAM_DEFINE_FLOAT(INAV_FLOW_K, 0.0165f); +PARAM_DEFINE_FLOAT(INAV_FLOW_Q_MIN, 0.5f); PARAM_DEFINE_FLOAT(INAV_SONAR_FILT, 0.02f); PARAM_DEFINE_FLOAT(INAV_SONAR_ERR, 0.5f); PARAM_DEFINE_FLOAT(INAV_LAND_T, 3.0f); @@ -66,6 +67,7 @@ int parameters_init(struct position_estimator_inav_param_handles *h) h->w_pos_flow = param_find("INAV_W_POS_FLOW"); h->w_acc_bias = param_find("INAV_W_ACC_BIAS"); h->flow_k = param_find("INAV_FLOW_K"); + h->flow_q_min = param_find("INAV_FLOW_Q_MIN"); h->sonar_filt = param_find("INAV_SONAR_FILT"); h->sonar_err = param_find("INAV_SONAR_ERR"); h->land_t = param_find("INAV_LAND_T"); @@ -86,6 +88,7 @@ int parameters_update(const struct position_estimator_inav_param_handles *h, str param_get(h->w_pos_flow, &(p->w_pos_flow)); param_get(h->w_acc_bias, &(p->w_acc_bias)); param_get(h->flow_k, &(p->flow_k)); + param_get(h->flow_q_min, &(p->flow_q_min)); param_get(h->sonar_filt, &(p->sonar_filt)); param_get(h->sonar_err, &(p->sonar_err)); param_get(h->land_t, &(p->land_t)); diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.h b/src/modules/position_estimator_inav/position_estimator_inav_params.h index 61570aea7a..562915f498 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.h +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.h @@ -50,6 +50,7 @@ struct position_estimator_inav_params { float w_pos_flow; float w_acc_bias; float flow_k; + float flow_q_min; float sonar_filt; float sonar_err; float land_t; @@ -67,6 +68,7 @@ struct position_estimator_inav_param_handles { param_t w_pos_flow; param_t w_acc_bias; param_t flow_k; + param_t flow_q_min; param_t sonar_filt; param_t sonar_err; param_t land_t; From 81a4df0953e738041d9fdc2b2eb353a635f3003b Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 6 Oct 2013 08:44:30 +0200 Subject: [PATCH 021/447] sensors: slow down updates rate to 200Hz to free some CPU time --- src/modules/sensors/sensors.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 085da905c6..c9513c0caa 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -1541,8 +1541,8 @@ Sensors::task_main() /* rate limit vehicle status updates to 5Hz */ orb_set_interval(_vcontrol_mode_sub, 200); - /* rate limit gyro to 250 Hz (the gyro signal is lowpassed accordingly earlier) */ - orb_set_interval(_gyro_sub, 4); + /* rate limit gyro to 200 Hz (the gyro signal is lowpassed accordingly earlier) */ + orb_set_interval(_gyro_sub, 5); /* * do advertisements From c8c74ea7769ca0db190be81db2d8e1b960702b9d Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 7 Oct 2013 11:59:23 +0200 Subject: [PATCH 022/447] position_estimator_inav: major optimization, poll only attitude topic, publish at 100 Hz --- .../position_estimator_inav_main.c | 54 ++++++++++--------- 1 file changed, 28 insertions(+), 26 deletions(-) diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index 8807020d0d..3d91b45cdf 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -79,7 +79,7 @@ static bool verbose_mode = false; static const hrt_abstime gps_timeout = 1000000; // GPS timeout = 1s static const hrt_abstime flow_timeout = 1000000; // optical flow timeout = 1s static const uint32_t updates_counter_len = 1000000; -static const uint32_t pub_interval = 4000; // limit publish rate to 250 Hz +static const uint32_t pub_interval = 10000; // limit publish rate to 100 Hz __EXPORT int position_estimator_inav_main(int argc, char *argv[]); @@ -308,14 +308,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } /* main loop */ - struct pollfd fds[7] = { - { .fd = parameter_update_sub, .events = POLLIN }, - { .fd = actuator_sub, .events = POLLIN }, - { .fd = armed_sub, .events = POLLIN }, + struct pollfd fds[1] = { { .fd = vehicle_attitude_sub, .events = POLLIN }, - { .fd = sensor_combined_sub, .events = POLLIN }, - { .fd = optical_flow_sub, .events = POLLIN }, - { .fd = vehicle_gps_position_sub, .events = POLLIN } }; if (!thread_should_exit) { @@ -323,7 +317,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } while (!thread_should_exit) { - int ret = poll(fds, 7, 10); // wait maximal this 10 ms = 100 Hz minimum rate + int ret = poll(fds, 1, 20); // wait maximal 20 ms = 50 Hz minimum rate hrt_abstime t = hrt_absolute_time(); if (ret < 0) { @@ -333,36 +327,42 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) continue; } else if (ret > 0) { + /* act on attitude updates */ + + /* vehicle attitude */ + orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &att); + attitude_updates++; + + bool updated; + /* parameter update */ - if (fds[0].revents & POLLIN) { - /* read from param to clear updated flag */ + orb_check(parameter_update_sub, &updated); + + if (updated) { struct parameter_update_s update; - orb_copy(ORB_ID(parameter_update), parameter_update_sub, - &update); - /* update parameters */ + orb_copy(ORB_ID(parameter_update), parameter_update_sub, &update); parameters_update(&pos_inav_param_handles, ¶ms); } /* actuator */ - if (fds[1].revents & POLLIN) { + orb_check(actuator_sub, &updated); + + if (updated) { orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_sub, &actuator); } /* armed */ - if (fds[2].revents & POLLIN) { + orb_check(armed_sub, &updated); + + if (updated) { orb_copy(ORB_ID(actuator_armed), armed_sub, &armed); } - /* vehicle attitude */ - if (fds[3].revents & POLLIN) { - orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &att); - attitude_updates++; - } - /* sensor combined */ - if (fds[4].revents & POLLIN) { - orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor); + orb_check(sensor_combined_sub, &updated); + if (updated) { + orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor); if (sensor.accelerometer_counter > accel_counter) { if (att.R_valid) { /* correct accel bias, now only for Z */ @@ -399,7 +399,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } /* optical flow */ - if (fds[5].revents & POLLIN) { + orb_check(optical_flow_sub, &updated); + if (updated) { orb_copy(ORB_ID(optical_flow), optical_flow_sub, &flow); if (flow.ground_distance_m > 0.31f && flow.ground_distance_m < 4.0f && (flow.ground_distance_m != sonar_prev || t - sonar_time < 150000)) { @@ -436,7 +437,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } /* vehicle GPS position */ - if (fds[6].revents & POLLIN) { + orb_check(vehicle_gps_position_sub, &updated); + if (updated) { orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, &gps); if (gps.fix_type >= 3) { /* hysteresis for GPS quality */ From 537484f60d37f7f04d2ecaeb4139e2c316565eb2 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 7 Oct 2013 11:59:54 +0200 Subject: [PATCH 023/447] Revert "sensors: slow down updates rate to 200Hz to free some CPU time" This reverts commit 81a4df0953e738041d9fdc2b2eb353a635f3003b. --- src/modules/sensors/sensors.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index c9513c0caa..085da905c6 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -1541,8 +1541,8 @@ Sensors::task_main() /* rate limit vehicle status updates to 5Hz */ orb_set_interval(_vcontrol_mode_sub, 200); - /* rate limit gyro to 200 Hz (the gyro signal is lowpassed accordingly earlier) */ - orb_set_interval(_gyro_sub, 5); + /* rate limit gyro to 250 Hz (the gyro signal is lowpassed accordingly earlier) */ + orb_set_interval(_gyro_sub, 4); /* * do advertisements From 1da6565fc6c9095bb125745993e2180c39899d7f Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 7 Oct 2013 15:12:57 +0200 Subject: [PATCH 024/447] position_estimator_inav: code style fixed --- .../position_estimator_inav_main.c | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index 3d43f89add..b3ce594938 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -370,6 +370,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (updated) { orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor); + if (sensor.accelerometer_counter > accel_counter) { if (att.R_valid) { /* correct accel bias, now only for Z */ @@ -407,6 +408,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) /* optical flow */ orb_check(optical_flow_sub, &updated); + if (updated) { orb_copy(ORB_ID(optical_flow), optical_flow_sub, &flow); @@ -490,6 +492,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) /* vehicle GPS position */ orb_check(vehicle_gps_position_sub, &updated); + if (updated) { orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, &gps); @@ -606,17 +609,21 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) /* accelerometer bias correction */ float accel_bias_corr[3] = { 0.0f, 0.0f, 0.0f }; + if (use_gps) { accel_bias_corr[0] -= gps_corr[0][0] * params.w_pos_gps_p * params.w_pos_gps_p; accel_bias_corr[0] -= gps_corr[0][1] * params.w_pos_gps_v; accel_bias_corr[1] -= gps_corr[1][0] * params.w_pos_gps_p * params.w_pos_gps_p; accel_bias_corr[1] -= gps_corr[1][1] * params.w_pos_gps_v; } + if (use_flow) { accel_bias_corr[0] -= flow_corr[0] * params.w_pos_flow; accel_bias_corr[1] -= flow_corr[1] * params.w_pos_flow; } + accel_bias_corr[2] -= (baro_corr + baro_alt0) * params.w_alt_baro * params.w_alt_baro; + if (sonar_valid) { accel_bias_corr[2] -= sonar_corr * params.w_alt_sonar * params.w_alt_sonar; } @@ -624,9 +631,11 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) /* transform error vector from NED frame to body frame */ for (int i = 0; i < 3; i++) { float c = 0.0f; + for (int j = 0; j < 3; j++) { c += att.R[j][i] * accel_bias_corr[j]; } + accel_bias[i] += c * params.w_acc_bias * dt; } From 52cf8836fee0a3d01f4b8402e3fbd8f624ce230a Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 8 Oct 2013 16:38:49 +0200 Subject: [PATCH 025/447] position_estimator_inav: avoid triggering land detector on altitude reference changes, don't reset altitude on arming if sonar is valid --- .../position_estimator_inav_main.c | 30 +++++++++---------- 1 file changed, 15 insertions(+), 15 deletions(-) diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index b3ce594938..55c8e1fb95 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -363,6 +363,15 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (updated) { orb_copy(ORB_ID(actuator_armed), armed_sub, &armed); + /* reset ground level on arm if sonar is invalid */ + if (armed.armed && !flag_armed && t > sonar_valid_time + sonar_valid_timeout) { + flag_armed = armed.armed; + baro_alt0 -= z_est[0]; + z_est[0] = 0.0f; + alt_avg = 0.0f; + local_pos.ref_alt = baro_alt0; + local_pos.ref_timestamp = hrt_absolute_time(); + } } /* sensor combined */ @@ -433,6 +442,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) local_pos.ref_alt = baro_alt0; local_pos.ref_timestamp = hrt_absolute_time(); z_est[0] += sonar_corr; + alt_avg -= sonar_corr; sonar_corr = 0.0f; sonar_corr_filtered = 0.0f; sonar_valid_time = t; @@ -587,14 +597,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) float dt = t_prev > 0 ? (t - t_prev) / 1000000.0f : 0.0f; t_prev = t; - /* reset ground level on arm */ - if (armed.armed && !flag_armed) { - baro_alt0 -= z_est[0]; - z_est[0] = 0.0f; - local_pos.ref_alt = baro_alt0; - local_pos.ref_timestamp = hrt_absolute_time(); - } - bool use_gps = ref_xy_inited && gps_valid; bool use_flow = flow_valid; @@ -687,21 +689,21 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } /* detect land */ - alt_avg += (z_est[0] - alt_avg) * dt / params.land_t; - float alt_disp = z_est[0] - alt_avg; - alt_disp = alt_disp * alt_disp; + alt_avg += (- z_est[0] - alt_avg) * dt / params.land_t; + float alt_disp2 = - z_est[0] - alt_avg; + alt_disp2 = alt_disp2 * alt_disp2; float land_disp2 = params.land_disp * params.land_disp; /* get actual thrust output */ float thrust = armed.armed ? actuator.control[3] : 0.0f; if (landed) { - if (alt_disp > land_disp2 && thrust > params.land_thr) { + if (alt_disp2 > land_disp2 && thrust > params.land_thr) { landed = false; landed_time = 0; } } else { - if (alt_disp < land_disp2 && thrust < params.land_thr) { + if (alt_disp2 < land_disp2 && thrust < params.land_thr) { if (landed_time == 0) { landed_time = t; // land detected first time @@ -791,8 +793,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) orb_publish(ORB_ID(vehicle_global_position), vehicle_global_position_pub, &global_pos); } - - flag_armed = armed.armed; } warnx("stopped"); From ff92770305d015f11facc2c7b0305da7d5fbbf30 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 8 Oct 2013 19:27:20 +0200 Subject: [PATCH 026/447] multirotor_pos_control: track reference position even if not active to handle reference changes properly --- .../multirotor_pos_control.c | 21 +++++++++++-------- 1 file changed, 12 insertions(+), 9 deletions(-) diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c index 53cbf44306..e3bda64e52 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c @@ -234,8 +234,8 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) const float alt_ctl_dz = 0.2f; const float pos_ctl_dz = 0.05f; - float ref_alt = 0.0f; - hrt_abstime ref_alt_t = 0; + float ref_alt_prev = 0.0f; + hrt_abstime ref_alt_prev_t = 0; uint64_t local_ref_timestamp = 0; PID_t xy_pos_pids[2]; @@ -331,11 +331,12 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) t_prev = t; + orb_copy(ORB_ID(vehicle_local_position), local_pos_sub, &local_pos); + if (control_mode.flag_control_altitude_enabled || control_mode.flag_control_velocity_enabled || control_mode.flag_control_position_enabled) { orb_copy(ORB_ID(manual_control_setpoint), manual_sub, &manual); orb_copy(ORB_ID(vehicle_attitude), att_sub, &att); orb_copy(ORB_ID(vehicle_attitude_setpoint), att_sp_sub, &att_sp); - orb_copy(ORB_ID(vehicle_local_position), local_pos_sub, &local_pos); float z_sp_offs_max = params.z_vel_max / params.z_p * 2.0f; float xy_sp_offs_max = params.xy_vel_max / params.xy_p * 2.0f; @@ -344,14 +345,12 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) if (control_mode.flag_control_manual_enabled) { /* manual control */ /* check for reference point updates and correct setpoint */ - if (local_pos.ref_timestamp != ref_alt_t) { - if (ref_alt_t != 0) { - /* home alt changed, don't follow large ground level changes in manual flight */ - local_pos_sp.z += local_pos.ref_alt - ref_alt; + if (local_pos.ref_timestamp != ref_alt_prev_t) { + if (ref_alt_prev_t != 0) { + /* reference alt changed, don't follow large ground level changes in manual flight */ + local_pos_sp.z -= local_pos.ref_alt - ref_alt_prev; } - ref_alt_t = local_pos.ref_timestamp; - ref_alt = local_pos.ref_alt; // TODO also correct XY setpoint } @@ -675,6 +674,10 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) reset_auto_sp_z = true; } + /* track local position reference even when not controlling position */ + ref_alt_prev_t = local_pos.ref_timestamp; + ref_alt_prev = local_pos.ref_alt; + /* reset altitude controller integral (hovering throttle) to manual throttle after manual throttle control */ reset_int_z_manual = control_mode.flag_armed && control_mode.flag_control_manual_enabled && !control_mode.flag_control_climb_rate_enabled; From c96de846b3b0b88859329b8a89fbc22cb8a72bed Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 8 Oct 2013 21:18:17 +0200 Subject: [PATCH 027/447] Added vehicle_local_position.ref_surface_timestamp to track surface level changes. Reference altitude updated continuosly when sonar available. --- .../multirotor_pos_control.c | 14 ++-- .../position_estimator_inav_main.c | 71 ++++++++++++------- .../uORB/topics/vehicle_local_position.h | 1 + 3 files changed, 54 insertions(+), 32 deletions(-) diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c index e3bda64e52..8c3ca66713 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c @@ -235,7 +235,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) const float pos_ctl_dz = 0.05f; float ref_alt_prev = 0.0f; - hrt_abstime ref_alt_prev_t = 0; + hrt_abstime ref_surface_prev_t = 0; uint64_t local_ref_timestamp = 0; PID_t xy_pos_pids[2]; @@ -345,11 +345,10 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) if (control_mode.flag_control_manual_enabled) { /* manual control */ /* check for reference point updates and correct setpoint */ - if (local_pos.ref_timestamp != ref_alt_prev_t) { - if (ref_alt_prev_t != 0) { - /* reference alt changed, don't follow large ground level changes in manual flight */ - local_pos_sp.z -= local_pos.ref_alt - ref_alt_prev; - } + if (local_pos.ref_surface_timestamp != ref_surface_prev_t) { + /* reference alt changed, don't follow large ground level changes in manual flight */ + mavlink_log_info(mavlink_fd, "[mpc] update z sp %.2f -> %.2f = %.2f", ref_alt_prev, local_pos.ref_alt, local_pos.ref_alt - ref_alt_prev); + local_pos_sp.z += local_pos.ref_alt - ref_alt_prev; // TODO also correct XY setpoint } @@ -476,6 +475,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) } else { local_pos_sp.z = local_pos.ref_alt - global_pos_sp.altitude; } + att_sp.yaw_body = global_pos_sp.yaw; mavlink_log_info(mavlink_fd, "[mpc] new sp: %.7f, %.7f (%.2f, %.2f)", (double)sp_lat, sp_lon, (double)local_pos_sp.x, (double)local_pos_sp.y); @@ -675,7 +675,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) } /* track local position reference even when not controlling position */ - ref_alt_prev_t = local_pos.ref_timestamp; + ref_surface_prev_t = local_pos.ref_surface_timestamp; ref_alt_prev = local_pos.ref_alt; /* reset altitude controller integral (hovering throttle) to manual throttle after manual throttle control */ diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index 55c8e1fb95..7a0a919ee3 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -82,6 +82,7 @@ static const hrt_abstime sonar_valid_timeout = 1000000; // assume that altitude static const hrt_abstime flow_timeout = 1000000; // optical flow topic timeout = 1s static const uint32_t updates_counter_len = 1000000; static const uint32_t pub_interval = 10000; // limit publish rate to 100 Hz +static const float max_flow = 1.0f; // max flow value that can be used, rad/s __EXPORT int position_estimator_inav_main(int argc, char *argv[]); @@ -363,6 +364,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (updated) { orb_copy(ORB_ID(actuator_armed), armed_sub, &armed); + /* reset ground level on arm if sonar is invalid */ if (armed.armed && !flag_armed && t > sonar_valid_time + sonar_valid_timeout) { flag_armed = armed.armed; @@ -370,7 +372,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) z_est[0] = 0.0f; alt_avg = 0.0f; local_pos.ref_alt = baro_alt0; - local_pos.ref_timestamp = hrt_absolute_time(); + local_pos.ref_timestamp = t; + local_pos.ref_surface_timestamp = t; } } @@ -439,8 +442,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) /* new ground level */ baro_alt0 += sonar_corr; mavlink_log_info(mavlink_fd, "[inav] update ref: alt=%.3f", baro_alt0); - local_pos.ref_alt = baro_alt0; - local_pos.ref_timestamp = hrt_absolute_time(); + local_pos.ref_surface_timestamp = t; z_est[0] += sonar_corr; alt_avg -= sonar_corr; sonar_corr = 0.0f; @@ -465,32 +467,49 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (z_est[0] < - 0.31f && flow_q > params.flow_q_min && t < sonar_valid_time + sonar_valid_timeout && att.R[2][2] > 0.7) { /* distance to surface */ float flow_dist = -z_est[0] / att.R[2][2]; - /* convert raw flow to angular flow */ - float flow_ang[2]; - flow_ang[0] = flow.flow_raw_x * params.flow_k; - flow_ang[1] = flow.flow_raw_y * params.flow_k; - /* flow measurements vector */ - float flow_m[3]; - flow_m[0] = -flow_ang[0] * flow_dist; - flow_m[1] = -flow_ang[1] * flow_dist; - flow_m[2] = z_est[1]; - /* velocity in NED */ - float flow_v[2] = { 0.0f, 0.0f }; + /* check if flow if too large for accurate measurements */ + /* calculate estimated velocity in body frame */ + float body_v_est[2] = { 0.0f, 0.0f }; - /* project measurements vector to NED basis, skip Z component */ for (int i = 0; i < 2; i++) { - for (int j = 0; j < 3; j++) { - flow_v[i] += att.R[i][j] * flow_m[j]; - } + body_v_est[i] = att.R[0][i] * x_est[1] + att.R[1][i] * y_est[1] + att.R[2][i] * z_est[1]; } - /* velocity correction */ - flow_corr[0] = flow_v[0] - x_est[1]; - flow_corr[1] = flow_v[1] - y_est[1]; - /* adjust correction weight */ - float flow_q_weight = (flow_q - params.flow_q_min) / (1.0f - params.flow_q_min); - flow_w = att.R[2][2] * flow_q_weight; - flow_valid = true; + /* use flow only if it is not too large according to estimate */ + // TODO add timeout for flow to allow flying without GPS + if (fabsf(body_v_est[1] / flow_dist - att.rollspeed) < max_flow && + fabsf(body_v_est[0] / flow_dist + att.pitchspeed) < max_flow) { + /* convert raw flow to angular flow */ + float flow_ang[2]; + flow_ang[0] = flow.flow_raw_x * params.flow_k; + flow_ang[1] = flow.flow_raw_y * params.flow_k; + /* flow measurements vector */ + float flow_m[3]; + flow_m[0] = -flow_ang[0] * flow_dist; + flow_m[1] = -flow_ang[1] * flow_dist; + flow_m[2] = z_est[1]; + /* velocity in NED */ + float flow_v[2] = { 0.0f, 0.0f }; + + /* project measurements vector to NED basis, skip Z component */ + for (int i = 0; i < 2; i++) { + for (int j = 0; j < 3; j++) { + flow_v[i] += att.R[i][j] * flow_m[j]; + } + } + + /* velocity correction */ + flow_corr[0] = flow_v[0] - x_est[1]; + flow_corr[1] = flow_v[1] - y_est[1]; + /* adjust correction weight */ + float flow_q_weight = (flow_q - params.flow_q_min) / (1.0f - params.flow_q_min); + flow_w = att.R[2][2] * flow_q_weight; + flow_valid = true; + + } else { + flow_w = 0.0f; + flow_valid = false; + } } else { flow_w = 0.0f; @@ -607,6 +626,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) /* baro offset correction if sonar available */ if (sonar_valid) { baro_alt0 -= (baro_corr + baro_alt0) * params.w_alt_sonar * dt; + local_pos.ref_alt = baro_alt0; + local_pos.ref_timestamp = t; } /* accelerometer bias correction */ diff --git a/src/modules/uORB/topics/vehicle_local_position.h b/src/modules/uORB/topics/vehicle_local_position.h index 4271537829..a1425d6950 100644 --- a/src/modules/uORB/topics/vehicle_local_position.h +++ b/src/modules/uORB/topics/vehicle_local_position.h @@ -73,6 +73,7 @@ struct vehicle_local_position_s 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_t ref_timestamp; /**< Time when reference position was set */ + uint64_t ref_surface_timestamp; /**< Time when reference surface was set */ int32_t ref_lat; /**< Reference point latitude in 1E7 degrees */ int32_t ref_lon; /**< Reference point longitude in 1E7 degrees */ float ref_alt; /**< Reference altitude AMSL in meters, MUST be set to current (not at reference point!) ground level */ From e4f7767e81e9832e888be787aace15924b4c842e Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 8 Oct 2013 21:19:14 +0200 Subject: [PATCH 028/447] multirotor_pos_control: debug log messages removed --- src/modules/multirotor_pos_control/multirotor_pos_control.c | 1 - 1 file changed, 1 deletion(-) diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c index 8c3ca66713..3016fcc0c0 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c @@ -347,7 +347,6 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) /* check for reference point updates and correct setpoint */ if (local_pos.ref_surface_timestamp != ref_surface_prev_t) { /* reference alt changed, don't follow large ground level changes in manual flight */ - mavlink_log_info(mavlink_fd, "[mpc] update z sp %.2f -> %.2f = %.2f", ref_alt_prev, local_pos.ref_alt, local_pos.ref_alt - ref_alt_prev); local_pos_sp.z += local_pos.ref_alt - ref_alt_prev; // TODO also correct XY setpoint From 9d1027162fb5cf32a6ff22d0d52a5a37a780322c Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 9 Oct 2013 10:47:44 +0200 Subject: [PATCH 029/447] position_estimator_inav: use flow even if it's not accurate if GPS is not available to prevent estimation suspending when no GPS available --- .../position_estimator_inav_main.c | 143 +++++++++--------- 1 file changed, 72 insertions(+), 71 deletions(-) diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index 7a0a919ee3..eaa21f8eb6 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -76,10 +76,11 @@ static bool thread_running = false; /**< Deamon status flag */ static int position_estimator_inav_task; /**< Handle of deamon task / thread */ static bool verbose_mode = false; -static const hrt_abstime gps_timeout = 1000000; // GPS topic timeout = 1s +static const hrt_abstime gps_topic_timeout = 1000000; // GPS topic timeout = 1s +static const hrt_abstime flow_topic_timeout = 1000000; // optical flow topic timeout = 1s static const hrt_abstime sonar_timeout = 150000; // sonar timeout = 150ms static const hrt_abstime sonar_valid_timeout = 1000000; // assume that altitude == distance to surface during this time -static const hrt_abstime flow_timeout = 1000000; // optical flow topic timeout = 1s +static const hrt_abstime flow_valid_timeout = 1000000; // assume that altitude == distance to surface during this time static const uint32_t updates_counter_len = 1000000; static const uint32_t pub_interval = 10000; // limit publish rate to 100 Hz static const float max_flow = 1.0f; // max flow value that can be used, rad/s @@ -224,10 +225,12 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) float sonar_prev = 0.0f; hrt_abstime sonar_time = 0; // time of last sonar measurement (not filtered) hrt_abstime sonar_valid_time = 0; // time of last sonar measurement used for correction (filtered) + hrt_abstime flow_valid_time = 0; // time of last flow measurement used for correction (filtered) - bool gps_valid = false; - bool sonar_valid = false; - bool flow_valid = false; + bool gps_valid = false; // GPS is valid + bool sonar_valid = false; // sonar is valid + bool flow_valid = false; // flow is valid + bool flow_accurate = false; // flow should be accurate (this flag not updated if flow_valid == false) /* declare and safely initialize all structs */ struct actuator_controls_s actuator; @@ -424,42 +427,36 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (updated) { orb_copy(ORB_ID(optical_flow), optical_flow_sub, &flow); - if (flow.ground_distance_m > 0.31f && flow.ground_distance_m < 4.0f && att.R[2][2] > 0.7 && (flow.ground_distance_m != sonar_prev || t < sonar_time + sonar_timeout)) { - if (flow.ground_distance_m != sonar_prev) { - sonar_time = t; - sonar_prev = flow.ground_distance_m; - sonar_corr = -flow.ground_distance_m - z_est[0]; - sonar_corr_filtered += (sonar_corr - sonar_corr_filtered) * params.sonar_filt; + if (flow.ground_distance_m > 0.31f && flow.ground_distance_m < 4.0f && att.R[2][2] > 0.7 && flow.ground_distance_m != sonar_prev) { + sonar_time = t; + sonar_prev = flow.ground_distance_m; + sonar_corr = -flow.ground_distance_m - z_est[0]; + sonar_corr_filtered += (sonar_corr - sonar_corr_filtered) * params.sonar_filt; - if (fabsf(sonar_corr) > params.sonar_err) { - /* correction is too large: spike or new ground level? */ - if (fabsf(sonar_corr - sonar_corr_filtered) > params.sonar_err) { - /* spike detected, ignore */ - sonar_corr = 0.0f; - sonar_valid = false; - - } else { - /* new ground level */ - baro_alt0 += sonar_corr; - mavlink_log_info(mavlink_fd, "[inav] update ref: alt=%.3f", baro_alt0); - local_pos.ref_surface_timestamp = t; - z_est[0] += sonar_corr; - alt_avg -= sonar_corr; - sonar_corr = 0.0f; - sonar_corr_filtered = 0.0f; - sonar_valid_time = t; - sonar_valid = true; - } + if (fabsf(sonar_corr) > params.sonar_err) { + /* correction is too large: spike or new ground level? */ + if (fabsf(sonar_corr - sonar_corr_filtered) > params.sonar_err) { + /* spike detected, ignore */ + sonar_corr = 0.0f; + sonar_valid = false; } else { + /* new ground level */ + baro_alt0 += sonar_corr; + mavlink_log_info(mavlink_fd, "[inav] update ref: alt=%.3f", baro_alt0); + local_pos.ref_surface_timestamp = t; + z_est[0] += sonar_corr; + alt_avg -= sonar_corr; + sonar_corr = 0.0f; + sonar_corr_filtered = 0.0f; sonar_valid_time = t; sonar_valid = true; } - } - } else { - sonar_corr = 0.0f; - sonar_valid = false; + } else { + sonar_valid_time = t; + sonar_valid = true; + } } float flow_q = flow.quality / 255.0f; @@ -475,42 +472,38 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) body_v_est[i] = att.R[0][i] * x_est[1] + att.R[1][i] * y_est[1] + att.R[2][i] * z_est[1]; } - /* use flow only if it is not too large according to estimate */ - // TODO add timeout for flow to allow flying without GPS - if (fabsf(body_v_est[1] / flow_dist - att.rollspeed) < max_flow && - fabsf(body_v_est[0] / flow_dist + att.pitchspeed) < max_flow) { - /* convert raw flow to angular flow */ - float flow_ang[2]; - flow_ang[0] = flow.flow_raw_x * params.flow_k; - flow_ang[1] = flow.flow_raw_y * params.flow_k; - /* flow measurements vector */ - float flow_m[3]; - flow_m[0] = -flow_ang[0] * flow_dist; - flow_m[1] = -flow_ang[1] * flow_dist; - flow_m[2] = z_est[1]; - /* velocity in NED */ - float flow_v[2] = { 0.0f, 0.0f }; + /* set this flag if flow should be accurate according to current velocity and attitude rate estimate */ + flow_accurate = fabsf(body_v_est[1] / flow_dist - att.rollspeed) < max_flow && + fabsf(body_v_est[0] / flow_dist + att.pitchspeed) < max_flow; - /* project measurements vector to NED basis, skip Z component */ - for (int i = 0; i < 2; i++) { - for (int j = 0; j < 3; j++) { - flow_v[i] += att.R[i][j] * flow_m[j]; - } + /* convert raw flow to angular flow */ + float flow_ang[2]; + flow_ang[0] = flow.flow_raw_x * params.flow_k; + flow_ang[1] = flow.flow_raw_y * params.flow_k; + /* flow measurements vector */ + float flow_m[3]; + flow_m[0] = -flow_ang[0] * flow_dist; + flow_m[1] = -flow_ang[1] * flow_dist; + flow_m[2] = z_est[1]; + /* velocity in NED */ + float flow_v[2] = { 0.0f, 0.0f }; + + /* project measurements vector to NED basis, skip Z component */ + for (int i = 0; i < 2; i++) { + for (int j = 0; j < 3; j++) { + flow_v[i] += att.R[i][j] * flow_m[j]; } - - /* velocity correction */ - flow_corr[0] = flow_v[0] - x_est[1]; - flow_corr[1] = flow_v[1] - y_est[1]; - /* adjust correction weight */ - float flow_q_weight = (flow_q - params.flow_q_min) / (1.0f - params.flow_q_min); - flow_w = att.R[2][2] * flow_q_weight; - flow_valid = true; - - } else { - flow_w = 0.0f; - flow_valid = false; } + /* velocity correction */ + flow_corr[0] = flow_v[0] - x_est[1]; + flow_corr[1] = flow_v[1] - y_est[1]; + /* adjust correction weight */ + float flow_q_weight = (flow_q - params.flow_q_min) / (1.0f - params.flow_q_min); + flow_w = att.R[2][2] * flow_q_weight; + flow_valid = true; + flow_valid_time = t; + } else { flow_w = 0.0f; flow_valid = false; @@ -599,7 +592,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } /* check for timeout on FLOW topic */ - if ((flow_valid || sonar_valid) && t > flow.timestamp + flow_timeout) { + if ((flow_valid || sonar_valid) && t > flow.timestamp + flow_topic_timeout) { flow_valid = false; sonar_valid = false; warnx("FLOW timeout"); @@ -607,21 +600,29 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } /* check for timeout on GPS topic */ - if (gps_valid && t > gps.timestamp_position + gps_timeout) { + if (gps_valid && t > gps.timestamp_position + gps_topic_timeout) { gps_valid = false; warnx("GPS timeout"); mavlink_log_info(mavlink_fd, "[inav] GPS timeout"); } + /* check for sonar measurement timeout */ + if (sonar_valid && t > sonar_time + sonar_timeout) { + sonar_corr = 0.0f; + sonar_valid = false; + } + float dt = t_prev > 0 ? (t - t_prev) / 1000000.0f : 0.0f; t_prev = t; + /* use GPS if it's valid and reference position initialized */ bool use_gps = ref_xy_inited && gps_valid; - bool use_flow = flow_valid; + /* use flow if it's valid and (accurate or no GPS available) */ + bool use_flow = flow_valid && (flow_accurate || !use_gps); /* try to estimate xy even if no absolute position source available, * if using optical flow velocity will be valid */ - bool can_estimate_xy = use_gps || use_flow; + bool can_estimate_xy = use_gps || use_flow || (t < flow_valid_time + flow_valid_timeout); /* baro offset correction if sonar available */ if (sonar_valid) { @@ -696,7 +697,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) inertial_filter_correct(gps_corr[0][0], dt, x_est, 0, params.w_pos_gps_p); inertial_filter_correct(gps_corr[1][0], dt, y_est, 0, params.w_pos_gps_p); - if (gps.vel_ned_valid && t < gps.timestamp_velocity + gps_timeout) { + if (gps.vel_ned_valid && t < gps.timestamp_velocity + gps_topic_timeout) { inertial_filter_correct(gps_corr[0][1], dt, x_est, 1, params.w_pos_gps_v); inertial_filter_correct(gps_corr[1][1], dt, y_est, 1, params.w_pos_gps_v); } From 2fc011d20c7bd08bf5e076e79ba33a41f8fcbc30 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 9 Oct 2013 11:05:22 +0200 Subject: [PATCH 030/447] Remove vehicle_local_position.ref_surface_timestamp field, don't sync baro_offset and local_pos.ref_alt instead --- .../multirotor_pos_control.c | 4 +- .../position_estimator_inav_main.c | 42 ++++++++----------- .../uORB/topics/vehicle_local_position.h | 1 - 3 files changed, 20 insertions(+), 27 deletions(-) diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c index 3016fcc0c0..3baebf1ccd 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c @@ -345,7 +345,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) if (control_mode.flag_control_manual_enabled) { /* manual control */ /* check for reference point updates and correct setpoint */ - if (local_pos.ref_surface_timestamp != ref_surface_prev_t) { + if (local_pos.ref_timestamp != ref_surface_prev_t) { /* reference alt changed, don't follow large ground level changes in manual flight */ local_pos_sp.z += local_pos.ref_alt - ref_alt_prev; @@ -674,7 +674,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) } /* track local position reference even when not controlling position */ - ref_surface_prev_t = local_pos.ref_surface_timestamp; + ref_surface_prev_t = local_pos.ref_timestamp; ref_alt_prev = local_pos.ref_alt; /* reset altitude controller integral (hovering throttle) to manual throttle after manual throttle control */ diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index eaa21f8eb6..d37c49696d 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -180,7 +180,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) int baro_init_cnt = 0; int baro_init_num = 200; - float baro_alt0 = 0.0f; /* to determine while start up */ + float baro_offset = 0.0f; // baro offset from reference altitude, it's not the same as local_position.ref_alt if sonar available float alt_avg = 0.0f; bool landed = true; hrt_abstime landed_time = 0; @@ -299,15 +299,15 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) /* mean calculation over several measurements */ if (baro_init_cnt < baro_init_num) { - baro_alt0 += sensor.baro_alt_meter; + baro_offset += sensor.baro_alt_meter; baro_init_cnt++; } else { wait_baro = false; - baro_alt0 /= (float) baro_init_cnt; - warnx("init ref: alt=%.3f", baro_alt0); - mavlink_log_info(mavlink_fd, "[inav] init ref: alt=%.3f", baro_alt0); - local_pos.ref_alt = baro_alt0; + baro_offset /= (float) baro_init_cnt; + warnx("init ref: alt=%.3f", baro_offset); + mavlink_log_info(mavlink_fd, "[inav] init ref: alt=%.3f", baro_offset); + local_pos.ref_alt = baro_offset; local_pos.ref_timestamp = hrt_absolute_time(); local_pos.z_valid = true; local_pos.v_z_valid = true; @@ -323,10 +323,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { .fd = vehicle_attitude_sub, .events = POLLIN }, }; - if (!thread_should_exit) { - warnx("main loop started"); - } - while (!thread_should_exit) { int ret = poll(fds, 1, 20); // wait maximal 20 ms = 50 Hz minimum rate hrt_abstime t = hrt_absolute_time(); @@ -371,12 +367,11 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) /* reset ground level on arm if sonar is invalid */ if (armed.armed && !flag_armed && t > sonar_valid_time + sonar_valid_timeout) { flag_armed = armed.armed; - baro_alt0 -= z_est[0]; + baro_offset -= z_est[0]; z_est[0] = 0.0f; alt_avg = 0.0f; - local_pos.ref_alt = baro_alt0; + local_pos.ref_alt = baro_offset; local_pos.ref_timestamp = t; - local_pos.ref_surface_timestamp = t; } } @@ -415,7 +410,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } if (sensor.baro_counter > baro_counter) { - baro_corr = - sensor.baro_alt_meter - z_est[0]; + baro_corr = - sensor.baro_alt_meter - z_est[0]; // should be shifted by baro_offset before applying correction baro_counter = sensor.baro_counter; baro_updates++; } @@ -442,9 +437,10 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } else { /* new ground level */ - baro_alt0 += sonar_corr; - mavlink_log_info(mavlink_fd, "[inav] update ref: alt=%.3f", baro_alt0); - local_pos.ref_surface_timestamp = t; + baro_offset += sonar_corr; + mavlink_log_info(mavlink_fd, "[inav] update ref: alt=%.3f", baro_offset); + local_pos.ref_alt += sonar_corr; + local_pos.ref_timestamp = t; z_est[0] += sonar_corr; alt_avg -= sonar_corr; sonar_corr = 0.0f; @@ -619,16 +615,14 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) bool use_gps = ref_xy_inited && gps_valid; /* use flow if it's valid and (accurate or no GPS available) */ bool use_flow = flow_valid && (flow_accurate || !use_gps); - /* try to estimate xy even if no absolute position source available, * if using optical flow velocity will be valid */ bool can_estimate_xy = use_gps || use_flow || (t < flow_valid_time + flow_valid_timeout); - /* baro offset correction if sonar available */ + /* baro offset correction if sonar available, + * don't touch reference altitude, local_pos.ref_alt != baro_offset after this */ if (sonar_valid) { - baro_alt0 -= (baro_corr + baro_alt0) * params.w_alt_sonar * dt; - local_pos.ref_alt = baro_alt0; - local_pos.ref_timestamp = t; + baro_offset -= (baro_corr + baro_offset) * params.w_alt_sonar * dt; } /* accelerometer bias correction */ @@ -646,7 +640,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) accel_bias_corr[1] -= flow_corr[1] * params.w_pos_flow; } - accel_bias_corr[2] -= (baro_corr + baro_alt0) * params.w_alt_baro * params.w_alt_baro; + accel_bias_corr[2] -= (baro_corr + baro_offset) * params.w_alt_baro * params.w_alt_baro; if (sonar_valid) { accel_bias_corr[2] -= sonar_corr * params.w_alt_sonar * params.w_alt_sonar; @@ -671,7 +665,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) inertial_filter_correct(sonar_corr, dt, z_est, 0, params.w_alt_sonar); } - inertial_filter_correct(baro_corr + baro_alt0, dt, z_est, 0, params.w_alt_baro); + inertial_filter_correct(baro_corr + baro_offset, dt, z_est, 0, params.w_alt_baro); inertial_filter_correct(accel_corr[2], dt, z_est, 2, params.w_alt_acc); if (can_estimate_xy) { diff --git a/src/modules/uORB/topics/vehicle_local_position.h b/src/modules/uORB/topics/vehicle_local_position.h index a1425d6950..4271537829 100644 --- a/src/modules/uORB/topics/vehicle_local_position.h +++ b/src/modules/uORB/topics/vehicle_local_position.h @@ -73,7 +73,6 @@ struct vehicle_local_position_s 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_t ref_timestamp; /**< Time when reference position was set */ - uint64_t ref_surface_timestamp; /**< Time when reference surface was set */ int32_t ref_lat; /**< Reference point latitude in 1E7 degrees */ int32_t ref_lon; /**< Reference point longitude in 1E7 degrees */ float ref_alt; /**< Reference altitude AMSL in meters, MUST be set to current (not at reference point!) ground level */ From b7d8d77c0f3b68430e9683316c41b0d9df4abd7d Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 9 Oct 2013 11:41:00 +0200 Subject: [PATCH 031/447] position_estimator_inav: if flow is inaccurate, but used for correction with less weight --- .../position_estimator_inav/position_estimator_inav_main.c | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index d37c49696d..506bc1bf05 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -497,6 +497,10 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) /* adjust correction weight */ float flow_q_weight = (flow_q - params.flow_q_min) / (1.0f - params.flow_q_min); flow_w = att.R[2][2] * flow_q_weight; + /* if flow is not accurate, lower weight for it */ + // TODO make this more fuzzy + if (!flow_accurate) + flow_w *= 0.2f; flow_valid = true; flow_valid_time = t; From aedb97f38ed137b4a45152df154819745f998e2f Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 10 Oct 2013 12:35:05 +0200 Subject: [PATCH 032/447] multirotor_pos_control: run mainloop at 100Hz --- src/modules/multirotor_pos_control/multirotor_pos_control.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c index 3baebf1ccd..7abeade80e 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c @@ -680,8 +680,8 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) /* reset altitude controller integral (hovering throttle) to manual throttle after manual throttle control */ reset_int_z_manual = control_mode.flag_armed && control_mode.flag_control_manual_enabled && !control_mode.flag_control_climb_rate_enabled; - /* run at approximately 50 Hz */ - usleep(20000); + /* run at approximately 100 Hz */ + usleep(10000); } warnx("stopped"); From 86c53bee43ab7cb7cb18e9270af0c729440d14ed Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 10 Oct 2013 19:10:45 +0200 Subject: [PATCH 033/447] position_estimator_inav: use independent estimate for distance to ground (sonar), WIP --- .../position_estimator_inav_main.c | 38 ++++++++----------- src/modules/sdlog2/sdlog2.c | 2 + src/modules/sdlog2/sdlog2_messages.h | 4 +- .../uORB/topics/vehicle_local_position.h | 5 +++ 4 files changed, 26 insertions(+), 23 deletions(-) diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index 506bc1bf05..6df7c69b2f 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -180,7 +180,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) int baro_init_cnt = 0; int baro_init_num = 200; - float baro_offset = 0.0f; // baro offset from reference altitude, it's not the same as local_position.ref_alt if sonar available + float baro_offset = 0.0f; // baro offset for reference altitude, initialized only once + float surface_offset = 0.0f; // ground level offset from reference altitude float alt_avg = 0.0f; bool landed = true; hrt_abstime landed_time = 0; @@ -365,7 +366,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(actuator_armed), armed_sub, &armed); /* reset ground level on arm if sonar is invalid */ - if (armed.armed && !flag_armed && t > sonar_valid_time + sonar_valid_timeout) { + if (armed.armed && !flag_armed) { flag_armed = armed.armed; baro_offset -= z_est[0]; z_est[0] = 0.0f; @@ -410,7 +411,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } if (sensor.baro_counter > baro_counter) { - baro_corr = - sensor.baro_alt_meter - z_est[0]; // should be shifted by baro_offset before applying correction + baro_corr = baro_offset - sensor.baro_alt_meter - z_est[0]; baro_counter = sensor.baro_counter; baro_updates++; } @@ -425,7 +426,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (flow.ground_distance_m > 0.31f && flow.ground_distance_m < 4.0f && att.R[2][2] > 0.7 && flow.ground_distance_m != sonar_prev) { sonar_time = t; sonar_prev = flow.ground_distance_m; - sonar_corr = -flow.ground_distance_m - z_est[0]; + sonar_corr = flow.ground_distance_m + surface_offset + z_est[0]; sonar_corr_filtered += (sonar_corr - sonar_corr_filtered) * params.sonar_filt; if (fabsf(sonar_corr) > params.sonar_err) { @@ -437,12 +438,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } else { /* new ground level */ - baro_offset += sonar_corr; - mavlink_log_info(mavlink_fd, "[inav] update ref: alt=%.3f", baro_offset); - local_pos.ref_alt += sonar_corr; - local_pos.ref_timestamp = t; - z_est[0] += sonar_corr; - alt_avg -= sonar_corr; + surface_offset -= sonar_corr; + mavlink_log_info(mavlink_fd, "[inav] new surface level: %.3f", surface_offset); + alt_avg -= sonar_corr; // TODO check this sonar_corr = 0.0f; sonar_corr_filtered = 0.0f; sonar_valid_time = t; @@ -459,7 +457,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (z_est[0] < - 0.31f && flow_q > params.flow_q_min && t < sonar_valid_time + sonar_valid_timeout && att.R[2][2] > 0.7) { /* distance to surface */ - float flow_dist = -z_est[0] / att.R[2][2]; + float flow_dist = (- z_est[0] - surface_offset) / att.R[2][2]; /* check if flow if too large for accurate measurements */ /* calculate estimated velocity in body frame */ float body_v_est[2] = { 0.0f, 0.0f }; @@ -626,7 +624,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) /* baro offset correction if sonar available, * don't touch reference altitude, local_pos.ref_alt != baro_offset after this */ if (sonar_valid) { - baro_offset -= (baro_corr + baro_offset) * params.w_alt_sonar * dt; + surface_offset -= sonar_corr * params.w_alt_sonar * dt; } /* accelerometer bias correction */ @@ -644,11 +642,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) accel_bias_corr[1] -= flow_corr[1] * params.w_pos_flow; } - accel_bias_corr[2] -= (baro_corr + baro_offset) * params.w_alt_baro * params.w_alt_baro; - - if (sonar_valid) { - accel_bias_corr[2] -= sonar_corr * params.w_alt_sonar * params.w_alt_sonar; - } + accel_bias_corr[2] -= baro_corr * params.w_alt_baro * params.w_alt_baro; /* transform error vector from NED frame to body frame */ for (int i = 0; i < 3; i++) { @@ -665,11 +659,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) inertial_filter_predict(dt, z_est); /* inertial filter correction for altitude */ - if (sonar_valid) { - inertial_filter_correct(sonar_corr, dt, z_est, 0, params.w_alt_sonar); - } - - inertial_filter_correct(baro_corr + baro_offset, dt, z_est, 0, params.w_alt_baro); + inertial_filter_correct(baro_corr, dt, z_est, 0, params.w_alt_baro); inertial_filter_correct(accel_corr[2], dt, z_est, 2, params.w_alt_acc); if (can_estimate_xy) { @@ -773,6 +763,10 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) local_pos.vz = z_est[1]; local_pos.landed = landed; local_pos.yaw = att.yaw; + local_pos.dist_bottom_valid = t < sonar_valid_time + sonar_valid_timeout; + if (local_pos.dist_bottom_valid) { + local_pos.dist_bottom = -z_est[0] - surface_offset; + } local_pos.timestamp = t; diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index ec8033202d..7541910d1f 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1046,8 +1046,10 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.body.log_LPOS.ref_lat = buf.local_pos.ref_lat; log_msg.body.log_LPOS.ref_lon = buf.local_pos.ref_lon; log_msg.body.log_LPOS.ref_alt = buf.local_pos.ref_alt; + log_msg.body.log_LPOS.dist_bottom = buf.local_pos.dist_bottom; log_msg.body.log_LPOS.xy_flags = (buf.local_pos.xy_valid ? 1 : 0) | (buf.local_pos.v_xy_valid ? 2 : 0) | (buf.local_pos.xy_global ? 8 : 0); log_msg.body.log_LPOS.z_flags = (buf.local_pos.z_valid ? 1 : 0) | (buf.local_pos.v_z_valid ? 2 : 0) | (buf.local_pos.z_global ? 8 : 0); + log_msg.body.log_LPOS.dist_flags = (buf.local_pos.dist_bottom_valid ? 1 : 0); log_msg.body.log_LPOS.landed = buf.local_pos.landed; LOGBUFFER_WRITE_AND_COUNT(LPOS); } diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 0e88da0547..32dd4f7140 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -109,8 +109,10 @@ struct log_LPOS_s { int32_t ref_lat; int32_t ref_lon; float ref_alt; + float dist_bottom; uint8_t xy_flags; uint8_t z_flags; + uint8_t dist_flags; uint8_t landed; }; @@ -281,7 +283,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(ATSP, "ffff", "RollSP,PitchSP,YawSP,ThrustSP"), LOG_FORMAT(IMU, "fffffffff", "AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ"), LOG_FORMAT(SENS, "ffff", "BaroPres,BaroAlt,BaroTemp,DiffPres"), - LOG_FORMAT(LPOS, "ffffffLLfBBB", "X,Y,Z,VX,VY,VZ,RefLat,RefLon,RefAlt,XYFlags,ZFlags,Landed"), + LOG_FORMAT(LPOS, "ffffffLLffBBBB", "X,Y,Z,VX,VY,VZ,RefLat,RefLon,RefAlt,DstB,XYFl,ZFl,DstFl,Land"), LOG_FORMAT(LPSP, "ffff", "X,Y,Z,Yaw"), LOG_FORMAT(GPS, "QBffLLfffff", "GPSTime,FixType,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog"), LOG_FORMAT(ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"), diff --git a/src/modules/uORB/topics/vehicle_local_position.h b/src/modules/uORB/topics/vehicle_local_position.h index 4271537829..807fc6c099 100644 --- a/src/modules/uORB/topics/vehicle_local_position.h +++ b/src/modules/uORB/topics/vehicle_local_position.h @@ -77,6 +77,11 @@ struct vehicle_local_position_s int32_t ref_lon; /**< Reference point longitude in 1E7 degrees */ float ref_alt; /**< Reference altitude AMSL in meters, MUST be set to current (not at reference point!) ground level */ bool landed; /**< true if vehicle is landed */ + /* Distance to surface */ + float dist_bottom; /**< Distance to bottom surface (ground) */ + float v_dist_bottom; /**< Distance to bottom surface (ground) change rate */ + uint64_t surface_bottom_timestamp; /**< Time when new bottom surface found */ + bool dist_bottom_valid; /**< true if distance to bottom surface is valid */ }; /** From dc09182b950e5ad8fea35ad69d9957b72a9bbee0 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 11 Oct 2013 11:38:36 +0200 Subject: [PATCH 034/447] Added "bottom distance" switch, multirotor_pos_control implemented: use bottom distance to surface to control altitde --- src/modules/commander/commander.cpp | 21 ++++++-- .../commander/state_machine_helper.cpp | 10 ++++ .../multirotor_pos_control.c | 48 ++++++++++++++----- .../position_estimator_inav_main.c | 6 +-- src/modules/sensors/sensor_params.c | 1 + src/modules/sensors/sensors.cpp | 17 +++++++ .../uORB/topics/manual_control_setpoint.h | 1 + src/modules/uORB/topics/rc_channels.h | 29 +++++------ .../uORB/topics/vehicle_control_mode.h | 1 + src/modules/uORB/topics/vehicle_status.h | 13 ++--- 10 files changed, 106 insertions(+), 41 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index a9f6a23513..064e803997 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1398,13 +1398,13 @@ check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_sta /* land switch */ if (!isfinite(sp_man->return_switch)) { - current_status->return_switch = RETURN_SWITCH_NONE; + current_status->return_switch = SWITCH_OFF; } else if (sp_man->return_switch > STICK_ON_OFF_LIMIT) { - current_status->return_switch = RETURN_SWITCH_RETURN; + current_status->return_switch = SWITCH_ON; } else { - current_status->return_switch = RETURN_SWITCH_NONE; + current_status->return_switch = SWITCH_OFF; } /* assisted switch */ @@ -1418,7 +1418,7 @@ check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_sta current_status->assisted_switch = ASSISTED_SWITCH_SEATBELT; } - /* mission switch */ + /* mission switch */ if (!isfinite(sp_man->mission_switch)) { current_status->mission_switch = MISSION_SWITCH_MISSION; @@ -1428,6 +1428,17 @@ check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_sta } else { current_status->mission_switch = MISSION_SWITCH_MISSION; } + + /* distance bottom switch */ + if (!isfinite(sp_man->dist_bottom_switch)) { + current_status->dist_bottom_switch = SWITCH_OFF; + + } else if (sp_man->dist_bottom_switch > STICK_ON_OFF_LIMIT) { + current_status->dist_bottom_switch = SWITCH_ON; + + } else { + current_status->dist_bottom_switch = SWITCH_OFF; + } } transition_result_t @@ -1548,7 +1559,7 @@ check_navigation_state_machine(struct vehicle_status_s *status, struct vehicle_c /* switch to AUTO mode */ if (status->rc_signal_found_once && !status->rc_signal_lost) { /* act depending on switches when manual control enabled */ - if (status->return_switch == RETURN_SWITCH_RETURN) { + if (status->return_switch == SWITCH_ON) { /* RTL */ res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_RTL, control_mode); diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 58f7238ff3..579a0d3d1f 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -424,6 +424,16 @@ navigation_state_transition(struct vehicle_status_s *status, navigation_state_t } } + bool use_dist_bottom_prev = control_mode->flag_use_dist_bottom; + control_mode->flag_use_dist_bottom = control_mode->flag_control_manual_enabled && + control_mode->flag_control_altitude_enabled && status->dist_bottom_switch == SWITCH_ON; + + if (ret == TRANSITION_NOT_CHANGED && control_mode->flag_use_dist_bottom != use_dist_bottom_prev) { + // TODO really, navigation state not changed, set this to force publishing control_mode + ret = TRANSITION_CHANGED; + navigation_state_changed = true; + } + return ret; } diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c index 7abeade80e..f283a1eb45 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c @@ -229,14 +229,16 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) bool reset_auto_sp_xy = true; bool reset_auto_sp_z = true; bool reset_takeoff_sp = true; + bool reset_z_sp_dist = false; + float surface_offset = 0.0f; // Z of ground level + float z_sp_dist = 0.0f; // distance to ground setpoint (positive) hrt_abstime t_prev = 0; const float alt_ctl_dz = 0.2f; const float pos_ctl_dz = 0.05f; - float ref_alt_prev = 0.0f; - hrt_abstime ref_surface_prev_t = 0; uint64_t local_ref_timestamp = 0; + uint64_t surface_bottom_timestamp = 0; PID_t xy_pos_pids[2]; PID_t xy_vel_pids[2]; @@ -250,7 +252,6 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) parameters_init(¶ms_h); parameters_update(¶ms_h, ¶ms); - for (int i = 0; i < 2; i++) { pid_init(&(xy_pos_pids[i]), params.xy_p, 0.0f, params.xy_d, 1.0f, 0.0f, PID_MODE_DERIVATIV_SET, 0.02f); pid_init(&(xy_vel_pids[i]), params.xy_vel_p, params.xy_vel_i, params.xy_vel_d, 1.0f, params.tilt_max, PID_MODE_DERIVATIV_CALC_NO_SP, 0.02f); @@ -345,28 +346,48 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) if (control_mode.flag_control_manual_enabled) { /* manual control */ /* check for reference point updates and correct setpoint */ - if (local_pos.ref_timestamp != ref_surface_prev_t) { + //if (local_pos.ref_timestamp != ref_prev_t) { /* reference alt changed, don't follow large ground level changes in manual flight */ - local_pos_sp.z += local_pos.ref_alt - ref_alt_prev; - + //local_pos_sp.z += local_pos.ref_alt - ref_alt_prev; // TODO also correct XY setpoint - } + //} - /* reset setpoints to current position if needed */ if (control_mode.flag_control_altitude_enabled) { + /* reset setpoints to current position if needed */ if (reset_man_sp_z) { reset_man_sp_z = false; local_pos_sp.z = local_pos.z; + reset_z_sp_dist = true; mavlink_log_info(mavlink_fd, "[mpc] reset alt sp: %.2f", (double) - local_pos_sp.z); } + /* if distance to surface is available, use it */ + if (control_mode.flag_use_dist_bottom && local_pos.dist_bottom_valid) { + if (reset_z_sp_dist) { + reset_z_sp_dist = false; + surface_offset = local_pos.z + local_pos.dist_bottom; + z_sp_dist = -local_pos_sp.z + surface_offset; + mavlink_log_info(mavlink_fd, "[mpc] surface offset: %.2f", surface_offset); + } else { + if (local_pos.surface_bottom_timestamp != surface_bottom_timestamp) { + /* new surface level */ + z_sp_dist += local_pos.z + local_pos.dist_bottom - surface_offset; + } + surface_offset = local_pos.z + local_pos.dist_bottom; + } + /* move altitude setpoint according to ground distance */ + local_pos_sp.z = surface_offset - z_sp_dist; + } + /* move altitude setpoint with throttle stick */ float z_sp_ctl = scale_control(manual.throttle - 0.5f, 0.5f, alt_ctl_dz); if (z_sp_ctl != 0.0f) { sp_move_rate[2] = -z_sp_ctl * params.z_vel_max; local_pos_sp.z += sp_move_rate[2] * dt; + z_sp_dist -= sp_move_rate[2] * dt; // this will have effect only if distance sensor used + // TODO add z_sp_dist correction here if (local_pos_sp.z > local_pos.z + z_sp_offs_max) { local_pos_sp.z = local_pos.z + z_sp_offs_max; @@ -673,13 +694,16 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) reset_auto_sp_z = true; } - /* track local position reference even when not controlling position */ - ref_surface_prev_t = local_pos.ref_timestamp; - ref_alt_prev = local_pos.ref_alt; - /* reset altitude controller integral (hovering throttle) to manual throttle after manual throttle control */ reset_int_z_manual = control_mode.flag_armed && control_mode.flag_control_manual_enabled && !control_mode.flag_control_climb_rate_enabled; + /* reset distance setpoint if distance is not available */ + if (!local_pos.dist_bottom_valid) { + reset_z_sp_dist = true; + } + + surface_bottom_timestamp = local_pos.surface_bottom_timestamp; + /* run at approximately 100 Hz */ usleep(10000); } diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index 6df7c69b2f..8cd161075d 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -439,12 +439,12 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } else { /* new ground level */ surface_offset -= sonar_corr; - mavlink_log_info(mavlink_fd, "[inav] new surface level: %.3f", surface_offset); - alt_avg -= sonar_corr; // TODO check this sonar_corr = 0.0f; sonar_corr_filtered = 0.0f; sonar_valid_time = t; sonar_valid = true; + local_pos.surface_bottom_timestamp = t; + mavlink_log_info(mavlink_fd, "[inav] new surface level: %.2f", surface_offset); } } else { @@ -521,14 +521,12 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (gps_valid) { if (gps.eph_m > 10.0f) { gps_valid = false; - warnx("GPS signal lost"); mavlink_log_info(mavlink_fd, "[inav] GPS signal lost"); } } else { if (gps.eph_m < 5.0f) { gps_valid = true; - warnx("GPS signal found"); mavlink_log_info(mavlink_fd, "[inav] GPS signal found"); } } diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 4d719e0e15..6f5c20d391 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -188,6 +188,7 @@ PARAM_DEFINE_INT32(RC_MAP_MODE_SW, 5); PARAM_DEFINE_INT32(RC_MAP_RETURN_SW, 0); PARAM_DEFINE_INT32(RC_MAP_ASSIST_SW, 6); PARAM_DEFINE_INT32(RC_MAP_MISSIO_SW, 0); +PARAM_DEFINE_INT32(RC_MAP_DIST_B_SW, 0); //PARAM_DEFINE_INT32(RC_MAP_OFFB_SW, 0); diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 085da905c6..78b2fa8eee 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -315,6 +315,7 @@ private: int rc_map_return_sw; int rc_map_assisted_sw; int rc_map_mission_sw; + int rc_map_dist_bottom_sw; // int rc_map_offboard_ctrl_mode_sw; @@ -360,6 +361,7 @@ private: param_t rc_map_return_sw; param_t rc_map_assisted_sw; param_t rc_map_mission_sw; + param_t rc_map_dist_bottom_sw; // param_t rc_map_offboard_ctrl_mode_sw; @@ -578,6 +580,7 @@ Sensors::Sensors() : /* optional mode switches, not mapped per default */ _parameter_handles.rc_map_assisted_sw = param_find("RC_MAP_ASSIST_SW"); _parameter_handles.rc_map_mission_sw = param_find("RC_MAP_MISSIO_SW"); + _parameter_handles.rc_map_dist_bottom_sw = param_find("RC_MAP_DIST_B_SW"); // _parameter_handles.rc_map_offboard_ctrl_mode_sw = param_find("RC_MAP_OFFB_SW"); @@ -726,6 +729,10 @@ Sensors::parameters_update() warnx("Failed getting mission sw chan index"); } + if (param_get(_parameter_handles.rc_map_dist_bottom_sw, &(_parameters.rc_map_dist_bottom_sw)) != OK) { + warnx("Failed getting distance bottom sw chan index"); + } + if (param_get(_parameter_handles.rc_map_flaps, &(_parameters.rc_map_flaps)) != OK) { warnx("Failed getting flaps chan index"); } @@ -754,6 +761,7 @@ Sensors::parameters_update() _rc.function[RETURN] = _parameters.rc_map_return_sw - 1; _rc.function[ASSISTED] = _parameters.rc_map_assisted_sw - 1; _rc.function[MISSION] = _parameters.rc_map_mission_sw - 1; + _rc.function[DIST_BOTTOM] = _parameters.rc_map_dist_bottom_sw - 1; _rc.function[FLAPS] = _parameters.rc_map_flaps - 1; @@ -1330,6 +1338,8 @@ Sensors::ppm_poll() manual_control.return_switch = NAN; manual_control.assisted_switch = NAN; manual_control.mission_switch = NAN; + manual_control.dist_bottom_switch = NAN; + // manual_control.auto_offboard_input_switch = NAN; manual_control.flaps = NAN; @@ -1442,6 +1452,9 @@ Sensors::ppm_poll() /* mission switch input */ manual_control.mission_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MISSION]].scaled); + /* distance bottom switch input */ + manual_control.dist_bottom_switch = limit_minus_one_to_one(_rc.chan[_rc.function[DIST_BOTTOM]].scaled); + /* flaps */ if (_rc.function[FLAPS] >= 0) { @@ -1460,6 +1473,10 @@ Sensors::ppm_poll() manual_control.mission_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MISSION]].scaled); } + if (_rc.function[DIST_BOTTOM] >= 0) { + manual_control.dist_bottom_switch = limit_minus_one_to_one(_rc.chan[_rc.function[DIST_BOTTOM]].scaled); + } + // if (_rc.function[OFFBOARD_MODE] >= 0) { // manual_control.auto_offboard_input_switch = limit_minus_one_to_one(_rc.chan[_rc.function[OFFBOARD_MODE]].scaled); // } diff --git a/src/modules/uORB/topics/manual_control_setpoint.h b/src/modules/uORB/topics/manual_control_setpoint.h index eac6d6e986..6e14d4beec 100644 --- a/src/modules/uORB/topics/manual_control_setpoint.h +++ b/src/modules/uORB/topics/manual_control_setpoint.h @@ -60,6 +60,7 @@ struct manual_control_setpoint_s { float return_switch; /**< land 2 position switch (mandatory): land, no effect */ float assisted_switch; /**< assisted 2 position switch (optional): seatbelt, simple */ float mission_switch; /**< mission 2 position switch (optional): mission, loiter */ + float dist_bottom_switch; /**< distance bottom 2 position switch (optional): off, on */ /** * Any of the channels below may not be available and be set to NaN diff --git a/src/modules/uORB/topics/rc_channels.h b/src/modules/uORB/topics/rc_channels.h index 5a85801439..8b952c28f1 100644 --- a/src/modules/uORB/topics/rc_channels.h +++ b/src/modules/uORB/topics/rc_channels.h @@ -63,20 +63,21 @@ enum RC_CHANNELS_FUNCTION { THROTTLE = 0, - ROLL = 1, - PITCH = 2, - YAW = 3, - MODE = 4, - RETURN = 5, - ASSISTED = 6, - MISSION = 7, - OFFBOARD_MODE = 8, - FLAPS = 9, - AUX_1 = 10, - AUX_2 = 11, - AUX_3 = 12, - AUX_4 = 13, - AUX_5 = 14, + ROLL, + PITCH, + YAW, + MODE, + RETURN, + ASSISTED, + MISSION, + DIST_BOTTOM, + OFFBOARD_MODE, + FLAPS, + AUX_1, + AUX_2, + AUX_3, + AUX_4, + AUX_5, RC_CHANNELS_FUNCTION_MAX /**< indicates the number of functions. There can be more functions than RC channels. */ }; diff --git a/src/modules/uORB/topics/vehicle_control_mode.h b/src/modules/uORB/topics/vehicle_control_mode.h index 093c6775d1..cd004844cf 100644 --- a/src/modules/uORB/topics/vehicle_control_mode.h +++ b/src/modules/uORB/topics/vehicle_control_mode.h @@ -79,6 +79,7 @@ struct vehicle_control_mode_s bool flag_control_position_enabled; /**< true if position is controlled */ bool flag_control_altitude_enabled; /**< true if altitude is controlled */ bool flag_control_climb_rate_enabled; /**< true if climb rate is controlled */ + bool flag_use_dist_bottom; /**< true if bottom distance sensor should be used for altitude control */ bool flag_control_auto_enabled; // TEMP uint8_t auto_state; // TEMP navigation state for AUTO modes diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index 6bc63cbaea..96ae9cbcec 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -106,16 +106,16 @@ typedef enum { ASSISTED_SWITCH_EASY } assisted_switch_pos_t; -typedef enum { - RETURN_SWITCH_NONE = 0, - RETURN_SWITCH_RETURN -} return_switch_pos_t; - typedef enum { MISSION_SWITCH_NONE = 0, MISSION_SWITCH_MISSION } mission_switch_pos_t; +typedef enum { + SWITCH_OFF = 0, + SWITCH_ON +} on_off_switch_pos_t; + enum VEHICLE_MODE_FLAG { VEHICLE_MODE_FLAG_SAFETY_ARMED = 128, VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED = 64, @@ -187,9 +187,10 @@ struct vehicle_status_s bool is_rotary_wing; mode_switch_pos_t mode_switch; - return_switch_pos_t return_switch; + on_off_switch_pos_t return_switch; assisted_switch_pos_t assisted_switch; mission_switch_pos_t mission_switch; + on_off_switch_pos_t dist_bottom_switch; bool condition_battery_voltage_valid; bool condition_system_in_air_restore; /**< true if we can restore in mid air */ From c314f31068ebeba858d98f5411bbdfc2812a8f6b Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 11 Oct 2013 11:58:21 +0200 Subject: [PATCH 035/447] multirotor_pos_control: update altitude setpoint if reference altitude changed --- .../multirotor_pos_control.c | 18 ++++++++++++------ 1 file changed, 12 insertions(+), 6 deletions(-) diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c index f283a1eb45..8d6898cf2e 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c @@ -70,11 +70,13 @@ #include "multirotor_pos_control_params.h" #include "thrust_pid.h" - static bool thread_should_exit = false; /**< Deamon exit flag */ static bool thread_running = false; /**< Deamon status flag */ static int deamon_task; /**< Handle of deamon task / thread */ +static const float alt_ctl_dz = 0.2f; +static const float pos_ctl_dz = 0.05f; + __EXPORT int multirotor_pos_control_main(int argc, char *argv[]); /** @@ -233,9 +235,10 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) float surface_offset = 0.0f; // Z of ground level float z_sp_dist = 0.0f; // distance to ground setpoint (positive) + float ref_alt = 0.0f; + hrt_abstime ref_timestamp = 0; + hrt_abstime t_prev = 0; - const float alt_ctl_dz = 0.2f; - const float pos_ctl_dz = 0.05f; uint64_t local_ref_timestamp = 0; uint64_t surface_bottom_timestamp = 0; @@ -346,11 +349,11 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) if (control_mode.flag_control_manual_enabled) { /* manual control */ /* check for reference point updates and correct setpoint */ - //if (local_pos.ref_timestamp != ref_prev_t) { + if (local_pos.ref_timestamp != ref_timestamp) { /* reference alt changed, don't follow large ground level changes in manual flight */ - //local_pos_sp.z += local_pos.ref_alt - ref_alt_prev; + local_pos_sp.z += local_pos.ref_alt - ref_alt; // TODO also correct XY setpoint - //} + } if (control_mode.flag_control_altitude_enabled) { /* reset setpoints to current position if needed */ @@ -704,6 +707,9 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) surface_bottom_timestamp = local_pos.surface_bottom_timestamp; + ref_alt = local_pos.ref_alt; + ref_timestamp = local_pos.ref_timestamp; + /* run at approximately 100 Hz */ usleep(10000); } From 74af8d2c459f1509e2b1361f0b699c0dd758a34d Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 11 Oct 2013 16:48:52 +0200 Subject: [PATCH 036/447] multirotor_pos_control: reset distance z setpoint when distance_bottom switch switched on --- src/modules/multirotor_pos_control/multirotor_pos_control.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c index 8d6898cf2e..d94b18ead5 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c @@ -701,7 +701,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) reset_int_z_manual = control_mode.flag_armed && control_mode.flag_control_manual_enabled && !control_mode.flag_control_climb_rate_enabled; /* reset distance setpoint if distance is not available */ - if (!local_pos.dist_bottom_valid) { + if (!local_pos.dist_bottom_valid || !control_mode.flag_use_dist_bottom) { reset_z_sp_dist = true; } From 2b1a11b16dd77a5b2a427e7a32a2e398b249ebad Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 11 Oct 2013 20:36:40 +0200 Subject: [PATCH 037/447] position_estimator_inav: bug fixed, allow to disable GPS by setting INAV_W_POS_GPS_P = 0 --- .../position_estimator_inav/position_estimator_inav_main.c | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index 8cd161075d..81271ab528 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -454,10 +454,11 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } float flow_q = flow.quality / 255.0f; + float dist_bottom = - z_est[0] - surface_offset; - if (z_est[0] < - 0.31f && flow_q > params.flow_q_min && t < sonar_valid_time + sonar_valid_timeout && att.R[2][2] > 0.7) { + if (dist_bottom > 0.3f && flow_q > params.flow_q_min && (t < sonar_valid_time + sonar_valid_timeout) && att.R[2][2] > 0.7) { /* distance to surface */ - float flow_dist = (- z_est[0] - surface_offset) / att.R[2][2]; + float flow_dist = dist_bottom / att.R[2][2]; /* check if flow if too large for accurate measurements */ /* calculate estimated velocity in body frame */ float body_v_est[2] = { 0.0f, 0.0f }; @@ -612,7 +613,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) t_prev = t; /* use GPS if it's valid and reference position initialized */ - bool use_gps = ref_xy_inited && gps_valid; + bool use_gps = ref_xy_inited && gps_valid && params.w_pos_gps_p > 0.00001f; /* use flow if it's valid and (accurate or no GPS available) */ bool use_flow = flow_valid && (flow_accurate || !use_gps); /* try to estimate xy even if no absolute position source available, From c0c366d6ee076ca812fa9672709c1e66fafdb32b Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 12 Oct 2013 11:20:20 +0200 Subject: [PATCH 038/447] position_estimator_inav: estimate distance to bottom rate, increase time of position estimation on only accelerometer, reduce weight for GPS if flow available --- .../position_estimator_inav_main.c | 55 ++++++++++++------- .../position_estimator_inav_params.c | 3 + .../position_estimator_inav_params.h | 2 + .../uORB/topics/vehicle_local_position.h | 2 +- 4 files changed, 42 insertions(+), 20 deletions(-) diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index 81271ab528..f9fb5ab1bc 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -79,8 +79,8 @@ static bool verbose_mode = false; static const hrt_abstime gps_topic_timeout = 1000000; // GPS topic timeout = 1s static const hrt_abstime flow_topic_timeout = 1000000; // optical flow topic timeout = 1s static const hrt_abstime sonar_timeout = 150000; // sonar timeout = 150ms -static const hrt_abstime sonar_valid_timeout = 1000000; // assume that altitude == distance to surface during this time -static const hrt_abstime flow_valid_timeout = 1000000; // assume that altitude == distance to surface during this time +static const hrt_abstime sonar_valid_timeout = 1000000; // estimate sonar distance during this time after sonar loss +static const hrt_abstime xy_src_timeout = 2000000; // estimate position during this time after position sources loss static const uint32_t updates_counter_len = 1000000; static const uint32_t pub_interval = 10000; // limit publish rate to 100 Hz static const float max_flow = 1.0f; // max flow value that can be used, rad/s @@ -182,6 +182,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) int baro_init_num = 200; float baro_offset = 0.0f; // baro offset for reference altitude, initialized only once float surface_offset = 0.0f; // ground level offset from reference altitude + float surface_offset_rate = 0.0f; // surface offset change rate float alt_avg = 0.0f; bool landed = true; hrt_abstime landed_time = 0; @@ -226,7 +227,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) float sonar_prev = 0.0f; hrt_abstime sonar_time = 0; // time of last sonar measurement (not filtered) hrt_abstime sonar_valid_time = 0; // time of last sonar measurement used for correction (filtered) - hrt_abstime flow_valid_time = 0; // time of last flow measurement used for correction (filtered) + hrt_abstime xy_src_time = 0; // time of last available position data bool gps_valid = false; // GPS is valid bool sonar_valid = false; // sonar is valid @@ -434,6 +435,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (fabsf(sonar_corr - sonar_corr_filtered) > params.sonar_err) { /* spike detected, ignore */ sonar_corr = 0.0f; + surface_offset_rate = 0.0f; sonar_valid = false; } else { @@ -441,6 +443,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) surface_offset -= sonar_corr; sonar_corr = 0.0f; sonar_corr_filtered = 0.0f; + surface_offset_rate = 0.0f; sonar_valid_time = t; sonar_valid = true; local_pos.surface_bottom_timestamp = t; @@ -448,8 +451,10 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } } else { + /* correction is ok, use it */ sonar_valid_time = t; sonar_valid = true; + surface_offset_rate = -sonar_corr * params.w_alt_sonar; } } @@ -495,13 +500,12 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) flow_corr[1] = flow_v[1] - y_est[1]; /* adjust correction weight */ float flow_q_weight = (flow_q - params.flow_q_min) / (1.0f - params.flow_q_min); - flow_w = att.R[2][2] * flow_q_weight; - /* if flow is not accurate, lower weight for it */ + flow_w = att.R[2][2] * flow_q_weight / fmaxf(1.0f, flow_dist); + /* if flow is not accurate, reduce weight for it */ // TODO make this more fuzzy if (!flow_accurate) - flow_w *= 0.2f; + flow_w *= 0.05f; flow_valid = true; - flow_valid_time = t; } else { flow_w = 0.0f; @@ -606,6 +610,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) /* check for sonar measurement timeout */ if (sonar_valid && t > sonar_time + sonar_timeout) { sonar_corr = 0.0f; + surface_offset_rate = 0.0f; sonar_valid = false; } @@ -616,24 +621,35 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) bool use_gps = ref_xy_inited && gps_valid && params.w_pos_gps_p > 0.00001f; /* use flow if it's valid and (accurate or no GPS available) */ bool use_flow = flow_valid && (flow_accurate || !use_gps); - /* try to estimate xy even if no absolute position source available, - * if using optical flow velocity will be valid */ - bool can_estimate_xy = use_gps || use_flow || (t < flow_valid_time + flow_valid_timeout); + /* try to estimate position during some time after position sources lost */ + if (use_gps || use_flow) { + xy_src_time = t; + } + bool can_estimate_xy = (t < xy_src_time + xy_src_timeout); /* baro offset correction if sonar available, * don't touch reference altitude, local_pos.ref_alt != baro_offset after this */ if (sonar_valid) { - surface_offset -= sonar_corr * params.w_alt_sonar * dt; + surface_offset += surface_offset_rate * dt; } /* accelerometer bias correction */ float accel_bias_corr[3] = { 0.0f, 0.0f, 0.0f }; + float w_pos_gps_p = params.w_pos_gps_p; + float w_pos_gps_v = params.w_pos_gps_v; + + /* reduce GPS weight if optical flow is good */ + if (use_flow && flow_accurate) { + w_pos_gps_p *= params.w_gps_flow; + w_pos_gps_v *= params.w_gps_flow; + } + if (use_gps) { - accel_bias_corr[0] -= gps_corr[0][0] * params.w_pos_gps_p * params.w_pos_gps_p; - accel_bias_corr[0] -= gps_corr[0][1] * params.w_pos_gps_v; - accel_bias_corr[1] -= gps_corr[1][0] * params.w_pos_gps_p * params.w_pos_gps_p; - accel_bias_corr[1] -= gps_corr[1][1] * params.w_pos_gps_v; + accel_bias_corr[0] -= gps_corr[0][0] * w_pos_gps_p * w_pos_gps_p; + accel_bias_corr[0] -= gps_corr[0][1] * w_pos_gps_v; + accel_bias_corr[1] -= gps_corr[1][0] * w_pos_gps_p * w_pos_gps_p; + accel_bias_corr[1] -= gps_corr[1][1] * w_pos_gps_v; } if (use_flow) { @@ -681,12 +697,12 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } if (use_gps) { - inertial_filter_correct(gps_corr[0][0], dt, x_est, 0, params.w_pos_gps_p); - inertial_filter_correct(gps_corr[1][0], dt, y_est, 0, params.w_pos_gps_p); + inertial_filter_correct(gps_corr[0][0], dt, x_est, 0, w_pos_gps_p); + inertial_filter_correct(gps_corr[1][0], dt, y_est, 0, w_pos_gps_p); if (gps.vel_ned_valid && t < gps.timestamp_velocity + gps_topic_timeout) { - inertial_filter_correct(gps_corr[0][1], dt, x_est, 1, params.w_pos_gps_v); - inertial_filter_correct(gps_corr[1][1], dt, y_est, 1, params.w_pos_gps_v); + inertial_filter_correct(gps_corr[0][1], dt, x_est, 1, w_pos_gps_v); + inertial_filter_correct(gps_corr[1][1], dt, y_est, 1, w_pos_gps_v); } } @@ -765,6 +781,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) local_pos.dist_bottom_valid = t < sonar_valid_time + sonar_valid_timeout; if (local_pos.dist_bottom_valid) { local_pos.dist_bottom = -z_est[0] - surface_offset; + local_pos.dist_bottom_rate = -z_est[1] - surface_offset_rate; } local_pos.timestamp = t; diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.c b/src/modules/position_estimator_inav/position_estimator_inav_params.c index b3c32b1805..0a00ae6bbc 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.c @@ -47,6 +47,7 @@ PARAM_DEFINE_FLOAT(INAV_W_POS_GPS_P, 1.0f); PARAM_DEFINE_FLOAT(INAV_W_POS_GPS_V, 2.0f); PARAM_DEFINE_FLOAT(INAV_W_POS_ACC, 10.0f); PARAM_DEFINE_FLOAT(INAV_W_POS_FLOW, 5.0f); +PARAM_DEFINE_FLOAT(INAV_W_GPS_FLOW, 0.1f); PARAM_DEFINE_FLOAT(INAV_W_ACC_BIAS, 0.0f); PARAM_DEFINE_FLOAT(INAV_FLOW_K, 0.0165f); PARAM_DEFINE_FLOAT(INAV_FLOW_Q_MIN, 0.5f); @@ -65,6 +66,7 @@ int parameters_init(struct position_estimator_inav_param_handles *h) h->w_pos_gps_v = param_find("INAV_W_POS_GPS_V"); h->w_pos_acc = param_find("INAV_W_POS_ACC"); h->w_pos_flow = param_find("INAV_W_POS_FLOW"); + h->w_gps_flow = param_find("INAV_W_GPS_FLOW"); h->w_acc_bias = param_find("INAV_W_ACC_BIAS"); h->flow_k = param_find("INAV_FLOW_K"); h->flow_q_min = param_find("INAV_FLOW_Q_MIN"); @@ -86,6 +88,7 @@ int parameters_update(const struct position_estimator_inav_param_handles *h, str param_get(h->w_pos_gps_v, &(p->w_pos_gps_v)); param_get(h->w_pos_acc, &(p->w_pos_acc)); param_get(h->w_pos_flow, &(p->w_pos_flow)); + param_get(h->w_gps_flow, &(p->w_gps_flow)); param_get(h->w_acc_bias, &(p->w_acc_bias)); param_get(h->flow_k, &(p->flow_k)); param_get(h->flow_q_min, &(p->flow_q_min)); diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.h b/src/modules/position_estimator_inav/position_estimator_inav_params.h index 562915f498..e394edfa42 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.h +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.h @@ -48,6 +48,7 @@ struct position_estimator_inav_params { float w_pos_gps_v; float w_pos_acc; float w_pos_flow; + float w_gps_flow; float w_acc_bias; float flow_k; float flow_q_min; @@ -66,6 +67,7 @@ struct position_estimator_inav_param_handles { param_t w_pos_gps_v; param_t w_pos_acc; param_t w_pos_flow; + param_t w_gps_flow; param_t w_acc_bias; param_t flow_k; param_t flow_q_min; diff --git a/src/modules/uORB/topics/vehicle_local_position.h b/src/modules/uORB/topics/vehicle_local_position.h index 807fc6c099..d567f2e020 100644 --- a/src/modules/uORB/topics/vehicle_local_position.h +++ b/src/modules/uORB/topics/vehicle_local_position.h @@ -79,7 +79,7 @@ struct vehicle_local_position_s bool landed; /**< true if vehicle is landed */ /* Distance to surface */ float dist_bottom; /**< Distance to bottom surface (ground) */ - float v_dist_bottom; /**< Distance to bottom surface (ground) change rate */ + float dist_bottom_rate; /**< Distance to bottom surface (ground) change rate */ uint64_t surface_bottom_timestamp; /**< Time when new bottom surface found */ bool dist_bottom_valid; /**< true if distance to bottom surface is valid */ }; From 5d556f185023b2c52dd98093f255cb9cee65e406 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 13 Oct 2013 19:43:17 +0200 Subject: [PATCH 039/447] position_estimator_inav: distance to surface estimation fixes --- .../position_estimator_inav_main.c | 35 ++++++++++++------- .../position_estimator_inav_params.c | 2 +- 2 files changed, 23 insertions(+), 14 deletions(-) diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index f9fb5ab1bc..8f789542ff 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -435,15 +435,14 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (fabsf(sonar_corr - sonar_corr_filtered) > params.sonar_err) { /* spike detected, ignore */ sonar_corr = 0.0f; - surface_offset_rate = 0.0f; sonar_valid = false; } else { /* new ground level */ surface_offset -= sonar_corr; + surface_offset_rate = 0.0f; sonar_corr = 0.0f; sonar_corr_filtered = 0.0f; - surface_offset_rate = 0.0f; sonar_valid_time = t; sonar_valid = true; local_pos.surface_bottom_timestamp = t; @@ -454,7 +453,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) /* correction is ok, use it */ sonar_valid_time = t; sonar_valid = true; - surface_offset_rate = -sonar_corr * params.w_alt_sonar; } } @@ -501,10 +499,12 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) /* adjust correction weight */ float flow_q_weight = (flow_q - params.flow_q_min) / (1.0f - params.flow_q_min); flow_w = att.R[2][2] * flow_q_weight / fmaxf(1.0f, flow_dist); + /* if flow is not accurate, reduce weight for it */ // TODO make this more fuzzy if (!flow_accurate) flow_w *= 0.05f; + flow_valid = true; } else { @@ -610,7 +610,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) /* check for sonar measurement timeout */ if (sonar_valid && t > sonar_time + sonar_timeout) { sonar_corr = 0.0f; - surface_offset_rate = 0.0f; sonar_valid = false; } @@ -621,30 +620,39 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) bool use_gps = ref_xy_inited && gps_valid && params.w_pos_gps_p > 0.00001f; /* use flow if it's valid and (accurate or no GPS available) */ bool use_flow = flow_valid && (flow_accurate || !use_gps); + /* try to estimate position during some time after position sources lost */ if (use_gps || use_flow) { - xy_src_time = t; + xy_src_time = t; } + bool can_estimate_xy = (t < xy_src_time + xy_src_timeout); - /* baro offset correction if sonar available, - * don't touch reference altitude, local_pos.ref_alt != baro_offset after this */ - if (sonar_valid) { + bool dist_bottom_valid = (t < sonar_valid_time + sonar_valid_timeout); + + if (dist_bottom_valid) { + /* surface distance prediction */ surface_offset += surface_offset_rate * dt; + + /* surface distance correction */ + if (sonar_valid) { + surface_offset_rate -= sonar_corr * params.w_alt_sonar * params.w_alt_sonar * dt; + surface_offset -= sonar_corr * params.w_alt_sonar * dt; + } } - /* accelerometer bias correction */ - float accel_bias_corr[3] = { 0.0f, 0.0f, 0.0f }; - + /* reduce GPS weight if optical flow is good */ float w_pos_gps_p = params.w_pos_gps_p; float w_pos_gps_v = params.w_pos_gps_v; - /* reduce GPS weight if optical flow is good */ if (use_flow && flow_accurate) { w_pos_gps_p *= params.w_gps_flow; w_pos_gps_v *= params.w_gps_flow; } + /* accelerometer bias correction */ + float accel_bias_corr[3] = { 0.0f, 0.0f, 0.0f }; + if (use_gps) { accel_bias_corr[0] -= gps_corr[0][0] * w_pos_gps_p * w_pos_gps_p; accel_bias_corr[0] -= gps_corr[0][1] * w_pos_gps_v; @@ -778,7 +786,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) local_pos.vz = z_est[1]; local_pos.landed = landed; local_pos.yaw = att.yaw; - local_pos.dist_bottom_valid = t < sonar_valid_time + sonar_valid_timeout; + local_pos.dist_bottom_valid = dist_bottom_valid; + if (local_pos.dist_bottom_valid) { local_pos.dist_bottom = -z_est[0] - surface_offset; local_pos.dist_bottom_rate = -z_est[1] - surface_offset_rate; diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.c b/src/modules/position_estimator_inav/position_estimator_inav_params.c index 0a00ae6bbc..b3f46fb997 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.c @@ -51,7 +51,7 @@ PARAM_DEFINE_FLOAT(INAV_W_GPS_FLOW, 0.1f); PARAM_DEFINE_FLOAT(INAV_W_ACC_BIAS, 0.0f); PARAM_DEFINE_FLOAT(INAV_FLOW_K, 0.0165f); PARAM_DEFINE_FLOAT(INAV_FLOW_Q_MIN, 0.5f); -PARAM_DEFINE_FLOAT(INAV_SONAR_FILT, 0.02f); +PARAM_DEFINE_FLOAT(INAV_SONAR_FILT, 0.05f); PARAM_DEFINE_FLOAT(INAV_SONAR_ERR, 0.5f); PARAM_DEFINE_FLOAT(INAV_LAND_T, 3.0f); PARAM_DEFINE_FLOAT(INAV_LAND_DISP, 0.7f); From 419cb4bc80d58928d632d0397db75de58534d685 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 13 Oct 2013 19:45:04 +0200 Subject: [PATCH 040/447] sdlog2: DIST (distance to surface) message added --- src/modules/sdlog2/sdlog2.c | 17 +++++++++++++++-- src/modules/sdlog2/sdlog2_messages.h | 15 +++++++++++---- 2 files changed, 26 insertions(+), 6 deletions(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 7541910d1f..76ff27e562 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -668,6 +668,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct log_GPSP_s log_GPSP; struct log_ESC_s log_ESC; struct log_GVSP_s log_GVSP; + struct log_DIST_s log_DIST; } body; } log_msg = { LOG_PACKET_HEADER_INIT(0) @@ -831,6 +832,9 @@ int sdlog2_thread_main(int argc, char *argv[]) uint16_t baro_counter = 0; uint16_t differential_pressure_counter = 0; + /* track changes in distance status */ + bool dist_bottom_present = false; + /* enable logging on start if needed */ if (log_on_start) sdlog2_start_log(); @@ -1046,12 +1050,21 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.body.log_LPOS.ref_lat = buf.local_pos.ref_lat; log_msg.body.log_LPOS.ref_lon = buf.local_pos.ref_lon; log_msg.body.log_LPOS.ref_alt = buf.local_pos.ref_alt; - log_msg.body.log_LPOS.dist_bottom = buf.local_pos.dist_bottom; log_msg.body.log_LPOS.xy_flags = (buf.local_pos.xy_valid ? 1 : 0) | (buf.local_pos.v_xy_valid ? 2 : 0) | (buf.local_pos.xy_global ? 8 : 0); log_msg.body.log_LPOS.z_flags = (buf.local_pos.z_valid ? 1 : 0) | (buf.local_pos.v_z_valid ? 2 : 0) | (buf.local_pos.z_global ? 8 : 0); - log_msg.body.log_LPOS.dist_flags = (buf.local_pos.dist_bottom_valid ? 1 : 0); log_msg.body.log_LPOS.landed = buf.local_pos.landed; LOGBUFFER_WRITE_AND_COUNT(LPOS); + + if (buf.local_pos.dist_bottom_valid) { + dist_bottom_present = true; + } + if (dist_bottom_present) { + log_msg.msg_type = LOG_DIST_MSG; + log_msg.body.log_DIST.bottom = buf.local_pos.dist_bottom; + log_msg.body.log_DIST.bottom_rate = buf.local_pos.dist_bottom_rate; + log_msg.body.log_DIST.flags = (buf.local_pos.dist_bottom_valid ? 1 : 0); + LOGBUFFER_WRITE_AND_COUNT(DIST); + } } /* --- LOCAL POSITION SETPOINT --- */ diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 32dd4f7140..7e11940b74 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -109,10 +109,8 @@ struct log_LPOS_s { int32_t ref_lat; int32_t ref_lon; float ref_alt; - float dist_bottom; uint8_t xy_flags; uint8_t z_flags; - uint8_t dist_flags; uint8_t landed; }; @@ -261,6 +259,14 @@ struct log_FWRV_s { char fw_revision[64]; }; +/* --- DIST - DISTANCE TO SURFACE --- */ +#define LOG_DIST_MSG 21 +struct log_DIST_s { + float bottom; + float bottom_rate; + uint8_t flags; +}; + #pragma pack(pop) @@ -283,7 +289,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(ATSP, "ffff", "RollSP,PitchSP,YawSP,ThrustSP"), LOG_FORMAT(IMU, "fffffffff", "AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ"), LOG_FORMAT(SENS, "ffff", "BaroPres,BaroAlt,BaroTemp,DiffPres"), - LOG_FORMAT(LPOS, "ffffffLLffBBBB", "X,Y,Z,VX,VY,VZ,RefLat,RefLon,RefAlt,DstB,XYFl,ZFl,DstFl,Land"), + LOG_FORMAT(LPOS, "ffffffLLfBBB", "X,Y,Z,VX,VY,VZ,RefLat,RefLon,RefAlt,XYFlags,ZFlags,Landed"), LOG_FORMAT(LPSP, "ffff", "X,Y,Z,Yaw"), LOG_FORMAT(GPS, "QBffLLfffff", "GPSTime,FixType,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog"), LOG_FORMAT(ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"), @@ -297,7 +303,8 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(GPSP, "BLLfffbBffff", "AltRel,Lat,Lon,Alt,Yaw,LoiterR,LoiterDir,NavCmd,P1,P2,P3,P4"), LOG_FORMAT(ESC, "HBBBHHHHHHfH", "Counter,NumESC,Conn,N,Ver,Adr,Volt,Amp,RPM,Temp,SetP,SetPRAW"), LOG_FORMAT(GVSP, "fff", "VX,VY,VZ"), - LOG_FORMAT(FWRV,"Z",FW_VERSION_STR), + LOG_FORMAT(FWRV, "Z", FW_VERSION_STR), + LOG_FORMAT(DIST, "ffB", "Bottom,BottomRate,Flags"), }; static const int log_formats_num = sizeof(log_formats) / sizeof(struct log_format_s); From fe21bb7198841d830cfb2f5d904c8e45532eb27e Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 13 Oct 2013 21:23:53 +0200 Subject: [PATCH 041/447] position_estimator_inav: surface offset estimation improved --- .../position_estimator_inav/position_estimator_inav_main.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index 8f789542ff..ddd2f62c0e 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -636,7 +636,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) /* surface distance correction */ if (sonar_valid) { - surface_offset_rate -= sonar_corr * params.w_alt_sonar * params.w_alt_sonar * dt; + surface_offset_rate -= sonar_corr * 0.5f * params.w_alt_sonar * params.w_alt_sonar * dt; surface_offset -= sonar_corr * params.w_alt_sonar * dt; } } From 9858ff049727a6ec60e4136a28187f60798695bd Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 13 Oct 2013 21:25:08 +0200 Subject: [PATCH 042/447] multirotor_pos_control: distance-based altitude hold rewritten --- .../multirotor_pos_control.c | 29 ++++++++++++++----- .../multirotor_pos_control_params.c | 12 ++++---- 2 files changed, 28 insertions(+), 13 deletions(-) diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c index d94b18ead5..6bc9912b07 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c @@ -233,7 +233,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) bool reset_takeoff_sp = true; bool reset_z_sp_dist = false; float surface_offset = 0.0f; // Z of ground level - float z_sp_dist = 0.0f; // distance to ground setpoint (positive) + float sp_z_dist = 0.0f; // distance to ground setpoint (positive) float ref_alt = 0.0f; hrt_abstime ref_timestamp = 0; @@ -346,6 +346,10 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) float xy_sp_offs_max = params.xy_vel_max / params.xy_p * 2.0f; float sp_move_rate[3] = { 0.0f, 0.0f, 0.0f }; + float sp_z = local_pos_sp.z; + float z = local_pos.z; + float vz = local_pos.vz; + if (control_mode.flag_control_manual_enabled) { /* manual control */ /* check for reference point updates and correct setpoint */ @@ -369,17 +373,23 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) if (reset_z_sp_dist) { reset_z_sp_dist = false; surface_offset = local_pos.z + local_pos.dist_bottom; - z_sp_dist = -local_pos_sp.z + surface_offset; + sp_z_dist = -local_pos_sp.z + surface_offset; mavlink_log_info(mavlink_fd, "[mpc] surface offset: %.2f", surface_offset); + } else { if (local_pos.surface_bottom_timestamp != surface_bottom_timestamp) { /* new surface level */ - z_sp_dist += local_pos.z + local_pos.dist_bottom - surface_offset; + sp_z_dist += local_pos.z + local_pos.dist_bottom - surface_offset; } + surface_offset = local_pos.z + local_pos.dist_bottom; } + + z = -local_pos.dist_bottom; + vz = -local_pos.dist_bottom_rate; /* move altitude setpoint according to ground distance */ - local_pos_sp.z = surface_offset - z_sp_dist; + local_pos_sp.z = surface_offset - sp_z_dist; + sp_z = -sp_z_dist; } /* move altitude setpoint with throttle stick */ @@ -388,7 +398,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) if (z_sp_ctl != 0.0f) { sp_move_rate[2] = -z_sp_ctl * params.z_vel_max; local_pos_sp.z += sp_move_rate[2] * dt; - z_sp_dist -= sp_move_rate[2] * dt; // this will have effect only if distance sensor used + sp_z_dist -= sp_move_rate[2] * dt; // TODO add z_sp_dist correction here if (local_pos_sp.z > local_pos.z + z_sp_offs_max) { @@ -541,6 +551,8 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) reset_man_sp_xy = true; reset_man_sp_z = true; + sp_z = local_pos_sp.z; + } else { /* no control (failsafe), loiter or stay on ground */ if (local_pos.landed) { @@ -578,6 +590,8 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) reset_auto_sp_xy = false; } + + sp_z = local_pos_sp.z; } /* publish local position setpoint */ @@ -585,7 +599,8 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) /* run position & altitude controllers, calculate velocity setpoint */ if (control_mode.flag_control_altitude_enabled) { - global_vel_sp.vz = pid_calculate(&z_pos_pid, local_pos_sp.z, local_pos.z, local_pos.vz - sp_move_rate[2], dt) + sp_move_rate[2]; + // TODO add feed-forward to PID library to allow limiting resulting value + global_vel_sp.vz = pid_calculate(&z_pos_pid, sp_z, z, vz - sp_move_rate[2], dt) + sp_move_rate[2]; } else { reset_man_sp_z = true; @@ -639,7 +654,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) mavlink_log_info(mavlink_fd, "[mpc] reset hovering thrust: %.2f", (double)i); } - thrust_sp[2] = thrust_pid_calculate(&z_vel_pid, global_vel_sp.vz, local_pos.vz, dt, att.R[2][2]); + thrust_sp[2] = thrust_pid_calculate(&z_vel_pid, global_vel_sp.vz, vz, dt, att.R[2][2]); att_sp.thrust = -thrust_sp[2]; } else { diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control_params.c b/src/modules/multirotor_pos_control/multirotor_pos_control_params.c index b7041e4d57..a7d974e636 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control_params.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control_params.c @@ -41,18 +41,18 @@ #include "multirotor_pos_control_params.h" /* controller parameters */ -PARAM_DEFINE_FLOAT(MPC_THR_MIN, 0.2f); -PARAM_DEFINE_FLOAT(MPC_THR_MAX, 0.8f); +PARAM_DEFINE_FLOAT(MPC_THR_MIN, 0.0f); +PARAM_DEFINE_FLOAT(MPC_THR_MAX, 1.0f); PARAM_DEFINE_FLOAT(MPC_Z_P, 1.0f); PARAM_DEFINE_FLOAT(MPC_Z_D, 0.0f); -PARAM_DEFINE_FLOAT(MPC_Z_VEL_P, 0.1f); -PARAM_DEFINE_FLOAT(MPC_Z_VEL_I, 0.0f); +PARAM_DEFINE_FLOAT(MPC_Z_VEL_P, 0.2f); +PARAM_DEFINE_FLOAT(MPC_Z_VEL_I, 0.1f); PARAM_DEFINE_FLOAT(MPC_Z_VEL_D, 0.0f); -PARAM_DEFINE_FLOAT(MPC_Z_VEL_MAX, 3.0f); +PARAM_DEFINE_FLOAT(MPC_Z_VEL_MAX, 2.0f); PARAM_DEFINE_FLOAT(MPC_XY_P, 0.5f); PARAM_DEFINE_FLOAT(MPC_XY_D, 0.0f); PARAM_DEFINE_FLOAT(MPC_XY_VEL_P, 0.2f); -PARAM_DEFINE_FLOAT(MPC_XY_VEL_I, 0.0f); +PARAM_DEFINE_FLOAT(MPC_XY_VEL_I, 0.1f); PARAM_DEFINE_FLOAT(MPC_XY_VEL_D, 0.0f); PARAM_DEFINE_FLOAT(MPC_XY_VEL_MAX, 5.0f); PARAM_DEFINE_FLOAT(MPC_TILT_MAX, 0.5f); From 7d4981d4b4389d28fa05d661ed52be41564b953b Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 18 Oct 2013 14:15:35 +0200 Subject: [PATCH 043/447] multirotor_pos_control: bug fixed --- .../multirotor_pos_control/multirotor_pos_control.c | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c index 6bc9912b07..b53bceb768 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c @@ -389,7 +389,6 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) vz = -local_pos.dist_bottom_rate; /* move altitude setpoint according to ground distance */ local_pos_sp.z = surface_offset - sp_z_dist; - sp_z = -sp_z_dist; } /* move altitude setpoint with throttle stick */ @@ -408,6 +407,12 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) local_pos_sp.z = local_pos.z - z_sp_offs_max; } } + + if (control_mode.flag_use_dist_bottom && local_pos.dist_bottom_valid) { + sp_z = -sp_z_dist; + } else { + sp_z = local_pos_sp.z; + } } if (control_mode.flag_control_position_enabled) { From 1c57d7de434d09893416137f9c72dca2f225cbc7 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 29 Sep 2013 19:00:45 +0200 Subject: [PATCH 044/447] using jacobians in fw attitude control --- src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp | 5 +++-- src/lib/ecl/attitude_fw/ecl_pitch_controller.h | 2 +- src/lib/ecl/attitude_fw/ecl_roll_controller.cpp | 5 +++-- src/lib/ecl/attitude_fw/ecl_roll_controller.h | 2 +- src/modules/fw_att_control/fw_att_control_main.cpp | 4 ++-- 5 files changed, 10 insertions(+), 8 deletions(-) diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp index d876f1a39b..59f4b10bee 100644 --- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp @@ -56,7 +56,7 @@ ECL_PitchController::ECL_PitchController() : { } -float ECL_PitchController::control(float pitch_setpoint, float pitch, float pitch_rate, float roll, float scaler, +float ECL_PitchController::control(float pitch_setpoint, float pitch, float pitch_rate, float roll, float yaw_rate, float scaler, bool lock_integrator, float airspeed_min, float airspeed_max, float airspeed) { /* get the usual dt estimate */ @@ -108,7 +108,8 @@ float ECL_PitchController::control(float pitch_setpoint, float pitch, float pitc float pitch_error = pitch_setpoint - pitch; /* rate setpoint from current error and time constant */ - _rate_setpoint = pitch_error / _tc; + float theta_dot_setpoint = pitch_error / _tc; + _rate_setpoint = cosf(roll) * theta_dot_setpoint + cosf(pitch) * sinf(roll) * yaw_rate; //jacobian /* add turn offset */ _rate_setpoint += turn_offset; diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.h b/src/lib/ecl/attitude_fw/ecl_pitch_controller.h index 1e6cec6a18..41aa486774 100644 --- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.h +++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.h @@ -56,7 +56,7 @@ class __EXPORT ECL_PitchController public: ECL_PitchController(); - float control(float pitch_setpoint, float pitch, float pitch_rate, float roll, float scaler = 1.0f, + float control(float pitch_setpoint, float pitch, float pitch_rate, float roll, float yaw_rate, float scaler = 1.0f, bool lock_integrator = false, float airspeed_min = 0.0f, float airspeed_max = 0.0f, float airspeed = (0.0f / 0.0f)); void reset_integrator(); diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp index b9a73fc029..c42e1856a8 100644 --- a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp @@ -58,7 +58,7 @@ ECL_RollController::ECL_RollController() : } -float ECL_RollController::control(float roll_setpoint, float roll, float roll_rate, +float ECL_RollController::control(float roll_setpoint, float roll, float roll_rate, float pitch, float yaw_rate, float scaler, bool lock_integrator, float airspeed_min, float airspeed_max, float airspeed) { /* get the usual dt estimate */ @@ -79,7 +79,8 @@ float ECL_RollController::control(float roll_setpoint, float roll, float roll_ra } float roll_error = roll_setpoint - roll; - _rate_setpoint = roll_error / _tc; + float phi_dot_setpoint = roll_error / _tc; + _rate_setpoint = phi_dot_setpoint - sinf(pitch) * yaw_rate; //jacobian /* limit the rate */ if (_max_rate > 0.01f) { diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.h b/src/lib/ecl/attitude_fw/ecl_roll_controller.h index 0d4ea9333a..e19812ea8e 100644 --- a/src/lib/ecl/attitude_fw/ecl_roll_controller.h +++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.h @@ -56,7 +56,7 @@ class __EXPORT ECL_RollController public: ECL_RollController(); - float control(float roll_setpoint, float roll, float roll_rate, + float control(float roll_setpoint, float roll, float roll_rate, float pitch, float yaw_rate, float scaler = 1.0f, bool lock_integrator = false, float airspeed_min = 0.0f, float airspeed_max = 0.0f, float airspeed = (0.0f / 0.0f)); void reset_integrator(); diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index eb67874db4..c8a59356ca 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -635,11 +635,11 @@ FixedwingAttitudeControl::task_main() } } - float roll_rad = _roll_ctrl.control(roll_sp, _att.roll, _att.rollspeed, + float roll_rad = _roll_ctrl.control(roll_sp, _att.roll, _att.rollspeed, _att.pitch, _att.yawspeed, airspeed_scaling, lock_integrator, _parameters.airspeed_min, _parameters.airspeed_max, airspeed); _actuators.control[0] = (isfinite(roll_rad)) ? roll_rad * actuator_scaling : 0.0f; - float pitch_rad = _pitch_ctrl.control(pitch_sp, _att.pitch, _att.pitchspeed, _att.roll, airspeed_scaling, + float pitch_rad = _pitch_ctrl.control(pitch_sp, _att.pitch, _att.pitchspeed, _att.roll, _att.yawspeed, airspeed_scaling, lock_integrator, _parameters.airspeed_min, _parameters.airspeed_max, airspeed); _actuators.control[1] = (isfinite(pitch_rad)) ? pitch_rad * actuator_scaling : 0.0f; From cb5e5e914351754356c85e61f2394d1cf91db71f Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 15 Oct 2013 18:34:13 +0200 Subject: [PATCH 045/447] fw att control: also transform rate estimate --- src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp | 11 ++++++++--- src/lib/ecl/attitude_fw/ecl_roll_controller.cpp | 11 +++++++++-- 2 files changed, 17 insertions(+), 5 deletions(-) diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp index 59f4b10bee..9b86d69715 100644 --- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp @@ -107,14 +107,19 @@ float ECL_PitchController::control(float pitch_setpoint, float pitch, float pitc turn_offset = -turn_offset; float pitch_error = pitch_setpoint - pitch; - /* rate setpoint from current error and time constant */ + /* /* Apply P controller: rate setpoint from current error and time constant */ float theta_dot_setpoint = pitch_error / _tc; - _rate_setpoint = cosf(roll) * theta_dot_setpoint + cosf(pitch) * sinf(roll) * yaw_rate; //jacobian + + /* Transform setpoint to body angular rates */ + _rate_setpoint = cosf(roll) * theta_dot_setpoint + cosf(pitch) * sinf(roll) * yaw_rate; //jacobian //XXX: use desired yaw_rate? /* add turn offset */ _rate_setpoint += turn_offset; - _rate_error = _rate_setpoint - pitch_rate; + /* Transform estimation to body angular rates */ + float pitch_rate_body = cosf(roll) * theta_dot_setpoint + cosf(pitch) * sinf(roll) * yaw_rate; //jacobian + + _rate_error = _rate_setpoint - pitch_rate_body; float ilimit_scaled = 0.0f; diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp index c42e1856a8..36186ce68b 100644 --- a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp @@ -79,8 +79,12 @@ float ECL_RollController::control(float roll_setpoint, float roll, float roll_ra } float roll_error = roll_setpoint - roll; + + /* Apply P controller */ float phi_dot_setpoint = roll_error / _tc; - _rate_setpoint = phi_dot_setpoint - sinf(pitch) * yaw_rate; //jacobian + + /* Transform setpoint to body angular rates */ + _rate_setpoint = phi_dot_setpoint - sinf(pitch) * yaw_rate; //jacobian //XXX: use desired yaw_rate? /* limit the rate */ if (_max_rate > 0.01f) { @@ -88,8 +92,11 @@ float ECL_RollController::control(float roll_setpoint, float roll, float roll_ra _rate_setpoint = (_rate_setpoint < -_max_rate) ? -_max_rate : _rate_setpoint; } - _rate_error = _rate_setpoint - roll_rate; + /* Transform estimation to body angular rates */ + float roll_rate_body = roll_rate - sinf(pitch) * yaw_rate; //jacobian + /* Calculate body angular rate error */ + _rate_error = _rate_setpoint - roll_rate_body; //body angular rate error float ilimit_scaled = 0.0f; From 17e0c5053ece4cbf53e659b5f60d640beaab7d50 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 15 Oct 2013 19:05:23 +0200 Subject: [PATCH 046/447] wip, fw att ctrl: coordinated turn --- .../ecl/attitude_fw/ecl_yaw_controller.cpp | 24 +++++++++++---- src/lib/ecl/attitude_fw/ecl_yaw_controller.h | 30 +++---------------- .../fw_att_control/fw_att_control_main.cpp | 2 +- 3 files changed, 23 insertions(+), 33 deletions(-) diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp index b786acf24f..9601fa5447 100644 --- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp @@ -47,12 +47,10 @@ ECL_YawController::ECL_YawController() : _last_run(0), - _last_error(0.0f), _last_output(0.0f), - _last_rate_hp_out(0.0f), - _last_rate_hp_in(0.0f), - _k_d_last(0.0f), - _integrator(0.0f) + _rate_setpoint(0.0f), + _max_deflection_rad(math::radians(45.0f)) + { } @@ -66,7 +64,21 @@ float ECL_YawController::control(float roll, float yaw_rate, float accel_y, floa float dt = (dt_micros > 500000) ? 0.0f : dt_micros / 1000000; - return 0.0f; +// float psi_dot = 0.0f; +// float denumerator = (speed_body[0] * cosf(att_sp->roll_body) * cosf(att_sp->pitch_body) + speed_body[2] * sinf(att_sp->pitch_body)); +// if(denumerator != 0.0f) { +// psi_dot = (speed_body[2] * phi_dot + 9.81f * sinf(att_sp->roll_body) * cosf(att_sp->pitch_body) + speed_body[0] * theta_dot * sinf(att_sp->roll_body)) +// / (speed_body[0] * cosf(att_sp->roll_body) * cosf(att_sp->pitch_body) + speed_body[2] * sinf(att_sp->pitch_body)); +// } + + /* Calculate desired yaw rate from coordinated turn constraint / (no side forces) */ + _last_output = 0.0f; + float denumerator = (speed_body_u * cosf(roll) * cosf(pitch) + speed_body_w * sinf(pitch)); + if(denumerator != 0.0f) { //XXX: floating point comparison + _last_output = (speed_body_w * roll_rate_desired + 9.81f * sinf(roll) * cosf(pitch) + speed_body_u * pitch_rate_desired * sinf(roll)) / denumerator; + } + + return math::constrain(_last_output, -_max_deflection_rad, _max_deflection_rad); } void ECL_YawController::reset_integrator() diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.h b/src/lib/ecl/attitude_fw/ecl_yaw_controller.h index 66b2279182..fe0abb956b 100644 --- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.h +++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.h @@ -50,39 +50,17 @@ public: float control(float roll, float yaw_rate, float accel_y, float scaler = 1.0f, bool lock_integrator = false, float airspeed_min = 0, float airspeed_max = 0, float aspeed = (0.0f / 0.0f)); - void reset_integrator(); - void set_k_side(float k_a) { - _k_side = k_a; - } - void set_k_i(float k_i) { - _k_i = k_i; - } - void set_k_d(float k_d) { - _k_d = k_d; - } - void set_k_roll_ff(float k_roll_ff) { - _k_roll_ff = k_roll_ff; - } - void set_integrator_max(float max) { - _integrator_max = max; + float get_desired_rate() { + return _rate_setpoint; } private: uint64_t _last_run; - float _k_side; - float _k_i; - float _k_d; - float _k_roll_ff; - float _integrator_max; - - float _last_error; + float _rate_setpoint; float _last_output; - float _last_rate_hp_out; - float _last_rate_hp_in; - float _k_d_last; - float _integrator; + float _max_deflection_rad; }; diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index c8a59356ca..8cb515dcca 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -661,7 +661,7 @@ FixedwingAttitudeControl::task_main() vehicle_rates_setpoint_s rates_sp; rates_sp.roll = _roll_ctrl.get_desired_rate(); rates_sp.pitch = _pitch_ctrl.get_desired_rate(); - rates_sp.yaw = 0.0f; // XXX not yet implemented + rates_sp.yaw = _yaw_ctrl.get_desired_rate(); rates_sp.timestamp = hrt_absolute_time(); From b21b9078e2d719bfd7af9580ac3b9b5957ef1d57 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 15 Oct 2013 22:00:56 +0200 Subject: [PATCH 047/447] wip, fw attitude ctrl: split into attitude and rate, compiles, untested --- .../ecl/attitude_fw/ecl_pitch_controller.cpp | 77 +++++++++++------ .../ecl/attitude_fw/ecl_pitch_controller.h | 17 +++- .../ecl/attitude_fw/ecl_roll_controller.cpp | 42 +++++---- src/lib/ecl/attitude_fw/ecl_roll_controller.h | 16 +++- .../ecl/attitude_fw/ecl_yaw_controller.cpp | 86 ++++++++++++++++--- src/lib/ecl/attitude_fw/ecl_yaw_controller.h | 62 ++++++++++++- .../fw_att_control/fw_att_control_main.cpp | 84 ++++++++++++++---- 7 files changed, 299 insertions(+), 85 deletions(-) diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp index 9b86d69715..30a46b0e7b 100644 --- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp @@ -56,29 +56,9 @@ ECL_PitchController::ECL_PitchController() : { } -float ECL_PitchController::control(float pitch_setpoint, float pitch, float pitch_rate, float roll, float yaw_rate, float scaler, - bool lock_integrator, float airspeed_min, float airspeed_max, float airspeed) +float ECL_PitchController::control_attitude(float pitch_setpoint, float roll, float pitch, float airspeed) { - /* get the usual dt estimate */ - uint64_t dt_micros = ecl_elapsed_time(&_last_run); - _last_run = ecl_absolute_time(); - float dt = dt_micros / 1000000; - - /* lock integral for long intervals */ - if (dt_micros > 500000) - lock_integrator = true; - - float k_roll_ff = math::max((_k_p - _k_i * _tc) * _tc - _k_d, 0.0f); - float k_i_rate = _k_i * _tc; - - /* input conditioning */ - if (!isfinite(airspeed)) { - /* airspeed is NaN, +- INF or not available, pick center of band */ - airspeed = 0.5f * (airspeed_min + airspeed_max); - } else if (airspeed < airspeed_min) { - airspeed = airspeed_min; - } /* flying inverted (wings upside down) ? */ bool inverted = false; @@ -106,20 +86,61 @@ float ECL_PitchController::control(float pitch_setpoint, float pitch, float pitc if (inverted) turn_offset = -turn_offset; + /* Calculate the error */ float pitch_error = pitch_setpoint - pitch; - /* /* Apply P controller: rate setpoint from current error and time constant */ - float theta_dot_setpoint = pitch_error / _tc; - /* Transform setpoint to body angular rates */ - _rate_setpoint = cosf(roll) * theta_dot_setpoint + cosf(pitch) * sinf(roll) * yaw_rate; //jacobian //XXX: use desired yaw_rate? + /* Apply P controller: rate setpoint from current error and time constant */ + float _rate_setpoint = pitch_error / _tc; /* add turn offset */ _rate_setpoint += turn_offset; - /* Transform estimation to body angular rates */ - float pitch_rate_body = cosf(roll) * theta_dot_setpoint + cosf(pitch) * sinf(roll) * yaw_rate; //jacobian + /* limit the rate */ //XXX: move to body angluar rates + if (_max_rate_pos > 0.01f && _max_rate_neg > 0.01f) { + if (_rate_setpoint > 0.0f) { + _rate_setpoint = (_rate_setpoint > _max_rate_pos) ? _max_rate_pos : _rate_setpoint; + } else { + _rate_setpoint = (_rate_setpoint < -_max_rate_neg) ? -_max_rate_neg : _rate_setpoint; + } - _rate_error = _rate_setpoint - pitch_rate_body; + } + + return _rate_setpoint; +} + +float ECL_PitchController::control_bodyrate(float roll, float pitch, + float pitch_rate, float yaw_rate, + float yaw_rate_setpoint, + float airspeed_min, float airspeed_max, float airspeed, float scaler, bool lock_integrator) +{ + /* get the usual dt estimate */ + uint64_t dt_micros = ecl_elapsed_time(&_last_run); + _last_run = ecl_absolute_time(); + + float dt = dt_micros / 1000000; + + /* lock integral for long intervals */ + if (dt_micros > 500000) + lock_integrator = true; + + float k_roll_ff = math::max((_k_p - _k_i * _tc) * _tc - _k_d, 0.0f); + float k_i_rate = _k_i * _tc; + + /* input conditioning */ + if (!isfinite(airspeed)) { + /* airspeed is NaN, +- INF or not available, pick center of band */ + airspeed = 0.5f * (airspeed_min + airspeed_max); + } else if (airspeed < airspeed_min) { + airspeed = airspeed_min; + } + + /* Transform setpoint to body angular rates */ + _bodyrate_setpoint = cosf(roll) * _rate_setpoint + cosf(pitch) * sinf(roll) * yaw_rate_setpoint; //jacobian + + /* Transform estimation to body angular rates */ + float pitch_bodyrate = cosf(roll) * pitch_rate + cosf(pitch) * sinf(roll) * yaw_rate; //jacobian + + _rate_error = _bodyrate_setpoint - pitch_bodyrate; float ilimit_scaled = 0.0f; diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.h b/src/lib/ecl/attitude_fw/ecl_pitch_controller.h index 41aa486774..ef6129d026 100644 --- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.h +++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.h @@ -36,6 +36,7 @@ * Definition of a simple orthogonal pitch PID controller. * * @author Lorenz Meier + * @author Thomas Gubler * * Acknowledgements: * @@ -51,13 +52,18 @@ #include #include -class __EXPORT ECL_PitchController +class __EXPORT ECL_PitchController //XXX: create controller superclass { public: ECL_PitchController(); - float control(float pitch_setpoint, float pitch, float pitch_rate, float roll, float yaw_rate, float scaler = 1.0f, - bool lock_integrator = false, float airspeed_min = 0.0f, float airspeed_max = 0.0f, float airspeed = (0.0f / 0.0f)); + float control_attitude(float pitch_setpoint, float roll, float pitch, float airspeed); + + + float control_bodyrate(float roll, float pitch, + float pitch_rate, float yaw_rate, + float yaw_rate_setpoint, + float airspeed_min = 0.0f, float airspeed_max = 0.0f, float airspeed = (0.0f / 0.0f), float scaler = 1.0f, bool lock_integrator = false); void reset_integrator(); @@ -94,6 +100,10 @@ public: return _rate_setpoint; } + float get_desired_bodyrate() { + return _bodyrate_setpoint; + } + private: uint64_t _last_run; @@ -109,6 +119,7 @@ private: float _integrator; float _rate_error; float _rate_setpoint; + float _bodyrate_setpoint; float _max_deflection_rad; }; diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp index 36186ce68b..3afda31765 100644 --- a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp @@ -53,18 +53,38 @@ ECL_RollController::ECL_RollController() : _integrator(0.0f), _rate_error(0.0f), _rate_setpoint(0.0f), + _bodyrate_setpoint(0.0f), _max_deflection_rad(math::radians(45.0f)) { } -float ECL_RollController::control(float roll_setpoint, float roll, float roll_rate, float pitch, float yaw_rate, - float scaler, bool lock_integrator, float airspeed_min, float airspeed_max, float airspeed) +float ECL_RollController::control_attitude(float roll_setpoint, float roll) +{ + + /* Calculate error */ + float roll_error = roll_setpoint - 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(float pitch, + float roll_rate, float yaw_rate, + float yaw_rate_setpoint, + float airspeed_min, float airspeed_max, float airspeed, float scaler, bool lock_integrator) { /* get the usual dt estimate */ uint64_t dt_micros = ecl_elapsed_time(&_last_run); _last_run = ecl_absolute_time(); - float dt = (dt_micros > 500000) ? 0.0f : dt_micros / 1000000; float k_ff = math::max((_k_p - _k_i * _tc) * _tc - _k_d, 0.0f); @@ -78,25 +98,15 @@ float ECL_RollController::control(float roll_setpoint, float roll, float roll_ra airspeed = airspeed_min; } - float roll_error = roll_setpoint - roll; - - /* Apply P controller */ - float phi_dot_setpoint = roll_error / _tc; /* Transform setpoint to body angular rates */ - _rate_setpoint = phi_dot_setpoint - sinf(pitch) * yaw_rate; //jacobian //XXX: use desired yaw_rate? - - /* limit the rate */ - 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; - } + _bodyrate_setpoint = _rate_setpoint - sinf(pitch) * yaw_rate_setpoint; //jacobian /* Transform estimation to body angular rates */ - float roll_rate_body = roll_rate - sinf(pitch) * yaw_rate; //jacobian + float roll_bodyrate = roll_rate - sinf(pitch) * yaw_rate; //jacobian /* Calculate body angular rate error */ - _rate_error = _rate_setpoint - roll_rate_body; //body angular rate error + _rate_error = _bodyrate_setpoint - roll_bodyrate; //body angular rate error float ilimit_scaled = 0.0f; diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.h b/src/lib/ecl/attitude_fw/ecl_roll_controller.h index e19812ea8e..f94db0dc77 100644 --- a/src/lib/ecl/attitude_fw/ecl_roll_controller.h +++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.h @@ -36,6 +36,7 @@ * Definition of a simple orthogonal roll PID controller. * * @author Lorenz Meier + * @author Thomas Gubler * * Acknowledgements: * @@ -51,13 +52,17 @@ #include #include -class __EXPORT ECL_RollController +class __EXPORT ECL_RollController //XXX: create controller superclass { public: ECL_RollController(); - float control(float roll_setpoint, float roll, float roll_rate, float pitch, float yaw_rate, - float scaler = 1.0f, bool lock_integrator = false, float airspeed_min = 0.0f, float airspeed_max = 0.0f, float airspeed = (0.0f / 0.0f)); + float control_attitude(float roll_setpoint, float roll); + + float control_bodyrate(float pitch, + float roll_rate, float yaw_rate, + float yaw_rate_setpoint, + float airspeed_min = 0.0f, float airspeed_max = 0.0f, float airspeed = (0.0f / 0.0f), float scaler = 1.0f, bool lock_integrator = false); void reset_integrator(); @@ -90,6 +95,10 @@ public: return _rate_setpoint; } + float get_desired_bodyrate() { + return _bodyrate_setpoint; + } + private: uint64_t _last_run; float _tc; @@ -102,6 +111,7 @@ private: float _integrator; float _rate_error; float _rate_setpoint; + float _bodyrate_setpoint; float _max_deflection_rad; }; diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp index 9601fa5447..d2cd9cf041 100644 --- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp @@ -47,37 +47,95 @@ ECL_YawController::ECL_YawController() : _last_run(0), + _tc(0.1f), _last_output(0.0f), + _integrator(0.0f), + _rate_error(0.0f), _rate_setpoint(0.0f), + _bodyrate_setpoint(0.0f), _max_deflection_rad(math::radians(45.0f)) { } -float ECL_YawController::control(float roll, float yaw_rate, float accel_y, float scaler, bool lock_integrator, - float airspeed_min, float airspeed_max, float aspeed) +float ECL_YawController::control_attitude(float roll, float pitch, + float speed_body_u, float speed_body_w, + float roll_rate_setpoint, float pitch_rate_setpoint) +{ + /* Calculate desired yaw rate from coordinated turn constraint / (no side forces) */ + _rate_setpoint = 0.0f; + float denumerator = (speed_body_u * cosf(roll) * cosf(pitch) + speed_body_w * sinf(pitch)); + if(denumerator != 0.0f) { //XXX: floating point comparison + _rate_setpoint = (speed_body_w * roll_rate_setpoint + 9.81f * sinf(roll) * cosf(pitch) + speed_body_u * pitch_rate_setpoint * sinf(roll)) / denumerator; + } + + /* 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_YawController::control_bodyrate(float roll, float pitch, + float pitch_rate, float yaw_rate, + float pitch_rate_setpoint, + float airspeed_min, float airspeed_max, float airspeed, float scaler, bool lock_integrator) { /* get the usual dt estimate */ uint64_t dt_micros = ecl_elapsed_time(&_last_run); _last_run = ecl_absolute_time(); - float dt = (dt_micros > 500000) ? 0.0f : dt_micros / 1000000; -// float psi_dot = 0.0f; -// float denumerator = (speed_body[0] * cosf(att_sp->roll_body) * cosf(att_sp->pitch_body) + speed_body[2] * sinf(att_sp->pitch_body)); -// if(denumerator != 0.0f) { -// psi_dot = (speed_body[2] * phi_dot + 9.81f * sinf(att_sp->roll_body) * cosf(att_sp->pitch_body) + speed_body[0] * theta_dot * sinf(att_sp->roll_body)) -// / (speed_body[0] * cosf(att_sp->roll_body) * cosf(att_sp->pitch_body) + speed_body[2] * sinf(att_sp->pitch_body)); -// } + float k_ff = math::max((_k_p - _k_i * _tc) * _tc - _k_d, 0.0f); + float k_i_rate = _k_i * _tc; - /* Calculate desired yaw rate from coordinated turn constraint / (no side forces) */ - _last_output = 0.0f; - float denumerator = (speed_body_u * cosf(roll) * cosf(pitch) + speed_body_w * sinf(pitch)); - if(denumerator != 0.0f) { //XXX: floating point comparison - _last_output = (speed_body_w * roll_rate_desired + 9.81f * sinf(roll) * cosf(pitch) + speed_body_u * pitch_rate_desired * sinf(roll)) / denumerator; + /* input conditioning */ + if (!isfinite(airspeed)) { + /* airspeed is NaN, +- INF or not available, pick center of band */ + airspeed = 0.5f * (airspeed_min + airspeed_max); + } else if (airspeed < airspeed_min) { + airspeed = airspeed_min; } + + /* Transform setpoint to body angular rates */ + _bodyrate_setpoint = -sinf(roll) * pitch_rate_setpoint * cosf(roll)*cosf(pitch) * _rate_setpoint; //jacobian + + /* Transform estimation to body angular rates */ + float yaw_bodyrate = -sinf(roll) * pitch_rate * cosf(roll)*cosf(pitch) * yaw_rate; //jacobian + + /* Calculate body angular rate error */ + _rate_error = _bodyrate_setpoint - yaw_bodyrate; //body angular rate error + + float ilimit_scaled = 0.0f; + + if (!lock_integrator && k_i_rate > 0.0f && airspeed > 0.5f * airspeed_min) { + + float id = _rate_error * k_i_rate * dt * scaler; + + /* + * anti-windup: do not allow integrator to increase into the + * wrong direction if actuator is at limit + */ + if (_last_output < -_max_deflection_rad) { + /* only allow motion to center: increase value */ + id = math::max(id, 0.0f); + } else if (_last_output > _max_deflection_rad) { + /* only allow motion to center: decrease value */ + id = math::min(id, 0.0f); + } + + _integrator += id; + } + + /* integrator limit */ + _integrator = math::constrain(_integrator, -ilimit_scaled, ilimit_scaled); + /* store non-limited output */ + _last_output = ((_rate_error * _k_d * scaler) + _integrator + (_rate_setpoint * k_ff)) * scaler; + return math::constrain(_last_output, -_max_deflection_rad, _max_deflection_rad); } diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.h b/src/lib/ecl/attitude_fw/ecl_yaw_controller.h index fe0abb956b..d5aaa617fe 100644 --- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.h +++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.h @@ -35,6 +35,15 @@ * @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 @@ -42,24 +51,69 @@ #include #include -class __EXPORT ECL_YawController +class __EXPORT ECL_YawController //XXX: create controller superclass { public: ECL_YawController(); - float control(float roll, float yaw_rate, float accel_y, float scaler = 1.0f, bool lock_integrator = false, - float airspeed_min = 0, float airspeed_max = 0, float aspeed = (0.0f / 0.0f)); + float control_attitude(float roll, float pitch, + float speed_body_u, float speed_body_w, + float roll_rate_setpoint, float pitch_rate_setpoint); + float control_bodyrate( float roll, float pitch, + float pitch_rate, float yaw_rate, + float pitch_rate_setpoint, + float airspeed_min, float airspeed_max, float airspeed, float scaler, bool lock_integrator); + + void reset_integrator(); + + void set_k_p(float k_p) { + _k_p = k_p; + } + void set_k_i(float k_i) { + _k_i = k_i; + } + void set_k_d(float k_d) { + _k_d = k_d; + } + void set_integrator_max(float max) { + _integrator_max = max; + } + void set_max_rate(float max_rate) { + _max_rate = max_rate; + } + void set_k_roll_ff(float roll_ff) { + _roll_ff = roll_ff; + } + + + float get_rate_error() { + return _rate_error; + } float get_desired_rate() { return _rate_setpoint; } + float get_desired_bodyrate() { + return _bodyrate_setpoint; + } + private: uint64_t _last_run; - float _rate_setpoint; + float _tc; + float _k_p; + float _k_i; + float _k_d; + float _integrator_max; + float _max_rate; + float _roll_ff; float _last_output; + float _integrator; + float _rate_error; + float _rate_setpoint; + float _bodyrate_setpoint; float _max_deflection_rad; }; diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index 8cb515dcca..422e94ba18 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -37,6 +37,7 @@ * Implementation of a generic attitude controller based on classic orthogonal PIDs. * * @author Lorenz Meier + * @author Thomas Gubler * */ @@ -62,6 +63,7 @@ #include #include #include +#include #include #include #include @@ -106,26 +108,28 @@ private: bool _task_should_exit; /**< if true, sensor task should exit */ int _control_task; /**< task handle for sensor task */ - int _att_sub; /**< vehicle attitude subscription */ - int _accel_sub; /**< accelerometer subscription */ + int _att_sub; /**< vehicle attitude subscription */ + int _accel_sub; /**< accelerometer subscription */ int _att_sp_sub; /**< vehicle attitude setpoint */ int _attitude_sub; /**< raw rc channels data subscription */ int _airspeed_sub; /**< airspeed subscription */ - int _vcontrol_mode_sub; /**< vehicle status subscription */ - int _params_sub; /**< notification of parameter updates */ - int _manual_sub; /**< notification of manual control updates */ + int _vcontrol_mode_sub; /**< vehicle status subscription */ + int _params_sub; /**< notification of parameter updates */ + int _manual_sub; /**< notification of manual control updates */ + int _global_pos_sub; /**< global position subscription */ orb_advert_t _rate_sp_pub; /**< rate setpoint publication */ orb_advert_t _attitude_sp_pub; /**< attitude setpoint point */ orb_advert_t _actuators_0_pub; /**< actuator control group 0 setpoint */ - struct vehicle_attitude_s _att; /**< vehicle attitude */ - struct accel_report _accel; /**< body frame accelerations */ + struct vehicle_attitude_s _att; /**< vehicle attitude */ + struct accel_report _accel; /**< body frame accelerations */ struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */ struct manual_control_setpoint_s _manual; /**< r/c channel data */ - struct airspeed_s _airspeed; /**< airspeed */ - struct vehicle_control_mode_s _vcontrol_mode; /**< vehicle control mode */ - struct actuator_controls_s _actuators; /**< actuator control inputs */ + struct airspeed_s _airspeed; /**< airspeed */ + struct vehicle_control_mode_s _vcontrol_mode; /**< vehicle control mode */ + struct actuator_controls_s _actuators; /**< actuator control inputs */ + struct vehicle_global_position_s _global_pos; /**< global position */ perf_counter_t _loop_perf; /**< loop performance counter */ @@ -226,6 +230,11 @@ private: */ void vehicle_setpoint_poll(); + /** + * Check for global position updates. + */ + void global_pos_poll(); + /** * Shim for calling task_main from task_create. */ @@ -261,6 +270,7 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : _vcontrol_mode_sub(-1), _params_sub(-1), _manual_sub(-1), + _global_pos_sub(-1), /* publications */ _rate_sp_pub(-1), @@ -375,7 +385,7 @@ FixedwingAttitudeControl::parameters_update() _roll_ctrl.set_max_rate(math::radians(_parameters.r_rmax)); /* yaw control parameters */ - _yaw_ctrl.set_k_side(math::radians(_parameters.y_p)); + _yaw_ctrl.set_k_p(math::radians(_parameters.y_p)); _yaw_ctrl.set_k_i(math::radians(_parameters.y_i)); _yaw_ctrl.set_k_d(math::radians(_parameters.y_d)); _yaw_ctrl.set_k_roll_ff(math::radians(_parameters.y_roll_feedforward)); @@ -452,6 +462,18 @@ FixedwingAttitudeControl::vehicle_setpoint_poll() } } +void +FixedwingAttitudeControl::global_pos_poll() +{ + /* check if there is a new setpoint */ + bool global_pos_updated; + orb_check(_global_pos_sub, &global_pos_updated); + + if (global_pos_updated) { + orb_copy(ORB_ID(vehicle_global_position), _global_pos_sub, &_global_pos); + } +} + void FixedwingAttitudeControl::task_main_trampoline(int argc, char *argv[]) { @@ -476,6 +498,7 @@ FixedwingAttitudeControl::task_main() _vcontrol_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); _manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); + _global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); /* rate limit vehicle status updates to 5Hz */ orb_set_interval(_vcontrol_mode_sub, 200); @@ -551,6 +574,8 @@ FixedwingAttitudeControl::task_main() vehicle_manual_poll(); + global_pos_poll(); + /* lock integrator until control is started */ bool lock_integrator; @@ -635,16 +660,41 @@ FixedwingAttitudeControl::task_main() } } - float roll_rad = _roll_ctrl.control(roll_sp, _att.roll, _att.rollspeed, _att.pitch, _att.yawspeed, - airspeed_scaling, lock_integrator, _parameters.airspeed_min, _parameters.airspeed_max, airspeed); + /* Prepare speed_body_u and speed_body_w */ + float speed_body_u = 0.0f; + float speed_body_w = 0.0f; + if(_att.R_valid) { + speed_body_u = _att.R[0][0] * _global_pos.vx + _att.R[0][1] * _global_pos.vy + _att.R[0][2] * _global_pos.vz; +// speed_body_v = _att.R[1][0] * _global_pos.vx + _att.R[1][1] * _global_pos.vy + _att.R[1][2] * _global_pos.vz; + speed_body_w = _att.R[2][0] * _global_pos.vx + _att.R[2][1] * _global_pos.vy + _att.R[2][2] * _global_pos.vz; + } else { + warnx("Did not get a valid R\n"); + } + + /* Run attitude controllers */ + _roll_ctrl.control_attitude(roll_sp, _att.roll); + _pitch_ctrl.control_attitude(roll_sp, _att.roll, _att.pitch, airspeed); + _yaw_ctrl.control_attitude(_att.roll, _att.pitch, + speed_body_u,speed_body_w, + _roll_ctrl.get_desired_rate(), _pitch_ctrl.get_desired_rate()); //runs last, because is depending on output of roll and pitch attitude + + /* Run attitude RATE controllers which need the desired attitudes from above */ + float roll_rad = _roll_ctrl.control_bodyrate(_att.pitch, + _att.rollspeed, _att.yawspeed, + _yaw_ctrl.get_desired_rate(), + _parameters.airspeed_min, _parameters.airspeed_max, airspeed, airspeed_scaling, lock_integrator); _actuators.control[0] = (isfinite(roll_rad)) ? roll_rad * actuator_scaling : 0.0f; - float pitch_rad = _pitch_ctrl.control(pitch_sp, _att.pitch, _att.pitchspeed, _att.roll, _att.yawspeed, airspeed_scaling, - lock_integrator, _parameters.airspeed_min, _parameters.airspeed_max, airspeed); + float pitch_rad = _pitch_ctrl.control_bodyrate(_att.roll, _att.pitch, + _att.pitchspeed, _att.yawspeed, + _yaw_ctrl.get_desired_rate(), + _parameters.airspeed_min, _parameters.airspeed_max, airspeed, airspeed_scaling, lock_integrator); _actuators.control[1] = (isfinite(pitch_rad)) ? pitch_rad * actuator_scaling : 0.0f; - float yaw_rad = _yaw_ctrl.control(_att.roll, _att.yawspeed, _accel.y, airspeed_scaling, lock_integrator, - _parameters.airspeed_min, _parameters.airspeed_max, airspeed); + float yaw_rad = _yaw_ctrl.control_bodyrate( _att.roll, _att.pitch, + _att.pitchspeed, _att.yawspeed, + _pitch_ctrl.get_desired_rate(), + _parameters.airspeed_min, _parameters.airspeed_max, airspeed, airspeed_scaling, lock_integrator); _actuators.control[2] = (isfinite(yaw_rad)) ? yaw_rad * actuator_scaling : 0.0f; /* throttle passed through */ From a0ea0901b555b4c7732dbc2da22339d82f581e48 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 17 Oct 2013 20:29:54 +0200 Subject: [PATCH 048/447] wip, minor bugfixes in fw att control --- src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp | 2 +- src/modules/fw_att_control/fw_att_control_main.cpp | 8 ++++---- src/modules/fw_att_control/fw_att_control_params.c | 4 +++- 3 files changed, 8 insertions(+), 6 deletions(-) diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp index 30a46b0e7b..e1f4d3eef0 100644 --- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp @@ -90,7 +90,7 @@ float ECL_PitchController::control_attitude(float pitch_setpoint, float roll, fl float pitch_error = pitch_setpoint - pitch; /* Apply P controller: rate setpoint from current error and time constant */ - float _rate_setpoint = pitch_error / _tc; + _rate_setpoint = pitch_error / _tc; /* add turn offset */ _rate_setpoint += turn_offset; diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index 422e94ba18..bc9d6771b6 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -673,7 +673,7 @@ FixedwingAttitudeControl::task_main() /* Run attitude controllers */ _roll_ctrl.control_attitude(roll_sp, _att.roll); - _pitch_ctrl.control_attitude(roll_sp, _att.roll, _att.pitch, airspeed); + _pitch_ctrl.control_attitude(pitch_sp, _att.roll, _att.pitch, airspeed); _yaw_ctrl.control_attitude(_att.roll, _att.pitch, speed_body_u,speed_body_w, _roll_ctrl.get_desired_rate(), _pitch_ctrl.get_desired_rate()); //runs last, because is depending on output of roll and pitch attitude @@ -709,9 +709,9 @@ FixedwingAttitudeControl::task_main() * only once available */ vehicle_rates_setpoint_s rates_sp; - rates_sp.roll = _roll_ctrl.get_desired_rate(); - rates_sp.pitch = _pitch_ctrl.get_desired_rate(); - rates_sp.yaw = _yaw_ctrl.get_desired_rate(); + rates_sp.roll = _roll_ctrl.get_desired_bodyrate(); + rates_sp.pitch = _pitch_ctrl.get_desired_bodyrate(); + rates_sp.yaw = _yaw_ctrl.get_desired_bodyrate(); rates_sp.timestamp = hrt_absolute_time(); diff --git a/src/modules/fw_att_control/fw_att_control_params.c b/src/modules/fw_att_control/fw_att_control_params.c index 97aa275de2..717608159a 100644 --- a/src/modules/fw_att_control/fw_att_control_params.c +++ b/src/modules/fw_att_control/fw_att_control_params.c @@ -44,6 +44,8 @@ #include +//XXX resolve unclear naming of paramters: FW_P_D --> FW_PR_P + /* * Controller parameters, accessible via MAVLink * @@ -130,7 +132,7 @@ PARAM_DEFINE_FLOAT(FW_Y_P, 0); PARAM_DEFINE_FLOAT(FW_Y_I, 0); PARAM_DEFINE_FLOAT(FW_Y_IMAX, 15.0f); PARAM_DEFINE_FLOAT(FW_Y_D, 0); -PARAM_DEFINE_FLOAT(FW_Y_ROLLFF, 1); +PARAM_DEFINE_FLOAT(FW_Y_ROLLFF, 0); PARAM_DEFINE_FLOAT(FW_AIRSPD_MIN, 9.0f); PARAM_DEFINE_FLOAT(FW_AIRSPD_TRIM, 12.0f); PARAM_DEFINE_FLOAT(FW_AIRSPD_MAX, 18.0f); From b9ef3636f5210588d0aa219a163d3ef5edd6a204 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 24 Oct 2013 17:43:50 +0200 Subject: [PATCH 049/447] change wrong comment --- src/modules/fw_att_control/fw_att_control_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index bc9d6771b6..7f3e7893ef 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -465,7 +465,7 @@ FixedwingAttitudeControl::vehicle_setpoint_poll() void FixedwingAttitudeControl::global_pos_poll() { - /* check if there is a new setpoint */ + /* check if there is a new global position */ bool global_pos_updated; orb_check(_global_pos_sub, &global_pos_updated); From 8f74aab468d565101eaa1e0f104b0297343fe2ed Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 19 Oct 2013 14:14:44 +0200 Subject: [PATCH 050/447] fw att control: bugfixes for integrator --- .../ecl/attitude_fw/ecl_pitch_controller.cpp | 11 +++---- .../ecl/attitude_fw/ecl_roll_controller.cpp | 13 ++++---- .../ecl/attitude_fw/ecl_yaw_controller.cpp | 30 ++++++++++++------- src/lib/ecl/attitude_fw/ecl_yaw_controller.h | 4 +++ .../fw_att_control/fw_att_control_main.cpp | 18 +++++++---- .../fw_att_control/fw_att_control_params.c | 1 + 6 files changed, 48 insertions(+), 29 deletions(-) diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp index e1f4d3eef0..5fe55163f3 100644 --- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp @@ -116,8 +116,7 @@ float ECL_PitchController::control_bodyrate(float roll, float pitch, /* get the usual dt estimate */ uint64_t dt_micros = ecl_elapsed_time(&_last_run); _last_run = ecl_absolute_time(); - - float dt = dt_micros / 1000000; + float dt = (dt_micros > 500000) ? 0.0f : (float)dt_micros * 1e-6f; /* lock integral for long intervals */ if (dt_micros > 500000) @@ -142,11 +141,9 @@ float ECL_PitchController::control_bodyrate(float roll, float pitch, _rate_error = _bodyrate_setpoint - pitch_bodyrate; - float ilimit_scaled = 0.0f; - if (!lock_integrator && k_i_rate > 0.0f && airspeed > 0.5f * airspeed_min) { - float id = _rate_error * k_i_rate * dt * scaler; + float id = _rate_error * dt; /* * anti-windup: do not allow integrator to increase into the @@ -164,9 +161,9 @@ float ECL_PitchController::control_bodyrate(float roll, float pitch, } /* integrator limit */ - _integrator = math::constrain(_integrator, -ilimit_scaled, ilimit_scaled); + _integrator = math::constrain(_integrator, -_integrator_max, _integrator_max); /* store non-limited output */ - _last_output = ((_rate_error * _k_d * scaler) + _integrator + (_rate_setpoint * k_roll_ff)) * scaler; + _last_output = ((_rate_error * _k_d * scaler) + _integrator * k_i_rate * scaler + (_rate_setpoint * k_roll_ff)) * scaler; return math::constrain(_last_output, -_max_deflection_rad, _max_deflection_rad); } diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp index 3afda31765..1aac827923 100644 --- a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp @@ -45,6 +45,7 @@ #include #include #include +#include ECL_RollController::ECL_RollController() : _last_run(0), @@ -85,7 +86,7 @@ float ECL_RollController::control_bodyrate(float pitch, /* get the usual dt estimate */ uint64_t dt_micros = ecl_elapsed_time(&_last_run); _last_run = ecl_absolute_time(); - float dt = (dt_micros > 500000) ? 0.0f : dt_micros / 1000000; + float dt = (dt_micros > 500000) ? 0.0f : (float)dt_micros * 1e-6f; float k_ff = math::max((_k_p - _k_i * _tc) * _tc - _k_d, 0.0f); float k_i_rate = _k_i * _tc; @@ -108,11 +109,9 @@ float ECL_RollController::control_bodyrate(float pitch, /* Calculate body angular rate error */ _rate_error = _bodyrate_setpoint - roll_bodyrate; //body angular rate error - float ilimit_scaled = 0.0f; - if (!lock_integrator && k_i_rate > 0.0f && airspeed > 0.5f * airspeed_min) { - float id = _rate_error * k_i_rate * dt * scaler; + float id = _rate_error * dt; /* * anti-windup: do not allow integrator to increase into the @@ -130,9 +129,11 @@ float ECL_RollController::control_bodyrate(float pitch, } /* integrator limit */ - _integrator = math::constrain(_integrator, -ilimit_scaled, ilimit_scaled); + _integrator = math::constrain(_integrator, -_integrator_max, _integrator_max); + //warnx("roll: _integrator: %.4f, _integrator_max: %.4f", (double)_integrator, (double)_integrator_max); + /* store non-limited output */ - _last_output = ((_rate_error * _k_d * scaler) + _integrator + (_rate_setpoint * k_ff)) * scaler; + _last_output = ((_rate_error * _k_d * scaler) + _integrator * k_i_rate * scaler + (_rate_setpoint * k_ff)) * scaler; return math::constrain(_last_output, -_max_deflection_rad, _max_deflection_rad); } diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp index d2cd9cf041..7dfed3379f 100644 --- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp @@ -44,6 +44,7 @@ #include #include #include +#include ECL_YawController::ECL_YawController() : _last_run(0), @@ -53,7 +54,8 @@ ECL_YawController::ECL_YawController() : _rate_error(0.0f), _rate_setpoint(0.0f), _bodyrate_setpoint(0.0f), - _max_deflection_rad(math::radians(45.0f)) + _max_deflection_rad(math::radians(45.0f)), + _coordinated(1.0f) { @@ -63,11 +65,18 @@ float ECL_YawController::control_attitude(float roll, float pitch, float speed_body_u, float speed_body_w, float roll_rate_setpoint, float pitch_rate_setpoint) { +// static int counter = 0; /* Calculate desired yaw rate from coordinated turn constraint / (no side forces) */ _rate_setpoint = 0.0f; - float denumerator = (speed_body_u * cosf(roll) * cosf(pitch) + speed_body_w * sinf(pitch)); - if(denumerator != 0.0f) { //XXX: floating point comparison - _rate_setpoint = (speed_body_w * roll_rate_setpoint + 9.81f * sinf(roll) * cosf(pitch) + speed_body_u * pitch_rate_setpoint * sinf(roll)) / denumerator; + if (_coordinated > 0.1) { + float denumerator = (speed_body_u * cosf(roll) * cosf(pitch) + speed_body_w * sinf(pitch)); + if(denumerator != 0.0f) { //XXX: floating point comparison + _rate_setpoint = (speed_body_w * roll_rate_setpoint + 9.81f * sinf(roll) * cosf(pitch) + speed_body_u * pitch_rate_setpoint * sinf(roll)) / denumerator; + } + +// 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 @@ -76,6 +85,9 @@ float ECL_YawController::control_attitude(float roll, float pitch, _rate_setpoint = (_rate_setpoint < -_max_rate) ? -_max_rate : _rate_setpoint; } + +// counter++; + return _rate_setpoint; } @@ -87,7 +99,7 @@ float ECL_YawController::control_bodyrate(float roll, float pitch, /* get the usual dt estimate */ uint64_t dt_micros = ecl_elapsed_time(&_last_run); _last_run = ecl_absolute_time(); - float dt = (dt_micros > 500000) ? 0.0f : dt_micros / 1000000; + float dt = (dt_micros > 500000) ? 0.0f : (float)dt_micros * 1e-6f; float k_ff = math::max((_k_p - _k_i * _tc) * _tc - _k_d, 0.0f); float k_i_rate = _k_i * _tc; @@ -110,11 +122,9 @@ float ECL_YawController::control_bodyrate(float roll, float pitch, /* Calculate body angular rate error */ _rate_error = _bodyrate_setpoint - yaw_bodyrate; //body angular rate error - float ilimit_scaled = 0.0f; - if (!lock_integrator && k_i_rate > 0.0f && airspeed > 0.5f * airspeed_min) { - float id = _rate_error * k_i_rate * dt * scaler; + float id = _rate_error * dt; /* * anti-windup: do not allow integrator to increase into the @@ -132,9 +142,9 @@ float ECL_YawController::control_bodyrate(float roll, float pitch, } /* integrator limit */ - _integrator = math::constrain(_integrator, -ilimit_scaled, ilimit_scaled); + _integrator = math::constrain(_integrator, -_integrator_max, _integrator_max); /* store non-limited output */ - _last_output = ((_rate_error * _k_d * scaler) + _integrator + (_rate_setpoint * k_ff)) * scaler; + _last_output = ((_rate_error * _k_d * scaler) + _integrator * k_i_rate * scaler + (_rate_setpoint * k_ff)) * scaler; return math::constrain(_last_output, -_max_deflection_rad, _max_deflection_rad); } diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.h b/src/lib/ecl/attitude_fw/ecl_yaw_controller.h index d5aaa617fe..0250fcb352 100644 --- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.h +++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.h @@ -85,6 +85,9 @@ public: void set_k_roll_ff(float roll_ff) { _roll_ff = roll_ff; } + void set_coordinated(float coordinated) { + _coordinated = coordinated; + } float get_rate_error() { @@ -115,6 +118,7 @@ private: float _rate_setpoint; float _bodyrate_setpoint; float _max_deflection_rad; + float _coordinated; }; diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index 7f3e7893ef..91981b08a0 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -155,6 +155,7 @@ private: float y_d; float y_roll_feedforward; float y_integrator_max; + float y_coordinated; float airspeed_min; float airspeed_trim; @@ -181,6 +182,7 @@ private: param_t y_d; param_t y_roll_feedforward; param_t y_integrator_max; + param_t y_coordinated; param_t airspeed_min; param_t airspeed_trim; @@ -289,25 +291,27 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : _parameter_handles.p_i = param_find("FW_P_I"); _parameter_handles.p_rmax_pos = param_find("FW_P_RMAX_POS"); _parameter_handles.p_rmax_neg = param_find("FW_P_RMAX_NEG"); - _parameter_handles.p_integrator_max = param_find("FW_P_integrator_max"); + _parameter_handles.p_integrator_max = param_find("FW_P_IMAX"); _parameter_handles.p_roll_feedforward = param_find("FW_P_ROLLFF"); _parameter_handles.r_p = param_find("FW_R_P"); _parameter_handles.r_d = param_find("FW_R_D"); _parameter_handles.r_i = param_find("FW_R_I"); - _parameter_handles.r_integrator_max = param_find("FW_R_integrator_max"); + _parameter_handles.r_integrator_max = param_find("FW_R_IMAX"); _parameter_handles.r_rmax = param_find("FW_R_RMAX"); _parameter_handles.y_p = param_find("FW_Y_P"); _parameter_handles.y_i = param_find("FW_Y_I"); _parameter_handles.y_d = param_find("FW_Y_D"); _parameter_handles.y_roll_feedforward = param_find("FW_Y_ROLLFF"); - _parameter_handles.y_integrator_max = param_find("FW_Y_integrator_max"); + _parameter_handles.y_integrator_max = param_find("FW_Y_IMAX"); _parameter_handles.airspeed_min = param_find("FW_AIRSPD_MIN"); _parameter_handles.airspeed_trim = param_find("FW_AIRSPD_TRIM"); _parameter_handles.airspeed_max = param_find("FW_AIRSPD_MAX"); + _parameter_handles.y_coordinated = param_find("FW_Y_COORD"); + /* fetch initial parameter values */ parameters_update(); } @@ -361,6 +365,7 @@ FixedwingAttitudeControl::parameters_update() param_get(_parameter_handles.y_d, &(_parameters.y_d)); param_get(_parameter_handles.y_roll_feedforward, &(_parameters.y_roll_feedforward)); param_get(_parameter_handles.y_integrator_max, &(_parameters.y_integrator_max)); + param_get(_parameter_handles.y_coordinated, &(_parameters.y_coordinated)); param_get(_parameter_handles.airspeed_min, &(_parameters.airspeed_min)); param_get(_parameter_handles.airspeed_trim, &(_parameters.airspeed_trim)); @@ -390,6 +395,7 @@ FixedwingAttitudeControl::parameters_update() _yaw_ctrl.set_k_d(math::radians(_parameters.y_d)); _yaw_ctrl.set_k_roll_ff(math::radians(_parameters.y_roll_feedforward)); _yaw_ctrl.set_integrator_max(math::radians(_parameters.y_integrator_max)); + _yaw_ctrl.set_coordinated(_parameters.y_coordinated); return OK; } @@ -709,9 +715,9 @@ FixedwingAttitudeControl::task_main() * only once available */ vehicle_rates_setpoint_s rates_sp; - rates_sp.roll = _roll_ctrl.get_desired_bodyrate(); - rates_sp.pitch = _pitch_ctrl.get_desired_bodyrate(); - rates_sp.yaw = _yaw_ctrl.get_desired_bodyrate(); + rates_sp.roll = _roll_ctrl.get_desired_rate(); + rates_sp.pitch = _pitch_ctrl.get_desired_rate(); + rates_sp.yaw = _yaw_ctrl.get_desired_rate(); rates_sp.timestamp = hrt_absolute_time(); diff --git a/src/modules/fw_att_control/fw_att_control_params.c b/src/modules/fw_att_control/fw_att_control_params.c index 717608159a..efe40b1efe 100644 --- a/src/modules/fw_att_control/fw_att_control_params.c +++ b/src/modules/fw_att_control/fw_att_control_params.c @@ -136,3 +136,4 @@ PARAM_DEFINE_FLOAT(FW_Y_ROLLFF, 0); PARAM_DEFINE_FLOAT(FW_AIRSPD_MIN, 9.0f); PARAM_DEFINE_FLOAT(FW_AIRSPD_TRIM, 12.0f); PARAM_DEFINE_FLOAT(FW_AIRSPD_MAX, 18.0f); +PARAM_DEFINE_FLOAT(FW_Y_COORD, 1.0f); From ccc6dd73a02f83d8fb857ca25cfe998a3b1303d4 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 19 Oct 2013 19:12:03 +0200 Subject: [PATCH 051/447] consistent and safer fix for dt calculation --- src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp | 2 +- src/lib/ecl/attitude_fw/ecl_roll_controller.cpp | 6 +++++- src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp | 7 ++++++- 3 files changed, 12 insertions(+), 3 deletions(-) diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp index 5fe55163f3..eb1031cd30 100644 --- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp @@ -116,7 +116,7 @@ float ECL_PitchController::control_bodyrate(float roll, float pitch, /* get the usual dt estimate */ uint64_t dt_micros = ecl_elapsed_time(&_last_run); _last_run = ecl_absolute_time(); - float dt = (dt_micros > 500000) ? 0.0f : (float)dt_micros * 1e-6f; + float dt = (float)dt_micros * 1e-6f; /* lock integral for long intervals */ if (dt_micros > 500000) diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp index 1aac827923..ab3ac0a9d3 100644 --- a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp @@ -86,7 +86,11 @@ float ECL_RollController::control_bodyrate(float pitch, /* get the usual dt estimate */ uint64_t dt_micros = ecl_elapsed_time(&_last_run); _last_run = ecl_absolute_time(); - float dt = (dt_micros > 500000) ? 0.0f : (float)dt_micros * 1e-6f; + float dt = (float)dt_micros * 1e-6f; + + /* lock integral for long intervals */ + if (dt_micros > 500000) + lock_integrator = true; float k_ff = math::max((_k_p - _k_i * _tc) * _tc - _k_d, 0.0f); float k_i_rate = _k_i * _tc; diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp index 7dfed3379f..e56a8d08d4 100644 --- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp @@ -99,7 +99,12 @@ float ECL_YawController::control_bodyrate(float roll, float pitch, /* get the usual dt estimate */ uint64_t dt_micros = ecl_elapsed_time(&_last_run); _last_run = ecl_absolute_time(); - float dt = (dt_micros > 500000) ? 0.0f : (float)dt_micros * 1e-6f; + float dt = (float)dt_micros * 1e-6f; + + /* lock integral for long intervals */ + if (dt_micros > 500000) + lock_integrator = true; + float k_ff = math::max((_k_p - _k_i * _tc) * _tc - _k_d, 0.0f); float k_i_rate = _k_i * _tc; From 1e89f30120f30f0cac9c4bdd7b81ee2da96bcc8f Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 24 Oct 2013 17:57:21 +0200 Subject: [PATCH 052/447] constrain integrator part in control output until startup detection is available for safety reasons --- src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp | 5 +++-- src/lib/ecl/attitude_fw/ecl_roll_controller.cpp | 5 +++-- src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp | 5 +++-- src/modules/fw_att_control/fw_att_control_params.c | 6 +++--- 4 files changed, 12 insertions(+), 9 deletions(-) diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp index eb1031cd30..4b43e73e6b 100644 --- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp @@ -161,9 +161,10 @@ float ECL_PitchController::control_bodyrate(float roll, float pitch, } /* integrator limit */ - _integrator = math::constrain(_integrator, -_integrator_max, _integrator_max); + //xxx: until start detection is available: integral part in control signal is limited here + float integrator_constrained = math::constrain(_integrator * k_i_rate, -_integrator_max, _integrator_max); /* store non-limited output */ - _last_output = ((_rate_error * _k_d * scaler) + _integrator * k_i_rate * scaler + (_rate_setpoint * k_roll_ff)) * scaler; + _last_output = ((_rate_error * _k_d * scaler) + integrator_constrained * scaler + (_rate_setpoint * k_roll_ff)) * scaler; return math::constrain(_last_output, -_max_deflection_rad, _max_deflection_rad); } diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp index ab3ac0a9d3..658a0a5016 100644 --- a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp @@ -133,11 +133,12 @@ float ECL_RollController::control_bodyrate(float pitch, } /* integrator limit */ - _integrator = math::constrain(_integrator, -_integrator_max, _integrator_max); + //xxx: until start detection is available: integral part in control signal is limited here + float integrator_constrained = math::constrain(_integrator * k_i_rate, -_integrator_max, _integrator_max); //warnx("roll: _integrator: %.4f, _integrator_max: %.4f", (double)_integrator, (double)_integrator_max); /* store non-limited output */ - _last_output = ((_rate_error * _k_d * scaler) + _integrator * k_i_rate * scaler + (_rate_setpoint * k_ff)) * scaler; + _last_output = ((_rate_error * _k_d * scaler) + integrator_constrained * scaler + (_rate_setpoint * k_ff)) * scaler; return math::constrain(_last_output, -_max_deflection_rad, _max_deflection_rad); } diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp index e56a8d08d4..b6938a59a2 100644 --- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp @@ -147,9 +147,10 @@ float ECL_YawController::control_bodyrate(float roll, float pitch, } /* integrator limit */ - _integrator = math::constrain(_integrator, -_integrator_max, _integrator_max); + //xxx: until start detection is available: integral part in control signal is limited here + float integrator_constrained = math::constrain(_integrator * k_i_rate, -_integrator_max, _integrator_max); /* store non-limited output */ - _last_output = ((_rate_error * _k_d * scaler) + _integrator * k_i_rate * scaler + (_rate_setpoint * k_ff)) * scaler; + _last_output = ((_rate_error * _k_d * scaler) + integrator_constrained * scaler + (_rate_setpoint * k_ff)) * scaler; return math::constrain(_last_output, -_max_deflection_rad, _max_deflection_rad); } diff --git a/src/modules/fw_att_control/fw_att_control_params.c b/src/modules/fw_att_control/fw_att_control_params.c index efe40b1efe..39b78e20a8 100644 --- a/src/modules/fw_att_control/fw_att_control_params.c +++ b/src/modules/fw_att_control/fw_att_control_params.c @@ -85,7 +85,7 @@ PARAM_DEFINE_FLOAT(FW_P_RMAX_NEG, 0.0f); // @Description This limits the range in degrees the integrator can wind up to. // @Range 0.0 to 45.0 // @Increment 1.0 -PARAM_DEFINE_FLOAT(FW_P_IMAX, 15.0f); +PARAM_DEFINE_FLOAT(FW_P_IMAX, 0.2f); // @DisplayName Roll feedforward gain. // @Description This compensates during turns and ensures the nose stays level. @@ -119,7 +119,7 @@ PARAM_DEFINE_FLOAT(FW_R_I, 0.0f); // @Description This limits the range in degrees the integrator can wind up to. // @Range 0.0 to 45.0 // @Increment 1.0 -PARAM_DEFINE_FLOAT(FW_R_IMAX, 15.0f); +PARAM_DEFINE_FLOAT(FW_R_IMAX, 0.2f); // @DisplayName Maximum Roll Rate // @Description This limits the maximum roll rate the controller will output (in degrees per second). Setting a value of zero disables the limit. @@ -130,7 +130,7 @@ PARAM_DEFINE_FLOAT(FW_R_RMAX, 60); PARAM_DEFINE_FLOAT(FW_Y_P, 0); PARAM_DEFINE_FLOAT(FW_Y_I, 0); -PARAM_DEFINE_FLOAT(FW_Y_IMAX, 15.0f); +PARAM_DEFINE_FLOAT(FW_Y_IMAX, 0.2f); PARAM_DEFINE_FLOAT(FW_Y_D, 0); PARAM_DEFINE_FLOAT(FW_Y_ROLLFF, 0); PARAM_DEFINE_FLOAT(FW_AIRSPD_MIN, 9.0f); From feb75f08cba0d18267f9d463db49f7c1db310596 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 19 Oct 2013 22:07:27 +0200 Subject: [PATCH 053/447] wip, clean up pid in fw att --- src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp | 15 ++++++++------- src/lib/ecl/attitude_fw/ecl_roll_controller.cpp | 14 +++++++------- src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp | 16 +++++++++------- 3 files changed, 24 insertions(+), 21 deletions(-) diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp index eb1031cd30..5b8897eaad 100644 --- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp @@ -122,8 +122,8 @@ float ECL_PitchController::control_bodyrate(float roll, float pitch, if (dt_micros > 500000) lock_integrator = true; - float k_roll_ff = math::max((_k_p - _k_i * _tc) * _tc - _k_d, 0.0f); - float k_i_rate = _k_i * _tc; +// float k_ff = math::max((_k_p - _k_i * _tc) * _tc - _k_d, 0.0f); + float k_ff = 0; /* input conditioning */ if (!isfinite(airspeed)) { @@ -141,13 +141,12 @@ float ECL_PitchController::control_bodyrate(float roll, float pitch, _rate_error = _bodyrate_setpoint - pitch_bodyrate; - if (!lock_integrator && k_i_rate > 0.0f && airspeed > 0.5f * airspeed_min) { + if (!lock_integrator && _k_i > 0.0f && airspeed > 0.5f * airspeed_min) { float id = _rate_error * dt; /* - * anti-windup: do not allow integrator to increase into the - * wrong direction if actuator is at limit + * anti-windup: do not allow integrator to increase if actuator is at limit */ if (_last_output < -_max_deflection_rad) { /* only allow motion to center: increase value */ @@ -162,8 +161,10 @@ float ECL_PitchController::control_bodyrate(float roll, float pitch, /* integrator limit */ _integrator = math::constrain(_integrator, -_integrator_max, _integrator_max); - /* store non-limited output */ - _last_output = ((_rate_error * _k_d * scaler) + _integrator * k_i_rate * scaler + (_rate_setpoint * k_roll_ff)) * scaler; + + /* Apply PI rate controller and store non-limited output */ + //xxx: naming of gain variables (k_d <--> k_p) + _last_output = (_rate_error * _k_d + _integrator * _k_i * _rate_setpoint * k_ff) * scaler * scaler; //scaler^2 is proportional to 1/airspeed^2 return math::constrain(_last_output, -_max_deflection_rad, _max_deflection_rad); } diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp index ab3ac0a9d3..01a114d59f 100644 --- a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp @@ -92,8 +92,8 @@ float ECL_RollController::control_bodyrate(float pitch, if (dt_micros > 500000) lock_integrator = true; - float k_ff = math::max((_k_p - _k_i * _tc) * _tc - _k_d, 0.0f); - float k_i_rate = _k_i * _tc; +// float k_ff = math::max((_k_p - _k_i * _tc) * _tc - _k_d, 0.0f); + float k_ff = 0; //xxx: param /* input conditioning */ if (!isfinite(airspeed)) { @@ -113,13 +113,12 @@ float ECL_RollController::control_bodyrate(float pitch, /* Calculate body angular rate error */ _rate_error = _bodyrate_setpoint - roll_bodyrate; //body angular rate error - if (!lock_integrator && k_i_rate > 0.0f && airspeed > 0.5f * airspeed_min) { + if (!lock_integrator && _k_i > 0.0f && airspeed > 0.5f * airspeed_min) { float id = _rate_error * dt; /* - * anti-windup: do not allow integrator to increase into the - * wrong direction if actuator is at limit + * anti-windup: do not allow integrator to increase if actuator is at limit */ if (_last_output < -_max_deflection_rad) { /* only allow motion to center: increase value */ @@ -136,8 +135,9 @@ float ECL_RollController::control_bodyrate(float pitch, _integrator = math::constrain(_integrator, -_integrator_max, _integrator_max); //warnx("roll: _integrator: %.4f, _integrator_max: %.4f", (double)_integrator, (double)_integrator_max); - /* store non-limited output */ - _last_output = ((_rate_error * _k_d * scaler) + _integrator * k_i_rate * scaler + (_rate_setpoint * k_ff)) * scaler; + /* Apply PI rate controller and store non-limited output */ + //xxx: naming of gain variables + _last_output = (_rate_error * _k_d + _integrator * _k_i * _rate_setpoint * k_ff) * scaler * scaler; //scaler^2 is proportional to 1/airspeed^2 return math::constrain(_last_output, -_max_deflection_rad, _max_deflection_rad); } diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp index e56a8d08d4..5c9cc820fd 100644 --- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp @@ -106,8 +106,9 @@ float ECL_YawController::control_bodyrate(float roll, float pitch, lock_integrator = true; - float k_ff = math::max((_k_p - _k_i * _tc) * _tc - _k_d, 0.0f); - float k_i_rate = _k_i * _tc; +// float k_ff = math::max((_k_p - _k_i * _tc) * _tc - _k_d, 0.0f); + float k_ff = 0; + /* input conditioning */ if (!isfinite(airspeed)) { @@ -127,13 +128,12 @@ float ECL_YawController::control_bodyrate(float roll, float pitch, /* Calculate body angular rate error */ _rate_error = _bodyrate_setpoint - yaw_bodyrate; //body angular rate error - if (!lock_integrator && k_i_rate > 0.0f && airspeed > 0.5f * airspeed_min) { + if (!lock_integrator && _k_i > 0.0f && airspeed > 0.5f * airspeed_min) { float id = _rate_error * dt; /* - * anti-windup: do not allow integrator to increase into the - * wrong direction if actuator is at limit + * anti-windup: do not allow integrator to increase if actuator is at limit */ if (_last_output < -_max_deflection_rad) { /* only allow motion to center: increase value */ @@ -148,8 +148,10 @@ float ECL_YawController::control_bodyrate(float roll, float pitch, /* integrator limit */ _integrator = math::constrain(_integrator, -_integrator_max, _integrator_max); - /* store non-limited output */ - _last_output = ((_rate_error * _k_d * scaler) + _integrator * k_i_rate * scaler + (_rate_setpoint * k_ff)) * scaler; + + /* Apply PI rate controller and store non-limited output */ + //xxx: naming of gain variables (k_d <--> k_p) + _last_output = (_rate_error * _k_d + _integrator * _k_i * _rate_setpoint * k_ff) * scaler * scaler; //scaler^2 is proportional to 1/airspeed^2 return math::constrain(_last_output, -_max_deflection_rad, _max_deflection_rad); } From 3c14483df247fb842cabdbb02206912f7f4350cf Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 20 Oct 2013 00:35:20 +0200 Subject: [PATCH 054/447] fw att ctrl: rename some variables --- src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp | 3 +-- src/lib/ecl/attitude_fw/ecl_roll_controller.cpp | 3 +-- src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp | 3 +-- src/modules/fw_att_control/fw_att_control_params.c | 2 +- 4 files changed, 4 insertions(+), 7 deletions(-) diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp index 5b8897eaad..8b9ba9c626 100644 --- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp @@ -163,8 +163,7 @@ float ECL_PitchController::control_bodyrate(float roll, float pitch, _integrator = math::constrain(_integrator, -_integrator_max, _integrator_max); /* Apply PI rate controller and store non-limited output */ - //xxx: naming of gain variables (k_d <--> k_p) - _last_output = (_rate_error * _k_d + _integrator * _k_i * _rate_setpoint * k_ff) * scaler * scaler; //scaler^2 is proportional to 1/airspeed^2 + _last_output = (_rate_error * _k_p + _integrator * _k_i * _rate_setpoint * k_ff) * scaler * scaler; //scaler^2 is proportional to 1/airspeed^2 return math::constrain(_last_output, -_max_deflection_rad, _max_deflection_rad); } diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp index 01a114d59f..f3909593a4 100644 --- a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp @@ -136,8 +136,7 @@ float ECL_RollController::control_bodyrate(float pitch, //warnx("roll: _integrator: %.4f, _integrator_max: %.4f", (double)_integrator, (double)_integrator_max); /* Apply PI rate controller and store non-limited output */ - //xxx: naming of gain variables - _last_output = (_rate_error * _k_d + _integrator * _k_i * _rate_setpoint * k_ff) * scaler * scaler; //scaler^2 is proportional to 1/airspeed^2 + _last_output = (_rate_error * _k_p + _integrator * _k_i * _rate_setpoint * k_ff) * scaler * scaler; //scaler^2 is proportional to 1/airspeed^2 return math::constrain(_last_output, -_max_deflection_rad, _max_deflection_rad); } diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp index 5c9cc820fd..84d9ebd88c 100644 --- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp @@ -150,8 +150,7 @@ float ECL_YawController::control_bodyrate(float roll, float pitch, _integrator = math::constrain(_integrator, -_integrator_max, _integrator_max); /* Apply PI rate controller and store non-limited output */ - //xxx: naming of gain variables (k_d <--> k_p) - _last_output = (_rate_error * _k_d + _integrator * _k_i * _rate_setpoint * k_ff) * scaler * scaler; //scaler^2 is proportional to 1/airspeed^2 + _last_output = (_rate_error * _k_p + _integrator * _k_i * _rate_setpoint * k_ff) * scaler * scaler; //scaler^2 is proportional to 1/airspeed^2 return math::constrain(_last_output, -_max_deflection_rad, _max_deflection_rad); } diff --git a/src/modules/fw_att_control/fw_att_control_params.c b/src/modules/fw_att_control/fw_att_control_params.c index efe40b1efe..d61d3dd4f6 100644 --- a/src/modules/fw_att_control/fw_att_control_params.c +++ b/src/modules/fw_att_control/fw_att_control_params.c @@ -44,7 +44,7 @@ #include -//XXX resolve unclear naming of paramters: FW_P_D --> FW_PR_P +//XXX resolve unclear naming of paramters: FW_P_P --> FW_PR_P /* * Controller parameters, accessible via MAVLink From 146279b20fc65705e31e03298a1d3eea8911d0f8 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 20 Oct 2013 15:03:11 +0200 Subject: [PATCH 055/447] wip fw ctrl, several small bugfixes, set limit to 1 --- .../ecl/attitude_fw/ecl_pitch_controller.cpp | 11 ++--- .../ecl/attitude_fw/ecl_pitch_controller.h | 1 - .../ecl/attitude_fw/ecl_roll_controller.cpp | 15 +++--- src/lib/ecl/attitude_fw/ecl_roll_controller.h | 1 - .../ecl/attitude_fw/ecl_yaw_controller.cpp | 9 ++-- src/lib/ecl/attitude_fw/ecl_yaw_controller.h | 1 - .../fw_att_control/fw_att_control_main.cpp | 49 ++++++++++--------- .../fw_att_control/fw_att_control_params.c | 1 + 8 files changed, 44 insertions(+), 44 deletions(-) diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp index 8b9ba9c626..531512493b 100644 --- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp @@ -51,8 +51,7 @@ ECL_PitchController::ECL_PitchController() : _last_output(0.0f), _integrator(0.0f), _rate_error(0.0f), - _rate_setpoint(0.0f), - _max_deflection_rad(math::radians(45.0f)) + _rate_setpoint(0.0f) { } @@ -148,10 +147,10 @@ float ECL_PitchController::control_bodyrate(float roll, float pitch, /* * anti-windup: do not allow integrator to increase if actuator is at limit */ - if (_last_output < -_max_deflection_rad) { + if (_last_output < -1.0f) { /* only allow motion to center: increase value */ id = math::max(id, 0.0f); - } else if (_last_output > _max_deflection_rad) { + } else if (_last_output > 1.0f) { /* only allow motion to center: decrease value */ id = math::min(id, 0.0f); } @@ -163,9 +162,9 @@ float ECL_PitchController::control_bodyrate(float roll, float pitch, _integrator = math::constrain(_integrator, -_integrator_max, _integrator_max); /* Apply PI rate controller and store non-limited output */ - _last_output = (_rate_error * _k_p + _integrator * _k_i * _rate_setpoint * k_ff) * scaler * scaler; //scaler^2 is proportional to 1/airspeed^2 + _last_output = (_rate_error * _k_p + _integrator * _k_i + _rate_setpoint * k_ff) * scaler * scaler; //scaler is proportional to 1/airspeed - return math::constrain(_last_output, -_max_deflection_rad, _max_deflection_rad); + return math::constrain(_last_output, -1.0f, 1.0f); } void ECL_PitchController::reset_integrator() diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.h b/src/lib/ecl/attitude_fw/ecl_pitch_controller.h index ef6129d026..ba8ed3e6f2 100644 --- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.h +++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.h @@ -120,7 +120,6 @@ private: float _rate_error; float _rate_setpoint; float _bodyrate_setpoint; - float _max_deflection_rad; }; #endif // ECL_PITCH_CONTROLLER_H diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp index f3909593a4..e5bd7b4f7f 100644 --- a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp @@ -54,8 +54,7 @@ ECL_RollController::ECL_RollController() : _integrator(0.0f), _rate_error(0.0f), _rate_setpoint(0.0f), - _bodyrate_setpoint(0.0f), - _max_deflection_rad(math::radians(45.0f)) + _bodyrate_setpoint(0.0f) { } @@ -96,6 +95,7 @@ float ECL_RollController::control_bodyrate(float pitch, float k_ff = 0; //xxx: param /* input conditioning */ +// warnx("airspeed pre %.4f", (double)airspeed); if (!isfinite(airspeed)) { /* airspeed is NaN, +- INF or not available, pick center of band */ airspeed = 0.5f * (airspeed_min + airspeed_max); @@ -120,10 +120,10 @@ float ECL_RollController::control_bodyrate(float pitch, /* * anti-windup: do not allow integrator to increase if actuator is at limit */ - if (_last_output < -_max_deflection_rad) { + if (_last_output < -1.0f) { /* only allow motion to center: increase value */ id = math::max(id, 0.0f); - } else if (_last_output > _max_deflection_rad) { + } else if (_last_output > 1.0f) { /* only allow motion to center: decrease value */ id = math::min(id, 0.0f); } @@ -133,12 +133,13 @@ float ECL_RollController::control_bodyrate(float pitch, /* integrator limit */ _integrator = math::constrain(_integrator, -_integrator_max, _integrator_max); - //warnx("roll: _integrator: %.4f, _integrator_max: %.4f", (double)_integrator, (double)_integrator_max); + warnx("roll: _integrator: %.4f, _integrator_max: %.4f, airspeed %.4f, _k_i %.4f", (double)_integrator, (double)_integrator_max, (double)airspeed, (double)_k_i); /* Apply PI rate controller and store non-limited output */ - _last_output = (_rate_error * _k_p + _integrator * _k_i * _rate_setpoint * k_ff) * scaler * scaler; //scaler^2 is proportional to 1/airspeed^2 + _last_output = (_rate_error * _k_p + _integrator * _k_i + _rate_setpoint * k_ff) * scaler * scaler; //scaler is proportional to 1/airspeed + warnx("roll: _last_output %.4f", (double)_last_output); - return math::constrain(_last_output, -_max_deflection_rad, _max_deflection_rad); + return math::constrain(_last_output, -1.0f, 1.0f); } void ECL_RollController::reset_integrator() diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.h b/src/lib/ecl/attitude_fw/ecl_roll_controller.h index f94db0dc77..dd7d1bf534 100644 --- a/src/lib/ecl/attitude_fw/ecl_roll_controller.h +++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.h @@ -112,7 +112,6 @@ private: float _rate_error; float _rate_setpoint; float _bodyrate_setpoint; - float _max_deflection_rad; }; #endif // ECL_ROLL_CONTROLLER_H diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp index 84d9ebd88c..bc036e0821 100644 --- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp @@ -54,7 +54,6 @@ ECL_YawController::ECL_YawController() : _rate_error(0.0f), _rate_setpoint(0.0f), _bodyrate_setpoint(0.0f), - _max_deflection_rad(math::radians(45.0f)), _coordinated(1.0f) { @@ -135,10 +134,10 @@ float ECL_YawController::control_bodyrate(float roll, float pitch, /* * anti-windup: do not allow integrator to increase if actuator is at limit */ - if (_last_output < -_max_deflection_rad) { + if (_last_output < -1.0f) { /* only allow motion to center: increase value */ id = math::max(id, 0.0f); - } else if (_last_output > _max_deflection_rad) { + } else if (_last_output > 1.0f) { /* only allow motion to center: decrease value */ id = math::min(id, 0.0f); } @@ -150,9 +149,9 @@ float ECL_YawController::control_bodyrate(float roll, float pitch, _integrator = math::constrain(_integrator, -_integrator_max, _integrator_max); /* Apply PI rate controller and store non-limited output */ - _last_output = (_rate_error * _k_p + _integrator * _k_i * _rate_setpoint * k_ff) * scaler * scaler; //scaler^2 is proportional to 1/airspeed^2 + _last_output = (_rate_error * _k_p + _integrator * _k_i + _rate_setpoint * k_ff) * scaler * scaler; //scaler is proportional to 1/airspeed - return math::constrain(_last_output, -_max_deflection_rad, _max_deflection_rad); + return math::constrain(_last_output, -1.0f, 1.0f); } void ECL_YawController::reset_integrator() diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.h b/src/lib/ecl/attitude_fw/ecl_yaw_controller.h index 0250fcb352..5c00fa8731 100644 --- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.h +++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.h @@ -117,7 +117,6 @@ private: float _rate_error; float _rate_setpoint; float _bodyrate_setpoint; - float _max_deflection_rad; float _coordinated; }; diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index 91981b08a0..1b30462a88 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -156,6 +156,7 @@ private: float y_roll_feedforward; float y_integrator_max; float y_coordinated; + float y_rmax; float airspeed_min; float airspeed_trim; @@ -183,6 +184,7 @@ private: param_t y_roll_feedforward; param_t y_integrator_max; param_t y_coordinated; + param_t y_rmax; param_t airspeed_min; param_t airspeed_trim; @@ -305,6 +307,7 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : _parameter_handles.y_d = param_find("FW_Y_D"); _parameter_handles.y_roll_feedforward = param_find("FW_Y_ROLLFF"); _parameter_handles.y_integrator_max = param_find("FW_Y_IMAX"); + _parameter_handles.y_rmax = param_find("FW_Y_RMAX"); _parameter_handles.airspeed_min = param_find("FW_AIRSPD_MIN"); _parameter_handles.airspeed_trim = param_find("FW_AIRSPD_TRIM"); @@ -366,6 +369,7 @@ FixedwingAttitudeControl::parameters_update() param_get(_parameter_handles.y_roll_feedforward, &(_parameters.y_roll_feedforward)); param_get(_parameter_handles.y_integrator_max, &(_parameters.y_integrator_max)); param_get(_parameter_handles.y_coordinated, &(_parameters.y_coordinated)); + param_get(_parameter_handles.y_rmax, &(_parameters.y_rmax)); param_get(_parameter_handles.airspeed_min, &(_parameters.airspeed_min)); param_get(_parameter_handles.airspeed_trim, &(_parameters.airspeed_trim)); @@ -373,29 +377,30 @@ FixedwingAttitudeControl::parameters_update() /* pitch control parameters */ _pitch_ctrl.set_time_constant(_parameters.tconst); - _pitch_ctrl.set_k_p(math::radians(_parameters.p_p)); - _pitch_ctrl.set_k_i(math::radians(_parameters.p_i)); - _pitch_ctrl.set_k_d(math::radians(_parameters.p_d)); - _pitch_ctrl.set_integrator_max(math::radians(_parameters.p_integrator_max)); + _pitch_ctrl.set_k_p(_parameters.p_p); + _pitch_ctrl.set_k_i(_parameters.p_i); + _pitch_ctrl.set_k_d(_parameters.p_d); + _pitch_ctrl.set_integrator_max(_parameters.p_integrator_max); _pitch_ctrl.set_max_rate_pos(math::radians(_parameters.p_rmax_pos)); _pitch_ctrl.set_max_rate_neg(math::radians(_parameters.p_rmax_neg)); - _pitch_ctrl.set_roll_ff(math::radians(_parameters.p_roll_feedforward)); + _pitch_ctrl.set_roll_ff(_parameters.p_roll_feedforward); /* roll control parameters */ _roll_ctrl.set_time_constant(_parameters.tconst); - _roll_ctrl.set_k_p(math::radians(_parameters.r_p)); - _roll_ctrl.set_k_i(math::radians(_parameters.r_i)); - _roll_ctrl.set_k_d(math::radians(_parameters.r_d)); - _roll_ctrl.set_integrator_max(math::radians(_parameters.r_integrator_max)); + _roll_ctrl.set_k_p(_parameters.r_p); + _roll_ctrl.set_k_i(_parameters.r_i); + _roll_ctrl.set_k_d(_parameters.r_d); + _roll_ctrl.set_integrator_max(_parameters.r_integrator_max); _roll_ctrl.set_max_rate(math::radians(_parameters.r_rmax)); /* yaw control parameters */ - _yaw_ctrl.set_k_p(math::radians(_parameters.y_p)); - _yaw_ctrl.set_k_i(math::radians(_parameters.y_i)); - _yaw_ctrl.set_k_d(math::radians(_parameters.y_d)); - _yaw_ctrl.set_k_roll_ff(math::radians(_parameters.y_roll_feedforward)); - _yaw_ctrl.set_integrator_max(math::radians(_parameters.y_integrator_max)); + _yaw_ctrl.set_k_p(_parameters.y_p); + _yaw_ctrl.set_k_i(_parameters.y_i); + _yaw_ctrl.set_k_d(_parameters.y_d); + _yaw_ctrl.set_k_roll_ff(_parameters.y_roll_feedforward); + _yaw_ctrl.set_integrator_max(_parameters.y_integrator_max); _yaw_ctrl.set_coordinated(_parameters.y_coordinated); + _yaw_ctrl.set_max_rate(math::radians(_parameters.y_rmax)); return OK; } @@ -437,6 +442,7 @@ FixedwingAttitudeControl::vehicle_airspeed_poll() if (airspeed_updated) { orb_copy(ORB_ID(airspeed), _airspeed_sub, &_airspeed); + warnx("airspeed poll: ind: %.4f, true: %.4f", _airspeed.indicated_airspeed_m_s, _airspeed.true_airspeed_m_s); return true; } @@ -596,9 +602,6 @@ FixedwingAttitudeControl::task_main() if (_vcontrol_mode.flag_control_attitude_enabled) { - /* scale from radians to normalized -1 .. 1 range */ - const float actuator_scaling = 1.0f / (M_PI_F / 4.0f); - /* scale around tuning airspeed */ float airspeed; @@ -685,23 +688,23 @@ FixedwingAttitudeControl::task_main() _roll_ctrl.get_desired_rate(), _pitch_ctrl.get_desired_rate()); //runs last, because is depending on output of roll and pitch attitude /* Run attitude RATE controllers which need the desired attitudes from above */ - float roll_rad = _roll_ctrl.control_bodyrate(_att.pitch, + float roll_u = _roll_ctrl.control_bodyrate(_att.pitch, _att.rollspeed, _att.yawspeed, _yaw_ctrl.get_desired_rate(), _parameters.airspeed_min, _parameters.airspeed_max, airspeed, airspeed_scaling, lock_integrator); - _actuators.control[0] = (isfinite(roll_rad)) ? roll_rad * actuator_scaling : 0.0f; + _actuators.control[0] = (isfinite(roll_u)) ? roll_u : 0.0f; - float pitch_rad = _pitch_ctrl.control_bodyrate(_att.roll, _att.pitch, + float pitch_u = _pitch_ctrl.control_bodyrate(_att.roll, _att.pitch, _att.pitchspeed, _att.yawspeed, _yaw_ctrl.get_desired_rate(), _parameters.airspeed_min, _parameters.airspeed_max, airspeed, airspeed_scaling, lock_integrator); - _actuators.control[1] = (isfinite(pitch_rad)) ? pitch_rad * actuator_scaling : 0.0f; + _actuators.control[1] = (isfinite(roll_u)) ? roll_u : 0.0f; - float yaw_rad = _yaw_ctrl.control_bodyrate( _att.roll, _att.pitch, + float yaw_u = _yaw_ctrl.control_bodyrate( _att.roll, _att.pitch, _att.pitchspeed, _att.yawspeed, _pitch_ctrl.get_desired_rate(), _parameters.airspeed_min, _parameters.airspeed_max, airspeed, airspeed_scaling, lock_integrator); - _actuators.control[2] = (isfinite(yaw_rad)) ? yaw_rad * actuator_scaling : 0.0f; + _actuators.control[2] = (isfinite(yaw_u)) ? yaw_u : 0.0f; /* throttle passed through */ _actuators.control[3] = (isfinite(throttle_sp)) ? throttle_sp : 0.0f; diff --git a/src/modules/fw_att_control/fw_att_control_params.c b/src/modules/fw_att_control/fw_att_control_params.c index d61d3dd4f6..c48f3a5e3d 100644 --- a/src/modules/fw_att_control/fw_att_control_params.c +++ b/src/modules/fw_att_control/fw_att_control_params.c @@ -136,4 +136,5 @@ PARAM_DEFINE_FLOAT(FW_Y_ROLLFF, 0); PARAM_DEFINE_FLOAT(FW_AIRSPD_MIN, 9.0f); PARAM_DEFINE_FLOAT(FW_AIRSPD_TRIM, 12.0f); PARAM_DEFINE_FLOAT(FW_AIRSPD_MAX, 18.0f); +PARAM_DEFINE_FLOAT(FW_Y_RMAX, 60); PARAM_DEFINE_FLOAT(FW_Y_COORD, 1.0f); From 273bc52acc73cb43f31acf5869d0e536def0eec8 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 20 Oct 2013 19:22:08 +0200 Subject: [PATCH 056/447] fw att ctrl: transpose R --- src/modules/fw_att_control/fw_att_control_main.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index 7f3e7893ef..b59a97136e 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -664,9 +664,9 @@ FixedwingAttitudeControl::task_main() float speed_body_u = 0.0f; float speed_body_w = 0.0f; if(_att.R_valid) { - speed_body_u = _att.R[0][0] * _global_pos.vx + _att.R[0][1] * _global_pos.vy + _att.R[0][2] * _global_pos.vz; -// speed_body_v = _att.R[1][0] * _global_pos.vx + _att.R[1][1] * _global_pos.vy + _att.R[1][2] * _global_pos.vz; - speed_body_w = _att.R[2][0] * _global_pos.vx + _att.R[2][1] * _global_pos.vy + _att.R[2][2] * _global_pos.vz; + speed_body_u = _att.R[0][0] * _global_pos.vx + _att.R[1][0] * _global_pos.vy + _att.R[2][0] * _global_pos.vz; +// speed_body_v = _att.R[0][1] * _global_pos.vx + _att.R[1][1] * _global_pos.vy + _att.R[2][1] * _global_pos.vz; + speed_body_w = _att.R[0][2] * _global_pos.vx + _att.R[1][2] * _global_pos.vy + _att.R[2][2] * _global_pos.vz; } else { warnx("Did not get a valid R\n"); } From 61fafcbc785525641b137389e3e5cd96b72cfbc1 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 24 Oct 2013 16:23:32 +0200 Subject: [PATCH 057/447] fw ctrl: rename parameters to more consistent names --- .../fw_att_control/fw_att_control_main.cpp | 24 +++++++-------- .../fw_att_control/fw_att_control_params.c | 30 +++++++++---------- 2 files changed, 27 insertions(+), 27 deletions(-) diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index 1b30462a88..2772593305 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -288,25 +288,25 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : _airspeed_valid(false) { _parameter_handles.tconst = param_find("FW_ATT_TC"); - _parameter_handles.p_p = param_find("FW_P_P"); - _parameter_handles.p_d = param_find("FW_P_D"); - _parameter_handles.p_i = param_find("FW_P_I"); + _parameter_handles.p_p = param_find("FW_PR_P"); + _parameter_handles.p_d = param_find("FW_PR_D"); + _parameter_handles.p_i = param_find("FW_PR_I"); _parameter_handles.p_rmax_pos = param_find("FW_P_RMAX_POS"); _parameter_handles.p_rmax_neg = param_find("FW_P_RMAX_NEG"); - _parameter_handles.p_integrator_max = param_find("FW_P_IMAX"); + _parameter_handles.p_integrator_max = param_find("FW_PR_IMAX"); _parameter_handles.p_roll_feedforward = param_find("FW_P_ROLLFF"); - _parameter_handles.r_p = param_find("FW_R_P"); - _parameter_handles.r_d = param_find("FW_R_D"); - _parameter_handles.r_i = param_find("FW_R_I"); - _parameter_handles.r_integrator_max = param_find("FW_R_IMAX"); + _parameter_handles.r_p = param_find("FW_RR_P"); + _parameter_handles.r_d = param_find("FW_RR_D"); + _parameter_handles.r_i = param_find("FW_RR_I"); + _parameter_handles.r_integrator_max = param_find("FW_RR_IMAX"); _parameter_handles.r_rmax = param_find("FW_R_RMAX"); - _parameter_handles.y_p = param_find("FW_Y_P"); - _parameter_handles.y_i = param_find("FW_Y_I"); - _parameter_handles.y_d = param_find("FW_Y_D"); + _parameter_handles.y_p = param_find("FW_YR_P"); + _parameter_handles.y_i = param_find("FW_YR_I"); + _parameter_handles.y_d = param_find("FW_YR_D"); _parameter_handles.y_roll_feedforward = param_find("FW_Y_ROLLFF"); - _parameter_handles.y_integrator_max = param_find("FW_Y_IMAX"); + _parameter_handles.y_integrator_max = param_find("FW_YR_IMAX"); _parameter_handles.y_rmax = param_find("FW_Y_RMAX"); _parameter_handles.airspeed_min = param_find("FW_AIRSPD_MIN"); diff --git a/src/modules/fw_att_control/fw_att_control_params.c b/src/modules/fw_att_control/fw_att_control_params.c index c48f3a5e3d..07fac49890 100644 --- a/src/modules/fw_att_control/fw_att_control_params.c +++ b/src/modules/fw_att_control/fw_att_control_params.c @@ -50,7 +50,7 @@ * Controller parameters, accessible via MAVLink * */ - +//xxx: update descriptions // @DisplayName Attitude Time Constant // @Description This defines the latency between a step input and the achieved setpoint. Half a second is a good start value and fits for most average systems. Smaller systems may require smaller values, but as this will wear out servos faster, the value should only be decreased as needed. // @Range 0.4 to 1.0 seconds, in tens of seconds @@ -59,33 +59,33 @@ PARAM_DEFINE_FLOAT(FW_ATT_TC, 0.5f); // @DisplayName Proportional gain. // @Description This defines how much the elevator input will be commanded dependend on the current pitch error. // @Range 10 to 200, 1 increments -PARAM_DEFINE_FLOAT(FW_P_P, 40.0f); +PARAM_DEFINE_FLOAT(FW_PR_P, 0.3f); // @DisplayName Damping gain. // @Description This gain damps the airframe pitch rate. In particular relevant for flying wings. // @Range 0.0 to 10.0, 0.1 increments -PARAM_DEFINE_FLOAT(FW_P_D, 0.0f); +PARAM_DEFINE_FLOAT(FW_PR_D, 0.0f); //xxx: remove // @DisplayName Integrator gain. // @Description This gain defines how much control response will result out of a steady state error. It trims any constant error. // @Range 0 to 50.0 -PARAM_DEFINE_FLOAT(FW_P_I, 0.0f); +PARAM_DEFINE_FLOAT(FW_PR_I, 0.05f); // @DisplayName Maximum positive / up pitch rate. // @Description This limits the maximum pitch up angular rate the controller will output (in degrees per second). Setting a value of zero disables the limit. // @Range 0 to 90.0 degrees per seconds, in 1 increments -PARAM_DEFINE_FLOAT(FW_P_RMAX_POS, 0.0f); +PARAM_DEFINE_FLOAT(FW_P_RMAX_POS, 60.0f); // @DisplayName Maximum negative / down pitch rate. // @Description This limits the maximum pitch down up angular rate the controller will output (in degrees per second). Setting a value of zero disables the limit. // @Range 0 to 90.0 degrees per seconds, in 1 increments -PARAM_DEFINE_FLOAT(FW_P_RMAX_NEG, 0.0f); +PARAM_DEFINE_FLOAT(FW_P_RMAX_NEG, 60.0f); // @DisplayName Pitch Integrator Anti-Windup // @Description This limits the range in degrees the integrator can wind up to. // @Range 0.0 to 45.0 // @Increment 1.0 -PARAM_DEFINE_FLOAT(FW_P_IMAX, 15.0f); +PARAM_DEFINE_FLOAT(FW_PR_IMAX, 1000.0f); // @DisplayName Roll feedforward gain. // @Description This compensates during turns and ensures the nose stays level. @@ -99,27 +99,27 @@ PARAM_DEFINE_FLOAT(FW_P_ROLLFF, 1.0f); // @Range 10.0 200.0 // @Increment 10.0 // @User User -PARAM_DEFINE_FLOAT(FW_R_P, 40.0f); +PARAM_DEFINE_FLOAT(FW_RR_P, 0.5f); // @DisplayName Damping Gain // @Description Controls the roll rate to roll actuator output. It helps to reduce motions in turbulence. // @Range 0.0 10.0 // @Increment 1.0 // @User User -PARAM_DEFINE_FLOAT(FW_R_D, 0.0f); +PARAM_DEFINE_FLOAT(FW_RR_D, 0.0f); //xxx: remove // @DisplayName Integrator Gain // @Description This gain controls the contribution of the integral to roll actuator outputs. It trims out steady state errors. // @Range 0.0 100.0 // @Increment 5.0 // @User User -PARAM_DEFINE_FLOAT(FW_R_I, 0.0f); +PARAM_DEFINE_FLOAT(FW_RR_I, 0.05f); // @DisplayName Roll Integrator Anti-Windup // @Description This limits the range in degrees the integrator can wind up to. // @Range 0.0 to 45.0 // @Increment 1.0 -PARAM_DEFINE_FLOAT(FW_R_IMAX, 15.0f); +PARAM_DEFINE_FLOAT(FW_RR_IMAX, 1000.0f); // @DisplayName Maximum Roll Rate // @Description This limits the maximum roll rate the controller will output (in degrees per second). Setting a value of zero disables the limit. @@ -128,10 +128,10 @@ PARAM_DEFINE_FLOAT(FW_R_IMAX, 15.0f); PARAM_DEFINE_FLOAT(FW_R_RMAX, 60); -PARAM_DEFINE_FLOAT(FW_Y_P, 0); -PARAM_DEFINE_FLOAT(FW_Y_I, 0); -PARAM_DEFINE_FLOAT(FW_Y_IMAX, 15.0f); -PARAM_DEFINE_FLOAT(FW_Y_D, 0); +PARAM_DEFINE_FLOAT(FW_YR_P, 0.5); +PARAM_DEFINE_FLOAT(FW_YR_I, 0.05); +PARAM_DEFINE_FLOAT(FW_YR_IMAX, 1000.0f); +PARAM_DEFINE_FLOAT(FW_YR_D, 0); //xxx: remove PARAM_DEFINE_FLOAT(FW_Y_ROLLFF, 0); PARAM_DEFINE_FLOAT(FW_AIRSPD_MIN, 9.0f); PARAM_DEFINE_FLOAT(FW_AIRSPD_TRIM, 12.0f); From a5046fdfa02999f153502168682ade9bd5c15842 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 20 Oct 2013 20:10:31 +0200 Subject: [PATCH 058/447] fix wrong operation in yaw controller --- src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp | 4 +++- src/lib/ecl/attitude_fw/ecl_roll_controller.cpp | 4 ++-- src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp | 6 ++++-- 3 files changed, 9 insertions(+), 5 deletions(-) diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp index 531512493b..6268122882 100644 --- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp @@ -45,6 +45,7 @@ #include #include #include +#include ECL_PitchController::ECL_PitchController() : _last_run(0), @@ -163,7 +164,8 @@ float ECL_PitchController::control_bodyrate(float roll, float pitch, /* Apply PI rate controller and store non-limited output */ _last_output = (_rate_error * _k_p + _integrator * _k_i + _rate_setpoint * k_ff) * scaler * scaler; //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_roll_controller.cpp b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp index e5bd7b4f7f..9e20071311 100644 --- a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp @@ -133,11 +133,11 @@ float ECL_RollController::control_bodyrate(float pitch, /* integrator limit */ _integrator = math::constrain(_integrator, -_integrator_max, _integrator_max); - warnx("roll: _integrator: %.4f, _integrator_max: %.4f, airspeed %.4f, _k_i %.4f", (double)_integrator, (double)_integrator_max, (double)airspeed, (double)_k_i); +// warnx("roll: _integrator: %.4f, _integrator_max: %.4f, airspeed %.4f, _k_i %.4f", (double)_integrator, (double)_integrator_max, (double)airspeed, (double)_k_i); /* Apply PI rate controller and store non-limited output */ _last_output = (_rate_error * _k_p + _integrator * _k_i + _rate_setpoint * k_ff) * scaler * scaler; //scaler is proportional to 1/airspeed - warnx("roll: _last_output %.4f", (double)_last_output); +// 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_yaw_controller.cpp b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp index bc036e0821..bd1f318a72 100644 --- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp @@ -119,10 +119,10 @@ float ECL_YawController::control_bodyrate(float roll, float pitch, /* Transform setpoint to body angular rates */ - _bodyrate_setpoint = -sinf(roll) * pitch_rate_setpoint * cosf(roll)*cosf(pitch) * _rate_setpoint; //jacobian + _bodyrate_setpoint = -sinf(roll) * pitch_rate_setpoint + cosf(roll)*cosf(pitch) * _rate_setpoint; //jacobian /* Transform estimation to body angular rates */ - float yaw_bodyrate = -sinf(roll) * pitch_rate * cosf(roll)*cosf(pitch) * yaw_rate; //jacobian + float yaw_bodyrate = -sinf(roll) * pitch_rate + cosf(roll)*cosf(pitch) * yaw_rate; //jacobian /* Calculate body angular rate error */ _rate_error = _bodyrate_setpoint - yaw_bodyrate; //body angular rate error @@ -150,6 +150,8 @@ float ECL_YawController::control_bodyrate(float roll, float pitch, /* Apply PI rate controller and store non-limited output */ _last_output = (_rate_error * _k_p + _integrator * _k_i + _rate_setpoint * k_ff) * scaler * 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); } From 00825aa3cf7a04de148f763b96ee69f977d2656f Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 20 Oct 2013 17:36:49 +0200 Subject: [PATCH 059/447] fix small bug where roll instead of pitch was used --- src/modules/fw_att_control/fw_att_control_main.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index 2772593305..c29cf4cc30 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -442,7 +442,7 @@ FixedwingAttitudeControl::vehicle_airspeed_poll() if (airspeed_updated) { orb_copy(ORB_ID(airspeed), _airspeed_sub, &_airspeed); - warnx("airspeed poll: ind: %.4f, true: %.4f", _airspeed.indicated_airspeed_m_s, _airspeed.true_airspeed_m_s); +// warnx("airspeed poll: ind: %.4f, true: %.4f", _airspeed.indicated_airspeed_m_s, _airspeed.true_airspeed_m_s); return true; } @@ -698,7 +698,7 @@ FixedwingAttitudeControl::task_main() _att.pitchspeed, _att.yawspeed, _yaw_ctrl.get_desired_rate(), _parameters.airspeed_min, _parameters.airspeed_max, airspeed, airspeed_scaling, lock_integrator); - _actuators.control[1] = (isfinite(roll_u)) ? roll_u : 0.0f; + _actuators.control[1] = (isfinite(pitch_u)) ? pitch_u : 0.0f; float yaw_u = _yaw_ctrl.control_bodyrate( _att.roll, _att.pitch, _att.pitchspeed, _att.yawspeed, From 9fce5aa40226d4018794b7520759a6960965ce78 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 27 Oct 2013 21:56:04 +0100 Subject: [PATCH 060/447] change _update_height_demand for test --- src/lib/external_lgpl/tecs/tecs.cpp | 56 ++++++++++++++++++----------- src/lib/external_lgpl/tecs/tecs.h | 2 +- 2 files changed, 36 insertions(+), 22 deletions(-) diff --git a/src/lib/external_lgpl/tecs/tecs.cpp b/src/lib/external_lgpl/tecs/tecs.cpp index 864a9d24d2..af596dc5e7 100644 --- a/src/lib/external_lgpl/tecs/tecs.cpp +++ b/src/lib/external_lgpl/tecs/tecs.cpp @@ -2,6 +2,7 @@ #include "tecs.h" #include +#include using namespace math; @@ -199,33 +200,46 @@ void TECS::_update_speed_demand(void) _TAS_dem_last = _TAS_dem; } -void TECS::_update_height_demand(float demand) +void TECS::_update_height_demand(float demand, float state) { - // Apply 2 point moving average to demanded height - // This is required because height demand is only updated at 5Hz - _hgt_dem = 0.5f * (demand + _hgt_dem_in_old); - _hgt_dem_in_old = _hgt_dem; +// // Apply 2 point moving average to demanded height +// // This is required because height demand is only updated at 5Hz +// _hgt_dem = 0.5f * (demand + _hgt_dem_in_old); +// _hgt_dem_in_old = _hgt_dem; +// +// // printf("hgt_dem: %6.2f hgt_dem_last: %6.2f max_climb_rate: %6.2f\n", _hgt_dem, _hgt_dem_prev, +// // _maxClimbRate); +// +// // Limit height rate of change +// if ((_hgt_dem - _hgt_dem_prev) > (_maxClimbRate * 0.1f)) { +// _hgt_dem = _hgt_dem_prev + _maxClimbRate * 0.1f; +// +// } else if ((_hgt_dem - _hgt_dem_prev) < (-_maxSinkRate * 0.1f)) { +// _hgt_dem = _hgt_dem_prev - _maxSinkRate * 0.1f; +// } +// +// _hgt_dem_prev = _hgt_dem; +// +// // Apply first order lag to height demand +// _hgt_dem_adj = 0.05f * _hgt_dem + 0.95f * _hgt_dem_adj_last; +// _hgt_rate_dem = (_hgt_dem_adj - _hgt_dem_adj_last) / 0.1f; +// _hgt_dem_adj_last = _hgt_dem_adj; +// +// // printf("hgt_dem: %6.2f hgt_dem_adj: %6.2f hgt_dem_last: %6.2f hgt_rate_dem: %6.2f\n", _hgt_dem, _hgt_dem_adj, _hgt_dem_adj_last, +// // _hgt_rate_dem); - // printf("hgt_dem: %6.2f hgt_dem_last: %6.2f max_climb_rate: %6.2f\n", _hgt_dem, _hgt_dem_prev, - // _maxClimbRate); + _hgt_dem_adj = 0.05f * demand + 0.95f * _hgt_dem_adj_last; + _hgt_rate_dem = (demand-state)*0.1f; //xxx: parameter // Limit height rate of change - if ((_hgt_dem - _hgt_dem_prev) > (_maxClimbRate * 0.1f)) { - _hgt_dem = _hgt_dem_prev + _maxClimbRate * 0.1f; + if (_hgt_rate_dem > _maxClimbRate) { + _hgt_rate_dem = _maxClimbRate; - } else if ((_hgt_dem - _hgt_dem_prev) < (-_maxSinkRate * 0.1f)) { - _hgt_dem = _hgt_dem_prev - _maxSinkRate * 0.1f; + } else if (_hgt_rate_dem < -_maxSinkRate) { + _hgt_rate_dem = -_maxSinkRate; } - _hgt_dem_prev = _hgt_dem; - - // Apply first order lag to height demand - _hgt_dem_adj = 0.05f * _hgt_dem + 0.95f * _hgt_dem_adj_last; - _hgt_rate_dem = (_hgt_dem_adj - _hgt_dem_adj_last) / 0.1f; - _hgt_dem_adj_last = _hgt_dem_adj; - - // printf("hgt_dem: %6.2f hgt_dem_adj: %6.2f hgt_dem_last: %6.2f hgt_rate_dem: %6.2f\n", _hgt_dem, _hgt_dem_adj, _hgt_dem_adj_last, - // _hgt_rate_dem); + warnx("_hgt_rate_dem: %.4f, _hgt_dem_adj %.4f", _hgt_rate_dem, _hgt_dem_adj); } void TECS::_detect_underspeed(void) @@ -500,7 +514,7 @@ void TECS::update_pitch_throttle(const math::Dcm &rotMat, float pitch, float bar _update_speed_demand(); // Calculate the height demand - _update_height_demand(hgt_dem); + _update_height_demand(hgt_dem, baro_altitude); // Detect underspeed condition _detect_underspeed(); diff --git a/src/lib/external_lgpl/tecs/tecs.h b/src/lib/external_lgpl/tecs/tecs.h index 4a98c8e974..224f353664 100644 --- a/src/lib/external_lgpl/tecs/tecs.h +++ b/src/lib/external_lgpl/tecs/tecs.h @@ -327,7 +327,7 @@ private: void _update_speed_demand(void); // Update the demanded height - void _update_height_demand(float demand); + void _update_height_demand(float demand, float state); // Detect an underspeed condition void _detect_underspeed(void); From 5eea669d62b8a6a85b79c8ad3b5b8c08bb54987f Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 27 Oct 2013 22:03:49 +0100 Subject: [PATCH 061/447] add missing copy of variable --- src/lib/external_lgpl/tecs/tecs.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/lib/external_lgpl/tecs/tecs.cpp b/src/lib/external_lgpl/tecs/tecs.cpp index af596dc5e7..51a621f62b 100644 --- a/src/lib/external_lgpl/tecs/tecs.cpp +++ b/src/lib/external_lgpl/tecs/tecs.cpp @@ -229,6 +229,7 @@ void TECS::_update_height_demand(float demand, float state) // // _hgt_rate_dem); _hgt_dem_adj = 0.05f * demand + 0.95f * _hgt_dem_adj_last; + _hgt_dem_adj_last = _hgt_dem_adj; _hgt_rate_dem = (demand-state)*0.1f; //xxx: parameter // Limit height rate of change From e413ae7cbc1c5bbf9acafe6c23602e16cdc11121 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 29 Oct 2013 09:14:52 +0400 Subject: [PATCH 062/447] attitude_estimator_ekf: acceleration compensation based on GPS velocity --- .../attitude_estimator_ekf_main.cpp | 47 +++++++++++++++++-- 1 file changed, 44 insertions(+), 3 deletions(-) diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp index a70a14fe4b..c0f7a5bdb6 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp @@ -58,6 +58,7 @@ #include #include #include +#include #include #include @@ -214,6 +215,8 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds struct sensor_combined_s raw; memset(&raw, 0, sizeof(raw)); + struct vehicle_gps_position_s gps; + memset(&gps, 0, sizeof(gps)); struct vehicle_attitude_s att; memset(&att, 0, sizeof(att)); struct vehicle_control_mode_s control_mode; @@ -222,11 +225,16 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds uint64_t last_data = 0; uint64_t last_measurement = 0; + float vel_prev[3] = { 0.0f, 0.0f, 0.0f }; + /* subscribe to raw data */ int sub_raw = orb_subscribe(ORB_ID(sensor_combined)); /* rate-limit raw data updates to 333 Hz (sensors app publishes at 200, so this is just paranoid) */ orb_set_interval(sub_raw, 3); + /* subscribe to GPS */ + int sub_gps = orb_subscribe(ORB_ID(vehicle_gps_position)); + /* subscribe to param changes */ int sub_params = orb_subscribe(ORB_ID(parameter_update)); @@ -306,6 +314,7 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds /* get latest measurements */ orb_copy(ORB_ID(sensor_combined), sub_raw, &raw); + orb_copy(ORB_ID(vehicle_gps_position), sub_gps, &gps); if (!initialized) { // XXX disabling init for now @@ -352,9 +361,41 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds sensor_last_timestamp[1] = raw.timestamp; } - z_k[3] = raw.accelerometer_m_s2[0]; - z_k[4] = raw.accelerometer_m_s2[1]; - z_k[5] = raw.accelerometer_m_s2[2]; + float acc[3]; + if (gps.fix_type >= 3 && gps.eph_m < 10.0f && gps.vel_ned_valid && hrt_absolute_time() < gps.timestamp_velocity + 500000) { + /* calculate acceleration in NED frame */ + float acc_NED[3]; + acc_NED[0] = (gps.vel_n_m_s - vel_prev[0]) / dt; + acc_NED[1] = (gps.vel_e_m_s - vel_prev[1]) / dt; + acc_NED[2] = (gps.vel_d_m_s - vel_prev[2]) / dt; + + /* project acceleration to body frame */ + for (int i = 0; i < 3; i++) { + acc[i] = 0.0f; + for (int j = 0; j < 3; j++) { + acc[i] += att.R[j][i] * acc_NED[j]; + } + } + + } else { + acc[0] = 0.0f; + acc[1] = 0.0f; + acc[2] = 0.0f; + } + + if (gps.vel_ned_valid) { + vel_prev[0] = gps.vel_n_m_s; + vel_prev[1] = gps.vel_e_m_s; + vel_prev[2] = gps.vel_d_m_s; + } else { + vel_prev[0] = 0.0f; + vel_prev[1] = 0.0f; + vel_prev[2] = 0.0f; + } + + z_k[3] = raw.accelerometer_m_s2[0] - acc[0]; + z_k[4] = raw.accelerometer_m_s2[1] - acc[1]; + z_k[5] = raw.accelerometer_m_s2[2] - acc[2]; /* update magnetometer measurements */ if (sensor_last_count[2] != raw.magnetometer_counter) { From 947207e68da08b7cd14e7da0b44d000b1c435b11 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 31 Oct 2013 12:15:14 +0100 Subject: [PATCH 063/447] change calc of hgt_rate_dem to use the filtered height setpoint --- src/lib/external_lgpl/tecs/tecs.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/external_lgpl/tecs/tecs.cpp b/src/lib/external_lgpl/tecs/tecs.cpp index 51a621f62b..baf893dd20 100644 --- a/src/lib/external_lgpl/tecs/tecs.cpp +++ b/src/lib/external_lgpl/tecs/tecs.cpp @@ -231,7 +231,7 @@ void TECS::_update_height_demand(float demand, float state) _hgt_dem_adj = 0.05f * demand + 0.95f * _hgt_dem_adj_last; _hgt_dem_adj_last = _hgt_dem_adj; - _hgt_rate_dem = (demand-state)*0.1f; //xxx: parameter + _hgt_rate_dem = (_hgt_dem_adj-state)*0.1f; //xxx: parameter // Limit height rate of change if (_hgt_rate_dem > _maxClimbRate) { _hgt_rate_dem = _maxClimbRate; From d944eeabf08620413c4d99c26de339fd6f6e7e8c Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 31 Oct 2013 13:32:42 +0100 Subject: [PATCH 064/447] remove filtering of height setpoint --- src/lib/external_lgpl/tecs/tecs.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/external_lgpl/tecs/tecs.cpp b/src/lib/external_lgpl/tecs/tecs.cpp index baf893dd20..5cbc19dbba 100644 --- a/src/lib/external_lgpl/tecs/tecs.cpp +++ b/src/lib/external_lgpl/tecs/tecs.cpp @@ -228,7 +228,7 @@ void TECS::_update_height_demand(float demand, float state) // // printf("hgt_dem: %6.2f hgt_dem_adj: %6.2f hgt_dem_last: %6.2f hgt_rate_dem: %6.2f\n", _hgt_dem, _hgt_dem_adj, _hgt_dem_adj_last, // // _hgt_rate_dem); - _hgt_dem_adj = 0.05f * demand + 0.95f * _hgt_dem_adj_last; + _hgt_dem_adj = demand;//0.025f * demand + 0.975f * _hgt_dem_adj_last; _hgt_dem_adj_last = _hgt_dem_adj; _hgt_rate_dem = (_hgt_dem_adj-state)*0.1f; //xxx: parameter From c0100b5e17b87b1c721a115bdbe535ab606bd58e Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 31 Oct 2013 14:20:30 +0100 Subject: [PATCH 065/447] heightrate p as parameter --- src/lib/external_lgpl/tecs/tecs.cpp | 2 +- src/lib/external_lgpl/tecs/tecs.h | 5 +++++ src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 8 ++++++++ src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c | 2 ++ 4 files changed, 16 insertions(+), 1 deletion(-) diff --git a/src/lib/external_lgpl/tecs/tecs.cpp b/src/lib/external_lgpl/tecs/tecs.cpp index 5cbc19dbba..a94a06dda2 100644 --- a/src/lib/external_lgpl/tecs/tecs.cpp +++ b/src/lib/external_lgpl/tecs/tecs.cpp @@ -231,7 +231,7 @@ void TECS::_update_height_demand(float demand, float state) _hgt_dem_adj = demand;//0.025f * demand + 0.975f * _hgt_dem_adj_last; _hgt_dem_adj_last = _hgt_dem_adj; - _hgt_rate_dem = (_hgt_dem_adj-state)*0.1f; //xxx: parameter + _hgt_rate_dem = (_hgt_dem_adj-state)*_heightrate_p; // Limit height rate of change if (_hgt_rate_dem > _maxClimbRate) { _hgt_rate_dem = _maxClimbRate; diff --git a/src/lib/external_lgpl/tecs/tecs.h b/src/lib/external_lgpl/tecs/tecs.h index 224f353664..d07bd00dd6 100644 --- a/src/lib/external_lgpl/tecs/tecs.h +++ b/src/lib/external_lgpl/tecs/tecs.h @@ -178,6 +178,10 @@ public: _indicated_airspeed_max = airspeed; } + void set_heightrate_p(float heightrate_p) { + _heightrate_p = heightrate_p; + } + private: // Last time update_50Hz was called uint64_t _update_50hz_last_usec; @@ -201,6 +205,7 @@ private: float _vertAccLim; float _rollComp; float _spdWeight; + float _heightrate_p; // throttle demand in the range from 0.0 to 1.0 float _throttle_dem; diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index ffa7915a7d..dbdab5961a 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -206,6 +206,8 @@ private: float throttle_land_max; float loiter_hold_radius; + + float heightrate_p; } _parameters; /**< local copies of interesting parameters */ struct { @@ -240,6 +242,8 @@ private: param_t throttle_land_max; param_t loiter_hold_radius; + + param_t heightrate_p; } _parameter_handles; /**< handles for interesting parameters */ @@ -370,6 +374,7 @@ FixedwingPositionControl::FixedwingPositionControl() : _parameter_handles.roll_throttle_compensation = param_find("FW_T_RLL2THR"); _parameter_handles.speed_weight = param_find("FW_T_SPDWEIGHT"); _parameter_handles.pitch_damping = param_find("FW_T_PTCH_DAMP"); + _parameter_handles.heightrate_p = param_find("FW_T_HRATE_P"); /* fetch initial parameter values */ parameters_update(); @@ -435,6 +440,8 @@ FixedwingPositionControl::parameters_update() param_get(_parameter_handles.pitch_damping, &(_parameters.pitch_damping)); param_get(_parameter_handles.max_climb_rate, &(_parameters.max_climb_rate)); + param_get(_parameter_handles.heightrate_p, &(_parameters.heightrate_p)); + _l1_control.set_l1_damping(_parameters.l1_damping); _l1_control.set_l1_period(_parameters.l1_period); _l1_control.set_l1_roll_limit(math::radians(_parameters.roll_limit)); @@ -453,6 +460,7 @@ FixedwingPositionControl::parameters_update() _tecs.set_indicated_airspeed_min(_parameters.airspeed_min); _tecs.set_indicated_airspeed_max(_parameters.airspeed_min); _tecs.set_max_climb_rate(_parameters.max_climb_rate); + _tecs.set_heightrate_p(_parameters.heightrate_p); /* sanity check parameters */ if (_parameters.airspeed_max < _parameters.airspeed_min || 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 3bb872405e..a6aa45258c 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 @@ -111,3 +111,5 @@ PARAM_DEFINE_FLOAT(FW_T_PTCH_DAMP, 0.0f); PARAM_DEFINE_FLOAT(FW_T_SINK_MAX, 5.0f); + +PARAM_DEFINE_FLOAT(FW_T_HRATE_P, 0.05f); From 1214cbe71f57e980e26d520dda693c089265e1e3 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 31 Oct 2013 17:15:55 +0100 Subject: [PATCH 066/447] wip towards glide slope tracking for autoland --- .../fw_pos_control_l1_main.cpp | 24 ++++++++++++------- 1 file changed, 16 insertions(+), 8 deletions(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index ffa7915a7d..000fa5fa30 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -759,10 +759,10 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio _l1_control.navigate_waypoints(prev_wp, next_wp, current_position, ground_speed); } - /* do not go down too early */ - if (wp_distance > 50.0f) { - altitude_error = (_global_triplet.current.altitude + 25.0f) - _global_pos.alt; - } +// /* do not go down too early */ +// if (wp_distance > 50.0f) { +// altitude_error = (_global_triplet.current.altitude + 25.0f) - _global_pos.alt; +// } _att_sp.roll_body = _l1_control.nav_roll(); @@ -777,6 +777,9 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio float airspeed_land = _parameters.airspeed_min; float airspeed_approach = (_parameters.airspeed_min + _parameters.airspeed_trim) / 2.0f; + const float landing_slope_angle_rad = 20.0f/180.0f*M_PI_F; //xxx: param + float landing_slope_alt_desired = wp_distance * tanf(landing_slope_angle_rad); + if (altitude_error > -4.0f) { /* land with minimal speed */ @@ -798,9 +801,9 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio } else if (wp_distance < 60.0f && altitude_error > -20.0f) { - /* minimize speed to approach speed */ + /* minimize speed to approach speed, stay on glide slope */ - _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(airspeed_approach), + _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, landing_slope_alt_desired, calculate_target_airspeed(airspeed_approach), _airspeed.indicated_airspeed_m_s, eas2tas, false, flare_angle_rad, _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, @@ -808,9 +811,14 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio } else { - /* normal cruise speed */ + /* normal cruise speed + * intersect glide slope: if current position higher than slope sink to glide slope but continue horizontally if below slope */ + float altitude_desired = _global_pos.alt; + if (_global_pos.alt > landing_slope_alt_desired) { + altitude_desired = landing_slope_alt_desired; + } - _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_trim), + _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, altitude_desired, calculate_target_airspeed(_parameters.airspeed_trim), _airspeed.indicated_airspeed_m_s, eas2tas, false, math::radians(_parameters.pitch_limit_min), _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, From 18c5d901889e33167f0967f612fb7e57b3b4a018 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 31 Oct 2013 17:53:03 +0100 Subject: [PATCH 067/447] change logic to avoid overshoot over landing spot --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 000fa5fa30..be6cc0e8ce 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -799,10 +799,9 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio /* limit roll motion to prevent wings from touching the ground first */ _att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-10.0f), math::radians(10.0f)); - } else if (wp_distance < 60.0f && altitude_error > -20.0f) { + } else if (wp_distance < 60.0f || altitude_error > -20.0f) { /* minimize speed to approach speed, stay on glide slope */ - _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, landing_slope_alt_desired, calculate_target_airspeed(airspeed_approach), _airspeed.indicated_airspeed_m_s, eas2tas, false, flare_angle_rad, From 97a54d1b5e684be36819816c03a54fe8bfd1792f Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 31 Oct 2013 18:19:16 +0100 Subject: [PATCH 068/447] fix missing conversion to absolute altitude --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index be6cc0e8ce..97b659af8d 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -778,7 +778,7 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio float airspeed_approach = (_parameters.airspeed_min + _parameters.airspeed_trim) / 2.0f; const float landing_slope_angle_rad = 20.0f/180.0f*M_PI_F; //xxx: param - float landing_slope_alt_desired = wp_distance * tanf(landing_slope_angle_rad); + float landing_slope_alt_desired = wp_distance * tanf(landing_slope_angle_rad) + _global_triplet.current.altitude; if (altitude_error > -4.0f) { From a6b85cfbf76dbe0e829e503a09660ffb11f003d0 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 1 Nov 2013 14:02:55 +0100 Subject: [PATCH 069/447] add some comments for clarification --- .../fw_pos_control_l1/fw_pos_control_l1_main.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 97b659af8d..a68de45255 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -729,8 +729,8 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio } else if (global_triplet.current.nav_cmd == NAV_CMD_LAND) { + /* Horizontal landing control */ /* switch to heading hold for the last meters, continue heading hold after */ - float wp_distance = get_distance_to_next_waypoint(prev_wp.getX(), prev_wp.getY(), current_position.getX(), current_position.getY()); //warnx("wp dist: %d, alt err: %d, noret: %s", (int)wp_distance, (int)altitude_error, (land_noreturn) ? "YES" : "NO"); if (wp_distance < 15.0f || land_noreturn) { @@ -758,16 +758,16 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio /* normal navigation */ _l1_control.navigate_waypoints(prev_wp, next_wp, current_position, ground_speed); } + _att_sp.roll_body = _l1_control.nav_roll(); + _att_sp.yaw_body = _l1_control.nav_bearing(); + + + /* Vertical landing control */ // /* do not go down too early */ // if (wp_distance > 50.0f) { // altitude_error = (_global_triplet.current.altitude + 25.0f) - _global_pos.alt; // } - - - _att_sp.roll_body = _l1_control.nav_roll(); - _att_sp.yaw_body = _l1_control.nav_bearing(); - /* apply minimum pitch (flare) and limit roll if close to touch down, altitude error is negative (going down) */ // XXX this could make a great param From 116ee348a228481512965778f444dd1c5f373939 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 1 Nov 2013 14:37:29 +0100 Subject: [PATCH 070/447] landing: make altitude handling before helper waypoint L more clever --- .../fw_pos_control_l1_main.cpp | 31 ++++++++++++++++--- 1 file changed, 27 insertions(+), 4 deletions(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index a68de45255..a908995289 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -279,6 +279,11 @@ private: */ void vehicle_setpoint_poll(); + /** + * Get Altitude on the landing glide slope + */ + float getLandingSlopeAbsoluteAltitude(float wp_distance, float wp_altitude, float landing_slope_angle_rad); + /** * Control position. */ @@ -624,6 +629,11 @@ FixedwingPositionControl::calculate_gndspeed_undershoot() } } +float FixedwingPositionControl::getLandingSlopeAbsoluteAltitude(float wp_distance, float wp_altitude, float landing_slope_angle_rad) +{ + return wp_distance * tanf(landing_slope_angle_rad) + wp_altitude; +} + bool FixedwingPositionControl::control_position(const math::Vector2f ¤t_position, const math::Vector2f &ground_speed, const struct vehicle_global_position_set_triplet_s &global_triplet) @@ -778,7 +788,10 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio float airspeed_approach = (_parameters.airspeed_min + _parameters.airspeed_trim) / 2.0f; const float landing_slope_angle_rad = 20.0f/180.0f*M_PI_F; //xxx: param - float landing_slope_alt_desired = wp_distance * tanf(landing_slope_angle_rad) + _global_triplet.current.altitude; + const float landingslope_length = 64.0f; + const float L_wp_distance = cosf(landing_slope_angle_rad) * landingslope_length; + float L_altitude = getLandingSlopeAbsoluteAltitude(L_wp_distance, _global_triplet.current.altitude, landing_slope_angle_rad); + float landing_slope_alt_desired = getLandingSlopeAbsoluteAltitude(wp_distance, _global_triplet.current.altitude, landing_slope_angle_rad); if (altitude_error > -4.0f) { @@ -799,7 +812,7 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio /* limit roll motion to prevent wings from touching the ground first */ _att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-10.0f), math::radians(10.0f)); - } else if (wp_distance < 60.0f || altitude_error > -20.0f) { + } else if (wp_distance < L_wp_distance) { /* minimize speed to approach speed, stay on glide slope */ _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, landing_slope_alt_desired, calculate_target_airspeed(airspeed_approach), @@ -811,10 +824,20 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio } else { /* normal cruise speed - * intersect glide slope: if current position higher than slope sink to glide slope but continue horizontally if below slope */ + * intersect glide slope: + * if current position is higher or within 10m of slope follow the glide slope + * if current position is below slope -10m and above altitude at L (see documentation) continue horizontally + * if current position is below altitude at L, climb to altitude of L */ float altitude_desired = _global_pos.alt; - if (_global_pos.alt > landing_slope_alt_desired) { + if (_global_pos.alt > landing_slope_alt_desired - 10.0f) { + /* stay on slope */ altitude_desired = landing_slope_alt_desired; + } else if (_global_pos.alt < landing_slope_alt_desired - 10.0f && _global_pos.alt > L_altitude) { + /* continue horizontally */ + altitude_desired = _global_pos.alt; //xxx: dangerous, but we have the altitude < L_altitude protection + } else { + /* climb to L_altitude */ + altitude_desired = L_altitude; } _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, altitude_desired, calculate_target_airspeed(_parameters.airspeed_trim), From e539a89e225c1d6ab127d7953bceec7efecefbdf Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 1 Nov 2013 17:35:09 +0100 Subject: [PATCH 071/447] autoland slope parameters, variable rename for bugfix and code clarity, printfs --- .../fw_pos_control_l1_main.cpp | 41 ++++++++++++++----- .../fw_pos_control_l1_params.c | 3 ++ 2 files changed, 33 insertions(+), 11 deletions(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index a908995289..a4b2778885 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -206,6 +206,9 @@ private: float throttle_land_max; float loiter_hold_radius; + + float land_slope_angle; + float land_slope_length; } _parameters; /**< local copies of interesting parameters */ struct { @@ -240,6 +243,9 @@ private: param_t throttle_land_max; param_t loiter_hold_radius; + + param_t land_slope_angle; + param_t land_slope_length; } _parameter_handles; /**< handles for interesting parameters */ @@ -363,6 +369,9 @@ FixedwingPositionControl::FixedwingPositionControl() : _parameter_handles.throttle_cruise = param_find("FW_THR_CRUISE"); _parameter_handles.throttle_land_max = param_find("FW_THR_LND_MAX"); + _parameter_handles.land_slope_angle = param_find("FW_LND_ANG"); + _parameter_handles.land_slope_length = param_find("FW_LND_SLL"); + _parameter_handles.time_const = param_find("FW_T_TIME_CONST"); _parameter_handles.min_sink_rate = param_find("FW_T_SINK_MIN"); _parameter_handles.max_sink_rate = param_find("FW_T_SINK_MAX"); @@ -440,6 +449,11 @@ FixedwingPositionControl::parameters_update() param_get(_parameter_handles.pitch_damping, &(_parameters.pitch_damping)); param_get(_parameter_handles.max_climb_rate, &(_parameters.max_climb_rate)); + + param_get(_parameter_handles.land_slope_angle, &(_parameters.land_slope_angle)); + param_get(_parameter_handles.land_slope_length, &(_parameters.land_slope_length)); + + _l1_control.set_l1_damping(_parameters.l1_damping); _l1_control.set_l1_period(_parameters.l1_period); _l1_control.set_l1_roll_limit(math::radians(_parameters.roll_limit)); @@ -673,7 +687,7 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio if (_setpoint_valid) { /* current waypoint (the one currently heading for) */ - math::Vector2f next_wp(global_triplet.current.lat / 1e7f, global_triplet.current.lon / 1e7f); + math::Vector2f curr_wp(global_triplet.current.lat / 1e7f, global_triplet.current.lon / 1e7f); /* previous waypoint */ math::Vector2f prev_wp; @@ -711,7 +725,7 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio } else if (global_triplet.current.nav_cmd == NAV_CMD_WAYPOINT) { /* waypoint is a plain navigation waypoint */ - _l1_control.navigate_waypoints(prev_wp, next_wp, current_position, ground_speed); + _l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed); _att_sp.roll_body = _l1_control.nav_roll(); _att_sp.yaw_body = _l1_control.nav_bearing(); @@ -726,7 +740,7 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio global_triplet.current.nav_cmd == NAV_CMD_LOITER_UNLIMITED) { /* waypoint is a loiter waypoint */ - _l1_control.navigate_loiter(next_wp, current_position, global_triplet.current.loiter_radius, + _l1_control.navigate_loiter(curr_wp, current_position, global_triplet.current.loiter_radius, global_triplet.current.loiter_direction, ground_speed); _att_sp.roll_body = _l1_control.nav_roll(); _att_sp.yaw_body = _l1_control.nav_bearing(); @@ -741,7 +755,7 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio /* Horizontal landing control */ /* switch to heading hold for the last meters, continue heading hold after */ - float wp_distance = get_distance_to_next_waypoint(prev_wp.getX(), prev_wp.getY(), current_position.getX(), current_position.getY()); + float wp_distance = get_distance_to_next_waypoint(current_position.getX(), current_position.getY(), curr_wp.getX(), curr_wp.getY()); //warnx("wp dist: %d, alt err: %d, noret: %s", (int)wp_distance, (int)altitude_error, (land_noreturn) ? "YES" : "NO"); if (wp_distance < 15.0f || land_noreturn) { @@ -766,7 +780,7 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio } else { /* normal navigation */ - _l1_control.navigate_waypoints(prev_wp, next_wp, current_position, ground_speed); + _l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed); } _att_sp.roll_body = _l1_control.nav_roll(); _att_sp.yaw_body = _l1_control.nav_bearing(); @@ -787,9 +801,9 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio float airspeed_land = _parameters.airspeed_min; float airspeed_approach = (_parameters.airspeed_min + _parameters.airspeed_trim) / 2.0f; - const float landing_slope_angle_rad = 20.0f/180.0f*M_PI_F; //xxx: param - const float landingslope_length = 64.0f; - const float L_wp_distance = cosf(landing_slope_angle_rad) * landingslope_length; + float landing_slope_angle_rad = math::radians(_parameters.land_slope_angle); + float landingslope_length = _parameters.land_slope_length; + float L_wp_distance = cosf(landing_slope_angle_rad) * landingslope_length; float L_altitude = getLandingSlopeAbsoluteAltitude(L_wp_distance, _global_triplet.current.altitude, landing_slope_angle_rad); float landing_slope_alt_desired = getLandingSlopeAbsoluteAltitude(wp_distance, _global_triplet.current.altitude, landing_slope_angle_rad); @@ -812,15 +826,17 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio /* limit roll motion to prevent wings from touching the ground first */ _att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-10.0f), math::radians(10.0f)); + warnx("Landing: land with minimal speed"); + } else if (wp_distance < L_wp_distance) { - /* minimize speed to approach speed, stay on glide slope */ + /* minimize speed to approach speed, stay on landing slope */ _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, landing_slope_alt_desired, calculate_target_airspeed(airspeed_approach), _airspeed.indicated_airspeed_m_s, eas2tas, false, flare_angle_rad, _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max)); - + warnx("Landing: after L, stay on landing slope, alt_desired: %.4f (wp_distance: %.4f)", landing_slope_alt_desired, wp_distance); } else { /* normal cruise speed @@ -832,12 +848,15 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio if (_global_pos.alt > landing_slope_alt_desired - 10.0f) { /* stay on slope */ altitude_desired = landing_slope_alt_desired; + warnx("Landing: before L, stay on landing slope, alt_desired: %.4f (wp_distance: %.4f)", altitude_desired, wp_distance); } else if (_global_pos.alt < landing_slope_alt_desired - 10.0f && _global_pos.alt > L_altitude) { /* continue horizontally */ altitude_desired = _global_pos.alt; //xxx: dangerous, but we have the altitude < L_altitude protection + warnx("Landing: before L,continue horizontal at: %.4f, (landing_slope_alt_desired %.4f, wp_distance: %.4f, L_altitude: %.4f)", altitude_desired, landing_slope_alt_desired, wp_distance, L_altitude); } else { /* climb to L_altitude */ altitude_desired = L_altitude; + warnx("Landing: before L, below L, climb: %.4f (wp_distance: %.4f)", altitude_desired, wp_distance); } _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, altitude_desired, calculate_target_airspeed(_parameters.airspeed_trim), @@ -849,7 +868,7 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio } else if (global_triplet.current.nav_cmd == NAV_CMD_TAKEOFF) { - _l1_control.navigate_waypoints(prev_wp, next_wp, current_position, ground_speed); + _l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed); _att_sp.roll_body = _l1_control.nav_roll(); _att_sp.yaw_body = _l1_control.nav_bearing(); 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 3bb872405e..a3140fe48f 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 @@ -111,3 +111,6 @@ PARAM_DEFINE_FLOAT(FW_T_PTCH_DAMP, 0.0f); PARAM_DEFINE_FLOAT(FW_T_SINK_MAX, 5.0f); + +PARAM_DEFINE_FLOAT(FW_LND_ANG, 10.0f); +PARAM_DEFINE_FLOAT(FW_LND_SLL, 64.0f); From 4b2e08a85083f3925e951ec3de3ac41575f11985 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 2 Nov 2013 13:53:30 +0100 Subject: [PATCH 072/447] autoland, fix flare and max landing throttle --- .../fw_pos_control_l1_main.cpp | 21 +++++++++---------- 1 file changed, 10 insertions(+), 11 deletions(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index a4b2778885..af43097cc0 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -799,7 +799,7 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio float land_pitch_min = math::radians(5.0f); float throttle_land = _parameters.throttle_min + (_parameters.throttle_max - _parameters.throttle_min) * 0.1f; float airspeed_land = _parameters.airspeed_min; - float airspeed_approach = (_parameters.airspeed_min + _parameters.airspeed_trim) / 2.0f; + float airspeed_approach = 1.3f * _parameters.airspeed_min; float landing_slope_angle_rad = math::radians(_parameters.land_slope_angle); float landingslope_length = _parameters.land_slope_length; @@ -807,22 +807,22 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio float L_altitude = getLandingSlopeAbsoluteAltitude(L_wp_distance, _global_triplet.current.altitude, landing_slope_angle_rad); float landing_slope_alt_desired = getLandingSlopeAbsoluteAltitude(wp_distance, _global_triplet.current.altitude, landing_slope_angle_rad); - if (altitude_error > -4.0f) { + if (altitude_error > -10.0f || land_noreturn) { //be generous here as we currently have to rely on the user input for the waypoint, checking for land_noreturn to avoid unwanted climb out /* land with minimal speed */ /* force TECS to only control speed with pitch, altitude is only implicitely controlled now */ _tecs.set_speed_weight(2.0f); - _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(airspeed_land), - _airspeed.indicated_airspeed_m_s, eas2tas, - false, flare_angle_rad, - 0.0f, _parameters.throttle_max, throttle_land, - math::radians(-10.0f), math::radians(15.0f)); - /* kill the throttle if param requests it */ throttle_max = math::min(throttle_max, _parameters.throttle_land_max); + _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(airspeed_land), + _airspeed.indicated_airspeed_m_s, eas2tas, + false, flare_angle_rad, + 0.0f, throttle_max, throttle_land, + math::radians(flare_angle_rad), math::radians(15.0f)); + /* limit roll motion to prevent wings from touching the ground first */ _att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-10.0f), math::radians(10.0f)); @@ -839,8 +839,7 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio warnx("Landing: after L, stay on landing slope, alt_desired: %.4f (wp_distance: %.4f)", landing_slope_alt_desired, wp_distance); } else { - /* normal cruise speed - * intersect glide slope: + /* intersect glide slope: * if current position is higher or within 10m of slope follow the glide slope * if current position is below slope -10m and above altitude at L (see documentation) continue horizontally * if current position is below altitude at L, climb to altitude of L */ @@ -859,7 +858,7 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio warnx("Landing: before L, below L, climb: %.4f (wp_distance: %.4f)", altitude_desired, wp_distance); } - _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, altitude_desired, calculate_target_airspeed(_parameters.airspeed_trim), + _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, altitude_desired, calculate_target_airspeed(airspeed_approach), _airspeed.indicated_airspeed_m_s, eas2tas, false, math::radians(_parameters.pitch_limit_min), _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, From 5fec5fecdc3980c973cb8c6ad23a013c8214ace7 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 2 Nov 2013 15:13:03 +0100 Subject: [PATCH 073/447] change flare pitch setpoint --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index af43097cc0..08f9d3c15b 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -795,7 +795,7 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio /* apply minimum pitch (flare) and limit roll if close to touch down, altitude error is negative (going down) */ // XXX this could make a great param - float flare_angle_rad = math::radians(10.0f);//math::radians(global_triplet.current.param1) + float flare_angle_rad = -math::radians(5.0f);//math::radians(global_triplet.current.param1) float land_pitch_min = math::radians(5.0f); float throttle_land = _parameters.throttle_min + (_parameters.throttle_max - _parameters.throttle_min) * 0.1f; float airspeed_land = _parameters.airspeed_min; From a770bd5cf3d458baa85675815541512efce99b56 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 2 Nov 2013 18:59:55 +0400 Subject: [PATCH 074/447] attitude_estimator_ekf: correct acceleration continuously, not only on GPS updates --- .../attitude_estimator_ekf_main.cpp | 26 ++++++++++++------- 1 file changed, 16 insertions(+), 10 deletions(-) diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp index c0f7a5bdb6..efa6d97fdd 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp @@ -224,6 +224,7 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds uint64_t last_data = 0; uint64_t last_measurement = 0; + uint64_t last_gps = 0; float vel_prev[3] = { 0.0f, 0.0f, 0.0f }; @@ -363,17 +364,20 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds float acc[3]; if (gps.fix_type >= 3 && gps.eph_m < 10.0f && gps.vel_ned_valid && hrt_absolute_time() < gps.timestamp_velocity + 500000) { - /* calculate acceleration in NED frame */ - float acc_NED[3]; - acc_NED[0] = (gps.vel_n_m_s - vel_prev[0]) / dt; - acc_NED[1] = (gps.vel_e_m_s - vel_prev[1]) / dt; - acc_NED[2] = (gps.vel_d_m_s - vel_prev[2]) / dt; + if (last_gps != 0 && gps.timestamp_velocity != last_gps) { + float gps_dt = (gps.timestamp_velocity - last_gps) / 1000000.0f; + /* calculate acceleration in NED frame */ + float acc_NED[3]; + acc_NED[0] = (gps.vel_n_m_s - vel_prev[0]) / gps_dt; + acc_NED[1] = (gps.vel_e_m_s - vel_prev[1]) / gps_dt; + acc_NED[2] = (gps.vel_d_m_s - vel_prev[2]) / gps_dt; - /* project acceleration to body frame */ - for (int i = 0; i < 3; i++) { - acc[i] = 0.0f; - for (int j = 0; j < 3; j++) { - acc[i] += att.R[j][i] * acc_NED[j]; + /* project acceleration to body frame */ + for (int i = 0; i < 3; i++) { + acc[i] = 0.0f; + for (int j = 0; j < 3; j++) { + acc[i] += att.R[j][i] * acc_NED[j]; + } } } @@ -387,10 +391,12 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds vel_prev[0] = gps.vel_n_m_s; vel_prev[1] = gps.vel_e_m_s; vel_prev[2] = gps.vel_d_m_s; + last_gps = gps.timestamp_velocity; } else { vel_prev[0] = 0.0f; vel_prev[1] = 0.0f; vel_prev[2] = 0.0f; + last_gps = 0; } z_k[3] = raw.accelerometer_m_s2[0] - acc[0]; From 1cf9f72f628c5dbdf487e464699245cab61c1750 Mon Sep 17 00:00:00 2001 From: Jean Cyr Date: Sun, 3 Nov 2013 12:40:13 -0500 Subject: [PATCH 075/447] Add data manager module and fence support to navigator - Add function to geo.c to determine if global position is inside fence - Add navigator support/commands for maintaining fence coords. - Add data manager module to support persistence fence storage. Can store other data, but only used for fence at this time. - Add unit tests for data manager --- makefiles/config_px4fmu-v1_default.mk | 1 + makefiles/config_px4fmu-v2_default.mk | 1 + src/lib/geo/geo.c | 22 + src/lib/geo/geo.h | 14 + src/modules/dataman/dataman.c | 739 +++++++++++++++++++++++ src/modules/dataman/dataman.h | 116 ++++ src/modules/dataman/module.mk | 42 ++ src/modules/navigator/module.mk | 2 + src/modules/navigator/navigator_main.cpp | 325 ++++++++-- src/modules/uORB/objects_common.cpp | 3 + src/modules/uORB/topics/fence.h | 80 +++ src/systemcmds/tests/module.mk | 3 + src/systemcmds/tests/test_dataman.c | 179 ++++++ src/systemcmds/tests/tests.h | 1 + src/systemcmds/tests/tests_main.c | 1 + 15 files changed, 1470 insertions(+), 59 deletions(-) create mode 100644 src/modules/dataman/dataman.c create mode 100644 src/modules/dataman/dataman.h create mode 100644 src/modules/dataman/module.mk create mode 100644 src/modules/uORB/topics/fence.h create mode 100644 src/systemcmds/tests/test_dataman.c diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk index 3068270865..d116df1904 100644 --- a/makefiles/config_px4fmu-v1_default.mk +++ b/makefiles/config_px4fmu-v1_default.mk @@ -105,6 +105,7 @@ MODULES += modules/systemlib MODULES += modules/systemlib/mixer MODULES += modules/controllib MODULES += modules/uORB +MODULES += modules/dataman # # Libraries diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index 761fb8d9d6..ec4bb0b0b4 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -102,6 +102,7 @@ MODULES += modules/systemlib MODULES += modules/systemlib/mixer MODULES += modules/controllib MODULES += modules/uORB +MODULES += modules/dataman # # Libraries diff --git a/src/lib/geo/geo.c b/src/lib/geo/geo.c index 43105fdba7..8aeca5be75 100644 --- a/src/lib/geo/geo.c +++ b/src/lib/geo/geo.c @@ -465,4 +465,26 @@ __EXPORT float _wrap_360(float bearing) return bearing; } +__EXPORT bool inside_geofence(const struct vehicle_global_position_s *vehicle, const struct fence_s *fence) +{ + + /* Adaptation of algorithm originally presented as + * PNPOLY - Point Inclusion in Polygon Test + * W. Randolph Franklin (WRF) */ + + unsigned int i, j, vertices = fence->count; + bool c = false; + double lat = vehicle->lat / 1e7; + double lon = vehicle->lon / 1e7; + + // skip vertex 0 (return point) + for (i = 0, j = vertices - 1; i < vertices; j = i++) + if (((fence->vertices[i].lon) >= lon != (fence->vertices[j].lon >= lon)) && + (lat <= (fence->vertices[j].lat - fence->vertices[i].lat) * (lon - fence->vertices[i].lon) / + (fence->vertices[j].lon - fence->vertices[i].lon) + fence->vertices[i].lat)) + c = !c; + return c; +} + + diff --git a/src/lib/geo/geo.h b/src/lib/geo/geo.h index 123ff80f16..6bce9309b1 100644 --- a/src/lib/geo/geo.h +++ b/src/lib/geo/geo.h @@ -47,6 +47,9 @@ #pragma once +#include "uORB/topics/fence.h" +#include "uORB/topics/vehicle_global_position.h" + __BEGIN_DECLS #include @@ -126,4 +129,15 @@ __EXPORT float _wrap_360(float bearing); __EXPORT float _wrap_pi(float bearing); __EXPORT float _wrap_2pi(float bearing); +/** + * Return whether craft is inside geofence. + * + * Calculate whether point is inside arbitrary polygon + * @param craft pointer craft coordinates + * @param fence pointer to array of coordinates, one per vertex. First and last vertex are assumed connected + * @return true: craft is inside fence, false:craft is outside fence + */ +__EXPORT bool inside_geofence(const struct vehicle_global_position_s *craft, const struct fence_s *fence); + + __END_DECLS diff --git a/src/modules/dataman/dataman.c b/src/modules/dataman/dataman.c new file mode 100644 index 0000000000..05b77da20d --- /dev/null +++ b/src/modules/dataman/dataman.c @@ -0,0 +1,739 @@ +/**************************************************************************** + * + * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * Author: Lorenz Meier + * Jean Cyr + * + * 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 navigator_main.c + * Implementation of the main navigation state machine. + * + * Handles missions, geo fencing and failsafe navigation behavior. + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "dataman.h" + +/** + * data manager app start / stop handling function + * + * @ingroup apps + */ + +__EXPORT int dataman_main(int argc, char *argv[]); +__EXPORT ssize_t dm_read(dm_item_t item, unsigned char index, void *buffer, size_t buflen); +__EXPORT ssize_t dm_write(dm_item_t item, unsigned char index, dm_persitence_t persistence, const void *buffer, size_t buflen); +__EXPORT int dm_clear(dm_item_t item); +__EXPORT int dm_restart(dm_reset_reason restart_type); + +/* Types of function calls supported by the worker task */ +typedef enum { + dm_write_func = 0, + dm_read_func, + dm_clear_func, + dm_restart_func, + dm_number_of_funcs +} dm_function_t; + +/* Work task work item */ +typedef struct { + sq_entry_t link; /**< list linkage */ + sem_t wait_sem; + dm_function_t func; + ssize_t result; + union { + struct { + dm_item_t item; + unsigned char index; + dm_persitence_t persistence; + const void *buf; + size_t count; + } write_params; + struct { + dm_item_t item; + unsigned char index; + void *buf; + size_t count; + } read_params; + struct { + dm_item_t item; + } clear_params; + struct { + dm_reset_reason reason; + } restart_params; + }; +} work_q_item_t; + +/* Usage statistics */ +static unsigned g_func_counts[dm_number_of_funcs]; + +/* table of maximum number of instances for each item type */ +static const unsigned g_per_item_max_index[DM_KEY_NUM_KEYS] = { + DM_KEY_SAFE_POINTS_MAX, + DM_KEY_FENCE_POINTS_MAX, + DM_KEY_WAY_POINTS_MAX, +}; + +/* Table of offset for index 0 of each item type */ +static unsigned int g_key_offsets[DM_KEY_NUM_KEYS]; + +/* The data manager store file handle and file name */ +static int g_fd = -1, g_task_fd = -1; +static const char *k_data_manager_device_path = "/fs/microsd/dataman"; + +/* The data manager work queues */ + +typedef struct { + sq_queue_t q; + sem_t mutex; /* Mutual exclusion on work queue adds and deletes */ + unsigned size; + unsigned max_size; +} work_q_t; + +static work_q_t g_free_q; +static work_q_t g_work_q; + +sem_t g_work_queued_sema; +sem_t g_init_sema; + +static bool g_task_should_exit; /**< if true, sensor task should exit */ + +#define DM_SECTOR_HDR_SIZE 4 +static const unsigned k_sector_size = DM_MAX_DATA_SIZE + DM_SECTOR_HDR_SIZE; + +static void init_q(work_q_t *q) +{ + sq_init(&(q->q)); + sem_init(&(q->mutex), 1, 1); + q->size = q->max_size = 0; +} + +static void destroy_q(work_q_t *q) +{ + sem_destroy(&(q->mutex)); +} + +static inline void +lock_queue(work_q_t *q) +{ + sem_wait(&(q->mutex)); +} + +static inline void +unlock_queue(work_q_t *q) +{ + sem_post(&(q->mutex)); +} + +static work_q_item_t * +create_work_item(void) +{ + work_q_item_t *item; + + lock_queue(&g_free_q); + if ((item = (work_q_item_t *)sq_remfirst(&(g_free_q.q)))) + g_free_q.size--; + unlock_queue(&g_free_q); + + if (item == NULL) + item = (work_q_item_t *)malloc(sizeof(work_q_item_t)); + + if (item) + sem_init(&item->wait_sem, 1, 0); /* Caller will wait on this... initially locked */ + + return item; +} + +/* Work queue management functions */ +static void +enqueue_work_item(work_q_item_t *item) +{ + /* put the work item on the work queue */ + lock_queue(&g_work_q); + sq_addlast(&item->link, &(g_work_q.q)); + + if (++g_work_q.size > g_work_q.max_size) + g_work_q.max_size = g_work_q.size; + + unlock_queue(&g_work_q); + + /* tell the work thread that work is available */ + sem_post(&g_work_queued_sema); +} + +static void +destroy_work_item(work_q_item_t *item) +{ + sem_destroy(&item->wait_sem); + lock_queue(&g_free_q); + sq_addfirst(&item->link, &(g_free_q.q)); + + if (++g_free_q.size > g_free_q.max_size) + g_free_q.max_size = g_free_q.size; + + unlock_queue(&g_free_q); +} + +static work_q_item_t * +dequeue_work_item(void) +{ + work_q_item_t *work; + lock_queue(&g_work_q); + + if ((work = (work_q_item_t *)sq_remfirst(&g_work_q.q))) + g_work_q.size--; + + unlock_queue(&g_work_q); + return work; +} + +/* Calculate the offset in file of specific item */ +static int +calculate_offset(dm_item_t item, unsigned char index) +{ + + /* Make sure the item type is valid */ + if (item >= DM_KEY_NUM_KEYS) + return -1; + + /* Make sure the index for this item type is valid */ + if (index >= g_per_item_max_index[item]) + return -1; + + /* Calculate and return the item index based on type and index */ + return g_key_offsets[item] + (index * k_sector_size); +} + +/* Each data item is stored as follows + * + * byte 0: Length of user data item + * byte 1: Persistence of this data item + * byte DM_SECTOR_HDR_SIZE... : data item value + * + * The total size must not exceed k_sector_size + */ + +/* write to the data manager file */ +static ssize_t +_write(dm_item_t item, unsigned char index, dm_persitence_t persistence, const void *buf, size_t count) +{ + unsigned char buffer[k_sector_size]; + size_t len; + int offset; + + /* Get the offset for this item */ + offset = calculate_offset(item, index); + + if (offset < 0) + return -1; + + /* Make sure caller has not given us more data than we can handle */ + if (count > DM_MAX_DATA_SIZE) + return -1; + + /* Write out the data, prefixed with length and persistence level */ + buffer[0] = count; + buffer[1] = persistence; + buffer[2] = 0; + buffer[3] = 0; + memcpy(buffer + DM_SECTOR_HDR_SIZE, buf, count); + count += DM_SECTOR_HDR_SIZE; + + len = -1; + + if (lseek(g_task_fd, offset, SEEK_SET) == offset) + if ((len = write(g_task_fd, buffer, count)) == count) + fsync(g_task_fd); + + if (len != count) + return -1; + + /* All is well... return the number of user data written */ + return count - DM_SECTOR_HDR_SIZE; +} + +/* Retrieve from the data manager file */ +static ssize_t +_read(dm_item_t item, unsigned char index, void *buf, size_t count) +{ + unsigned char buffer[k_sector_size]; + int len, offset; + + /* Get the offset for this item */ + offset = calculate_offset(item, index); + + if (offset < 0) + return -1; + + /* Make sure the caller hasn't asked for more data than we can handle */ + if (count > DM_MAX_DATA_SIZE) + return -1; + + /* Read the prefix and data */ + len = -1; + if (lseek(g_task_fd, offset, SEEK_SET) == offset) + len = read(g_task_fd, buffer, count + DM_SECTOR_HDR_SIZE); + + /* Check for length issues */ + if (len < 0) + return -1; + + if (len == 0) + buffer[0] = 0; + + if (buffer[0] > 0) { + if (buffer[0] > count) + return -1; + + /* Looks good, copy it to the caller's buffer */ + memcpy(buf, buffer + DM_SECTOR_HDR_SIZE, buffer[0]); + } + + /* Return the number of bytes of caller data read */ + return buffer[0]; +} + +static int +_clear(dm_item_t item) +{ + int i, result = 0; + + int offset = calculate_offset(item, 0); + + if (offset < 0) + return -1; + + for (i = 0; (unsigned)i < g_per_item_max_index[item]; i++) { + char buf[1]; + + if (lseek(g_task_fd, offset, SEEK_SET) != offset) { + result = -1; + break; + } + + if (read(g_task_fd, buf, 1) < 1) + break; + + if (buf[0]) { + if (lseek(g_task_fd, offset, SEEK_SET) != offset) { + result = -1; + break; + } + + buf[0] = 0; + + if (write(g_task_fd, buf, 1) != 1) { + result = -1; + break; + } + } + + offset += k_sector_size; + } + + fsync(g_task_fd); + return result; +} + +/* Tell the data manager about the type of the last reset */ +static int +_restart(dm_reset_reason reason) +{ + unsigned char buffer[2]; + int offset, result = 0; + + /* We need to scan the entire file and invalidate and data that should not persist after the last reset */ + + /* Loop through all of the data segments and delete those that are not persistent */ + offset = 0; + + while (1) { + size_t len; + + /* Get data segment at current offset */ + if (lseek(g_task_fd, offset, SEEK_SET) != offset) { + result = -1; + break; + } + + len = read(g_task_fd, buffer, sizeof(buffer)); + + if (len == 0) + break; + + /* check if segment contains data */ + if (buffer[0]) { + int clear_entry = 0; + + /* Whether data gets deleted depends on reset type and data segment's persistence setting */ + if (reason == DM_INIT_REASON_POWER_ON) { + if (buffer[1] != DM_PERSIST_POWER_ON_RESET) { + clear_entry = 1; + } + + } else { + if ((buffer[1] != DM_PERSIST_POWER_ON_RESET) && (buffer[1] != DM_PERSIST_IN_FLIGHT_RESET)) { + clear_entry = 1; + } + } + + /* Set segment to unused if data does not persist */ + if (clear_entry) { + if (lseek(g_task_fd, offset, SEEK_SET) != offset) { + result = -1; + break; + } + + buffer[0] = 0; + + len = write(g_task_fd, buffer, 1); + + if (len != 1) { + result = -1; + break; + } + } + } + + offset += k_sector_size; + } + + fsync(g_task_fd); + + /* tell the caller how it went */ + return result; +} + +/* write to the data manager file */ +__EXPORT ssize_t +dm_write(dm_item_t item, unsigned char index, dm_persitence_t persistence, const void *buf, size_t count) +{ + work_q_item_t *work; + + if ((g_fd < 0) || g_task_should_exit) + return -1; + + /* Will return with queues locked */ + if ((work = create_work_item()) == NULL) + return -1; /* queues unlocked on failure */ + + work->func = dm_write_func; + work->write_params.item = item; + work->write_params.index = index; + work->write_params.persistence = persistence; + work->write_params.buf = buf; + work->write_params.count = count; + enqueue_work_item(work); + + sem_wait(&work->wait_sem); + ssize_t result = work->result; + destroy_work_item(work); + return result; +} + +/* Retrieve from the data manager file */ +__EXPORT ssize_t +dm_read(dm_item_t item, unsigned char index, void *buf, size_t count) +{ + work_q_item_t *work; + + if ((g_fd < 0) || g_task_should_exit) + return -1; + + /* Will return with queues locked */ + if ((work = create_work_item()) == NULL) + return -1; /* queues unlocked on failure */ + + work->func = dm_read_func; + work->read_params.item = item; + work->read_params.index = index; + work->read_params.buf = buf; + work->read_params.count = count; + enqueue_work_item(work); + + sem_wait(&work->wait_sem); + ssize_t result = work->result; + destroy_work_item(work); + return result; +} + +__EXPORT int +dm_clear(dm_item_t item) +{ + work_q_item_t *work; + + if ((g_fd < 0) || g_task_should_exit) + return -1; + + /* Will return with queues locked */ + if ((work = create_work_item()) == NULL) + return -1; /* queues unlocked on failure */ + + work->func = dm_clear_func; + work->clear_params.item = item; + enqueue_work_item(work); + + sem_wait(&work->wait_sem); + int result = work->result; + destroy_work_item(work); + return result; +} + +/* Tell the data manager about the type of the last reset */ +__EXPORT int +dm_restart(dm_reset_reason reason) +{ + work_q_item_t *work; + + if ((g_fd < 0) || g_task_should_exit) + return -1; + + /* Will return with queues locked */ + if ((work = create_work_item()) == NULL) + return -1; /* queues unlocked on failure */ + + work->func = dm_restart_func; + work->restart_params.reason = reason; + enqueue_work_item(work); + + sem_wait(&work->wait_sem); + int result = work->result; + destroy_work_item(work); + return result; +} + +static int +task_main(int argc, char *argv[]) +{ + work_q_item_t *work; + + /* inform about start */ + warnx("Initializing.."); + + /* Initialize global variables */ + g_key_offsets[0] = 0; + + for (unsigned i = 0; i < (DM_KEY_NUM_KEYS - 1); i++) + g_key_offsets[i + 1] = g_key_offsets[i] + (g_per_item_max_index[i] * k_sector_size); + + unsigned max_offset = g_key_offsets[DM_KEY_NUM_KEYS - 1] + (g_per_item_max_index[DM_KEY_NUM_KEYS - 1] * k_sector_size); + + for (unsigned i = 0; i < dm_number_of_funcs; i++) + g_func_counts[i] = 0; + + g_task_should_exit = false; + + init_q(&g_work_q); + init_q(&g_free_q); + + sem_init(&g_work_queued_sema, 1, 0); + + g_task_fd = open(k_data_manager_device_path, O_RDWR | O_CREAT | O_BINARY); + if (g_task_fd < 0) { + warnx("Could not open data manager file %s", k_data_manager_device_path); + return -1; + } + if (lseek(g_task_fd, max_offset, SEEK_SET) != max_offset) { + close(g_task_fd); + warnx("Could not seek data manager file %s", k_data_manager_device_path); + return -1; + } + fsync(g_task_fd); + + g_fd = g_task_fd; + + warnx("Initialized, data manager file '%s' size is %d bytes", k_data_manager_device_path, max_offset); + + sem_post(&g_init_sema); + + /* Start the endless loop, waiting for then processing work requests */ + while (true) { + + /* do we need to exit ??? */ + if ((g_task_should_exit) && (g_fd >= 0)) { + /* Close the file handle to stop further queueing */ + g_fd = -1; + } + + if (!g_task_should_exit) { + /* wait for work */ + sem_wait(&g_work_queued_sema); + } + + /* Empty the work queue */ + while ((work = dequeue_work_item())) { + + switch (work->func) { + case dm_write_func: + g_func_counts[dm_write_func]++; + work->result = + _write(work->write_params.item, work->write_params.index, work->write_params.persistence, work->write_params.buf, work->write_params.count); + break; + + case dm_read_func: + g_func_counts[dm_read_func]++; + work->result = + _read(work->read_params.item, work->read_params.index, work->read_params.buf, work->read_params.count); + break; + + case dm_clear_func: + g_func_counts[dm_clear_func]++; + work->result = _clear(work->clear_params.item); + break; + + case dm_restart_func: + g_func_counts[dm_restart_func]++; + work->result = _restart(work->restart_params.reason); + break; + + default: /* should never happen */ + work->result = -1; + break; + } + + /* Inform the caller that work is done */ + sem_post(&work->wait_sem); + } + + /* time to go???? */ + if ((g_task_should_exit) && (g_fd < 0)) + break; + } + + close(g_task_fd); + g_task_fd = -1; + + /* Empty the work queue */ + for (;;) { + if ((work = (work_q_item_t *)sq_remfirst(&(g_free_q.q))) == NULL) + break; + + free(work); + } + + destroy_q(&g_work_q); + destroy_q(&g_free_q); + sem_destroy(&g_work_queued_sema); + + return 0; +} + +static int +start(void) +{ + int task; + + sem_init(&g_init_sema, 1, 0); + + /* start the task */ + if ((task = task_spawn_cmd("dataman", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 2048, task_main, NULL)) <= 0) { + warn("task start failed"); + return -1; + } + + /* wait for the thread to actuall initialize */ + sem_wait(&g_init_sema); + sem_destroy(&g_init_sema); + + return 0; +} + +static void +status(void) +{ + /* display usage statistics */ + warnx("Writes %d", g_func_counts[dm_write_func]); + warnx("Reads %d", g_func_counts[dm_read_func]); + warnx("Clears %d", g_func_counts[dm_clear_func]); + warnx("Restarts %d", g_func_counts[dm_restart_func]); + warnx("Max Q lengths work %d, free %d", g_work_q.max_size, g_free_q.max_size); +} + +static void +stop(void) +{ + /* Tell the worker task to shut down */ + g_task_should_exit = true; + sem_post(&g_work_queued_sema); +} + +static void +usage(void) +{ + errx(1, "usage: dataman {start|stop|status}"); +} + +int +dataman_main(int argc, char *argv[]) +{ + if (argc < 2) + usage(); + + if (!strcmp(argv[1], "start")) { + + if (g_fd >= 0) + errx(1, "already running"); + + start(); + + if (g_fd < 0) + errx(1, "start failed"); + + return 0; + } + + if (g_fd < 0) + errx(1, "not running"); + + if (!strcmp(argv[1], "stop")) + stop(); + else if (!strcmp(argv[1], "status")) + status(); + else + usage(); + + return 0; +} + diff --git a/src/modules/dataman/dataman.h b/src/modules/dataman/dataman.h new file mode 100644 index 0000000000..41ddfaf61b --- /dev/null +++ b/src/modules/dataman/dataman.h @@ -0,0 +1,116 @@ +/**************************************************************************** + * + * Copyright (c) 2013 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 dataman.h + * + * DATAMANAGER driver. + */ +#ifndef _DATAMANAGER_H +#define _DATAMANAGER_H + +#include +#include +#include + +#ifdef __cplusplus +extern "C" { +#endif + + /* Types of items that the data manager can store */ + typedef enum { + DM_KEY_SAFE_POINTS = 0, /* Safe points coordinates, safe point 0 is home point */ + DM_KEY_FENCE_POINTS, /* Fence vertex coordinates */ + DM_KEY_WAY_POINTS, /* Mission way point coordinates */ + DM_KEY_NUM_KEYS /* Total number of item types defined */ + } dm_item_t; + + /* The maximum number of instances for each item type */ + enum { + DM_KEY_SAFE_POINTS_MAX = 8, + DM_KEY_FENCE_POINTS_MAX = GEOFENCE_MAX_VERTICES, + DM_KEY_WAY_POINTS_MAX = MAVLINK_WPM_MAX_WP_COUNT, + }; + + /* Data persistence levels */ + typedef enum { + DM_PERSIST_POWER_ON_RESET = 0, /* Data survives all resets */ + DM_PERSIST_IN_FLIGHT_RESET, /* Data survives in-flight resets only */ + DM_PERSIST_VOLATILE /* Data does not survive resets */ + } dm_persitence_t; + + /* The reason for the last reset */ + typedef enum { + DM_INIT_REASON_POWER_ON = 0, /* Data survives resets */ + DM_INIT_REASON_IN_FLIGHT /* Data survives in-flight resets only */ + } dm_reset_reason; + + /* Maximum size in bytes of a single item instance */ + #define DM_MAX_DATA_SIZE 126 + + /* Retrieve from the data manager store */ + __EXPORT ssize_t + dm_read( + dm_item_t item, /* The item type to retrieve */ + unsigned char index, /* The index of the item */ + void *buffer, /* Pointer to caller data buffer */ + size_t buflen /* Length in bytes of data to retrieve */ + ); + + /* write to the data manager store */ + __EXPORT ssize_t + dm_write( + dm_item_t item, /* The item type to store */ + unsigned char index, /* The index of the item */ + dm_persitence_t persistence, /* The persistence level of this item */ + const void *buffer, /* Pointer to caller data buffer */ + size_t buflen /* Length in bytes of data to retrieve */ + ); + + /* Retrieve from the data manager store */ + __EXPORT int + dm_clear( + dm_item_t item /* The item type to clear */ + ); + + /* Tell the data manager about the type of the last reset */ + __EXPORT int + dm_restart( + dm_reset_reason restart_type /* The last reset type */ + ); + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/src/modules/dataman/module.mk b/src/modules/dataman/module.mk new file mode 100644 index 0000000000..dce7a62358 --- /dev/null +++ b/src/modules/dataman/module.mk @@ -0,0 +1,42 @@ +############################################################################ +# +# Copyright (c) 2013 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. +# +############################################################################ + +# +# Main Navigation Controller +# + +MODULE_COMMAND = dataman + +SRCS = dataman.c + +INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink diff --git a/src/modules/navigator/module.mk b/src/modules/navigator/module.mk index 0404b06c78..7f7443c43e 100644 --- a/src/modules/navigator/module.mk +++ b/src/modules/navigator/module.mk @@ -39,3 +39,5 @@ MODULE_COMMAND = navigator SRCS = navigator_main.cpp \ navigator_params.c + +INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index f6c44444aa..2181001c4a 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -2,6 +2,7 @@ * * Copyright (c) 2013 PX4 Development Team. All rights reserved. * Author: Lorenz Meier + * Jean Cyr * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -48,6 +49,8 @@ #include #include #include +#include +#include #include #include #include @@ -62,12 +65,14 @@ #include #include #include +#include #include #include #include #include #include #include +#include /** * navigator app start / stop handling function @@ -90,12 +95,27 @@ public: ~Navigator(); /** - * Start the sensors task. + * Start the navigator task. * * @return OK on success. */ int start(); + /** + * Display the navigator status. + */ + void status(); + + /** + * Load fence parameters. + */ + bool load_fence(unsigned vertices); + + /** + * Specify fence vertex position. + */ + void fence_point(int argc, char *argv[]); + private: bool _task_should_exit; /**< if true, sensor task should exit */ @@ -108,7 +128,10 @@ private: int _vstatus_sub; /**< vehicle status subscription */ int _params_sub; /**< notification of parameter updates */ int _manual_control_sub; /**< notification of manual control updates */ - int _mission_sub; + int _mission_sub; + int _capabilities_sub; + int _fence_sub; + int _fence_pub; orb_advert_t _triplet_pub; /**< position setpoint */ @@ -122,9 +145,16 @@ private: perf_counter_t _loop_perf; /**< loop performance counter */ - unsigned _mission_items_maxcount; /**< maximum number of mission items supported */ - struct mission_item_s * _mission_items; /**< storage for mission items */ - bool _mission_valid; /**< flag if mission is valid */ + unsigned _mission_items_maxcount; /**< maximum number of mission items supported */ + struct mission_item_s * _mission_items; /**< storage for mission items */ + bool _mission_valid; /**< flag if mission is valid */ + + + struct fence_s _fence; /**< storage for fence vertices */ + bool _fence_valid; /**< flag if fence is valid */ + bool _inside_fence; /**< vehicle is inside fence */ + + struct navigation_capabilities_s _nav_caps; /** manual control states */ float _seatbelt_hold_heading; /**< heading the system should hold in seatbelt mode */ @@ -169,6 +199,11 @@ private: */ void mission_poll(); + /** + * Retrieve mission. + */ + void mission_update(); + /** * Control throttle. */ @@ -192,6 +227,14 @@ private: * Main sensor collection task. */ void task_main() __attribute__((noreturn)); + + void publish_fence(unsigned vertices); + + void publish_mission(unsigned points); + + void publish_safepoints(unsigned points); + + bool fence_valid(const struct fence_s &fence); }; namespace navigator @@ -218,6 +261,10 @@ Navigator::Navigator() : _vstatus_sub(-1), _params_sub(-1), _manual_control_sub(-1), + _fence_sub(-1), + _fence_pub(-1), + _mission_sub(-1), + _capabilities_sub(-1), /* publications */ _triplet_pub(-1), @@ -227,17 +274,15 @@ Navigator::Navigator() : /* states */ _mission_items_maxcount(20), _mission_valid(false), - _loiter_hold(false) + _loiter_hold(false), + _fence_valid(false), + _inside_fence(true) { - _mission_items = (mission_item_s*)malloc(sizeof(mission_item_s) * _mission_items_maxcount); - if (!_mission_items) { - _mission_items_maxcount = 0; - warnx("no free RAM to allocate mission, rejecting any waypoints"); - } - + _global_pos.valid = false; + memset(&_fence, 0, sizeof(_fence)); _parameter_handles.throttle_cruise = param_find("NAV_DUMMY"); - /* fetch initial parameter values */ + /* fetch initial values */ parameters_update(); } @@ -283,10 +328,8 @@ Navigator::vehicle_status_poll() /* Check HIL state if vehicle status has changed */ orb_check(_vstatus_sub, &vstatus_updated); - if (vstatus_updated) { - + if (vstatus_updated) orb_copy(ORB_ID(vehicle_status), _vstatus_sub, &_vstatus); - } } void @@ -296,9 +339,8 @@ Navigator::vehicle_attitude_poll() bool att_updated; orb_check(_att_sub, &att_updated); - if (att_updated) { + if (att_updated) orb_copy(ORB_ID(vehicle_attitude), _att_sub, &_att); - } } void @@ -308,18 +350,22 @@ Navigator::mission_poll() bool mission_updated; orb_check(_mission_sub, &mission_updated); - if (mission_updated) { - - struct mission_s mission; - orb_copy(ORB_ID(mission), _mission_sub, &mission); + if (mission_updated) + mission_update(); +} +void +Navigator::mission_update() +{ + struct mission_s mission; + if (orb_copy(ORB_ID(mission), _mission_sub, &mission) == OK) { // XXX this is not optimal yet, but a first prototype / // test implementation if (mission.count <= _mission_items_maxcount) { /* - * Perform an atomic copy & state update - */ + * Perform an atomic copy & state update + */ irqstate_t flags = irqsave(); memcpy(_mission_items, mission.items, mission.count * sizeof(struct mission_item_s)); @@ -351,12 +397,22 @@ Navigator::task_main() */ _global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); _mission_sub = orb_subscribe(ORB_ID(mission)); + _capabilities_sub = orb_subscribe(ORB_ID(navigation_capabilities)); + _fence_sub = orb_subscribe(ORB_ID(fence)); _att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); _airspeed_sub = orb_subscribe(ORB_ID(airspeed)); _vstatus_sub = orb_subscribe(ORB_ID(vehicle_status)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); _manual_control_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); + // Load initial states + if (orb_copy(ORB_ID(vehicle_status), _vstatus_sub, &_vstatus) != OK) { + _vstatus.arming_state = ARMING_STATE_STANDBY; // for testing... commander may not be running + } + + orb_copy(ORB_ID(vehicle_attitude), _att_sub, &_att); + mission_update(); + /* rate limit vehicle status updates to 5Hz */ orb_set_interval(_vstatus_sub, 200); /* rate limit position updates to 50 Hz */ @@ -364,23 +420,35 @@ Navigator::task_main() parameters_update(); + _fence_valid = load_fence(GEOFENCE_MAX_VERTICES); + + /* load the craft capabilities */ + orb_copy(ORB_ID(navigation_capabilities), _capabilities_sub, &_nav_caps); + /* wakeup source(s) */ - struct pollfd fds[2]; + struct pollfd fds[5]; /* Setup of loop */ fds[0].fd = _params_sub; fds[0].events = POLLIN; fds[1].fd = _global_pos_sub; fds[1].events = POLLIN; + fds[2].fd = _fence_sub; + fds[2].events = POLLIN; + fds[3].fd = _capabilities_sub; + fds[3].events = POLLIN; + fds[4].fd = _mission_sub; + fds[4].events = POLLIN; while (!_task_should_exit) { - /* wait for up to 500ms for data */ + /* wait for up to 100ms for data */ int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100); /* timed out - periodic check for _task_should_exit, etc. */ - 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) { @@ -403,17 +471,34 @@ Navigator::task_main() parameters_update(); } + /* only update fence if it has changed */ + if (fds[2].revents & POLLIN) { + /* read from fence to clear updated flag */ + unsigned vertices; + orb_copy(ORB_ID(fence), _fence_sub, &vertices); + _fence_valid = load_fence(vertices); + } + + /* only update craft capabilities if they have changed */ + if (fds[3].revents & POLLIN) { + orb_copy(ORB_ID(navigation_capabilities), _capabilities_sub, &_nav_caps); + } + + if (fds[4].revents & POLLIN) { + mission_update(); + } + /* only run controller if position changed */ if (fds[1].revents & POLLIN) { - static uint64_t last_run = 0; float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f; last_run = hrt_absolute_time(); /* guard against too large deltaT's */ - if (deltaT > 1.0f) + if (deltaT > 1.0f) { deltaT = 0.01f; + } /* load local copies */ orb_copy(ORB_ID(vehicle_global_position), _global_pos_sub, &_global_pos); @@ -422,6 +507,10 @@ Navigator::task_main() mission_poll(); + if (_fence_valid && _global_pos.valid) { + _inside_fence = inside_geofence(&_global_pos, &_fence); + } + math::Vector2f ground_speed(_global_pos.vx, _global_pos.vy); // Current waypoint math::Vector2f next_wp(_global_triplet.current.lat / 1e7f, _global_triplet.current.lon / 1e7f); @@ -460,14 +549,14 @@ Navigator::task_main() if (_global_triplet.current.nav_cmd == NAV_CMD_WAYPOINT) { /* waypoint is a plain navigation waypoint */ - + } else if (_global_triplet.current.nav_cmd == NAV_CMD_LOITER_TURN_COUNT || - _global_triplet.current.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || - _global_triplet.current.nav_cmd == NAV_CMD_LOITER_UNLIMITED) { + _global_triplet.current.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || + _global_triplet.current.nav_cmd == NAV_CMD_LOITER_UNLIMITED) { /* waypoint is a loiter waypoint */ - + } // XXX at this point we always want no loiter hold if a @@ -528,9 +617,10 @@ Navigator::task_main() } perf_end(_loop_perf); + } - warnx("exiting.\n"); + warnx("exiting."); _navigator_task = -1; _exit(0); @@ -543,11 +633,11 @@ Navigator::start() /* start the task */ _navigator_task = task_spawn_cmd("navigator", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 5, - 2048, - (main_t)&Navigator::task_main_trampoline, - nullptr); + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 2048, + (main_t)&Navigator::task_main_trampoline, + nullptr); if (_navigator_task < 0) { warn("task start failed"); @@ -557,20 +647,139 @@ Navigator::start() return OK; } +void +Navigator::status() +{ + warnx("Global position is %svalid", _global_pos.valid ? "" : "in"); + if (_global_pos.valid) { + warnx("Longitude %5.5f degrees, latitude %5.5f degrees", _global_pos.lon / 1e7, _global_pos.lat / 1e7); + warnx("Altitude %5.5f meters, altitude above home %5.5f meters", + (double)_global_pos.alt, (double)_global_pos.relative_alt); + warnx("Ground velocity in m/s, x %5.5f, y %5.5f, z %5.5f", + (double)_global_pos.vx, (double)_global_pos.vy, (double)_global_pos.vz); + warnx("Compass heading in degrees %5.5f", (double)_global_pos.yaw * 57.2957795); + } + if (_fence_valid) { + warnx("Geofence is valid"); + warnx("Vertex longitude latitude"); + for (unsigned i = 0; i < _fence.count; i++) + warnx("%6u %9.5f %8.5f", i, (double)_fence.vertices[i].lon, (double)_fence.vertices[i].lat); + } else + warnx("Geofence not set"); +} + +void +Navigator::publish_fence(unsigned vertices) +{ + if (_fence_pub == -1) + _fence_pub = orb_advertise(ORB_ID(fence), &vertices); + else + orb_publish(ORB_ID(fence), _fence_pub, &vertices); +} + +bool +Navigator::fence_valid(const struct fence_s &fence) +{ + struct vehicle_global_position_s pos; + + // NULL fence is valid + if (fence.count == 0) { + return true; + } + + // Otherwise + if ((fence.count < 4) || (fence.count > GEOFENCE_MAX_VERTICES)) { + warnx("Fence must have at least 3 sides and not more than %d", GEOFENCE_MAX_VERTICES - 1); + return false; + } + + return true; +} + +bool +Navigator::load_fence(unsigned vertices) +{ + struct fence_s temp_fence; + + unsigned i; + for (i = 0; i < vertices; i++) { + if (dm_read(DM_KEY_FENCE_POINTS, i, temp_fence.vertices + i, sizeof(struct fence_vertex_s)) != sizeof(struct fence_vertex_s)) { + break; + } + } + + temp_fence.count = i; + + if (fence_valid(temp_fence)) + memcpy(&_fence, &temp_fence, sizeof(_fence)); + else + warnx("Invalid fence file, ignored!"); + + return _fence.count != 0; +} + +void +Navigator::fence_point(int argc, char *argv[]) +{ + int ix, last; + double lon, lat; + struct fence_vertex_s vertex; + char *end; + + if ((argc == 1) && (strcmp("-clear", argv[0]) == 0)) { + dm_clear(DM_KEY_FENCE_POINTS); + publish_fence(0); + return; + } + + if (argc < 3) + errx(1, "Specify: -clear | sequence latitude longitude [-publish]"); + + ix = atoi(argv[0]); + if (ix >= DM_KEY_FENCE_POINTS_MAX) + errx(1, "Sequence must be less than %d", DM_KEY_FENCE_POINTS_MAX); + + lat = strtod(argv[1], &end); + lon = strtod(argv[2], &end); + + last = 0; + if ((argc > 3) && (strcmp(argv[3], "-publish") == 0)) + last = 1; + + vertex.lat = (float)lat; + vertex.lon = (float)lon; + + if (dm_write(DM_KEY_FENCE_POINTS, ix, DM_PERSIST_POWER_ON_RESET, &vertex, sizeof(vertex)) == sizeof(vertex)) { + if (last) + publish_fence((unsigned)ix + 1); + return; + } + + errx(1, "can't store fence point"); +} + +static void usage() +{ + errx(1, "usage: navigator {start|stop|status|fence}"); +} + int navigator_main(int argc, char *argv[]) { - if (argc < 1) - errx(1, "usage: navigator {start|stop|status}"); + if (argc < 2) { + usage(); + } if (!strcmp(argv[1], "start")) { - if (navigator::g_navigator != nullptr) + if (navigator::g_navigator != nullptr) { errx(1, "already running"); + } navigator::g_navigator = new Navigator; - if (navigator::g_navigator == nullptr) + if (navigator::g_navigator == nullptr) { errx(1, "alloc failed"); + } if (OK != navigator::g_navigator->start()) { delete navigator::g_navigator; @@ -578,27 +787,25 @@ int navigator_main(int argc, char *argv[]) err(1, "start failed"); } - exit(0); + return 0; } + if (navigator::g_navigator == nullptr) + errx(1, "not running"); + if (!strcmp(argv[1], "stop")) { - if (navigator::g_navigator == nullptr) - errx(1, "not running"); - delete navigator::g_navigator; navigator::g_navigator = nullptr; - exit(0); + + } else if (!strcmp(argv[1], "status")) { + navigator::g_navigator->status(); + + } else if (!strcmp(argv[1], "fence")) { + navigator::g_navigator->fence_point(argc - 2, argv + 2); + + } else { + usage(); } - if (!strcmp(argv[1], "status")) { - if (navigator::g_navigator) { - errx(0, "running"); - - } else { - errx(1, "not running"); - } - } - - warnx("unrecognized command"); - return 1; + return 0; } diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index 3514dca24d..769fe90459 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -129,6 +129,9 @@ ORB_DEFINE(vehicle_global_velocity_setpoint, struct vehicle_global_velocity_setp #include "topics/mission.h" ORB_DEFINE(mission, struct mission_s); +#include "topics/fence.h" +ORB_DEFINE(fence, unsigned); + #include "topics/vehicle_attitude_setpoint.h" ORB_DEFINE(vehicle_attitude_setpoint, struct vehicle_attitude_setpoint_s); diff --git a/src/modules/uORB/topics/fence.h b/src/modules/uORB/topics/fence.h new file mode 100644 index 0000000000..6f16c51cf8 --- /dev/null +++ b/src/modules/uORB/topics/fence.h @@ -0,0 +1,80 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: @author Jean Cyr + * + * 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 fence.h + * Definition of geofence. + */ + +#ifndef TOPIC_FENCE_H_ +#define TOPIC_FENCE_H_ + +#include +#include +#include "../uORB.h" + +/** + * @addtogroup topics + * @{ + */ + +#define GEOFENCE_MAX_VERTICES 16 + +/** + * This is the position of a geofence vertex + * + */ +struct fence_vertex_s { + // Worst case float precision gives us 2 meter resolution at the equator + float lat; /**< latitude in degrees */ + float lon; /**< longitude in degrees */ +}; + +/** + * This is the position of a geofence + * + */ +struct fence_s { + unsigned count; /**< number of actual vertices */ + struct fence_vertex_s vertices[GEOFENCE_MAX_VERTICES]; +}; + +/** + * @} + */ + +/* register this as object request broker structure */ +ORB_DECLARE(fence); + +#endif diff --git a/src/systemcmds/tests/module.mk b/src/systemcmds/tests/module.mk index 6729ce4ae3..53c0e6b4f4 100644 --- a/src/systemcmds/tests/module.mk +++ b/src/systemcmds/tests/module.mk @@ -24,6 +24,9 @@ SRCS = test_adc.c \ test_uart_loopback.c \ test_uart_send.c \ test_mixer.cpp \ + test_dataman.c \ tests_file.c \ tests_main.c \ tests_param.c + +INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink diff --git a/src/systemcmds/tests/test_dataman.c b/src/systemcmds/tests/test_dataman.c new file mode 100644 index 0000000000..e33c5acebe --- /dev/null +++ b/src/systemcmds/tests/test_dataman.c @@ -0,0 +1,179 @@ +/**************************************************************************** + * px4/sensors/test_dataman.c + * + * Copyright (C) 2012 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 NuttX 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. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include + +#include + +#include +#include +#include +#include +#include +#include + +#include + +#include +#include +#include +#include + + +#include "tests.h" + +#include "dataman/dataman.h" + +static sem_t *sems; + +static int +task_main(int argc, char *argv[]) +{ + char buffer[DM_MAX_DATA_SIZE]; + hrt_abstime wstart, wend, rstart, rend; + + warnx("Starting dataman test task %s", argv[1]); + /* try to read an invalid item */ + int my_id = atoi(argv[1]); + /* try to read an invalid item */ + if (dm_read(DM_KEY_NUM_KEYS, 0, buffer, sizeof(buffer)) >= 0) { + warnx("%d read an invalid item failed", my_id); + goto fail; + } + /* try to read an invalid index */ + if (dm_read(DM_KEY_SAFE_POINTS, DM_KEY_SAFE_POINTS_MAX, buffer, sizeof(buffer)) >= 0) { + warnx("%d read an invalid index failed", my_id); + goto fail; + } + srand(hrt_absolute_time() ^ my_id); + unsigned hit = 0, miss = 0; + wstart = hrt_absolute_time(); + for (unsigned i = 0; i < 256; i++) { + memset(buffer, my_id, sizeof(buffer)); + buffer[1] = i; + unsigned hash = i ^ my_id; + unsigned len = (hash & 63) + 2; + if (dm_write(DM_KEY_WAY_POINTS, hash, DM_PERSIST_IN_FLIGHT_RESET, buffer, len) != len) { + warnx("%d write failed, index %d, length %d", my_id, hash, len); + goto fail; + } + usleep(rand() & ((64 * 1024) - 1)); + } + rstart = hrt_absolute_time(); + wend = rstart; + + for (unsigned i = 0; i < 256; i++) { + unsigned hash = i ^ my_id; + unsigned len2, len = (hash & 63) + 2; + if ((len2 = dm_read(DM_KEY_WAY_POINTS, hash, buffer, sizeof(buffer))) < 2) { + warnx("%d read failed length test, index %d", my_id, hash); + goto fail; + } + if (buffer[0] == my_id) { + hit++; + if (len2 != len) { + warnx("%d read failed length test, index %d, wanted %d, got %d", my_id, hash, len, len2); + goto fail; + } + if (buffer[1] != i) { + warnx("%d data verification failed, index %d, wanted %d, got %d", my_id, hash, my_id, buffer[1]); + goto fail; + } + } + else + miss++; + } + rend = hrt_absolute_time(); + warnx("Test %d pass, hit %d, miss %d, io time read %llums. write %llums.", + my_id, hit, miss, (rend - rstart) / 256000, (wend - wstart) / 256000); + sem_post(sems + my_id); + return 0; +fail: + warnx("Test %d fail, buffer %02x %02x %02x %02x %02x %02x", + my_id, buffer[0], buffer[1], buffer[2], buffer[3], buffer[4], buffer[5]); + sem_post(sems + my_id); + return -1; +} + +int test_dataman(int argc, char *argv[]) +{ + int i, num_tasks = 4; + char buffer[DM_MAX_DATA_SIZE]; + + if (argc > 1) + num_tasks = atoi(argv[1]); + + sems = (sem_t *)malloc(num_tasks * sizeof(sem_t)); + warnx("Running %d tasks", num_tasks); + for (i = 0; i < num_tasks; i++) { + int task; + char a[16]; + sprintf(a, "%d", i); + const char *av[2]; + av[0] = a; + av[1] = 0; + sem_init(sems + i, 1, 0); + /* start the task */ + if ((task = task_spawn_cmd("dataman", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 2048, task_main, av)) <= 0) { + warn("task start failed"); + } + } + for (i = 0; i < num_tasks; i++) { + sem_wait(sems + i); + sem_destroy(sems + i); + } + free(sems); + dm_restart(DM_INIT_REASON_IN_FLIGHT); + for (i = 0; i < 256; i++) { + if (dm_read(DM_KEY_WAY_POINTS, i, buffer, sizeof(buffer)) != 0) + break; + } + if (i >= 256) { + warnx("Restart in-flight failed"); + return -1; + + } + dm_restart(DM_INIT_REASON_POWER_ON); + for (i = 0; i < 256; i++) { + if (dm_read(DM_KEY_WAY_POINTS, i, buffer, sizeof(buffer)) != 0) { + warnx("Restart power-on failed"); + return -1; + } + } + return 0; +} diff --git a/src/systemcmds/tests/tests.h b/src/systemcmds/tests/tests.h index c483108cf0..4a5eaa0c50 100644 --- a/src/systemcmds/tests/tests.h +++ b/src/systemcmds/tests/tests.h @@ -101,6 +101,7 @@ extern int test_param(int argc, char *argv[]); extern int test_bson(int argc, char *argv[]); extern int test_file(int argc, char *argv[]); extern int test_mixer(int argc, char *argv[]); +extern int test_dataman(int argc, char *argv[]); __END_DECLS diff --git a/src/systemcmds/tests/tests_main.c b/src/systemcmds/tests/tests_main.c index 9eb9c632ec..9b1cfb1fe5 100644 --- a/src/systemcmds/tests/tests_main.c +++ b/src/systemcmds/tests/tests_main.c @@ -112,6 +112,7 @@ const struct { {"file", test_file, 0}, {"mixer", test_mixer, OPT_NOJIGTEST | OPT_NOALLTEST}, {"help", test_help, OPT_NOALLTEST | OPT_NOHELP | OPT_NOJIGTEST}, + {"dataman", test_dataman, OPT_NOALLTEST}, {NULL, NULL, 0} }; From 937b502d4c3fd582f7be736240f5971e8c0f7c2b Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 3 Nov 2013 20:33:56 +0100 Subject: [PATCH 076/447] increase landing speed to v_min * 1.3 for more safety --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 08f9d3c15b..9060eac90d 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -798,7 +798,7 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio float flare_angle_rad = -math::radians(5.0f);//math::radians(global_triplet.current.param1) float land_pitch_min = math::radians(5.0f); float throttle_land = _parameters.throttle_min + (_parameters.throttle_max - _parameters.throttle_min) * 0.1f; - float airspeed_land = _parameters.airspeed_min; + float airspeed_land = 1.3f * _parameters.airspeed_min; float airspeed_approach = 1.3f * _parameters.airspeed_min; float landing_slope_angle_rad = math::radians(_parameters.land_slope_angle); From 9d5f422d245de4ead6b37193e7862ba771febb83 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 3 Nov 2013 20:44:12 +0100 Subject: [PATCH 077/447] fix land_noreturn logic --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 9060eac90d..1a263b741d 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -766,7 +766,7 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio // target_bearing = get_bearing_to_next_waypoint(prev_wp.getX(), prev_wp.getY(), next_wp.getX(), next_wp.getY()); // } else { - if (!land_noreturn) + if (!land_noreturn) //set target_bearing in first occurrence target_bearing = _att.yaw; //} @@ -774,8 +774,7 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio _l1_control.navigate_heading(target_bearing, _att.yaw, ground_speed); - if (altitude_error > -5.0f) - land_noreturn = true; + land_noreturn = true; } else { From 89b8acd7a8dd82a7adbd38fb7fea4b422dea64e6 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 3 Nov 2013 22:04:32 +0100 Subject: [PATCH 078/447] fix initialization of attitude controllers --- src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp | 11 ++++++++++- src/lib/ecl/attitude_fw/ecl_roll_controller.cpp | 6 +++++- src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp | 11 +++++++---- src/lib/ecl/attitude_fw/ecl_yaw_controller.h | 2 -- 4 files changed, 22 insertions(+), 8 deletions(-) diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp index 4e27de45c9..ccaed14cea 100644 --- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp @@ -49,10 +49,19 @@ ECL_PitchController::ECL_PitchController() : _last_run(0), + _tc(0.1f), + _k_p(0.0f), + _k_i(0.0f), + _k_d(0.0f), + _integrator_max(0.0f), + _max_rate_pos(0.0f), + _max_rate_neg(0.0f), + _roll_ff(0.0f), _last_output(0.0f), _integrator(0.0f), _rate_error(0.0f), - _rate_setpoint(0.0f) + _rate_setpoint(0.0f), + _bodyrate_setpoint(0.0f) { } diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp index 0772f88bc7..4b0bfc6c42 100644 --- a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp @@ -50,13 +50,17 @@ ECL_RollController::ECL_RollController() : _last_run(0), _tc(0.1f), + _k_p(0.0f), + _k_i(0.0f), + _k_d(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) { - } float ECL_RollController::control_attitude(float roll_setpoint, float roll) diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp index 0de0eb439f..013bc191ac 100644 --- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp @@ -48,16 +48,19 @@ ECL_YawController::ECL_YawController() : _last_run(0), - _tc(0.1f), + _k_p(0.0f), + _k_i(0.0f), + _k_d(0.0f), + _integrator_max(0.0f), + _max_rate(0.0f), + _roll_ff(0.0f), _last_output(0.0f), _integrator(0.0f), _rate_error(0.0f), _rate_setpoint(0.0f), _bodyrate_setpoint(0.0f), - _coordinated(1.0f) - + _coordinated(0.0f) { - } float ECL_YawController::control_attitude(float roll, float pitch, diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.h b/src/lib/ecl/attitude_fw/ecl_yaw_controller.h index 5c00fa8731..154471cafb 100644 --- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.h +++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.h @@ -104,8 +104,6 @@ public: private: uint64_t _last_run; - - float _tc; float _k_p; float _k_i; float _k_d; From 3b1086b879a78eb48a04c36d0c1a9ad2e7eac6bf Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 3 Nov 2013 22:04:32 +0100 Subject: [PATCH 079/447] fix initialization of attitude controllers --- src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp | 11 ++++++++++- src/lib/ecl/attitude_fw/ecl_roll_controller.cpp | 6 +++++- src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp | 11 +++++++---- src/lib/ecl/attitude_fw/ecl_yaw_controller.h | 2 -- 4 files changed, 22 insertions(+), 8 deletions(-) diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp index 4e27de45c9..ccaed14cea 100644 --- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp @@ -49,10 +49,19 @@ ECL_PitchController::ECL_PitchController() : _last_run(0), + _tc(0.1f), + _k_p(0.0f), + _k_i(0.0f), + _k_d(0.0f), + _integrator_max(0.0f), + _max_rate_pos(0.0f), + _max_rate_neg(0.0f), + _roll_ff(0.0f), _last_output(0.0f), _integrator(0.0f), _rate_error(0.0f), - _rate_setpoint(0.0f) + _rate_setpoint(0.0f), + _bodyrate_setpoint(0.0f) { } diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp index 0772f88bc7..4b0bfc6c42 100644 --- a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp @@ -50,13 +50,17 @@ ECL_RollController::ECL_RollController() : _last_run(0), _tc(0.1f), + _k_p(0.0f), + _k_i(0.0f), + _k_d(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) { - } float ECL_RollController::control_attitude(float roll_setpoint, float roll) diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp index 0de0eb439f..013bc191ac 100644 --- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp @@ -48,16 +48,19 @@ ECL_YawController::ECL_YawController() : _last_run(0), - _tc(0.1f), + _k_p(0.0f), + _k_i(0.0f), + _k_d(0.0f), + _integrator_max(0.0f), + _max_rate(0.0f), + _roll_ff(0.0f), _last_output(0.0f), _integrator(0.0f), _rate_error(0.0f), _rate_setpoint(0.0f), _bodyrate_setpoint(0.0f), - _coordinated(1.0f) - + _coordinated(0.0f) { - } float ECL_YawController::control_attitude(float roll, float pitch, diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.h b/src/lib/ecl/attitude_fw/ecl_yaw_controller.h index 5c00fa8731..154471cafb 100644 --- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.h +++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.h @@ -104,8 +104,6 @@ public: private: uint64_t _last_run; - - float _tc; float _k_p; float _k_i; float _k_d; From 014e856c1f789b6c27d886bb55617aa7d235f4f6 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 4 Nov 2013 13:14:25 +0100 Subject: [PATCH 080/447] fw: make att controller more robust against invalid (nan) setpoints --- .../ecl/attitude_fw/ecl_yaw_controller.cpp | 5 ++ .../fw_att_control/fw_att_control_main.cpp | 67 ++++++++++++------- 2 files changed, 48 insertions(+), 24 deletions(-) diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp index 013bc191ac..04293efd6b 100644 --- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp @@ -90,6 +90,11 @@ float ECL_YawController::control_attitude(float roll, float pitch, // counter++; + if(!isfinite(_rate_setpoint)){ + warnx("yaw rate sepoint not finite"); + _rate_setpoint = 0.0f; + } + return _rate_setpoint; } diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index 78952cb8de..ffad691350 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -619,7 +619,8 @@ FixedwingAttitudeControl::task_main() float airspeed_scaling = _parameters.airspeed_trim / airspeed; //warnx("aspd scale: %6.2f act scale: %6.2f", airspeed_scaling, actuator_scaling); - float roll_sp, pitch_sp; + float roll_sp = 0.0f; + float pitch_sp = 0.0f; float throttle_sp = 0.0f; if (_vcontrol_mode.flag_control_velocity_enabled || _vcontrol_mode.flag_control_position_enabled) { @@ -643,6 +644,7 @@ FixedwingAttitudeControl::task_main() */ roll_sp = _manual.roll * 0.75f; pitch_sp = _manual.pitch * 0.75f; + warnx("copy(2) _att_sp.roll_body %.4f", _att_sp.roll_body); throttle_sp = _manual.throttle; _actuators.control[4] = _manual.flaps; @@ -681,33 +683,50 @@ FixedwingAttitudeControl::task_main() } /* Run attitude controllers */ - _roll_ctrl.control_attitude(roll_sp, _att.roll); - _pitch_ctrl.control_attitude(pitch_sp, _att.roll, _att.pitch, airspeed); - _yaw_ctrl.control_attitude(_att.roll, _att.pitch, - speed_body_u,speed_body_w, - _roll_ctrl.get_desired_rate(), _pitch_ctrl.get_desired_rate()); //runs last, because is depending on output of roll and pitch attitude + if (isfinite(roll_sp) && isfinite(pitch_sp)) { + _roll_ctrl.control_attitude(roll_sp, _att.roll); + _pitch_ctrl.control_attitude(pitch_sp, _att.roll, _att.pitch, airspeed); + _yaw_ctrl.control_attitude(_att.roll, _att.pitch, + speed_body_u,speed_body_w, + _roll_ctrl.get_desired_rate(), _pitch_ctrl.get_desired_rate()); //runs last, because is depending on output of roll and pitch attitude - /* Run attitude RATE controllers which need the desired attitudes from above */ - float roll_u = _roll_ctrl.control_bodyrate(_att.pitch, - _att.rollspeed, _att.yawspeed, - _yaw_ctrl.get_desired_rate(), - _parameters.airspeed_min, _parameters.airspeed_max, airspeed, airspeed_scaling, lock_integrator); - _actuators.control[0] = (isfinite(roll_u)) ? roll_u : 0.0f; + /* Run attitude RATE controllers which need the desired attitudes from above */ + float roll_u = _roll_ctrl.control_bodyrate(_att.pitch, + _att.rollspeed, _att.yawspeed, + _yaw_ctrl.get_desired_rate(), + _parameters.airspeed_min, _parameters.airspeed_max, airspeed, airspeed_scaling, lock_integrator); + _actuators.control[0] = (isfinite(roll_u)) ? roll_u : 0.0f; + if (!isfinite(roll_u)) { + warnx("roll_u %.4f", roll_u); + } - float pitch_u = _pitch_ctrl.control_bodyrate(_att.roll, _att.pitch, - _att.pitchspeed, _att.yawspeed, - _yaw_ctrl.get_desired_rate(), - _parameters.airspeed_min, _parameters.airspeed_max, airspeed, airspeed_scaling, lock_integrator); - _actuators.control[1] = (isfinite(pitch_u)) ? pitch_u : 0.0f; + float pitch_u = _pitch_ctrl.control_bodyrate(_att.roll, _att.pitch, + _att.pitchspeed, _att.yawspeed, + _yaw_ctrl.get_desired_rate(), + _parameters.airspeed_min, _parameters.airspeed_max, airspeed, airspeed_scaling, lock_integrator); + _actuators.control[1] = (isfinite(pitch_u)) ? pitch_u : 0.0f; + if (!isfinite(pitch_u)) { + warnx("pitch_u %.4f, _yaw_ctrl.get_desired_rate() %.4f, airspeed %.4f, airspeed_scaling %.4f, roll_sp %.4f, pitch_sp %.4f, _roll_ctrl.get_desired_rate() %.4f, _pitch_ctrl.get_desired_rate() %.4f att_sp.roll_body %.4f", + pitch_u, _yaw_ctrl.get_desired_rate(), airspeed, airspeed_scaling, roll_sp, pitch_sp, _roll_ctrl.get_desired_rate(), _pitch_ctrl.get_desired_rate(), _att_sp.roll_body); + } - float yaw_u = _yaw_ctrl.control_bodyrate( _att.roll, _att.pitch, - _att.pitchspeed, _att.yawspeed, - _pitch_ctrl.get_desired_rate(), - _parameters.airspeed_min, _parameters.airspeed_max, airspeed, airspeed_scaling, lock_integrator); - _actuators.control[2] = (isfinite(yaw_u)) ? yaw_u : 0.0f; + float yaw_u = _yaw_ctrl.control_bodyrate( _att.roll, _att.pitch, + _att.pitchspeed, _att.yawspeed, + _pitch_ctrl.get_desired_rate(), + _parameters.airspeed_min, _parameters.airspeed_max, airspeed, airspeed_scaling, lock_integrator); + _actuators.control[2] = (isfinite(yaw_u)) ? yaw_u : 0.0f; + if (!isfinite(yaw_u)) { + warnx("yaw_u %.4f", yaw_u); + } - /* throttle passed through */ - _actuators.control[3] = (isfinite(throttle_sp)) ? throttle_sp : 0.0f; + /* throttle passed through */ + _actuators.control[3] = (isfinite(throttle_sp)) ? throttle_sp : 0.0f; + if (!isfinite(throttle_sp)) { + warnx("throttle_sp %.4f", throttle_sp); + } + } else { + warnx("Non-finite setpoint roll_sp: %.4f, pitch_sp %.4f", roll_sp, pitch_sp); + } // warnx("aspd: %s: %6.2f, aspd scaling: %6.2f, controls: %5.2f %5.2f %5.2f %5.2f", (_airspeed_valid) ? "valid" : "unknown", // airspeed, airspeed_scaling, _actuators.control[0], _actuators.control[1], From 303ceab6ed7aca8cedd2d7703e6766c0477d7ab6 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 4 Nov 2013 13:14:25 +0100 Subject: [PATCH 081/447] fw: make att controller more robust against invalid (nan) setpoints --- .../ecl/attitude_fw/ecl_yaw_controller.cpp | 5 ++ .../fw_att_control/fw_att_control_main.cpp | 67 ++++++++++++------- 2 files changed, 48 insertions(+), 24 deletions(-) diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp index 013bc191ac..04293efd6b 100644 --- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp @@ -90,6 +90,11 @@ float ECL_YawController::control_attitude(float roll, float pitch, // counter++; + if(!isfinite(_rate_setpoint)){ + warnx("yaw rate sepoint not finite"); + _rate_setpoint = 0.0f; + } + return _rate_setpoint; } diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index 78952cb8de..ffad691350 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -619,7 +619,8 @@ FixedwingAttitudeControl::task_main() float airspeed_scaling = _parameters.airspeed_trim / airspeed; //warnx("aspd scale: %6.2f act scale: %6.2f", airspeed_scaling, actuator_scaling); - float roll_sp, pitch_sp; + float roll_sp = 0.0f; + float pitch_sp = 0.0f; float throttle_sp = 0.0f; if (_vcontrol_mode.flag_control_velocity_enabled || _vcontrol_mode.flag_control_position_enabled) { @@ -643,6 +644,7 @@ FixedwingAttitudeControl::task_main() */ roll_sp = _manual.roll * 0.75f; pitch_sp = _manual.pitch * 0.75f; + warnx("copy(2) _att_sp.roll_body %.4f", _att_sp.roll_body); throttle_sp = _manual.throttle; _actuators.control[4] = _manual.flaps; @@ -681,33 +683,50 @@ FixedwingAttitudeControl::task_main() } /* Run attitude controllers */ - _roll_ctrl.control_attitude(roll_sp, _att.roll); - _pitch_ctrl.control_attitude(pitch_sp, _att.roll, _att.pitch, airspeed); - _yaw_ctrl.control_attitude(_att.roll, _att.pitch, - speed_body_u,speed_body_w, - _roll_ctrl.get_desired_rate(), _pitch_ctrl.get_desired_rate()); //runs last, because is depending on output of roll and pitch attitude + if (isfinite(roll_sp) && isfinite(pitch_sp)) { + _roll_ctrl.control_attitude(roll_sp, _att.roll); + _pitch_ctrl.control_attitude(pitch_sp, _att.roll, _att.pitch, airspeed); + _yaw_ctrl.control_attitude(_att.roll, _att.pitch, + speed_body_u,speed_body_w, + _roll_ctrl.get_desired_rate(), _pitch_ctrl.get_desired_rate()); //runs last, because is depending on output of roll and pitch attitude - /* Run attitude RATE controllers which need the desired attitudes from above */ - float roll_u = _roll_ctrl.control_bodyrate(_att.pitch, - _att.rollspeed, _att.yawspeed, - _yaw_ctrl.get_desired_rate(), - _parameters.airspeed_min, _parameters.airspeed_max, airspeed, airspeed_scaling, lock_integrator); - _actuators.control[0] = (isfinite(roll_u)) ? roll_u : 0.0f; + /* Run attitude RATE controllers which need the desired attitudes from above */ + float roll_u = _roll_ctrl.control_bodyrate(_att.pitch, + _att.rollspeed, _att.yawspeed, + _yaw_ctrl.get_desired_rate(), + _parameters.airspeed_min, _parameters.airspeed_max, airspeed, airspeed_scaling, lock_integrator); + _actuators.control[0] = (isfinite(roll_u)) ? roll_u : 0.0f; + if (!isfinite(roll_u)) { + warnx("roll_u %.4f", roll_u); + } - float pitch_u = _pitch_ctrl.control_bodyrate(_att.roll, _att.pitch, - _att.pitchspeed, _att.yawspeed, - _yaw_ctrl.get_desired_rate(), - _parameters.airspeed_min, _parameters.airspeed_max, airspeed, airspeed_scaling, lock_integrator); - _actuators.control[1] = (isfinite(pitch_u)) ? pitch_u : 0.0f; + float pitch_u = _pitch_ctrl.control_bodyrate(_att.roll, _att.pitch, + _att.pitchspeed, _att.yawspeed, + _yaw_ctrl.get_desired_rate(), + _parameters.airspeed_min, _parameters.airspeed_max, airspeed, airspeed_scaling, lock_integrator); + _actuators.control[1] = (isfinite(pitch_u)) ? pitch_u : 0.0f; + if (!isfinite(pitch_u)) { + warnx("pitch_u %.4f, _yaw_ctrl.get_desired_rate() %.4f, airspeed %.4f, airspeed_scaling %.4f, roll_sp %.4f, pitch_sp %.4f, _roll_ctrl.get_desired_rate() %.4f, _pitch_ctrl.get_desired_rate() %.4f att_sp.roll_body %.4f", + pitch_u, _yaw_ctrl.get_desired_rate(), airspeed, airspeed_scaling, roll_sp, pitch_sp, _roll_ctrl.get_desired_rate(), _pitch_ctrl.get_desired_rate(), _att_sp.roll_body); + } - float yaw_u = _yaw_ctrl.control_bodyrate( _att.roll, _att.pitch, - _att.pitchspeed, _att.yawspeed, - _pitch_ctrl.get_desired_rate(), - _parameters.airspeed_min, _parameters.airspeed_max, airspeed, airspeed_scaling, lock_integrator); - _actuators.control[2] = (isfinite(yaw_u)) ? yaw_u : 0.0f; + float yaw_u = _yaw_ctrl.control_bodyrate( _att.roll, _att.pitch, + _att.pitchspeed, _att.yawspeed, + _pitch_ctrl.get_desired_rate(), + _parameters.airspeed_min, _parameters.airspeed_max, airspeed, airspeed_scaling, lock_integrator); + _actuators.control[2] = (isfinite(yaw_u)) ? yaw_u : 0.0f; + if (!isfinite(yaw_u)) { + warnx("yaw_u %.4f", yaw_u); + } - /* throttle passed through */ - _actuators.control[3] = (isfinite(throttle_sp)) ? throttle_sp : 0.0f; + /* throttle passed through */ + _actuators.control[3] = (isfinite(throttle_sp)) ? throttle_sp : 0.0f; + if (!isfinite(throttle_sp)) { + warnx("throttle_sp %.4f", throttle_sp); + } + } else { + warnx("Non-finite setpoint roll_sp: %.4f, pitch_sp %.4f", roll_sp, pitch_sp); + } // warnx("aspd: %s: %6.2f, aspd scaling: %6.2f, controls: %5.2f %5.2f %5.2f %5.2f", (_airspeed_valid) ? "valid" : "unknown", // airspeed, airspeed_scaling, _actuators.control[0], _actuators.control[1], From 22dbc03c011ea86f3a500949109c85676583db32 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 4 Nov 2013 13:18:20 +0100 Subject: [PATCH 082/447] remove unnecessary printf --- src/modules/fw_att_control/fw_att_control_main.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index ffad691350..c88b29056c 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -644,7 +644,6 @@ FixedwingAttitudeControl::task_main() */ roll_sp = _manual.roll * 0.75f; pitch_sp = _manual.pitch * 0.75f; - warnx("copy(2) _att_sp.roll_body %.4f", _att_sp.roll_body); throttle_sp = _manual.throttle; _actuators.control[4] = _manual.flaps; From c4b994d4ca67b21ce77cb8bc87e1096305ad4fad Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 4 Nov 2013 13:18:20 +0100 Subject: [PATCH 083/447] remove unnecessary printf --- src/modules/fw_att_control/fw_att_control_main.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index ffad691350..c88b29056c 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -644,7 +644,6 @@ FixedwingAttitudeControl::task_main() */ roll_sp = _manual.roll * 0.75f; pitch_sp = _manual.pitch * 0.75f; - warnx("copy(2) _att_sp.roll_body %.4f", _att_sp.roll_body); throttle_sp = _manual.throttle; _actuators.control[4] = _manual.flaps; From dbee6763672f431b386f59e3e9113cc036547347 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 5 Nov 2013 16:49:54 +0100 Subject: [PATCH 084/447] experiment with landing slope hight for more exact landing --- .../fw_pos_control_l1_main.cpp | 17 +++++++++-------- 1 file changed, 9 insertions(+), 8 deletions(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index a51eff5124..3aa3acb1a7 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -292,7 +292,7 @@ private: /** * Get Altitude on the landing glide slope */ - float getLandingSlopeAbsoluteAltitude(float wp_distance, float wp_altitude, float landing_slope_angle_rad); + float getLandingSlopeAbsoluteAltitude(float wp_distance, float wp_altitude, float landing_slope_angle_rad, float flare_relative_alt); /** * Control position. @@ -648,9 +648,9 @@ FixedwingPositionControl::calculate_gndspeed_undershoot() } } -float FixedwingPositionControl::getLandingSlopeAbsoluteAltitude(float wp_distance, float wp_altitude, float landing_slope_angle_rad) +float FixedwingPositionControl::getLandingSlopeAbsoluteAltitude(float wp_distance, float wp_altitude, float landing_slope_angle_rad, float flare_relative_alt) { - return wp_distance * tanf(landing_slope_angle_rad) + wp_altitude; + return wp_distance * tanf(landing_slope_angle_rad) + wp_altitude + flare_relative_alt; //flare_relative_alt is negative } bool @@ -807,16 +807,17 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio float landing_slope_angle_rad = math::radians(_parameters.land_slope_angle); float landingslope_length = _parameters.land_slope_length; + float flare_relative_alt = -10.0f; //negative!, be generous here as we currently have to rely on the user input for the waypoint //xxx: find better definition, e.g. as a function of the horizontal distance ("abflachbogen") float L_wp_distance = cosf(landing_slope_angle_rad) * landingslope_length; - float L_altitude = getLandingSlopeAbsoluteAltitude(L_wp_distance, _global_triplet.current.altitude, landing_slope_angle_rad); - float landing_slope_alt_desired = getLandingSlopeAbsoluteAltitude(wp_distance, _global_triplet.current.altitude, landing_slope_angle_rad); + float L_altitude = getLandingSlopeAbsoluteAltitude(L_wp_distance, _global_triplet.current.altitude, landing_slope_angle_rad, flare_relative_alt); + float landing_slope_alt_desired = getLandingSlopeAbsoluteAltitude(wp_distance, _global_triplet.current.altitude, landing_slope_angle_rad, flare_relative_alt); - if (altitude_error > -10.0f || land_noreturn) { //be generous here as we currently have to rely on the user input for the waypoint, checking for land_noreturn to avoid unwanted climb out + if (altitude_error > flare_relative_alt || land_noreturn) { //checking for land_noreturn to avoid unwanted climb out /* land with minimal speed */ - /* force TECS to only control speed with pitch, altitude is only implicitely controlled now */ - _tecs.set_speed_weight(2.0f); +// /* force TECS to only control speed with pitch, altitude is only implicitely controlled now */ +// _tecs.set_speed_weight(2.0f); /* kill the throttle if param requests it */ throttle_max = math::min(throttle_max, _parameters.throttle_land_max); From a34252d18f08e37153a777a6e2d79272b789b0c3 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 5 Nov 2013 16:49:54 +0100 Subject: [PATCH 085/447] experiment with landing slope hight for more exact landing --- .../fw_pos_control_l1_main.cpp | 17 +++++++++-------- 1 file changed, 9 insertions(+), 8 deletions(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 5363801298..45493b95f8 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -288,7 +288,7 @@ private: /** * Get Altitude on the landing glide slope */ - float getLandingSlopeAbsoluteAltitude(float wp_distance, float wp_altitude, float landing_slope_angle_rad); + float getLandingSlopeAbsoluteAltitude(float wp_distance, float wp_altitude, float landing_slope_angle_rad, float flare_relative_alt); /** * Control position. @@ -642,9 +642,9 @@ FixedwingPositionControl::calculate_gndspeed_undershoot() } } -float FixedwingPositionControl::getLandingSlopeAbsoluteAltitude(float wp_distance, float wp_altitude, float landing_slope_angle_rad) +float FixedwingPositionControl::getLandingSlopeAbsoluteAltitude(float wp_distance, float wp_altitude, float landing_slope_angle_rad, float flare_relative_alt) { - return wp_distance * tanf(landing_slope_angle_rad) + wp_altitude; + return wp_distance * tanf(landing_slope_angle_rad) + wp_altitude + flare_relative_alt; //flare_relative_alt is negative } bool @@ -801,16 +801,17 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio float landing_slope_angle_rad = math::radians(_parameters.land_slope_angle); float landingslope_length = _parameters.land_slope_length; + float flare_relative_alt = -10.0f; //negative!, be generous here as we currently have to rely on the user input for the waypoint //xxx: find better definition, e.g. as a function of the horizontal distance ("abflachbogen") float L_wp_distance = cosf(landing_slope_angle_rad) * landingslope_length; - float L_altitude = getLandingSlopeAbsoluteAltitude(L_wp_distance, _global_triplet.current.altitude, landing_slope_angle_rad); - float landing_slope_alt_desired = getLandingSlopeAbsoluteAltitude(wp_distance, _global_triplet.current.altitude, landing_slope_angle_rad); + float L_altitude = getLandingSlopeAbsoluteAltitude(L_wp_distance, _global_triplet.current.altitude, landing_slope_angle_rad, flare_relative_alt); + float landing_slope_alt_desired = getLandingSlopeAbsoluteAltitude(wp_distance, _global_triplet.current.altitude, landing_slope_angle_rad, flare_relative_alt); - if (altitude_error > -10.0f || land_noreturn) { //be generous here as we currently have to rely on the user input for the waypoint, checking for land_noreturn to avoid unwanted climb out + if (altitude_error > flare_relative_alt || land_noreturn) { //checking for land_noreturn to avoid unwanted climb out /* land with minimal speed */ - /* force TECS to only control speed with pitch, altitude is only implicitely controlled now */ - _tecs.set_speed_weight(2.0f); +// /* force TECS to only control speed with pitch, altitude is only implicitely controlled now */ +// _tecs.set_speed_weight(2.0f); /* kill the throttle if param requests it */ throttle_max = math::min(throttle_max, _parameters.throttle_land_max); From bdbe64026b0663489f31841fbc894befd839f732 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 6 Nov 2013 13:10:14 +0100 Subject: [PATCH 086/447] introduce experimental flare trajectory --- .../fw_pos_control_l1_main.cpp | 92 ++++++++++++------- .../fw_pos_control_l1_params.c | 2 +- 2 files changed, 61 insertions(+), 33 deletions(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 3aa3acb1a7..9de6f83660 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -160,7 +160,10 @@ private: /* land states */ /* not in non-abort mode for landing yet */ - bool land_noreturn; + bool land_noreturn_horizontal; + bool land_noreturn_vertical; + bool land_stayonground; + float flare_curve_alt_last; /* heading hold */ float target_bearing; @@ -292,7 +295,7 @@ private: /** * Get Altitude on the landing glide slope */ - float getLandingSlopeAbsoluteAltitude(float wp_distance, float wp_altitude, float landing_slope_angle_rad, float flare_relative_alt); + float getLandingSlopeAbsoluteAltitude(float wp_distance, float wp_altitude, float landing_slope_angle_rad, float horizontal_displacement); /** * Control position. @@ -353,7 +356,10 @@ FixedwingPositionControl::FixedwingPositionControl() : _airspeed_valid(false), _groundspeed_undershoot(0.0f), _global_pos_valid(false), - land_noreturn(false) + land_noreturn_horizontal(false), + land_noreturn_vertical(false), + land_stayonground(false), + flare_curve_alt_last(0.0f) { _nav_capabilities.turn_distance = 0.0f; @@ -374,7 +380,7 @@ FixedwingPositionControl::FixedwingPositionControl() : _parameter_handles.throttle_land_max = param_find("FW_THR_LND_MAX"); _parameter_handles.land_slope_angle = param_find("FW_LND_ANG"); - _parameter_handles.land_slope_length = param_find("FW_LND_SLL"); + _parameter_handles.land_slope_length = param_find("FW_LND_SLLR"); _parameter_handles.time_const = param_find("FW_T_TIME_CONST"); _parameter_handles.min_sink_rate = param_find("FW_T_SINK_MIN"); @@ -648,9 +654,9 @@ FixedwingPositionControl::calculate_gndspeed_undershoot() } } -float FixedwingPositionControl::getLandingSlopeAbsoluteAltitude(float wp_distance, float wp_altitude, float landing_slope_angle_rad, float flare_relative_alt) +float FixedwingPositionControl::getLandingSlopeAbsoluteAltitude(float wp_distance, float wp_altitude, float landing_slope_angle_rad, float horizontal_displacement) { - return wp_distance * tanf(landing_slope_angle_rad) + wp_altitude + flare_relative_alt; //flare_relative_alt is negative + return (wp_distance - horizontal_displacement) * tanf(landing_slope_angle_rad) + wp_altitude; //flare_relative_alt is negative } bool @@ -762,7 +768,8 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio /* switch to heading hold for the last meters, continue heading hold after */ float wp_distance = get_distance_to_next_waypoint(current_position.getX(), current_position.getY(), curr_wp.getX(), curr_wp.getY()); //warnx("wp dist: %d, alt err: %d, noret: %s", (int)wp_distance, (int)altitude_error, (land_noreturn) ? "YES" : "NO"); - if (wp_distance < 15.0f || land_noreturn) { + const float heading_hold_distance = 15.0f; + if (wp_distance < heading_hold_distance || land_noreturn_horizontal) { /* heading hold, along the line connecting this and the last waypoint */ @@ -771,15 +778,15 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio // target_bearing = get_bearing_to_next_waypoint(prev_wp.getX(), prev_wp.getY(), next_wp.getX(), next_wp.getY()); // } else { - if (!land_noreturn) //set target_bearing in first occurrence + if (!land_noreturn_horizontal) //set target_bearing in first occurrence target_bearing = _att.yaw; //} - warnx("NORET: %d, target_bearing: %d, yaw: %d", (int)land_noreturn, (int)math::degrees(target_bearing), (int)math::degrees(_att.yaw)); +// warnx("NORET: %d, target_bearing: %d, yaw: %d", (int)land_noreturn_horizontal, (int)math::degrees(target_bearing), (int)math::degrees(_att.yaw)); _l1_control.navigate_heading(target_bearing, _att.yaw, ground_speed); - land_noreturn = true; + land_noreturn_horizontal = true; } else { @@ -791,6 +798,7 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio /* Vertical landing control */ + //xxx: using the tecs altitude controller for slope control for now // /* do not go down too early */ // if (wp_distance > 50.0f) { @@ -806,13 +814,19 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio float airspeed_approach = 1.3f * _parameters.airspeed_min; float landing_slope_angle_rad = math::radians(_parameters.land_slope_angle); - float landingslope_length = _parameters.land_slope_length; - float flare_relative_alt = -10.0f; //negative!, be generous here as we currently have to rely on the user input for the waypoint //xxx: find better definition, e.g. as a function of the horizontal distance ("abflachbogen") - float L_wp_distance = cosf(landing_slope_angle_rad) * landingslope_length; - float L_altitude = getLandingSlopeAbsoluteAltitude(L_wp_distance, _global_triplet.current.altitude, landing_slope_angle_rad, flare_relative_alt); - float landing_slope_alt_desired = getLandingSlopeAbsoluteAltitude(wp_distance, _global_triplet.current.altitude, landing_slope_angle_rad, flare_relative_alt); + float flare_relative_alt = 15.0f; + float motor_lim_relative_alt = 10.0f;//be generous here as we currently have to rely on the user input for the waypoint + float L_wp_distance = get_distance_to_next_waypoint(prev_wp.getX(), prev_wp.getY(), curr_wp.getX(), curr_wp.getY()) * _parameters.land_slope_length; + float H1 = 10.0f; + float H0 = flare_relative_alt + H1; + float d1 = flare_relative_alt/tanf(landing_slope_angle_rad); + float flare_constant = (H0 * d1)/flare_relative_alt;//-flare_length/(logf(H1/H0)); + float flare_length = - logf(H1/H0) * flare_constant;//d1+20.0f;//-logf(0.01f/flare_relative_alt); + float horizontal_slope_displacement = (flare_length - d1); + float L_altitude = getLandingSlopeAbsoluteAltitude(L_wp_distance, _global_triplet.current.altitude, landing_slope_angle_rad, horizontal_slope_displacement); + float landing_slope_alt_desired = getLandingSlopeAbsoluteAltitude(wp_distance, _global_triplet.current.altitude, landing_slope_angle_rad, horizontal_slope_displacement); - if (altitude_error > flare_relative_alt || land_noreturn) { //checking for land_noreturn to avoid unwanted climb out + if (_global_pos.alt < _global_triplet.current.altitude + flare_relative_alt || land_noreturn_vertical) { //checking for land_noreturn to avoid unwanted climb out /* land with minimal speed */ @@ -820,18 +834,34 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio // _tecs.set_speed_weight(2.0f); /* kill the throttle if param requests it */ - throttle_max = math::min(throttle_max, _parameters.throttle_land_max); + throttle_max = _parameters.throttle_max; - _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(airspeed_land), + if (_global_pos.alt < _global_triplet.current.altitude + motor_lim_relative_alt) { + throttle_max = math::min(throttle_max, _parameters.throttle_land_max); + } + + float flare_curve_alt = _global_triplet.current.altitude + H0 * expf(-math::max(0.0f, flare_length - wp_distance)/flare_constant) - H1; + + /* avoid climbout */ + if (flare_curve_alt_last < flare_curve_alt && land_noreturn_vertical || land_stayonground) + { + flare_curve_alt = global_triplet.current.altitude; + land_stayonground = true; + } + + _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, flare_curve_alt, calculate_target_airspeed(airspeed_land), _airspeed.indicated_airspeed_m_s, eas2tas, false, flare_angle_rad, 0.0f, throttle_max, throttle_land, - math::radians(flare_angle_rad), math::radians(15.0f)); + flare_angle_rad, math::radians(15.0f)); /* limit roll motion to prevent wings from touching the ground first */ _att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-10.0f), math::radians(10.0f)); - warnx("Landing: land with minimal speed"); + land_noreturn_vertical = true; + warnx("Landing: flare, _global_pos.alt %.1f, flare_curve_alt %.1f, flare_curve_alt_last %.1f, flare_length %.1f, wp_distance %.1f", _global_pos.alt, flare_curve_alt, flare_curve_alt_last, flare_length, wp_distance); + + flare_curve_alt_last = flare_curve_alt; } else if (wp_distance < L_wp_distance) { @@ -841,26 +871,22 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio false, flare_angle_rad, _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max)); - warnx("Landing: after L, stay on landing slope, alt_desired: %.4f (wp_distance: %.4f)", landing_slope_alt_desired, wp_distance); + warnx("Landing: stay on slope, alt_desired: %.1f (wp_distance: %.1f), calculate_target_airspeed(airspeed_land) %.1f, horizontal_slope_displacement %.1f, d1 %.1f, flare_length %.1f", landing_slope_alt_desired, wp_distance, calculate_target_airspeed(airspeed_land), horizontal_slope_displacement, d1, flare_length); } else { /* intersect glide slope: * if current position is higher or within 10m of slope follow the glide slope - * if current position is below slope -10m and above altitude at L (see documentation) continue horizontally - * if current position is below altitude at L, climb to altitude of L */ + * if current position is below slope -10m continue on previous wp altitude until the intersection with the slope + * */ float altitude_desired = _global_pos.alt; if (_global_pos.alt > landing_slope_alt_desired - 10.0f) { /* stay on slope */ altitude_desired = landing_slope_alt_desired; - warnx("Landing: before L, stay on landing slope, alt_desired: %.4f (wp_distance: %.4f)", altitude_desired, wp_distance); - } else if (_global_pos.alt < landing_slope_alt_desired - 10.0f && _global_pos.alt > L_altitude) { + warnx("Landing: before L, stay on landing slope, alt_desired: %.1f (wp_distance: %.1f, L_wp_distance %.1f), calculate_target_airspeed(airspeed_land) %.1f, horizontal_slope_displacement %.1f", altitude_desired, wp_distance, L_wp_distance, calculate_target_airspeed(airspeed_land), horizontal_slope_displacement); + } else if (_global_pos.alt < landing_slope_alt_desired - 10.0f) { /* continue horizontally */ - altitude_desired = _global_pos.alt; //xxx: dangerous, but we have the altitude < L_altitude protection - warnx("Landing: before L,continue horizontal at: %.4f, (landing_slope_alt_desired %.4f, wp_distance: %.4f, L_altitude: %.4f)", altitude_desired, landing_slope_alt_desired, wp_distance, L_altitude); - } else { - /* climb to L_altitude */ - altitude_desired = L_altitude; - warnx("Landing: before L, below L, climb: %.4f (wp_distance: %.4f)", altitude_desired, wp_distance); + altitude_desired = _global_triplet.previous.altitude; //xxx: dangerous, but we have the altitude < L_altitude protection + warnx("Landing: before L,continue horizontal at last wp alt: %.4f, (landing_slope_alt_desired %.4f, wp_distance: %.4f, L_altitude: %.4f L_wp_distance: %.4f)", altitude_desired, landing_slope_alt_desired, wp_distance, L_altitude, L_wp_distance); } _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, altitude_desired, calculate_target_airspeed(airspeed_approach), @@ -957,7 +983,9 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio /* reset land state */ if (global_triplet.current.nav_cmd != NAV_CMD_LAND) { - land_noreturn = false; + land_noreturn_horizontal = false; + land_noreturn_vertical = false; + land_stayonground = false; } if (was_circle_mode && !_l1_control.circle_mode()) { 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 b905c66c3b..61d665b172 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 @@ -115,4 +115,4 @@ PARAM_DEFINE_FLOAT(FW_T_SINK_MAX, 5.0f); PARAM_DEFINE_FLOAT(FW_T_HRATE_P, 0.05f); PARAM_DEFINE_FLOAT(FW_LND_ANG, 10.0f); -PARAM_DEFINE_FLOAT(FW_LND_SLL, 64.0f); +PARAM_DEFINE_FLOAT(FW_LND_SLLR, 0.9f); From 7ad907e638103065051b2b857874588b41766ca8 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 6 Nov 2013 13:10:14 +0100 Subject: [PATCH 087/447] introduce experimental flare trajectory --- .../fw_pos_control_l1_main.cpp | 92 ++++++++++++------- .../fw_pos_control_l1_params.c | 2 +- 2 files changed, 61 insertions(+), 33 deletions(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 45493b95f8..413474a6e5 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -160,7 +160,10 @@ private: /* land states */ /* not in non-abort mode for landing yet */ - bool land_noreturn; + bool land_noreturn_horizontal; + bool land_noreturn_vertical; + bool land_stayonground; + float flare_curve_alt_last; /* heading hold */ float target_bearing; @@ -288,7 +291,7 @@ private: /** * Get Altitude on the landing glide slope */ - float getLandingSlopeAbsoluteAltitude(float wp_distance, float wp_altitude, float landing_slope_angle_rad, float flare_relative_alt); + float getLandingSlopeAbsoluteAltitude(float wp_distance, float wp_altitude, float landing_slope_angle_rad, float horizontal_displacement); /** * Control position. @@ -349,7 +352,10 @@ FixedwingPositionControl::FixedwingPositionControl() : _airspeed_valid(false), _groundspeed_undershoot(0.0f), _global_pos_valid(false), - land_noreturn(false) + land_noreturn_horizontal(false), + land_noreturn_vertical(false), + land_stayonground(false), + flare_curve_alt_last(0.0f) { _nav_capabilities.turn_distance = 0.0f; @@ -370,7 +376,7 @@ FixedwingPositionControl::FixedwingPositionControl() : _parameter_handles.throttle_land_max = param_find("FW_THR_LND_MAX"); _parameter_handles.land_slope_angle = param_find("FW_LND_ANG"); - _parameter_handles.land_slope_length = param_find("FW_LND_SLL"); + _parameter_handles.land_slope_length = param_find("FW_LND_SLLR"); _parameter_handles.time_const = param_find("FW_T_TIME_CONST"); _parameter_handles.min_sink_rate = param_find("FW_T_SINK_MIN"); @@ -642,9 +648,9 @@ FixedwingPositionControl::calculate_gndspeed_undershoot() } } -float FixedwingPositionControl::getLandingSlopeAbsoluteAltitude(float wp_distance, float wp_altitude, float landing_slope_angle_rad, float flare_relative_alt) +float FixedwingPositionControl::getLandingSlopeAbsoluteAltitude(float wp_distance, float wp_altitude, float landing_slope_angle_rad, float horizontal_displacement) { - return wp_distance * tanf(landing_slope_angle_rad) + wp_altitude + flare_relative_alt; //flare_relative_alt is negative + return (wp_distance - horizontal_displacement) * tanf(landing_slope_angle_rad) + wp_altitude; //flare_relative_alt is negative } bool @@ -756,7 +762,8 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio /* switch to heading hold for the last meters, continue heading hold after */ float wp_distance = get_distance_to_next_waypoint(current_position.getX(), current_position.getY(), curr_wp.getX(), curr_wp.getY()); //warnx("wp dist: %d, alt err: %d, noret: %s", (int)wp_distance, (int)altitude_error, (land_noreturn) ? "YES" : "NO"); - if (wp_distance < 15.0f || land_noreturn) { + const float heading_hold_distance = 15.0f; + if (wp_distance < heading_hold_distance || land_noreturn_horizontal) { /* heading hold, along the line connecting this and the last waypoint */ @@ -765,15 +772,15 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio // target_bearing = get_bearing_to_next_waypoint(prev_wp.getX(), prev_wp.getY(), next_wp.getX(), next_wp.getY()); // } else { - if (!land_noreturn) //set target_bearing in first occurrence + if (!land_noreturn_horizontal) //set target_bearing in first occurrence target_bearing = _att.yaw; //} - warnx("NORET: %d, target_bearing: %d, yaw: %d", (int)land_noreturn, (int)math::degrees(target_bearing), (int)math::degrees(_att.yaw)); +// warnx("NORET: %d, target_bearing: %d, yaw: %d", (int)land_noreturn_horizontal, (int)math::degrees(target_bearing), (int)math::degrees(_att.yaw)); _l1_control.navigate_heading(target_bearing, _att.yaw, ground_speed); - land_noreturn = true; + land_noreturn_horizontal = true; } else { @@ -785,6 +792,7 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio /* Vertical landing control */ + //xxx: using the tecs altitude controller for slope control for now // /* do not go down too early */ // if (wp_distance > 50.0f) { @@ -800,13 +808,19 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio float airspeed_approach = 1.3f * _parameters.airspeed_min; float landing_slope_angle_rad = math::radians(_parameters.land_slope_angle); - float landingslope_length = _parameters.land_slope_length; - float flare_relative_alt = -10.0f; //negative!, be generous here as we currently have to rely on the user input for the waypoint //xxx: find better definition, e.g. as a function of the horizontal distance ("abflachbogen") - float L_wp_distance = cosf(landing_slope_angle_rad) * landingslope_length; - float L_altitude = getLandingSlopeAbsoluteAltitude(L_wp_distance, _global_triplet.current.altitude, landing_slope_angle_rad, flare_relative_alt); - float landing_slope_alt_desired = getLandingSlopeAbsoluteAltitude(wp_distance, _global_triplet.current.altitude, landing_slope_angle_rad, flare_relative_alt); + float flare_relative_alt = 15.0f; + float motor_lim_relative_alt = 10.0f;//be generous here as we currently have to rely on the user input for the waypoint + float L_wp_distance = get_distance_to_next_waypoint(prev_wp.getX(), prev_wp.getY(), curr_wp.getX(), curr_wp.getY()) * _parameters.land_slope_length; + float H1 = 10.0f; + float H0 = flare_relative_alt + H1; + float d1 = flare_relative_alt/tanf(landing_slope_angle_rad); + float flare_constant = (H0 * d1)/flare_relative_alt;//-flare_length/(logf(H1/H0)); + float flare_length = - logf(H1/H0) * flare_constant;//d1+20.0f;//-logf(0.01f/flare_relative_alt); + float horizontal_slope_displacement = (flare_length - d1); + float L_altitude = getLandingSlopeAbsoluteAltitude(L_wp_distance, _global_triplet.current.altitude, landing_slope_angle_rad, horizontal_slope_displacement); + float landing_slope_alt_desired = getLandingSlopeAbsoluteAltitude(wp_distance, _global_triplet.current.altitude, landing_slope_angle_rad, horizontal_slope_displacement); - if (altitude_error > flare_relative_alt || land_noreturn) { //checking for land_noreturn to avoid unwanted climb out + if (_global_pos.alt < _global_triplet.current.altitude + flare_relative_alt || land_noreturn_vertical) { //checking for land_noreturn to avoid unwanted climb out /* land with minimal speed */ @@ -814,18 +828,34 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio // _tecs.set_speed_weight(2.0f); /* kill the throttle if param requests it */ - throttle_max = math::min(throttle_max, _parameters.throttle_land_max); + throttle_max = _parameters.throttle_max; - _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(airspeed_land), + if (_global_pos.alt < _global_triplet.current.altitude + motor_lim_relative_alt) { + throttle_max = math::min(throttle_max, _parameters.throttle_land_max); + } + + float flare_curve_alt = _global_triplet.current.altitude + H0 * expf(-math::max(0.0f, flare_length - wp_distance)/flare_constant) - H1; + + /* avoid climbout */ + if (flare_curve_alt_last < flare_curve_alt && land_noreturn_vertical || land_stayonground) + { + flare_curve_alt = global_triplet.current.altitude; + land_stayonground = true; + } + + _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, flare_curve_alt, calculate_target_airspeed(airspeed_land), _airspeed.indicated_airspeed_m_s, eas2tas, false, flare_angle_rad, 0.0f, throttle_max, throttle_land, - math::radians(flare_angle_rad), math::radians(15.0f)); + flare_angle_rad, math::radians(15.0f)); /* limit roll motion to prevent wings from touching the ground first */ _att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-10.0f), math::radians(10.0f)); - warnx("Landing: land with minimal speed"); + land_noreturn_vertical = true; + warnx("Landing: flare, _global_pos.alt %.1f, flare_curve_alt %.1f, flare_curve_alt_last %.1f, flare_length %.1f, wp_distance %.1f", _global_pos.alt, flare_curve_alt, flare_curve_alt_last, flare_length, wp_distance); + + flare_curve_alt_last = flare_curve_alt; } else if (wp_distance < L_wp_distance) { @@ -835,26 +865,22 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio false, flare_angle_rad, _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max)); - warnx("Landing: after L, stay on landing slope, alt_desired: %.4f (wp_distance: %.4f)", landing_slope_alt_desired, wp_distance); + warnx("Landing: stay on slope, alt_desired: %.1f (wp_distance: %.1f), calculate_target_airspeed(airspeed_land) %.1f, horizontal_slope_displacement %.1f, d1 %.1f, flare_length %.1f", landing_slope_alt_desired, wp_distance, calculate_target_airspeed(airspeed_land), horizontal_slope_displacement, d1, flare_length); } else { /* intersect glide slope: * if current position is higher or within 10m of slope follow the glide slope - * if current position is below slope -10m and above altitude at L (see documentation) continue horizontally - * if current position is below altitude at L, climb to altitude of L */ + * if current position is below slope -10m continue on previous wp altitude until the intersection with the slope + * */ float altitude_desired = _global_pos.alt; if (_global_pos.alt > landing_slope_alt_desired - 10.0f) { /* stay on slope */ altitude_desired = landing_slope_alt_desired; - warnx("Landing: before L, stay on landing slope, alt_desired: %.4f (wp_distance: %.4f)", altitude_desired, wp_distance); - } else if (_global_pos.alt < landing_slope_alt_desired - 10.0f && _global_pos.alt > L_altitude) { + warnx("Landing: before L, stay on landing slope, alt_desired: %.1f (wp_distance: %.1f, L_wp_distance %.1f), calculate_target_airspeed(airspeed_land) %.1f, horizontal_slope_displacement %.1f", altitude_desired, wp_distance, L_wp_distance, calculate_target_airspeed(airspeed_land), horizontal_slope_displacement); + } else if (_global_pos.alt < landing_slope_alt_desired - 10.0f) { /* continue horizontally */ - altitude_desired = _global_pos.alt; //xxx: dangerous, but we have the altitude < L_altitude protection - warnx("Landing: before L,continue horizontal at: %.4f, (landing_slope_alt_desired %.4f, wp_distance: %.4f, L_altitude: %.4f)", altitude_desired, landing_slope_alt_desired, wp_distance, L_altitude); - } else { - /* climb to L_altitude */ - altitude_desired = L_altitude; - warnx("Landing: before L, below L, climb: %.4f (wp_distance: %.4f)", altitude_desired, wp_distance); + altitude_desired = _global_triplet.previous.altitude; //xxx: dangerous, but we have the altitude < L_altitude protection + warnx("Landing: before L,continue horizontal at last wp alt: %.4f, (landing_slope_alt_desired %.4f, wp_distance: %.4f, L_altitude: %.4f L_wp_distance: %.4f)", altitude_desired, landing_slope_alt_desired, wp_distance, L_altitude, L_wp_distance); } _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, altitude_desired, calculate_target_airspeed(airspeed_approach), @@ -951,7 +977,9 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio /* reset land state */ if (global_triplet.current.nav_cmd != NAV_CMD_LAND) { - land_noreturn = false; + land_noreturn_horizontal = false; + land_noreturn_vertical = false; + land_stayonground = false; } if (was_circle_mode && !_l1_control.circle_mode()) { 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 a3140fe48f..4d7400ce66 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 @@ -113,4 +113,4 @@ PARAM_DEFINE_FLOAT(FW_T_PTCH_DAMP, 0.0f); PARAM_DEFINE_FLOAT(FW_T_SINK_MAX, 5.0f); PARAM_DEFINE_FLOAT(FW_LND_ANG, 10.0f); -PARAM_DEFINE_FLOAT(FW_LND_SLL, 64.0f); +PARAM_DEFINE_FLOAT(FW_LND_SLLR, 0.9f); From 56aa8c7e8d5c6c4ebd4afabfdda4493db1d9290e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 6 Nov 2013 14:43:29 +0100 Subject: [PATCH 088/447] Minor fixes for motor off case --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 413474a6e5..b4edb0b362 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -820,7 +820,7 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio float L_altitude = getLandingSlopeAbsoluteAltitude(L_wp_distance, _global_triplet.current.altitude, landing_slope_angle_rad, horizontal_slope_displacement); float landing_slope_alt_desired = getLandingSlopeAbsoluteAltitude(wp_distance, _global_triplet.current.altitude, landing_slope_angle_rad, horizontal_slope_displacement); - if (_global_pos.alt < _global_triplet.current.altitude + flare_relative_alt || land_noreturn_vertical) { //checking for land_noreturn to avoid unwanted climb out + if (((wp_distance < 2.0f * math::max(15.0f, flare_relative_alt)) && (_global_pos.alt < _global_triplet.current.altitude + flare_relative_alt)) || land_noreturn_vertical) { //checking for land_noreturn to avoid unwanted climb out /* land with minimal speed */ @@ -830,9 +830,9 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio /* kill the throttle if param requests it */ throttle_max = _parameters.throttle_max; - if (_global_pos.alt < _global_triplet.current.altitude + motor_lim_relative_alt) { + // if ((_global_pos.alt < _global_triplet.current.altitude + motor_lim_relative_alt)) { throttle_max = math::min(throttle_max, _parameters.throttle_land_max); - } + // } float flare_curve_alt = _global_triplet.current.altitude + H0 * expf(-math::max(0.0f, flare_length - wp_distance)/flare_constant) - H1; @@ -897,7 +897,7 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio _att_sp.yaw_body = _l1_control.nav_bearing(); /* apply minimum pitch and limit roll if target altitude is not within 10 meters */ - if (altitude_error > 10.0f) { + if (altitude_error > 15.0f) { /* enforce a minimum of 10 degrees pitch up on takeoff, or take parameter */ _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_min), From f2fdfd11b804151874eff52ded7c7fd12767baaa Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 6 Nov 2013 15:09:15 +0100 Subject: [PATCH 089/447] land motor lim independent of flare --- .../fw_pos_control_l1/fw_pos_control_l1_main.cpp | 12 ++++++++---- 1 file changed, 8 insertions(+), 4 deletions(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 94b3b75baf..9704eacb96 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -163,6 +163,7 @@ private: bool land_noreturn_horizontal; bool land_noreturn_vertical; bool land_stayonground; + bool land_motor_lim; float flare_curve_alt_last; /* heading hold */ float target_bearing; @@ -359,6 +360,7 @@ FixedwingPositionControl::FixedwingPositionControl() : land_noreturn_horizontal(false), land_noreturn_vertical(false), land_stayonground(false), + land_motor_lim(false), flare_curve_alt_last(0.0f) { _nav_capabilities.turn_distance = 0.0f; @@ -815,7 +817,7 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio float landing_slope_angle_rad = math::radians(_parameters.land_slope_angle); float flare_relative_alt = 15.0f; - float motor_lim_relative_alt = 10.0f;//be generous here as we currently have to rely on the user input for the waypoint + float motor_lim_horizontal_distance = 30.0f;//be generous here as we currently have to rely on the user input for the waypoint float L_wp_distance = get_distance_to_next_waypoint(prev_wp.getX(), prev_wp.getY(), curr_wp.getX(), curr_wp.getY()) * _parameters.land_slope_length; float H1 = 10.0f; float H0 = flare_relative_alt + H1; @@ -826,7 +828,7 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio float L_altitude = getLandingSlopeAbsoluteAltitude(L_wp_distance, _global_triplet.current.altitude, landing_slope_angle_rad, horizontal_slope_displacement); float landing_slope_alt_desired = getLandingSlopeAbsoluteAltitude(wp_distance, _global_triplet.current.altitude, landing_slope_angle_rad, horizontal_slope_displacement); - if (((wp_distance < 2.0f * math::max(15.0f, flare_relative_alt)) && (_global_pos.alt < _global_triplet.current.altitude + flare_relative_alt)) || land_noreturn_vertical) { //checking for land_noreturn to avoid unwanted climb out + if ( (_global_pos.alt < _global_triplet.current.altitude + flare_relative_alt) || land_noreturn_vertical) { //checking for land_noreturn to avoid unwanted climb out /* land with minimal speed */ @@ -836,9 +838,10 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio /* kill the throttle if param requests it */ throttle_max = _parameters.throttle_max; - // if ((_global_pos.alt < _global_triplet.current.altitude + motor_lim_relative_alt)) { + if (wp_distance < motor_lim_horizontal_distance || land_motor_lim) { throttle_max = math::min(throttle_max, _parameters.throttle_land_max); - // } + land_motor_lim = true; + } float flare_curve_alt = _global_triplet.current.altitude + H0 * expf(-math::max(0.0f, flare_length - wp_distance)/flare_constant) - H1; @@ -986,6 +989,7 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio land_noreturn_horizontal = false; land_noreturn_vertical = false; land_stayonground = false; + land_motor_lim = false; } if (was_circle_mode && !_l1_control.circle_mode()) { From 020964c78a7ec246cd437eeef57e0f57132874ea Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 6 Nov 2013 15:09:15 +0100 Subject: [PATCH 090/447] land motor lim independent of flare --- .../fw_pos_control_l1/fw_pos_control_l1_main.cpp | 12 ++++++++---- 1 file changed, 8 insertions(+), 4 deletions(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index b4edb0b362..c54a4d2d29 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -163,6 +163,7 @@ private: bool land_noreturn_horizontal; bool land_noreturn_vertical; bool land_stayonground; + bool land_motor_lim; float flare_curve_alt_last; /* heading hold */ float target_bearing; @@ -355,6 +356,7 @@ FixedwingPositionControl::FixedwingPositionControl() : land_noreturn_horizontal(false), land_noreturn_vertical(false), land_stayonground(false), + land_motor_lim(false), flare_curve_alt_last(0.0f) { _nav_capabilities.turn_distance = 0.0f; @@ -809,7 +811,7 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio float landing_slope_angle_rad = math::radians(_parameters.land_slope_angle); float flare_relative_alt = 15.0f; - float motor_lim_relative_alt = 10.0f;//be generous here as we currently have to rely on the user input for the waypoint + float motor_lim_horizontal_distance = 30.0f;//be generous here as we currently have to rely on the user input for the waypoint float L_wp_distance = get_distance_to_next_waypoint(prev_wp.getX(), prev_wp.getY(), curr_wp.getX(), curr_wp.getY()) * _parameters.land_slope_length; float H1 = 10.0f; float H0 = flare_relative_alt + H1; @@ -820,7 +822,7 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio float L_altitude = getLandingSlopeAbsoluteAltitude(L_wp_distance, _global_triplet.current.altitude, landing_slope_angle_rad, horizontal_slope_displacement); float landing_slope_alt_desired = getLandingSlopeAbsoluteAltitude(wp_distance, _global_triplet.current.altitude, landing_slope_angle_rad, horizontal_slope_displacement); - if (((wp_distance < 2.0f * math::max(15.0f, flare_relative_alt)) && (_global_pos.alt < _global_triplet.current.altitude + flare_relative_alt)) || land_noreturn_vertical) { //checking for land_noreturn to avoid unwanted climb out + if ( (_global_pos.alt < _global_triplet.current.altitude + flare_relative_alt) || land_noreturn_vertical) { //checking for land_noreturn to avoid unwanted climb out /* land with minimal speed */ @@ -830,9 +832,10 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio /* kill the throttle if param requests it */ throttle_max = _parameters.throttle_max; - // if ((_global_pos.alt < _global_triplet.current.altitude + motor_lim_relative_alt)) { + if (wp_distance < motor_lim_horizontal_distance || land_motor_lim) { throttle_max = math::min(throttle_max, _parameters.throttle_land_max); - // } + land_motor_lim = true; + } float flare_curve_alt = _global_triplet.current.altitude + H0 * expf(-math::max(0.0f, flare_length - wp_distance)/flare_constant) - H1; @@ -980,6 +983,7 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio land_noreturn_horizontal = false; land_noreturn_vertical = false; land_stayonground = false; + land_motor_lim = false; } if (was_circle_mode && !_l1_control.circle_mode()) { From 50405f9c7c2aac5518c366231d9d419ba38d5333 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 6 Nov 2013 16:17:49 +0100 Subject: [PATCH 091/447] land: fix logic before L to climb to L if last wp is below L --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 9704eacb96..0127160fb2 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -879,17 +879,17 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio /* intersect glide slope: * if current position is higher or within 10m of slope follow the glide slope - * if current position is below slope -10m continue on previous wp altitude until the intersection with the slope + * if current position is below slope -10m continue on maximum of previous wp altitude or L_altitude until the intersection with the slope * */ float altitude_desired = _global_pos.alt; if (_global_pos.alt > landing_slope_alt_desired - 10.0f) { /* stay on slope */ altitude_desired = landing_slope_alt_desired; warnx("Landing: before L, stay on landing slope, alt_desired: %.1f (wp_distance: %.1f, L_wp_distance %.1f), calculate_target_airspeed(airspeed_land) %.1f, horizontal_slope_displacement %.1f", altitude_desired, wp_distance, L_wp_distance, calculate_target_airspeed(airspeed_land), horizontal_slope_displacement); - } else if (_global_pos.alt < landing_slope_alt_desired - 10.0f) { + } else { /* continue horizontally */ - altitude_desired = _global_triplet.previous.altitude; //xxx: dangerous, but we have the altitude < L_altitude protection - warnx("Landing: before L,continue horizontal at last wp alt: %.4f, (landing_slope_alt_desired %.4f, wp_distance: %.4f, L_altitude: %.4f L_wp_distance: %.4f)", altitude_desired, landing_slope_alt_desired, wp_distance, L_altitude, L_wp_distance); + altitude_desired = math::max(_global_pos.alt, L_altitude); + warnx("Landing: before L,continue at: %.4f, (landing_slope_alt_desired %.4f, wp_distance: %.4f, L_altitude: %.4f L_wp_distance: %.4f)", altitude_desired, landing_slope_alt_desired, wp_distance, L_altitude, L_wp_distance); } _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, altitude_desired, calculate_target_airspeed(airspeed_approach), From f92e7cd8c7eb271b3672e3bb02fcc3a33b8e7808 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 6 Nov 2013 16:17:49 +0100 Subject: [PATCH 092/447] land: fix logic before L to climb to L if last wp is below L --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index c54a4d2d29..919990273e 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -873,17 +873,17 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio /* intersect glide slope: * if current position is higher or within 10m of slope follow the glide slope - * if current position is below slope -10m continue on previous wp altitude until the intersection with the slope + * if current position is below slope -10m continue on maximum of previous wp altitude or L_altitude until the intersection with the slope * */ float altitude_desired = _global_pos.alt; if (_global_pos.alt > landing_slope_alt_desired - 10.0f) { /* stay on slope */ altitude_desired = landing_slope_alt_desired; warnx("Landing: before L, stay on landing slope, alt_desired: %.1f (wp_distance: %.1f, L_wp_distance %.1f), calculate_target_airspeed(airspeed_land) %.1f, horizontal_slope_displacement %.1f", altitude_desired, wp_distance, L_wp_distance, calculate_target_airspeed(airspeed_land), horizontal_slope_displacement); - } else if (_global_pos.alt < landing_slope_alt_desired - 10.0f) { + } else { /* continue horizontally */ - altitude_desired = _global_triplet.previous.altitude; //xxx: dangerous, but we have the altitude < L_altitude protection - warnx("Landing: before L,continue horizontal at last wp alt: %.4f, (landing_slope_alt_desired %.4f, wp_distance: %.4f, L_altitude: %.4f L_wp_distance: %.4f)", altitude_desired, landing_slope_alt_desired, wp_distance, L_altitude, L_wp_distance); + altitude_desired = math::max(_global_pos.alt, L_altitude); + warnx("Landing: before L,continue at: %.4f, (landing_slope_alt_desired %.4f, wp_distance: %.4f, L_altitude: %.4f L_wp_distance: %.4f)", altitude_desired, landing_slope_alt_desired, wp_distance, L_altitude, L_wp_distance); } _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, altitude_desired, calculate_target_airspeed(airspeed_approach), From dd5de28ed92144d767c613ae1db76c0255cea554 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 6 Nov 2013 17:01:29 +0100 Subject: [PATCH 093/447] remove commander from hil startup scripts --- ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil | 5 ----- ROMFS/px4fmu_common/init.d/1001_rc_quad.hil | 5 ----- ROMFS/px4fmu_common/init.d/1002_rc_fw_state.hil | 5 ----- ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil | 5 ----- ROMFS/px4fmu_common/init.d/1004_rc_fw_Rascal110.hil | 5 ----- 5 files changed, 25 deletions(-) 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 5e4028bbb8..40a13b5d17 100644 --- a/ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil +++ b/ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil @@ -55,11 +55,6 @@ hil mode_pwm # param set MAV_TYPE 1 -# -# Start the commander (depends on orb, mavlink) -# -commander start - # # Check if we got an IO # diff --git a/ROMFS/px4fmu_common/init.d/1001_rc_quad.hil b/ROMFS/px4fmu_common/init.d/1001_rc_quad.hil index bbe3b7e288..9b664d63ed 100644 --- a/ROMFS/px4fmu_common/init.d/1001_rc_quad.hil +++ b/ROMFS/px4fmu_common/init.d/1001_rc_quad.hil @@ -60,11 +60,6 @@ hil mode_pwm # param set MAV_TYPE 2 -# -# Start the commander (depends on orb, mavlink) -# -commander start - # # Check if we got an IO # diff --git a/ROMFS/px4fmu_common/init.d/1002_rc_fw_state.hil b/ROMFS/px4fmu_common/init.d/1002_rc_fw_state.hil index d5782a89b8..7b9f41bf6d 100644 --- a/ROMFS/px4fmu_common/init.d/1002_rc_fw_state.hil +++ b/ROMFS/px4fmu_common/init.d/1002_rc_fw_state.hil @@ -52,11 +52,6 @@ hil mode_pwm # param set MAV_TYPE 1 -# -# Start the commander (depends on orb, mavlink) -# -commander start - # # Check if we got an IO # diff --git a/ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil b/ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil index 1c85e04f2a..0cc07ad343 100644 --- a/ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil +++ b/ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil @@ -60,11 +60,6 @@ hil mode_pwm # param set MAV_TYPE 2 -# -# Start the commander (depends on orb, mavlink) -# -commander start - # # Check if we got an IO # diff --git a/ROMFS/px4fmu_common/init.d/1004_rc_fw_Rascal110.hil b/ROMFS/px4fmu_common/init.d/1004_rc_fw_Rascal110.hil index 8c5e4b6a8f..344d784224 100644 --- a/ROMFS/px4fmu_common/init.d/1004_rc_fw_Rascal110.hil +++ b/ROMFS/px4fmu_common/init.d/1004_rc_fw_Rascal110.hil @@ -55,11 +55,6 @@ hil mode_pwm # param set MAV_TYPE 1 -# -# Start the commander (depends on orb, mavlink) -# -commander start - # # Check if we got an IO # From 249500188040b12d5930d839dfc94de270378072 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 6 Nov 2013 18:07:19 +0100 Subject: [PATCH 094/447] parametrize some landing parameters --- .../fw_pos_control_l1_main.cpp | 22 +++++++++++++++---- .../fw_pos_control_l1_params.c | 3 +++ 2 files changed, 21 insertions(+), 4 deletions(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 0127160fb2..0e5f805b46 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -215,6 +215,10 @@ private: float land_slope_angle; float land_slope_length; + float land_H1_virt; + float land_flare_alt_relative; + float land_thrust_lim_horizontal_distance; + } _parameters; /**< local copies of interesting parameters */ struct { @@ -254,6 +258,10 @@ private: param_t land_slope_angle; param_t land_slope_length; + param_t land_H1_virt; + param_t land_flare_alt_relative; + param_t land_thrust_lim_horizontal_distance; + } _parameter_handles; /**< handles for interesting parameters */ @@ -383,6 +391,9 @@ FixedwingPositionControl::FixedwingPositionControl() : _parameter_handles.land_slope_angle = param_find("FW_LND_ANG"); _parameter_handles.land_slope_length = param_find("FW_LND_SLLR"); + _parameter_handles.land_H1_virt = param_find("FW_LND_HVIRT"); + _parameter_handles.land_flare_alt_relative = param_find("FW_LND_FLALT"); + _parameter_handles.land_thrust_lim_horizontal_distance = param_find("FW_LND_TLDIST"); _parameter_handles.time_const = param_find("FW_T_TIME_CONST"); _parameter_handles.min_sink_rate = param_find("FW_T_SINK_MIN"); @@ -466,6 +477,9 @@ FixedwingPositionControl::parameters_update() param_get(_parameter_handles.land_slope_angle, &(_parameters.land_slope_angle)); param_get(_parameter_handles.land_slope_length, &(_parameters.land_slope_length)); + param_get(_parameter_handles.land_H1_virt, &(_parameters.land_H1_virt)); + param_get(_parameter_handles.land_flare_alt_relative, &(_parameters.land_flare_alt_relative)); + param_get(_parameter_handles.land_thrust_lim_horizontal_distance, &(_parameters.land_thrust_lim_horizontal_distance)); _l1_control.set_l1_damping(_parameters.l1_damping); _l1_control.set_l1_period(_parameters.l1_period); @@ -479,7 +493,7 @@ FixedwingPositionControl::parameters_update() _tecs.set_vertical_accel_limit(_parameters.vertical_accel_limit); _tecs.set_height_comp_filter_omega(_parameters.height_comp_filter_omega); _tecs.set_speed_comp_filter_omega(_parameters.speed_comp_filter_omega); - _tecs.set_roll_throttle_compensation(math::radians(_parameters.roll_throttle_compensation)); + _tecs.set_roll_throttle_compensation(_parameters.roll_throttle_compensation); _tecs.set_speed_weight(_parameters.speed_weight); _tecs.set_pitch_damping(_parameters.pitch_damping); _tecs.set_indicated_airspeed_min(_parameters.airspeed_min); @@ -816,10 +830,10 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio float airspeed_approach = 1.3f * _parameters.airspeed_min; float landing_slope_angle_rad = math::radians(_parameters.land_slope_angle); - float flare_relative_alt = 15.0f; - float motor_lim_horizontal_distance = 30.0f;//be generous here as we currently have to rely on the user input for the waypoint + float flare_relative_alt = _parameters.land_flare_alt_relative; + float motor_lim_horizontal_distance = _parameters.land_thrust_lim_horizontal_distance;//be generous here as we currently have to rely on the user input for the waypoint float L_wp_distance = get_distance_to_next_waypoint(prev_wp.getX(), prev_wp.getY(), curr_wp.getX(), curr_wp.getY()) * _parameters.land_slope_length; - float H1 = 10.0f; + float H1 = _parameters.land_H1_virt; float H0 = flare_relative_alt + H1; float d1 = flare_relative_alt/tanf(landing_slope_angle_rad); float flare_constant = (H0 * d1)/flare_relative_alt;//-flare_length/(logf(H1/H0)); 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 61d665b172..f206d808e4 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 @@ -116,3 +116,6 @@ PARAM_DEFINE_FLOAT(FW_T_HRATE_P, 0.05f); PARAM_DEFINE_FLOAT(FW_LND_ANG, 10.0f); PARAM_DEFINE_FLOAT(FW_LND_SLLR, 0.9f); +PARAM_DEFINE_FLOAT(FW_LND_HVIRT, 10.0f); +PARAM_DEFINE_FLOAT(FW_LND_FLALT, 15.0f); +PARAM_DEFINE_FLOAT(FW_LND_TLDIST, 30.0f); From 6fc430bd391463f89cfcfec2c38d19a0e0b13ba5 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 6 Nov 2013 18:07:19 +0100 Subject: [PATCH 095/447] parametrize some landing parameters --- .../fw_pos_control_l1_main.cpp | 22 +++++++++++++++---- .../fw_pos_control_l1_params.c | 3 +++ 2 files changed, 21 insertions(+), 4 deletions(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 919990273e..2e88d02493 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -213,6 +213,10 @@ private: float land_slope_angle; float land_slope_length; + float land_H1_virt; + float land_flare_alt_relative; + float land_thrust_lim_horizontal_distance; + } _parameters; /**< local copies of interesting parameters */ struct { @@ -250,6 +254,10 @@ private: param_t land_slope_angle; param_t land_slope_length; + param_t land_H1_virt; + param_t land_flare_alt_relative; + param_t land_thrust_lim_horizontal_distance; + } _parameter_handles; /**< handles for interesting parameters */ @@ -379,6 +387,9 @@ FixedwingPositionControl::FixedwingPositionControl() : _parameter_handles.land_slope_angle = param_find("FW_LND_ANG"); _parameter_handles.land_slope_length = param_find("FW_LND_SLLR"); + _parameter_handles.land_H1_virt = param_find("FW_LND_HVIRT"); + _parameter_handles.land_flare_alt_relative = param_find("FW_LND_FLALT"); + _parameter_handles.land_thrust_lim_horizontal_distance = param_find("FW_LND_TLDIST"); _parameter_handles.time_const = param_find("FW_T_TIME_CONST"); _parameter_handles.min_sink_rate = param_find("FW_T_SINK_MIN"); @@ -460,6 +471,9 @@ FixedwingPositionControl::parameters_update() param_get(_parameter_handles.land_slope_angle, &(_parameters.land_slope_angle)); param_get(_parameter_handles.land_slope_length, &(_parameters.land_slope_length)); + param_get(_parameter_handles.land_H1_virt, &(_parameters.land_H1_virt)); + param_get(_parameter_handles.land_flare_alt_relative, &(_parameters.land_flare_alt_relative)); + param_get(_parameter_handles.land_thrust_lim_horizontal_distance, &(_parameters.land_thrust_lim_horizontal_distance)); _l1_control.set_l1_damping(_parameters.l1_damping); @@ -474,7 +488,7 @@ FixedwingPositionControl::parameters_update() _tecs.set_vertical_accel_limit(_parameters.vertical_accel_limit); _tecs.set_height_comp_filter_omega(_parameters.height_comp_filter_omega); _tecs.set_speed_comp_filter_omega(_parameters.speed_comp_filter_omega); - _tecs.set_roll_throttle_compensation(math::radians(_parameters.roll_throttle_compensation)); + _tecs.set_roll_throttle_compensation(_parameters.roll_throttle_compensation); _tecs.set_speed_weight(_parameters.speed_weight); _tecs.set_pitch_damping(_parameters.pitch_damping); _tecs.set_indicated_airspeed_min(_parameters.airspeed_min); @@ -810,10 +824,10 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio float airspeed_approach = 1.3f * _parameters.airspeed_min; float landing_slope_angle_rad = math::radians(_parameters.land_slope_angle); - float flare_relative_alt = 15.0f; - float motor_lim_horizontal_distance = 30.0f;//be generous here as we currently have to rely on the user input for the waypoint + float flare_relative_alt = _parameters.land_flare_alt_relative; + float motor_lim_horizontal_distance = _parameters.land_thrust_lim_horizontal_distance;//be generous here as we currently have to rely on the user input for the waypoint float L_wp_distance = get_distance_to_next_waypoint(prev_wp.getX(), prev_wp.getY(), curr_wp.getX(), curr_wp.getY()) * _parameters.land_slope_length; - float H1 = 10.0f; + float H1 = _parameters.land_H1_virt; float H0 = flare_relative_alt + H1; float d1 = flare_relative_alt/tanf(landing_slope_angle_rad); float flare_constant = (H0 * d1)/flare_relative_alt;//-flare_length/(logf(H1/H0)); 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 4d7400ce66..43344513dc 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 @@ -114,3 +114,6 @@ PARAM_DEFINE_FLOAT(FW_T_SINK_MAX, 5.0f); PARAM_DEFINE_FLOAT(FW_LND_ANG, 10.0f); PARAM_DEFINE_FLOAT(FW_LND_SLLR, 0.9f); +PARAM_DEFINE_FLOAT(FW_LND_HVIRT, 10.0f); +PARAM_DEFINE_FLOAT(FW_LND_FLALT, 15.0f); +PARAM_DEFINE_FLOAT(FW_LND_TLDIST, 30.0f); From c75c5a5f30cde02002d4db16d803dc979ce8d4d5 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 6 Nov 2013 23:11:14 +0100 Subject: [PATCH 096/447] Fixedwing: Enable loiter mode, tested in HIL --- src/modules/commander/commander.cpp | 3 ++- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 2 +- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index ace13bb784..905d5dee55 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1539,7 +1539,8 @@ check_navigation_state_machine(struct vehicle_status_s *status, struct vehicle_c // TODO AUTO_LAND handling if (status->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) { /* don't switch to other states until takeoff not completed */ - if (local_pos->z > -takeoff_alt || status->condition_landed) { + // XXX: only respect the condition_landed when the local position is actually valid + if (status->condition_local_altitude_valid && (local_pos->z > -takeoff_alt || status->condition_landed)) { return TRANSITION_NOT_CHANGED; } } diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 0e5f805b46..345a08f7b5 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -711,7 +711,7 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio _tecs.set_speed_weight(_parameters.speed_weight); /* execute navigation once we have a setpoint */ - if (_setpoint_valid) { + if (_setpoint_valid && _control_mode.flag_control_auto_enabled) { /* current waypoint (the one currently heading for) */ math::Vector2f curr_wp(global_triplet.current.lat / 1e7f, global_triplet.current.lon / 1e7f); From 7f793d9753e3ae454096a190f4f4989e14b28597 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 6 Nov 2013 23:20:04 +0100 Subject: [PATCH 097/447] mavlink output instead of printf --- .../fw_pos_control_l1_main.cpp | 37 +++++++++++++++---- 1 file changed, 30 insertions(+), 7 deletions(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 0e5f805b46..e01b5813ca 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -83,6 +83,7 @@ #include #include #include +#include #include #include @@ -94,6 +95,8 @@ */ extern "C" __EXPORT int fw_pos_control_l1_main(int argc, char *argv[]); +static int mavlink_fd; + class FixedwingPositionControl { public: @@ -164,6 +167,8 @@ private: bool land_noreturn_vertical; bool land_stayonground; bool land_motor_lim; + bool land_onslope; + float flare_curve_alt_last; /* heading hold */ float target_bearing; @@ -369,8 +374,12 @@ FixedwingPositionControl::FixedwingPositionControl() : land_noreturn_vertical(false), land_stayonground(false), land_motor_lim(false), + land_onslope(false), flare_curve_alt_last(0.0f) { + + mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); + _nav_capabilities.turn_distance = 0.0f; _parameter_handles.l1_period = param_find("FW_L1_PERIOD"); @@ -854,7 +863,11 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio if (wp_distance < motor_lim_horizontal_distance || land_motor_lim) { throttle_max = math::min(throttle_max, _parameters.throttle_land_max); - land_motor_lim = true; + if (!land_motor_lim) { + land_motor_lim = true; + mavlink_log_info(mavlink_fd, "[POSCTRL] Landing, limit throttle"); + } + } float flare_curve_alt = _global_triplet.current.altitude + H0 * expf(-math::max(0.0f, flare_length - wp_distance)/flare_constant) - H1; @@ -874,9 +887,11 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio /* limit roll motion to prevent wings from touching the ground first */ _att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-10.0f), math::radians(10.0f)); - - land_noreturn_vertical = true; - warnx("Landing: flare, _global_pos.alt %.1f, flare_curve_alt %.1f, flare_curve_alt_last %.1f, flare_length %.1f, wp_distance %.1f", _global_pos.alt, flare_curve_alt, flare_curve_alt_last, flare_length, wp_distance); + if (!land_noreturn_vertical) { + mavlink_log_info(mavlink_fd, "[POSCTRL] Landing, flare"); + land_noreturn_vertical = true; + } + //warnx("Landing: flare, _global_pos.alt %.1f, flare_curve_alt %.1f, flare_curve_alt_last %.1f, flare_length %.1f, wp_distance %.1f", _global_pos.alt, flare_curve_alt, flare_curve_alt_last, flare_length, wp_distance); flare_curve_alt_last = flare_curve_alt; @@ -888,7 +903,14 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio false, flare_angle_rad, _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max)); - warnx("Landing: stay on slope, alt_desired: %.1f (wp_distance: %.1f), calculate_target_airspeed(airspeed_land) %.1f, horizontal_slope_displacement %.1f, d1 %.1f, flare_length %.1f", landing_slope_alt_desired, wp_distance, calculate_target_airspeed(airspeed_land), horizontal_slope_displacement, d1, flare_length); + //warnx("Landing: stay on slope, alt_desired: %.1f (wp_distance: %.1f), calculate_target_airspeed(airspeed_land) %.1f, horizontal_slope_displacement %.1f, d1 %.1f, flare_length %.1f", landing_slope_alt_desired, wp_distance, calculate_target_airspeed(airspeed_land), horizontal_slope_displacement, d1, flare_length); + + if (!land_onslope) { + + mavlink_log_info(mavlink_fd, "[POSCTRL] Landing, on slope"); + land_onslope = true; + } + } else { /* intersect glide slope: @@ -899,11 +921,11 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio if (_global_pos.alt > landing_slope_alt_desired - 10.0f) { /* stay on slope */ altitude_desired = landing_slope_alt_desired; - warnx("Landing: before L, stay on landing slope, alt_desired: %.1f (wp_distance: %.1f, L_wp_distance %.1f), calculate_target_airspeed(airspeed_land) %.1f, horizontal_slope_displacement %.1f", altitude_desired, wp_distance, L_wp_distance, calculate_target_airspeed(airspeed_land), horizontal_slope_displacement); + //warnx("Landing: before L, stay on landing slope, alt_desired: %.1f (wp_distance: %.1f, L_wp_distance %.1f), calculate_target_airspeed(airspeed_land) %.1f, horizontal_slope_displacement %.1f", altitude_desired, wp_distance, L_wp_distance, calculate_target_airspeed(airspeed_land), horizontal_slope_displacement); } else { /* continue horizontally */ altitude_desired = math::max(_global_pos.alt, L_altitude); - warnx("Landing: before L,continue at: %.4f, (landing_slope_alt_desired %.4f, wp_distance: %.4f, L_altitude: %.4f L_wp_distance: %.4f)", altitude_desired, landing_slope_alt_desired, wp_distance, L_altitude, L_wp_distance); + //warnx("Landing: before L,continue at: %.4f, (landing_slope_alt_desired %.4f, wp_distance: %.4f, L_altitude: %.4f L_wp_distance: %.4f)", altitude_desired, landing_slope_alt_desired, wp_distance, L_altitude, L_wp_distance); } _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, altitude_desired, calculate_target_airspeed(airspeed_approach), @@ -1004,6 +1026,7 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio land_noreturn_vertical = false; land_stayonground = false; land_motor_lim = false; + land_onslope = false; } if (was_circle_mode && !_l1_control.circle_mode()) { From 6064a2af7e8f92fa01c6a10a61c2c353ba93f4bd Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 6 Nov 2013 23:20:04 +0100 Subject: [PATCH 098/447] mavlink output instead of printf --- .../fw_pos_control_l1_main.cpp | 37 +++++++++++++++---- 1 file changed, 30 insertions(+), 7 deletions(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 2e88d02493..cd57feac74 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -83,6 +83,7 @@ #include #include #include +#include #include #include @@ -94,6 +95,8 @@ */ extern "C" __EXPORT int fw_pos_control_l1_main(int argc, char *argv[]); +static int mavlink_fd; + class FixedwingPositionControl { public: @@ -164,6 +167,8 @@ private: bool land_noreturn_vertical; bool land_stayonground; bool land_motor_lim; + bool land_onslope; + float flare_curve_alt_last; /* heading hold */ float target_bearing; @@ -365,8 +370,12 @@ FixedwingPositionControl::FixedwingPositionControl() : land_noreturn_vertical(false), land_stayonground(false), land_motor_lim(false), + land_onslope(false), flare_curve_alt_last(0.0f) { + + mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); + _nav_capabilities.turn_distance = 0.0f; _parameter_handles.l1_period = param_find("FW_L1_PERIOD"); @@ -848,7 +857,11 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio if (wp_distance < motor_lim_horizontal_distance || land_motor_lim) { throttle_max = math::min(throttle_max, _parameters.throttle_land_max); - land_motor_lim = true; + if (!land_motor_lim) { + land_motor_lim = true; + mavlink_log_info(mavlink_fd, "[POSCTRL] Landing, limit throttle"); + } + } float flare_curve_alt = _global_triplet.current.altitude + H0 * expf(-math::max(0.0f, flare_length - wp_distance)/flare_constant) - H1; @@ -868,9 +881,11 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio /* limit roll motion to prevent wings from touching the ground first */ _att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-10.0f), math::radians(10.0f)); - - land_noreturn_vertical = true; - warnx("Landing: flare, _global_pos.alt %.1f, flare_curve_alt %.1f, flare_curve_alt_last %.1f, flare_length %.1f, wp_distance %.1f", _global_pos.alt, flare_curve_alt, flare_curve_alt_last, flare_length, wp_distance); + if (!land_noreturn_vertical) { + mavlink_log_info(mavlink_fd, "[POSCTRL] Landing, flare"); + land_noreturn_vertical = true; + } + //warnx("Landing: flare, _global_pos.alt %.1f, flare_curve_alt %.1f, flare_curve_alt_last %.1f, flare_length %.1f, wp_distance %.1f", _global_pos.alt, flare_curve_alt, flare_curve_alt_last, flare_length, wp_distance); flare_curve_alt_last = flare_curve_alt; @@ -882,7 +897,14 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio false, flare_angle_rad, _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max)); - warnx("Landing: stay on slope, alt_desired: %.1f (wp_distance: %.1f), calculate_target_airspeed(airspeed_land) %.1f, horizontal_slope_displacement %.1f, d1 %.1f, flare_length %.1f", landing_slope_alt_desired, wp_distance, calculate_target_airspeed(airspeed_land), horizontal_slope_displacement, d1, flare_length); + //warnx("Landing: stay on slope, alt_desired: %.1f (wp_distance: %.1f), calculate_target_airspeed(airspeed_land) %.1f, horizontal_slope_displacement %.1f, d1 %.1f, flare_length %.1f", landing_slope_alt_desired, wp_distance, calculate_target_airspeed(airspeed_land), horizontal_slope_displacement, d1, flare_length); + + if (!land_onslope) { + + mavlink_log_info(mavlink_fd, "[POSCTRL] Landing, on slope"); + land_onslope = true; + } + } else { /* intersect glide slope: @@ -893,11 +915,11 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio if (_global_pos.alt > landing_slope_alt_desired - 10.0f) { /* stay on slope */ altitude_desired = landing_slope_alt_desired; - warnx("Landing: before L, stay on landing slope, alt_desired: %.1f (wp_distance: %.1f, L_wp_distance %.1f), calculate_target_airspeed(airspeed_land) %.1f, horizontal_slope_displacement %.1f", altitude_desired, wp_distance, L_wp_distance, calculate_target_airspeed(airspeed_land), horizontal_slope_displacement); + //warnx("Landing: before L, stay on landing slope, alt_desired: %.1f (wp_distance: %.1f, L_wp_distance %.1f), calculate_target_airspeed(airspeed_land) %.1f, horizontal_slope_displacement %.1f", altitude_desired, wp_distance, L_wp_distance, calculate_target_airspeed(airspeed_land), horizontal_slope_displacement); } else { /* continue horizontally */ altitude_desired = math::max(_global_pos.alt, L_altitude); - warnx("Landing: before L,continue at: %.4f, (landing_slope_alt_desired %.4f, wp_distance: %.4f, L_altitude: %.4f L_wp_distance: %.4f)", altitude_desired, landing_slope_alt_desired, wp_distance, L_altitude, L_wp_distance); + //warnx("Landing: before L,continue at: %.4f, (landing_slope_alt_desired %.4f, wp_distance: %.4f, L_altitude: %.4f L_wp_distance: %.4f)", altitude_desired, landing_slope_alt_desired, wp_distance, L_altitude, L_wp_distance); } _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, altitude_desired, calculate_target_airspeed(airspeed_approach), @@ -998,6 +1020,7 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio land_noreturn_vertical = false; land_stayonground = false; land_motor_lim = false; + land_onslope = false; } if (was_circle_mode && !_l1_control.circle_mode()) { From c3944f49b13a7a85b8505faeb7085765887d9287 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 7 Nov 2013 20:17:12 +0400 Subject: [PATCH 099/447] position_estimator_inav: minor baro offset changes --- .../position_estimator_inav_main.c | 15 ++++++++------- 1 file changed, 8 insertions(+), 7 deletions(-) diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index 8f4f9e0805..5be59f9652 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -312,7 +312,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } else { wait_baro = false; baro_offset /= (float) baro_init_cnt; - corr_baro = -baro_offset; warnx("baro offs: %.2f", baro_offset); mavlink_log_info(mavlink_fd, "[inav] baro offs: %.2f", baro_offset); local_pos.z_valid = true; @@ -369,11 +368,11 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (updated) { orb_copy(ORB_ID(actuator_armed), armed_sub, &armed); - /* reset ground level on arm if sonar is invalid */ + /* reset ground level on arm */ if (armed.armed && !flag_armed) { flag_armed = armed.armed; baro_offset -= z_est[0]; - corr_baro = -baro_offset; + corr_baro = 0.0f; local_pos.ref_alt -= z_est[0]; local_pos.ref_timestamp = t; z_est[0] = 0.0f; @@ -416,7 +415,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } if (sensor.baro_counter > baro_counter) { - corr_baro = - sensor.baro_alt_meter - z_est[0]; + corr_baro = baro_offset - sensor.baro_alt_meter - z_est[0]; baro_counter = sensor.baro_counter; baro_updates++; } @@ -667,7 +666,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) /* baro offset correction */ if (use_gps_z) { - baro_offset += corr_gps[2][0] * w_z_gps_p * dt; + float offs_corr = corr_gps[2][0] * w_z_gps_p * dt; + baro_offset += offs_corr; + baro_counter += offs_corr; } /* accelerometer bias correction */ @@ -689,7 +690,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) accel_bias_corr[1] -= corr_flow[1] * params.w_xy_flow; } - accel_bias_corr[2] -= (corr_baro + baro_offset) * params.w_z_baro * params.w_z_baro; + accel_bias_corr[2] -= corr_baro * params.w_z_baro * params.w_z_baro; /* transform error vector from NED frame to body frame */ for (int i = 0; i < 3; i++) { @@ -706,7 +707,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) inertial_filter_predict(dt, z_est); /* inertial filter correction for altitude */ - inertial_filter_correct(corr_baro + baro_offset, dt, z_est, 0, params.w_z_baro); + inertial_filter_correct(corr_baro, dt, z_est, 0, params.w_z_baro); inertial_filter_correct(corr_gps[2][0], dt, z_est, 0, w_z_gps_p); inertial_filter_correct(corr_gps[2][1], dt, z_est, 1, w_z_gps_v); inertial_filter_correct(corr_acc[2], dt, z_est, 2, params.w_z_acc); From 668f9dc552040b77d298330ff2dc1dcccdb5b3da Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 8 Nov 2013 16:45:22 +0100 Subject: [PATCH 100/447] enable seatbelt without position lock for non rotary wing vehicles --- src/modules/commander/state_machine_helper.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 490fc8fc6b..a50af7dafe 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -239,8 +239,9 @@ main_state_transition(struct vehicle_status_s *current_state, main_state_t new_m case MAIN_STATE_SEATBELT: /* need at minimum altitude estimate */ - if (current_state->condition_local_altitude_valid || - current_state->condition_global_position_valid) { + if (!current_state->is_rotary_wing || + (current_state->condition_local_altitude_valid || + current_state->condition_global_position_valid)) { ret = TRANSITION_CHANGED; } From d5ae5f237db466ddc1ec7b097738a52671c54aef Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 8 Nov 2013 23:00:41 +0400 Subject: [PATCH 101/447] mc_att_control_vector fixes --- .../ecl_mc_att_control_vector.cpp | 13 ++-- .../mc_att_control_vector_main.cpp | 78 +++++++------------ 2 files changed, 36 insertions(+), 55 deletions(-) diff --git a/src/modules/mc_att_control_vector/ecl_mc_att_control_vector.cpp b/src/modules/mc_att_control_vector/ecl_mc_att_control_vector.cpp index 252cdafab5..589c31c0d6 100644 --- a/src/modules/mc_att_control_vector/ecl_mc_att_control_vector.cpp +++ b/src/modules/mc_att_control_vector/ecl_mc_att_control_vector.cpp @@ -77,12 +77,13 @@ void ECL_MCAttControlVector::control(float dt, const math::Dcm &R_nb, float yaw, // desired thrust in body frame // avoid division by zero // compensates for thrust loss due to roll/pitch - if (F_des(2) >= 0.1f) { - thrust = F_des(2) / R_bn(2, 2); - } else { - F_des(2) = 0.1f; - thrust= F_des(2) / R_bn(2, 2); - } + //XXX disable this for first time + //if (F_des(2) >= 0.1f) { + // thrust = F_des(2) / R_bn(2, 2); + //} else { + // F_des(2) = 0.1f; + // thrust= F_des(2) / R_bn(2, 2); + //} math::Vector3 x_B_des; math::Vector3 y_B_des; diff --git a/src/modules/mc_att_control_vector/mc_att_control_vector_main.cpp b/src/modules/mc_att_control_vector/mc_att_control_vector_main.cpp index 368d343bde..e0b36067c7 100644 --- a/src/modules/mc_att_control_vector/mc_att_control_vector_main.cpp +++ b/src/modules/mc_att_control_vector/mc_att_control_vector_main.cpp @@ -62,17 +62,16 @@ #include #include #include -#include +#include +#include #include #include #include #include -#include #include #include #include -//#include #include "ecl_mc_att_control_vector.h" /** @@ -111,7 +110,7 @@ private: int _accel_sub; /**< accelerometer subscription */ int _att_sp_sub; /**< vehicle attitude setpoint */ int _attitude_sub; /**< raw rc channels data subscription */ - int _vstatus_sub; /**< vehicle status subscription */ + int _control_mode_sub; /**< vehicle control mode subscription */ int _params_sub; /**< notification of parameter updates */ int _manual_sub; /**< notification of manual control updates */ int _arming_sub; /**< arming status of outputs */ @@ -123,7 +122,7 @@ private: struct accel_report _accel; /**< body frame accelerations */ struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */ struct manual_control_setpoint_s _manual; /**< r/c channel data */ - struct vehicle_status_s _vstatus; /**< vehicle status */ + struct vehicle_control_mode_s _control_mode; /**< vehicle control mode */ struct actuator_controls_s _actuators; /**< actuator control inputs */ struct actuator_armed_s _arming; /**< actuator arming status */ @@ -178,9 +177,9 @@ private: void control_update(); /** - * Check for changes in vehicle status. + * Check for changes in vehicle control mode. */ - void vehicle_status_poll(); + void vehicle_control_mode_poll(); /** * Check for changes in manual inputs. @@ -233,7 +232,7 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : /* subscriptions */ _att_sub(-1), _accel_sub(-1), - _vstatus_sub(-1), + _control_mode_sub(-1), _params_sub(-1), _manual_sub(-1), _arming_sub(-1), @@ -320,16 +319,16 @@ MulticopterAttitudeControl::parameters_update() } void -MulticopterAttitudeControl::vehicle_status_poll() +MulticopterAttitudeControl::vehicle_control_mode_poll() { - bool vstatus_updated; + bool control_mode_updated; /* Check HIL state if vehicle status has changed */ - orb_check(_vstatus_sub, &vstatus_updated); + orb_check(_control_mode_sub, &control_mode_updated); - if (vstatus_updated) { + if (control_mode_updated) { - orb_copy(ORB_ID(vehicle_status), _vstatus_sub, &_vstatus); + orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode); } } @@ -404,14 +403,13 @@ MulticopterAttitudeControl::task_main() _att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); _att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); _accel_sub = orb_subscribe(ORB_ID(sensor_accel)); - _vstatus_sub = orb_subscribe(ORB_ID(vehicle_status)); + _control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); _manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); _arming_sub = orb_subscribe(ORB_ID(actuator_armed)); - /* rate limit vehicle status updates to 5Hz */ - orb_set_interval(_vstatus_sub, 200); - orb_set_interval(_att_sub, 100); + /* rate limit attitude updates to 100Hz */ + orb_set_interval(_att_sub, 10); parameters_update(); @@ -421,7 +419,7 @@ MulticopterAttitudeControl::task_main() /* get an initial update for all sensor and status data */ vehicle_setpoint_poll(); vehicle_accel_poll(); - vehicle_status_poll(); + vehicle_control_mode_poll(); vehicle_manual_poll(); arming_status_poll(); @@ -463,8 +461,6 @@ MulticopterAttitudeControl::task_main() /* only run controller if attitude changed */ if (fds[1].revents & POLLIN) { - - static uint64_t last_run = 0; float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f; last_run = hrt_absolute_time(); @@ -477,15 +473,9 @@ MulticopterAttitudeControl::task_main() orb_copy(ORB_ID(vehicle_attitude), _att_sub, &_att); vehicle_setpoint_poll(); - vehicle_accel_poll(); - - /* check vehicle status for changes to publication state */ - vehicle_status_poll(); - - /* check for arming status changes */ + vehicle_control_mode_poll(); arming_status_poll(); - vehicle_manual_poll(); /* lock integrator until armed */ @@ -497,33 +487,22 @@ MulticopterAttitudeControl::task_main() } /* decide if in auto or full manual control */ - float roll_sp, pitch_sp; + float roll_sp = 0.0f; + float pitch_sp = 0.0f; float throttle_sp = 0.0f; float yaw_sp = 0.0f; - if (_vstatus.state_machine == SYSTEM_STATE_MANUAL || - (_vstatus.state_machine == SYSTEM_STATE_STABILIZED)) { - - /* - * Scale down roll and pitch as the setpoints are radians - * and a typical remote can only do 45 degrees, the mapping is - * -1..+1 to -45..+45 degrees or -0.75..+0.75 radians. - * - * With this mapping the stick angle is a 1:1 representation of - * the commanded attitude. If more than 45 degrees are desired, - * a scaling parameter can be applied to the remote. - */ - roll_sp = _manual.roll * 0.75f; - pitch_sp = _manual.pitch * 0.75f; - yaw_sp = _manual.yaw; + if (_control_mode.flag_control_manual_enabled) { + roll_sp = _manual.roll; + pitch_sp = _manual.pitch; + yaw_sp = _manual.yaw; // XXX move yaw setpoint throttle_sp = _manual.throttle; - } else if (_vstatus.state_machine == SYSTEM_STATE_AUTO) { - - roll_sp = _att_sp.roll_body; - pitch_sp = _att_sp.pitch_body; - yaw_sp = _att_sp.yaw_body; - throttle_sp = _att_sp.thrust; + } else if (_control_mode.flag_control_auto_enabled) { + roll_sp = _att_sp.roll_body; + pitch_sp = _att_sp.pitch_body; + yaw_sp = _att_sp.yaw_body; + throttle_sp = _att_sp.thrust; } // XXX take rotation matrix directly from att_sp for auto mode @@ -532,6 +511,7 @@ MulticopterAttitudeControl::task_main() math::Vector3 rates_des; math::Dcm R_nb(_att.R); + // XXX actually it's not euler angles rates, but body rates, rename it (?) math::Vector3 angular_rates(_att.rollspeed, _att.pitchspeed, _att.yawspeed); _att_control.control(deltaT, R_nb, _att.yaw, F_des, From 550c611a6e2bf0affc7063adb1c6bae66dd3d4ca Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 8 Nov 2013 23:01:13 +0400 Subject: [PATCH 102/447] Add mc_att_control_vector to apps list --- makefiles/config_px4fmu-v1_default.mk | 1 + makefiles/config_px4fmu-v2_default.mk | 1 + 2 files changed, 2 insertions(+) diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk index 3068270865..efffa7a32d 100644 --- a/makefiles/config_px4fmu-v1_default.mk +++ b/makefiles/config_px4fmu-v1_default.mk @@ -83,6 +83,7 @@ MODULES += examples/flow_position_estimator MODULES += modules/fw_pos_control_l1 MODULES += modules/fw_att_control MODULES += modules/multirotor_att_control +MODULES += modules/mc_att_control_vector MODULES += modules/multirotor_pos_control MODULES += examples/flow_position_control MODULES += examples/flow_speed_control diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index 761fb8d9d6..e6c81487f2 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -82,6 +82,7 @@ MODULES += examples/flow_position_estimator MODULES += modules/fw_pos_control_l1 MODULES += modules/fw_att_control MODULES += modules/multirotor_att_control +MODULES += modules/mc_att_control_vector MODULES += modules/multirotor_pos_control # From a1b80ec3f356aa19544eaa318bc188d57877f16f Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 8 Nov 2013 21:27:00 +0100 Subject: [PATCH 103/447] fw: att fix initialization and add parameter to disable coordinated turns at low speed --- .../ecl/attitude_fw/ecl_yaw_controller.cpp | 7 +++-- src/lib/ecl/attitude_fw/ecl_yaw_controller.h | 8 +++--- .../fw_att_control/fw_att_control_main.cpp | 28 +++++++++++++------ .../fw_att_control/fw_att_control_params.c | 2 +- 4 files changed, 29 insertions(+), 16 deletions(-) diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp index 04293efd6b..a4ecc48a28 100644 --- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp @@ -59,21 +59,22 @@ ECL_YawController::ECL_YawController() : _rate_error(0.0f), _rate_setpoint(0.0f), _bodyrate_setpoint(0.0f), - _coordinated(0.0f) + _coordinated_min_speed(1.0f) { } float ECL_YawController::control_attitude(float roll, float pitch, - float speed_body_u, float speed_body_w, + float speed_body_u, float speed_body_v, float speed_body_w, float roll_rate_setpoint, float pitch_rate_setpoint) { // static int counter = 0; /* Calculate desired yaw rate from coordinated turn constraint / (no side forces) */ _rate_setpoint = 0.0f; - if (_coordinated > 0.1) { + if (sqrtf(speed_body_u * speed_body_u + speed_body_v * speed_body_v + speed_body_w * speed_body_w) > _coordinated_min_speed) { float denumerator = (speed_body_u * cosf(roll) * cosf(pitch) + speed_body_w * sinf(pitch)); if(denumerator != 0.0f) { //XXX: floating point comparison _rate_setpoint = (speed_body_w * roll_rate_setpoint + 9.81f * sinf(roll) * cosf(pitch) + speed_body_u * pitch_rate_setpoint * sinf(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) { diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.h b/src/lib/ecl/attitude_fw/ecl_yaw_controller.h index 154471cafb..b5ae1e6afc 100644 --- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.h +++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.h @@ -57,7 +57,7 @@ public: ECL_YawController(); float control_attitude(float roll, float pitch, - float speed_body_u, float speed_body_w, + float speed_body_u, float speed_body_v, float speed_body_w, float roll_rate_setpoint, float pitch_rate_setpoint); float control_bodyrate( float roll, float pitch, @@ -85,8 +85,8 @@ public: void set_k_roll_ff(float roll_ff) { _roll_ff = roll_ff; } - void set_coordinated(float coordinated) { - _coordinated = coordinated; + void set_coordinated_min_speed(float coordinated_min_speed) { + _coordinated_min_speed = coordinated_min_speed; } @@ -115,7 +115,7 @@ private: float _rate_error; float _rate_setpoint; float _bodyrate_setpoint; - float _coordinated; + float _coordinated_min_speed; }; diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index c88b29056c..a5f3f1d91d 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -155,7 +155,7 @@ private: float y_d; float y_roll_feedforward; float y_integrator_max; - float y_coordinated; + float y_coordinated_min_speed; float y_rmax; float airspeed_min; @@ -183,7 +183,7 @@ private: param_t y_d; param_t y_roll_feedforward; param_t y_integrator_max; - param_t y_coordinated; + param_t y_coordinated_min_speed; param_t y_rmax; param_t airspeed_min; @@ -287,6 +287,17 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : _setpoint_valid(false), _airspeed_valid(false) { + /* safely initialize structs */ + vehicle_attitude_s _att = {0}; + accel_report _accel = {0}; + vehicle_attitude_setpoint_s _att_sp = {0}; + manual_control_setpoint_s _manual = {0}; + airspeed_s _airspeed = {0}; + vehicle_control_mode_s _vcontrol_mode = {0}; + actuator_controls_s _actuators = {0}; + vehicle_global_position_s _global_pos = {0}; + + _parameter_handles.tconst = param_find("FW_ATT_TC"); _parameter_handles.p_p = param_find("FW_PR_P"); _parameter_handles.p_d = param_find("FW_PR_D"); @@ -313,7 +324,7 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : _parameter_handles.airspeed_trim = param_find("FW_AIRSPD_TRIM"); _parameter_handles.airspeed_max = param_find("FW_AIRSPD_MAX"); - _parameter_handles.y_coordinated = param_find("FW_Y_COORD"); + _parameter_handles.y_coordinated_min_speed = param_find("FW_YCO_VMIN"); /* fetch initial parameter values */ parameters_update(); @@ -368,7 +379,7 @@ FixedwingAttitudeControl::parameters_update() param_get(_parameter_handles.y_d, &(_parameters.y_d)); param_get(_parameter_handles.y_roll_feedforward, &(_parameters.y_roll_feedforward)); param_get(_parameter_handles.y_integrator_max, &(_parameters.y_integrator_max)); - param_get(_parameter_handles.y_coordinated, &(_parameters.y_coordinated)); + param_get(_parameter_handles.y_coordinated_min_speed, &(_parameters.y_coordinated_min_speed)); param_get(_parameter_handles.y_rmax, &(_parameters.y_rmax)); param_get(_parameter_handles.airspeed_min, &(_parameters.airspeed_min)); @@ -399,7 +410,7 @@ FixedwingAttitudeControl::parameters_update() _yaw_ctrl.set_k_d(_parameters.y_d); _yaw_ctrl.set_k_roll_ff(_parameters.y_roll_feedforward); _yaw_ctrl.set_integrator_max(_parameters.y_integrator_max); - _yaw_ctrl.set_coordinated(_parameters.y_coordinated); + _yaw_ctrl.set_coordinated_min_speed(_parameters.y_coordinated_min_speed); _yaw_ctrl.set_max_rate(math::radians(_parameters.y_rmax)); return OK; @@ -672,10 +683,11 @@ FixedwingAttitudeControl::task_main() /* Prepare speed_body_u and speed_body_w */ float speed_body_u = 0.0f; + float speed_body_v = 0.0f; float speed_body_w = 0.0f; if(_att.R_valid) { speed_body_u = _att.R[0][0] * _global_pos.vx + _att.R[1][0] * _global_pos.vy + _att.R[2][0] * _global_pos.vz; -// speed_body_v = _att.R[0][1] * _global_pos.vx + _att.R[1][1] * _global_pos.vy + _att.R[2][1] * _global_pos.vz; + speed_body_v = _att.R[0][1] * _global_pos.vx + _att.R[1][1] * _global_pos.vy + _att.R[2][1] * _global_pos.vz; speed_body_w = _att.R[0][2] * _global_pos.vx + _att.R[1][2] * _global_pos.vy + _att.R[2][2] * _global_pos.vz; } else { warnx("Did not get a valid R\n"); @@ -686,7 +698,7 @@ FixedwingAttitudeControl::task_main() _roll_ctrl.control_attitude(roll_sp, _att.roll); _pitch_ctrl.control_attitude(pitch_sp, _att.roll, _att.pitch, airspeed); _yaw_ctrl.control_attitude(_att.roll, _att.pitch, - speed_body_u,speed_body_w, + speed_body_u, speed_body_v, speed_body_w, _roll_ctrl.get_desired_rate(), _pitch_ctrl.get_desired_rate()); //runs last, because is depending on output of roll and pitch attitude /* Run attitude RATE controllers which need the desired attitudes from above */ @@ -709,7 +721,7 @@ FixedwingAttitudeControl::task_main() pitch_u, _yaw_ctrl.get_desired_rate(), airspeed, airspeed_scaling, roll_sp, pitch_sp, _roll_ctrl.get_desired_rate(), _pitch_ctrl.get_desired_rate(), _att_sp.roll_body); } - float yaw_u = _yaw_ctrl.control_bodyrate( _att.roll, _att.pitch, + float yaw_u = _yaw_ctrl.control_bodyrate(_att.roll, _att.pitch, _att.pitchspeed, _att.yawspeed, _pitch_ctrl.get_desired_rate(), _parameters.airspeed_min, _parameters.airspeed_max, airspeed, airspeed_scaling, lock_integrator); diff --git a/src/modules/fw_att_control/fw_att_control_params.c b/src/modules/fw_att_control/fw_att_control_params.c index 41e37e61f7..796ab3f906 100644 --- a/src/modules/fw_att_control/fw_att_control_params.c +++ b/src/modules/fw_att_control/fw_att_control_params.c @@ -137,4 +137,4 @@ PARAM_DEFINE_FLOAT(FW_AIRSPD_MIN, 9.0f); PARAM_DEFINE_FLOAT(FW_AIRSPD_TRIM, 12.0f); PARAM_DEFINE_FLOAT(FW_AIRSPD_MAX, 18.0f); PARAM_DEFINE_FLOAT(FW_Y_RMAX, 60); -PARAM_DEFINE_FLOAT(FW_Y_COORD, 1.0f); +PARAM_DEFINE_FLOAT(FW_YCO_VMIN, 1.0f); From b172bcd9122e186d152b7bacf6495f7f27efc80c Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 8 Nov 2013 21:27:16 +0100 Subject: [PATCH 104/447] fw pos ctrl: struct initialization --- .../fw_pos_control_l1/fw_pos_control_l1_main.cpp | 14 ++++++++++++++ 1 file changed, 14 insertions(+) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 602681ce02..2606956208 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -380,6 +380,20 @@ FixedwingPositionControl::FixedwingPositionControl() : mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); + /* safely initialize structs */ + vehicle_attitude_s _att = {0}; + vehicle_attitude_setpoint_s _att_sp = {0}; + navigation_capabilities_s _nav_capabilities = {0}; + manual_control_setpoint_s _manual = {0}; + airspeed_s _airspeed = {0}; + vehicle_control_mode_s _control_mode = {0}; + vehicle_global_position_s _global_pos = {0}; + vehicle_global_position_set_triplet_s _global_triplet = {0}; + accel_report _accel = {0}; + + + + _nav_capabilities.turn_distance = 0.0f; _parameter_handles.l1_period = param_find("FW_L1_PERIOD"); From 22ef0c77352e9a60cecc51fc219dbad4b9bd9d0d Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 10 Nov 2013 19:24:37 +0100 Subject: [PATCH 105/447] fix MEAS airspeed and airspeed calibration --- src/drivers/airspeed/airspeed.h | 2 +- src/drivers/ets_airspeed/ets_airspeed.cpp | 2 +- src/drivers/meas_airspeed/meas_airspeed.cpp | 10 ++++- .../commander/airspeed_calibration.cpp | 44 ++++++++++++++++--- src/modules/sensors/sensors.cpp | 1 + src/modules/systemlib/airspeed.c | 2 +- .../uORB/topics/differential_pressure.h | 4 +- 7 files changed, 52 insertions(+), 13 deletions(-) diff --git a/src/drivers/airspeed/airspeed.h b/src/drivers/airspeed/airspeed.h index 0487848133..62c0d1f17c 100644 --- a/src/drivers/airspeed/airspeed.h +++ b/src/drivers/airspeed/airspeed.h @@ -119,7 +119,7 @@ protected: virtual int collect() = 0; work_s _work; - uint16_t _max_differential_pressure_pa; + float _max_differential_pressure_pa; bool _sensor_ok; int _measure_ticks; bool _collect_phase; diff --git a/src/drivers/ets_airspeed/ets_airspeed.cpp b/src/drivers/ets_airspeed/ets_airspeed.cpp index 9d8ad084e5..de371bf324 100644 --- a/src/drivers/ets_airspeed/ets_airspeed.cpp +++ b/src/drivers/ets_airspeed/ets_airspeed.cpp @@ -180,7 +180,7 @@ ETSAirspeed::collect() differential_pressure_s report; report.timestamp = hrt_absolute_time(); report.error_count = perf_event_count(_comms_errors); - report.differential_pressure_pa = diff_pres_pa; + report.differential_pressure_pa = (float)diff_pres_pa; report.voltage = 0; report.max_differential_pressure_pa = _max_differential_pressure_pa; diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp index b3003fc1b0..3cd6d67203 100644 --- a/src/drivers/meas_airspeed/meas_airspeed.cpp +++ b/src/drivers/meas_airspeed/meas_airspeed.cpp @@ -36,6 +36,7 @@ * @author Lorenz Meier * @author Sarthak Kaingade * @author Simon Wilks + * @author Thomas Gubler * * Driver for the MEAS Spec series connected via I2C. * @@ -76,6 +77,7 @@ #include #include #include +#include #include #include @@ -184,7 +186,7 @@ MEASAirspeed::collect() //diff_pres_pa -= _diff_pres_offset; int16_t dp_raw = 0, dT_raw = 0; dp_raw = (val[0] << 8) + val[1]; - dp_raw = 0x3FFF & dp_raw; + dp_raw = 0x3FFF & dp_raw; //mask the used bits dT_raw = (val[2] << 8) + val[3]; dT_raw = (0xFFE0 & dT_raw) >> 5; float temperature = ((200 * dT_raw) / 2047) - 50; @@ -193,7 +195,11 @@ MEASAirspeed::collect() // Calculate differential pressure. As its centered around 8000 // and can go positive or negative, enforce absolute value - uint16_t diff_press_pa = abs(dp_raw - (16384 / 2.0f)); +// uint16_t diff_press_pa = abs(dp_raw - (16384 / 2.0f)); + const float P_min = -1.0f; + const float P_max = 1.0f; + float diff_press_pa = math::max(0.0f, fabsf( ( ((float)dp_raw - 0.1f*16383.0f) * (P_max-P_min)/(0.8f*16383.0f) + P_min) * 6894.8f) - _diff_pres_offset); + struct differential_pressure_s report; // Track maximum differential pressure measured (so we can work out top speed). diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp index 248eb4a66e..1809f96888 100644 --- a/src/modules/commander/airspeed_calibration.cpp +++ b/src/modules/commander/airspeed_calibration.cpp @@ -37,12 +37,15 @@ */ #include "airspeed_calibration.h" +#include "calibration_messages.h" #include "commander_helper.h" #include +#include #include #include #include +#include #include #include #include @@ -55,10 +58,13 @@ #endif static const int ERROR = -1; +static const char *sensor_name = "dpress"; + int do_airspeed_calibration(int mavlink_fd) { /* give directions */ - mavlink_log_info(mavlink_fd, "airspeed calibration starting, keep it still"); + mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name); + mavlink_log_info(mavlink_fd, "don't move system"); const int calibration_count = 2500; @@ -68,6 +74,28 @@ int do_airspeed_calibration(int mavlink_fd) int calibration_counter = 0; float diff_pres_offset = 0.0f; + /* Reset sensor parameters */ + struct airspeed_scale airscale = { + 0.0f, + 1.0f, + }; + + bool paramreset_successful = false; + int fd = open(AIRSPEED_DEVICE_PATH, 0); + if (fd > 0) { + if (OK == ioctl(fd, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale)) { + paramreset_successful = true; + } + close(fd); + } + + if (!paramreset_successful) { + warn("WARNING: failed to set scale / offsets for airspeed sensor"); + mavlink_log_critical(mavlink_fd, "could not reset dpress sensor"); + mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name); + return ERROR; + } + while (calibration_counter < calibration_count) { /* wait blocking for new data */ @@ -82,9 +110,12 @@ int do_airspeed_calibration(int mavlink_fd) diff_pres_offset += diff_pres.differential_pressure_pa; calibration_counter++; + if (calibration_counter % (calibration_count / 20) == 0) + mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, (calibration_counter * 100) / calibration_count); + } else if (poll_ret == 0) { /* any poll failure for 1s is a reason to abort */ - mavlink_log_info(mavlink_fd, "airspeed calibration aborted"); + mavlink_log_critical(mavlink_fd, CAL_FAILED_MSG, sensor_name); close(diff_pres_sub); return ERROR; } @@ -95,7 +126,7 @@ int do_airspeed_calibration(int mavlink_fd) if (isfinite(diff_pres_offset)) { if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) { - mavlink_log_critical(mavlink_fd, "Setting offs failed!"); + mavlink_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG); close(diff_pres_sub); return ERROR; } @@ -105,17 +136,18 @@ int do_airspeed_calibration(int mavlink_fd) if (save_ret != 0) { warn("WARNING: auto-save of params to storage failed"); - mavlink_log_info(mavlink_fd, "FAILED storing calibration"); + mavlink_log_critical(mavlink_fd, CAL_FAILED_SAVE_PARAMS_MSG); close(diff_pres_sub); return ERROR; } - mavlink_log_info(mavlink_fd, "airspeed calibration done"); + mavlink_log_info(mavlink_fd, CAL_DONE_MSG, sensor_name); + tune_neutral(); close(diff_pres_sub); return OK; } else { - mavlink_log_info(mavlink_fd, "airspeed calibration FAILED (NaN)"); + mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name); close(diff_pres_sub); return ERROR; } diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index da0c11372c..6d38b98ec1 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -1121,6 +1121,7 @@ Sensors::parameter_update_poll(bool forced) if (OK != ioctl(fd, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale)) warn("WARNING: failed to set scale / offsets for airspeed sensor"); + close(fd); } #if 0 diff --git a/src/modules/systemlib/airspeed.c b/src/modules/systemlib/airspeed.c index 310fbf60ff..1a479c2058 100644 --- a/src/modules/systemlib/airspeed.c +++ b/src/modules/systemlib/airspeed.c @@ -59,7 +59,7 @@ float calc_indicated_airspeed(float differential_pressure) { - if (differential_pressure > 0) { + if (differential_pressure > 0.0f) { return sqrtf((2.0f*differential_pressure) / CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C); } else { return -sqrtf((2.0f*fabsf(differential_pressure)) / CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C); diff --git a/src/modules/uORB/topics/differential_pressure.h b/src/modules/uORB/topics/differential_pressure.h index e4d2c92ceb..5d94d42883 100644 --- a/src/modules/uORB/topics/differential_pressure.h +++ b/src/modules/uORB/topics/differential_pressure.h @@ -54,8 +54,8 @@ struct differential_pressure_s { uint64_t timestamp; /**< microseconds since system boot, needed to integrate */ uint64_t error_count; - uint16_t differential_pressure_pa; /**< Differential pressure reading */ - uint16_t max_differential_pressure_pa; /**< Maximum differential pressure reading */ + float differential_pressure_pa; /**< Differential pressure reading */ + float max_differential_pressure_pa; /**< Maximum differential pressure reading */ float voltage; /**< Voltage from analog airspeed sensors (voltage divider already compensated) */ float temperature; /**< Temperature provided by sensor */ From 38172497c116548320c74696d795f9198e0bf4e4 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 13 Nov 2013 11:05:22 +0100 Subject: [PATCH 106/447] reintroduce feedforward --- src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp | 3 ++- src/lib/ecl/attitude_fw/ecl_pitch_controller.h | 10 ++++++++++ src/lib/ecl/attitude_fw/ecl_roll_controller.cpp | 3 ++- src/lib/ecl/attitude_fw/ecl_roll_controller.h | 10 ++++++++++ src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp | 3 ++- src/lib/ecl/attitude_fw/ecl_yaw_controller.h | 13 ++++++++++++- .../fw_att_control/fw_att_control_main.cpp | 16 ++++++++++++++++ .../fw_att_control/fw_att_control_params.c | 6 +++++- 8 files changed, 59 insertions(+), 5 deletions(-) diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp index ccaed14cea..d7dbbebd42 100644 --- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp @@ -53,6 +53,7 @@ ECL_PitchController::ECL_PitchController() : _k_p(0.0f), _k_i(0.0f), _k_d(0.0f), + _k_ff(0.0f), _integrator_max(0.0f), _max_rate_pos(0.0f), _max_rate_neg(0.0f), @@ -173,7 +174,7 @@ float ECL_PitchController::control_bodyrate(float roll, float pitch, float integrator_constrained = math::constrain(_integrator * _k_i, -_integrator_max, _integrator_max); /* Apply PI rate controller and store non-limited output */ - _last_output = (_rate_error * _k_p + integrator_constrained + _rate_setpoint * k_ff) * scaler * scaler; //scaler is proportional to 1/airspeed + _last_output = (_bodyrate_setpoint * _k_ff +_rate_error * _k_p + integrator_constrained + _rate_setpoint * k_ff) * scaler * scaler; //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 index ba8ed3e6f2..2ca0490fdc 100644 --- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.h +++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.h @@ -73,21 +73,30 @@ public: void set_k_p(float k_p) { _k_p = k_p; } + void set_k_i(float k_i) { _k_i = k_i; } void set_k_d(float k_d) { _k_d = k_d; } + + void set_k_ff(float k_ff) { + _k_ff = k_ff; + } + void set_integrator_max(float max) { _integrator_max = max; } + void set_max_rate_pos(float max_rate_pos) { _max_rate_pos = 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; } @@ -111,6 +120,7 @@ private: float _k_p; float _k_i; float _k_d; + float _k_ff; float _integrator_max; float _max_rate_pos; float _max_rate_neg; diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp index 4b0bfc6c42..bd6c9da712 100644 --- a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp @@ -53,6 +53,7 @@ ECL_RollController::ECL_RollController() : _k_p(0.0f), _k_i(0.0f), _k_d(0.0f), + _k_ff(0.0f), _integrator_max(0.0f), _max_rate(0.0f), _last_output(0.0f), @@ -141,7 +142,7 @@ float ECL_RollController::control_bodyrate(float pitch, //warnx("roll: _integrator: %.4f, _integrator_max: %.4f", (double)_integrator, (double)_integrator_max); /* Apply PI rate controller and store non-limited output */ - _last_output = (_rate_error * _k_p + integrator_constrained + _rate_setpoint * k_ff) * scaler * scaler; //scaler is proportional to 1/airspeed + _last_output = (_bodyrate_setpoint * _k_ff + _rate_error * _k_p + integrator_constrained + _rate_setpoint * k_ff) * scaler * scaler; //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 index dd7d1bf534..efc7b89441 100644 --- a/src/lib/ecl/attitude_fw/ecl_roll_controller.h +++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.h @@ -71,18 +71,27 @@ public: _tc = time_constant; } } + void set_k_p(float k_p) { _k_p = k_p; } + void set_k_i(float k_i) { _k_i = k_i; } + void set_k_d(float k_d) { _k_d = k_d; } + + void set_k_ff(float k_ff) { + _k_ff = k_ff; + } + void set_integrator_max(float max) { _integrator_max = max; } + void set_max_rate(float max_rate) { _max_rate = max_rate; } @@ -105,6 +114,7 @@ private: float _k_p; float _k_i; float _k_d; + float _k_ff; float _integrator_max; float _max_rate; float _last_output; diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp index a4ecc48a28..7c366aaf2d 100644 --- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp @@ -51,6 +51,7 @@ ECL_YawController::ECL_YawController() : _k_p(0.0f), _k_i(0.0f), _k_d(0.0f), + _k_ff(0.0f), _integrator_max(0.0f), _max_rate(0.0f), _roll_ff(0.0f), @@ -159,7 +160,7 @@ float ECL_YawController::control_bodyrate(float roll, float pitch, float integrator_constrained = math::constrain(_integrator * _k_i, -_integrator_max, _integrator_max); /* Apply PI rate controller and store non-limited output */ - _last_output = (_rate_error * _k_p + integrator_constrained + _rate_setpoint * k_ff) * scaler * scaler; //scaler is proportional to 1/airspeed + _last_output = (_bodyrate_setpoint * _k_ff + _rate_error * _k_p + integrator_constrained + _rate_setpoint * k_ff) * scaler * 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); diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.h b/src/lib/ecl/attitude_fw/ecl_yaw_controller.h index b5ae1e6afc..f15645fcf6 100644 --- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.h +++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.h @@ -69,22 +69,32 @@ public: void set_k_p(float k_p) { _k_p = k_p; - } + } + void set_k_i(float k_i) { _k_i = k_i; } + void set_k_d(float k_d) { _k_d = k_d; } + + void set_k_ff(float k_ff) { + _k_ff = k_ff; + } + void set_integrator_max(float max) { _integrator_max = max; } + void set_max_rate(float max_rate) { _max_rate = max_rate; } + void set_k_roll_ff(float roll_ff) { _roll_ff = roll_ff; } + void set_coordinated_min_speed(float coordinated_min_speed) { _coordinated_min_speed = coordinated_min_speed; } @@ -107,6 +117,7 @@ private: float _k_p; float _k_i; float _k_d; + float _k_ff; float _integrator_max; float _max_rate; float _roll_ff; diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index a5f3f1d91d..53f89c7c4a 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -141,6 +141,7 @@ private: float p_p; float p_d; float p_i; + float p_ff; float p_rmax_pos; float p_rmax_neg; float p_integrator_max; @@ -148,11 +149,13 @@ private: float r_p; float r_d; float r_i; + float r_ff; float r_integrator_max; float r_rmax; float y_p; float y_i; float y_d; + float y_ff; float y_roll_feedforward; float y_integrator_max; float y_coordinated_min_speed; @@ -169,6 +172,7 @@ private: param_t p_p; param_t p_d; param_t p_i; + param_t p_ff; param_t p_rmax_pos; param_t p_rmax_neg; param_t p_integrator_max; @@ -176,11 +180,13 @@ private: param_t r_p; param_t r_d; param_t r_i; + param_t r_ff; param_t r_integrator_max; param_t r_rmax; param_t y_p; param_t y_i; param_t y_d; + param_t y_ff; param_t y_roll_feedforward; param_t y_integrator_max; param_t y_coordinated_min_speed; @@ -302,6 +308,7 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : _parameter_handles.p_p = param_find("FW_PR_P"); _parameter_handles.p_d = param_find("FW_PR_D"); _parameter_handles.p_i = param_find("FW_PR_I"); + _parameter_handles.p_ff = param_find("FW_PR_FF"); _parameter_handles.p_rmax_pos = param_find("FW_P_RMAX_POS"); _parameter_handles.p_rmax_neg = param_find("FW_P_RMAX_NEG"); _parameter_handles.p_integrator_max = param_find("FW_PR_IMAX"); @@ -310,12 +317,14 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : _parameter_handles.r_p = param_find("FW_RR_P"); _parameter_handles.r_d = param_find("FW_RR_D"); _parameter_handles.r_i = param_find("FW_RR_I"); + _parameter_handles.r_ff = param_find("FW_RR_FF"); _parameter_handles.r_integrator_max = param_find("FW_RR_IMAX"); _parameter_handles.r_rmax = param_find("FW_R_RMAX"); _parameter_handles.y_p = param_find("FW_YR_P"); _parameter_handles.y_i = param_find("FW_YR_I"); _parameter_handles.y_d = param_find("FW_YR_D"); + _parameter_handles.y_ff = param_find("FW_YR_FF"); _parameter_handles.y_roll_feedforward = param_find("FW_Y_ROLLFF"); _parameter_handles.y_integrator_max = param_find("FW_YR_IMAX"); _parameter_handles.y_rmax = param_find("FW_Y_RMAX"); @@ -363,6 +372,7 @@ FixedwingAttitudeControl::parameters_update() param_get(_parameter_handles.p_p, &(_parameters.p_p)); param_get(_parameter_handles.p_d, &(_parameters.p_d)); param_get(_parameter_handles.p_i, &(_parameters.p_i)); + param_get(_parameter_handles.p_ff, &(_parameters.p_ff)); param_get(_parameter_handles.p_rmax_pos, &(_parameters.p_rmax_pos)); param_get(_parameter_handles.p_rmax_neg, &(_parameters.p_rmax_neg)); param_get(_parameter_handles.p_integrator_max, &(_parameters.p_integrator_max)); @@ -371,12 +381,15 @@ FixedwingAttitudeControl::parameters_update() param_get(_parameter_handles.r_p, &(_parameters.r_p)); param_get(_parameter_handles.r_d, &(_parameters.r_d)); param_get(_parameter_handles.r_i, &(_parameters.r_i)); + param_get(_parameter_handles.r_ff, &(_parameters.r_ff)); + param_get(_parameter_handles.r_integrator_max, &(_parameters.r_integrator_max)); param_get(_parameter_handles.r_rmax, &(_parameters.r_rmax)); param_get(_parameter_handles.y_p, &(_parameters.y_p)); param_get(_parameter_handles.y_i, &(_parameters.y_i)); param_get(_parameter_handles.y_d, &(_parameters.y_d)); + param_get(_parameter_handles.y_ff, &(_parameters.y_ff)); param_get(_parameter_handles.y_roll_feedforward, &(_parameters.y_roll_feedforward)); param_get(_parameter_handles.y_integrator_max, &(_parameters.y_integrator_max)); param_get(_parameter_handles.y_coordinated_min_speed, &(_parameters.y_coordinated_min_speed)); @@ -391,6 +404,7 @@ FixedwingAttitudeControl::parameters_update() _pitch_ctrl.set_k_p(_parameters.p_p); _pitch_ctrl.set_k_i(_parameters.p_i); _pitch_ctrl.set_k_d(_parameters.p_d); + _pitch_ctrl.set_k_ff(_parameters.p_ff); _pitch_ctrl.set_integrator_max(_parameters.p_integrator_max); _pitch_ctrl.set_max_rate_pos(math::radians(_parameters.p_rmax_pos)); _pitch_ctrl.set_max_rate_neg(math::radians(_parameters.p_rmax_neg)); @@ -401,6 +415,7 @@ FixedwingAttitudeControl::parameters_update() _roll_ctrl.set_k_p(_parameters.r_p); _roll_ctrl.set_k_i(_parameters.r_i); _roll_ctrl.set_k_d(_parameters.r_d); + _roll_ctrl.set_k_ff(_parameters.r_ff); _roll_ctrl.set_integrator_max(_parameters.r_integrator_max); _roll_ctrl.set_max_rate(math::radians(_parameters.r_rmax)); @@ -408,6 +423,7 @@ FixedwingAttitudeControl::parameters_update() _yaw_ctrl.set_k_p(_parameters.y_p); _yaw_ctrl.set_k_i(_parameters.y_i); _yaw_ctrl.set_k_d(_parameters.y_d); + _yaw_ctrl.set_k_ff(_parameters.y_ff); _yaw_ctrl.set_k_roll_ff(_parameters.y_roll_feedforward); _yaw_ctrl.set_integrator_max(_parameters.y_integrator_max); _yaw_ctrl.set_coordinated_min_speed(_parameters.y_coordinated_min_speed); diff --git a/src/modules/fw_att_control/fw_att_control_params.c b/src/modules/fw_att_control/fw_att_control_params.c index 796ab3f906..240749fed2 100644 --- a/src/modules/fw_att_control/fw_att_control_params.c +++ b/src/modules/fw_att_control/fw_att_control_params.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013 PX4 Development Team. All rights reserved. +f * Copyright (c) 2013 PX4 Development Team. All rights reserved. * Author: Lorenz Meier * * Redistribution and use in source and binary forms, with or without @@ -138,3 +138,7 @@ PARAM_DEFINE_FLOAT(FW_AIRSPD_TRIM, 12.0f); PARAM_DEFINE_FLOAT(FW_AIRSPD_MAX, 18.0f); PARAM_DEFINE_FLOAT(FW_Y_RMAX, 60); PARAM_DEFINE_FLOAT(FW_YCO_VMIN, 1.0f); + +PARAM_DEFINE_FLOAT(FW_RR_FF, 0.0f); +PARAM_DEFINE_FLOAT(FW_PR_FF, 0.0f); +PARAM_DEFINE_FLOAT(FW_YR_FF, 0.0f); From cfaf0ada45a9b99a83221c6c51a50090c55a1483 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 13 Nov 2013 23:14:05 +0400 Subject: [PATCH 107/447] Heading in VFR message fixed --- src/modules/mavlink/orb_listener.c | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/modules/mavlink/orb_listener.c b/src/modules/mavlink/orb_listener.c index abc91d34fa..2804a8191f 100644 --- a/src/modules/mavlink/orb_listener.c +++ b/src/modules/mavlink/orb_listener.c @@ -54,6 +54,7 @@ #include #include #include +#include #include @@ -248,7 +249,7 @@ l_vehicle_attitude(const struct listener *l) if (t >= last_sent_vfr + 100000) { last_sent_vfr = t; float groundspeed = sqrtf(global_pos.vx * global_pos.vx + global_pos.vy * global_pos.vy); - uint16_t heading = (att.yaw + M_PI_F) / M_PI_F * 180.0f; + uint16_t heading = _wrap_2pi(att.yaw) / M_PI_F * 180.0f; float throttle = actuators_effective_0.control_effective[3] * (UINT16_MAX - 1); mavlink_msg_vfr_hud_send(MAVLINK_COMM_0, airspeed.true_airspeed_m_s, groundspeed, heading, throttle, global_pos.alt, -global_pos.vz); } From e7f4d91022af363905b758ab39bde1addcb69517 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 13 Nov 2013 23:15:02 +0400 Subject: [PATCH 108/447] geo: cleanup of wrap_XXX functions --- src/lib/geo/geo.c | 54 +++++++++++++++++++++++++++++++---------------- 1 file changed, 36 insertions(+), 18 deletions(-) diff --git a/src/lib/geo/geo.c b/src/lib/geo/geo.c index 43105fdba7..9e533b92d8 100644 --- a/src/lib/geo/geo.c +++ b/src/lib/geo/geo.c @@ -390,22 +390,22 @@ __EXPORT int get_distance_to_arc(struct crosstrack_error_s * crosstrack_error, d __EXPORT float _wrap_pi(float bearing) { /* value is inf or NaN */ - if (!isfinite(bearing) || bearing == 0) { + if (!isfinite(bearing)) { return bearing; } int c = 0; - - while (bearing > M_PI_F && c < 30) { + while (bearing > M_PI_F) { bearing -= M_TWOPI_F; - c++; + if (c++ > 3) + return NAN; } c = 0; - - while (bearing <= -M_PI_F && c < 30) { + while (bearing <= -M_PI_F) { bearing += M_TWOPI_F; - c++; + if (c++ > 3) + return NAN; } return bearing; @@ -418,12 +418,18 @@ __EXPORT float _wrap_2pi(float bearing) return bearing; } - while (bearing >= M_TWOPI_F) { - bearing = bearing - M_TWOPI_F; + int c = 0; + while (bearing > M_TWOPI_F) { + bearing -= M_TWOPI_F; + if (c++ > 3) + return NAN; } - while (bearing < 0.0f) { - bearing = bearing + M_TWOPI_F; + c = 0; + while (bearing <= 0.0f) { + bearing += M_TWOPI_F; + if (c++ > 3) + return NAN; } return bearing; @@ -436,12 +442,18 @@ __EXPORT float _wrap_180(float bearing) return bearing; } + int c = 0; while (bearing > 180.0f) { - bearing = bearing - 360.0f; + bearing -= 360.0f; + if (c++ > 3) + return NAN; } - while (bearing <= -180.0f) { - bearing = bearing + 360.0f; + c = 0; + while (bearing <= -180.0f) { + bearing += 360.0f; + if (c++ > 3) + return NAN; } return bearing; @@ -454,12 +466,18 @@ __EXPORT float _wrap_360(float bearing) return bearing; } - while (bearing >= 360.0f) { - bearing = bearing - 360.0f; + int c = 0; + while (bearing > 360.0f) { + bearing -= 360.0f; + if (c++ > 3) + return NAN; } - while (bearing < 0.0f) { - bearing = bearing + 360.0f; + c = 0; + while (bearing <= 0.0f) { + bearing += 360.0f; + if (c++ > 3) + return NAN; } return bearing; From c31d065340a44ef121069b5b82395526b910d3e8 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 14 Nov 2013 11:59:18 +0400 Subject: [PATCH 109/447] Revert "Heading in VFR message fixed" This reverts commit cfaf0ada45a9b99a83221c6c51a50090c55a1483. --- src/modules/mavlink/orb_listener.c | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/modules/mavlink/orb_listener.c b/src/modules/mavlink/orb_listener.c index 2804a8191f..abc91d34fa 100644 --- a/src/modules/mavlink/orb_listener.c +++ b/src/modules/mavlink/orb_listener.c @@ -54,7 +54,6 @@ #include #include #include -#include #include @@ -249,7 +248,7 @@ l_vehicle_attitude(const struct listener *l) if (t >= last_sent_vfr + 100000) { last_sent_vfr = t; float groundspeed = sqrtf(global_pos.vx * global_pos.vx + global_pos.vy * global_pos.vy); - uint16_t heading = _wrap_2pi(att.yaw) / M_PI_F * 180.0f; + uint16_t heading = (att.yaw + M_PI_F) / M_PI_F * 180.0f; float throttle = actuators_effective_0.control_effective[3] * (UINT16_MAX - 1); mavlink_msg_vfr_hud_send(MAVLINK_COMM_0, airspeed.true_airspeed_m_s, groundspeed, heading, throttle, global_pos.alt, -global_pos.vz); } From f337d62f2da3c4765acc8b88a4cef3381a89c6e7 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 14 Nov 2013 14:36:52 +0100 Subject: [PATCH 110/447] fw attitude: fix handling of invalid airspeed --- src/modules/fw_att_control/fw_att_control_main.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index 53f89c7c4a..ff3f133065 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -635,9 +635,9 @@ FixedwingAttitudeControl::task_main() /* if airspeed is smaller than min, the sensor is not giving good readings */ if (!_airspeed_valid || - (_airspeed.indicated_airspeed_m_s < _parameters.airspeed_min) || + (_airspeed.indicated_airspeed_m_s < 0.1f * _parameters.airspeed_min) || !isfinite(_airspeed.indicated_airspeed_m_s)) { - airspeed = _parameters.airspeed_min + (_parameters.airspeed_max - _parameters.airspeed_min) / 2.0f; + airspeed = _parameters.airspeed_trim; } else { airspeed = _airspeed.indicated_airspeed_m_s; From ebbe4d2235452fd77575bc084bb519987e566ea9 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 15 Nov 2013 22:43:07 +0100 Subject: [PATCH 111/447] initial wip version of launchdetector --- makefiles/config_px4fmu-v1_default.mk | 1 + makefiles/config_px4fmu-v2_default.mk | 1 + .../launchdetection/CatapultLaunchMethod.cpp | 85 +++++++++++++++++ .../launchdetection/CatapultLaunchMethod.h | 71 ++++++++++++++ src/lib/launchdetection/LaunchDetector.cpp | 94 +++++++++++++++++++ src/lib/launchdetection/LaunchDetector.h | 72 ++++++++++++++ src/lib/launchdetection/LaunchMethod.h | 54 +++++++++++ .../launchdetection/launchdetection_params.c | 67 +++++++++++++ src/lib/launchdetection/module.mk | 40 ++++++++ .../fw_pos_control_l1_main.cpp | 61 ++++++++---- 10 files changed, 529 insertions(+), 17 deletions(-) create mode 100644 src/lib/launchdetection/CatapultLaunchMethod.cpp create mode 100644 src/lib/launchdetection/CatapultLaunchMethod.h create mode 100644 src/lib/launchdetection/LaunchDetector.cpp create mode 100644 src/lib/launchdetection/LaunchDetector.h create mode 100644 src/lib/launchdetection/LaunchMethod.h create mode 100644 src/lib/launchdetection/launchdetection_params.c create mode 100644 src/lib/launchdetection/module.mk diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk index 3068270865..b3ce02f017 100644 --- a/makefiles/config_px4fmu-v1_default.mk +++ b/makefiles/config_px4fmu-v1_default.mk @@ -116,6 +116,7 @@ MODULES += lib/ecl MODULES += lib/external_lgpl MODULES += lib/geo MODULES += lib/conversion +MODULES += lib/launchdetection # # Demo apps diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index 761fb8d9d6..17fca68b99 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -113,6 +113,7 @@ MODULES += lib/ecl MODULES += lib/external_lgpl MODULES += lib/geo MODULES += lib/conversion +MODULES += lib/launchdetection # # Demo apps diff --git a/src/lib/launchdetection/CatapultLaunchMethod.cpp b/src/lib/launchdetection/CatapultLaunchMethod.cpp new file mode 100644 index 0000000000..0a95b46f60 --- /dev/null +++ b/src/lib/launchdetection/CatapultLaunchMethod.cpp @@ -0,0 +1,85 @@ +/**************************************************************************** + * + * 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 documentation4 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 CatapultLaunchMethod.cpp + * Catpult Launch detection + * + * Authors and acknowledgements in header. + */ + +#include "CatapultLaunchMethod.h" + +CatapultLaunchMethod::CatapultLaunchMethod() : + last_timestamp(0), + integrator(0.0f), + launchDetected(false), + threshold_accel(NULL, "LAUN_CAT_A", false), + threshold_time(NULL, "LAUN_CAT_T", false) +{ + +} + +CatapultLaunchMethod::~CatapultLaunchMethod() { + +} + +void CatapultLaunchMethod::update(float accel_x) +{ + float dt = (float)hrt_elapsed_time(&last_timestamp) * 1e-6f; + last_timestamp = hrt_absolute_time(); + + if (accel_x > threshold_accel.get()) { + integrator += accel_x * dt; + if (integrator > threshold_accel.get() * threshold_time.get()) { + launchDetected = true; + } + + } else { + /* reset integrator */ + integrator = 0.0f; + launchDetected = false; + } + +} + +bool CatapultLaunchMethod::getLaunchDetected() +{ + return launchDetected; +} + +void CatapultLaunchMethod::updateParams() +{ + threshold_accel.update(); + threshold_time.update(); +} diff --git a/src/lib/launchdetection/CatapultLaunchMethod.h b/src/lib/launchdetection/CatapultLaunchMethod.h new file mode 100644 index 0000000000..e943f11e9d --- /dev/null +++ b/src/lib/launchdetection/CatapultLaunchMethod.h @@ -0,0 +1,71 @@ +/**************************************************************************** + * + * 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 documentation4 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 CatapultLaunchMethod.h + * Catpult Launch detection + * + * @author Thomas Gubler + */ + +#ifndef CATAPULTLAUNCHMETHOD_H_ +#define CATAPULTLAUNCHMETHOD_H_ + +#include "LaunchMethod.h" + +#include +#include + +class CatapultLaunchMethod : public LaunchMethod +{ +public: + CatapultLaunchMethod(); + ~CatapultLaunchMethod(); + + void update(float accel_x); + bool getLaunchDetected(); + void updateParams(); + +private: + hrt_abstime last_timestamp; +// float threshold_accel_raw; +// float threshold_time; + float integrator; + bool launchDetected; + + control::BlockParamFloat threshold_accel; + control::BlockParamFloat threshold_time; + +}; + +#endif /* CATAPULTLAUNCHMETHOD_H_ */ diff --git a/src/lib/launchdetection/LaunchDetector.cpp b/src/lib/launchdetection/LaunchDetector.cpp new file mode 100644 index 0000000000..2545e0a7e2 --- /dev/null +++ b/src/lib/launchdetection/LaunchDetector.cpp @@ -0,0 +1,94 @@ +/**************************************************************************** + * + * 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 launchDetection.cpp + * Auto Detection for different launch methods (e.g. catapult) + * + * Authors and acknowledgements in header. + */ + +#include "LaunchDetector.h" +#include "CatapultLaunchMethod.h" +#include + +LaunchDetector::LaunchDetector() : + launchdetection_on(NULL, "LAUN_ALL_ON", false) +{ + /* init all detectors */ + launchMethods[0] = new CatapultLaunchMethod(); + + + /* update all parameters of all detectors */ + updateParams(); +} + +LaunchDetector::~LaunchDetector() +{ + +} + +void LaunchDetector::update(float accel_x) +{ + if (launchdetection_on.get() == 1) { + for (uint8_t i = 0; i < sizeof(launchMethods)/sizeof(LaunchMethod); i++) { + launchMethods[i]->update(accel_x); + } + } +} + +bool LaunchDetector::getLaunchDetected() +{ + if (launchdetection_on.get() == 1) { + for (uint8_t i = 0; i < sizeof(launchMethods)/sizeof(LaunchMethod); i++) { + if(launchMethods[i]->getLaunchDetected()) { + return true; + } + } + } + + return false; +} + +void LaunchDetector::updateParams() { + + warnx(" LaunchDetector::updateParams()"); + //launchdetection_on.update(); + + for (uint8_t i = 0; i < sizeof(launchMethods)/sizeof(LaunchMethod); i++) { + //launchMethods[i]->updateParams(); + warnx("updating component %d", i); + } + + warnx(" LaunchDetector::updateParams() ended"); +} diff --git a/src/lib/launchdetection/LaunchDetector.h b/src/lib/launchdetection/LaunchDetector.h new file mode 100644 index 0000000000..9818911437 --- /dev/null +++ b/src/lib/launchdetection/LaunchDetector.h @@ -0,0 +1,72 @@ +/**************************************************************************** + * + * 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 documentation4 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 LaunchDetector.h + * Auto Detection for different launch methods (e.g. catapult) + * + * @author Thomas Gubler + */ + +#ifndef LAUNCHDETECTOR_H +#define LAUNCHDETECTOR_H + +#include +#include + +#include "LaunchMethod.h" + +#include + +class __EXPORT LaunchDetector +{ +public: + LaunchDetector(); + ~LaunchDetector(); + + void update(float accel_x); + bool getLaunchDetected(); + void updateParams(); + bool launchDetectionEnabled() { return (bool)launchdetection_on.get(); }; + +// virtual bool getLaunchDetected(); +protected: +private: + LaunchMethod* launchMethods[1]; + control::BlockParamInt launchdetection_on; + + +}; + + +#endif // LAUNCHDETECTOR_H diff --git a/src/lib/launchdetection/LaunchMethod.h b/src/lib/launchdetection/LaunchMethod.h new file mode 100644 index 0000000000..bfb5f8cb4c --- /dev/null +++ b/src/lib/launchdetection/LaunchMethod.h @@ -0,0 +1,54 @@ +/**************************************************************************** + * + * 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 documentation4 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 LaunchMethod.h + * Base class for different launch methods + * + * @author Thomas Gubler + */ + +#ifndef LAUNCHMETHOD_H_ +#define LAUNCHMETHOD_H_ + +class LaunchMethod +{ +public: + virtual void update(float accel_x) = 0; + virtual bool getLaunchDetected() = 0; + virtual void updateParams() = 0; +protected: +private: +}; + +#endif /* LAUNCHMETHOD_H_ */ diff --git a/src/lib/launchdetection/launchdetection_params.c b/src/lib/launchdetection/launchdetection_params.c new file mode 100644 index 0000000000..536749c88c --- /dev/null +++ b/src/lib/launchdetection/launchdetection_params.c @@ -0,0 +1,67 @@ +/**************************************************************************** + * + * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * Author: Lorenz Meier + * + * 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 fw_pos_control_l1_params.c + * + * Parameters defined by the L1 position control task + * + * @author Lorenz Meier + */ + +#include + +#include + +/* + * Launch detection parameters, accessible via MAVLink + * + */ + +/* Catapult Launch detection */ + +// @DisplayName Switch to enable launchdetection +// @Description if set to 1 launchdetection is enabled +// @Range 0 or 1 +PARAM_DEFINE_INT32(LAUN_ALL_ON, 0); + +// @DisplayName Catapult Accelerometer Threshold +// @Description LAUN_CAT_A * LAUN_CAT_T serves as threshold to trigger launch detection +// @Range > 0 +PARAM_DEFINE_FLOAT(LAUN_CAT_A, 30.0f); + +// @DisplayName Catapult Time Threshold +// @Description LAUN_CAT_A * LAUN_CAT_T serves as threshold to trigger launch detection +// @Range > 0, in seconds +PARAM_DEFINE_FLOAT(LAUN_CAT_T, 0.05f); diff --git a/src/lib/launchdetection/module.mk b/src/lib/launchdetection/module.mk new file mode 100644 index 0000000000..13648b74ca --- /dev/null +++ b/src/lib/launchdetection/module.mk @@ -0,0 +1,40 @@ +############################################################################ +# +# 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 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. +# +############################################################################ + +# +# Launchdetection Library +# + +SRCS = LaunchDetector.cpp \ + CatapultLaunchMethod.cpp \ + launchdetection_params.c diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index a9648b207d..99428ea50a 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -86,6 +86,7 @@ #include #include +#include /** * L1 control app start / stop handling function @@ -164,6 +165,9 @@ private: /* heading hold */ float target_bearing; + /* Launch detection */ + LaunchDetector launchDetector; + /* throttle and airspeed states */ float _airspeed_error; ///< airspeed error to setpoint in m/s bool _airspeed_valid; ///< flag if a valid airspeed estimate exists @@ -338,7 +342,8 @@ FixedwingPositionControl::FixedwingPositionControl() : _airspeed_valid(false), _groundspeed_undershoot(0.0f), _global_pos_valid(false), - land_noreturn(false) + land_noreturn(false), + launchDetector() { _nav_capabilities.turn_distance = 0.0f; @@ -464,6 +469,8 @@ FixedwingPositionControl::parameters_update() return 1; } + launchDetector.updateParams(); + return OK; } @@ -818,30 +825,50 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio } else if (global_triplet.current.nav_cmd == NAV_CMD_TAKEOFF) { + /* Perform launch detection */ + bool do_fly_takeoff = false; + warnx("Launch detection running"); + if (launchDetector.launchDetectionEnabled()) { + launchDetector.update(_accel.x); + if (launchDetector.getLaunchDetected()) { + do_fly_takeoff = true; + warnx("Launch detected. Taking off!"); + } + } else { + /* no takeoff detection --> fly */ + do_fly_takeoff = true; + } + + _l1_control.navigate_waypoints(prev_wp, next_wp, current_position, ground_speed); _att_sp.roll_body = _l1_control.nav_roll(); _att_sp.yaw_body = _l1_control.nav_bearing(); - /* apply minimum pitch and limit roll if target altitude is not within 10 meters */ - if (altitude_error > 10.0f) { + if (do_fly_takeoff) { - /* enforce a minimum of 10 degrees pitch up on takeoff, or take parameter */ - _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_min), - _airspeed.indicated_airspeed_m_s, eas2tas, - true, math::max(math::radians(global_triplet.current.param1), math::radians(10.0f)), - _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, - math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max)); + /* apply minimum pitch and limit roll if target altitude is not within 10 meters */ + if (altitude_error > 10.0f) { - /* limit roll motion to ensure enough lift */ - _att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-15.0f), math::radians(15.0f)); + /* enforce a minimum of 10 degrees pitch up on takeoff, or take parameter */ + _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_min), + _airspeed.indicated_airspeed_m_s, eas2tas, + true, math::max(math::radians(global_triplet.current.param1), math::radians(10.0f)), + _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, + math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max)); + /* limit roll motion to ensure enough lift */ + _att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-15.0f), math::radians(15.0f)); + + } else { + + _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_trim), + _airspeed.indicated_airspeed_m_s, eas2tas, + false, math::radians(_parameters.pitch_limit_min), + _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, + math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max)); + } } else { - - _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_trim), - _airspeed.indicated_airspeed_m_s, eas2tas, - false, math::radians(_parameters.pitch_limit_min), - _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, - math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max)); + throttle_max = 0.0f; } } From fefaab91cf3a67a9d2f66e94fdb2708709635095 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 17 Nov 2013 13:41:14 +0100 Subject: [PATCH 112/447] l1: change a max to min in wp switch-distance calculation --- src/lib/ecl/l1/ecl_l1_pos_controller.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/ecl/l1/ecl_l1_pos_controller.cpp b/src/lib/ecl/l1/ecl_l1_pos_controller.cpp index 27d76f959c..31d7cecb7b 100644 --- a/src/lib/ecl/l1/ecl_l1_pos_controller.cpp +++ b/src/lib/ecl/l1/ecl_l1_pos_controller.cpp @@ -70,7 +70,7 @@ float ECL_L1_Pos_Controller::target_bearing() float ECL_L1_Pos_Controller::switch_distance(float wp_radius) { /* following [2], switching on L1 distance */ - return math::max(wp_radius, _L1_distance); + return math::min(wp_radius, _L1_distance); } bool ECL_L1_Pos_Controller::reached_loiter_target(void) From 178b9b0a697eb0325689b271f8ffbba528dede14 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 17 Nov 2013 23:03:19 +0400 Subject: [PATCH 113/447] mc_att_control_vector: initial rewrite, WIP --- .../ecl_mc_att_control_vector.cpp | 147 ------------- .../ecl_mc_att_control_vector.h | 76 ------- .../mc_att_control_vector_main.cpp | 200 ++++++------------ src/modules/mc_att_control_vector/module.mk | 3 +- 4 files changed, 64 insertions(+), 362 deletions(-) delete mode 100644 src/modules/mc_att_control_vector/ecl_mc_att_control_vector.cpp delete mode 100644 src/modules/mc_att_control_vector/ecl_mc_att_control_vector.h diff --git a/src/modules/mc_att_control_vector/ecl_mc_att_control_vector.cpp b/src/modules/mc_att_control_vector/ecl_mc_att_control_vector.cpp deleted file mode 100644 index 589c31c0d6..0000000000 --- a/src/modules/mc_att_control_vector/ecl_mc_att_control_vector.cpp +++ /dev/null @@ -1,147 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2013 PX4 Development Team. All rights reserved. - * Author: Tobias Naegeli - * - * 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 ecl_mc_att_control_vector.cpp - * - * Multirotor attitude controller based on concepts in: - * - * Minimum Snap Trajectory Generation and Control for Quadrotors - * http://www.seas.upenn.edu/~dmel/mellingerICRA11.pdf - * - * @author Tobias Naegeli - * @author Lorenz Meier - * - */ - -#include -#include "ecl_mc_att_control_vector.h" - -ECL_MCAttControlVector::ECL_MCAttControlVector() : - _integral_error(0.0f, 0.0f), - _integral_max(1000.0f, 1000.0f), - _integral_lock(false) - { - - } - -void ECL_MCAttControlVector::control(float dt, const math::Dcm &R_nb, float yaw, const math::Vector &F_des_in, - float Kp, float Kd, float Ki, const math::Vector &angular_rates, - math::Vector &rates_des, float &thrust) -{ - // XXX - bool earth = true; - bool integral_reset = false; - - math::Matrix R_bn = R_nb.transpose(); - - float cy = cosf(yaw); - float sy = sinf(yaw); - - math::Matrix RYaw = math::Dcm(cy,-sy,0.0f,sy,cy,0.0f,0.0f,0.0f,1.0f); - math::Vector z_b = math::Vector3(R_bn(0,2), R_bn(1,2), R_bn(2,2)); - - math::Vector3 F_des = F_des_in; - - // desired thrust in body frame - // avoid division by zero - // compensates for thrust loss due to roll/pitch - //XXX disable this for first time - //if (F_des(2) >= 0.1f) { - // thrust = F_des(2) / R_bn(2, 2); - //} else { - // F_des(2) = 0.1f; - // thrust= F_des(2) / R_bn(2, 2); - //} - - math::Vector3 x_B_des; - math::Vector3 y_B_des; - math::Vector3 z_B_des; - - // desired body z axis - if (earth) { - z_B_des = (F_des / F_des.norm()); - } else { - z_B_des = RYaw * (F_des / F_des.norm()); - } - - // desired direction in world coordinates (yaw angle) - math::Vector3 x_C(cy, sy, 0.0f); - // desired body y axis - y_B_des = z_B_des.cross(x_C) / (z_B_des.cross(x_C)).norm(); - // desired body x axis - x_B_des = y_B_des.cross(z_B_des); - // desired Rotation Matrix - math::Matrix R_des = math::Dcm(x_B_des(0), x_B_des(1), x_B_des(2), - y_B_des(0), y_B_des(1), y_B_des(2), - z_B_des(0), z_B_des(1), z_B_des(2)); - - - // Attitude Controller - // P controller - - // error rotation matrix - math::Matrix e_R = (R_des.transpose() * R_bn - R_bn.transpose() * R_des) * 0.5f; - - // error rotation vector - math::Vector e_R_v(3); - e_R_v(0) = e_R(1,2); - e_R_v(1) = e_R(0,2); - e_R_v(2) = e_R(0,1); - - - // include an integral part - math::Vector intError = math::Vector3(0.0f, 0.0f, 0.0f); - if (!_integral_lock) { - if (thrust > 0.3f && !integral_reset) { - if (fabsf(_integral_error(0)) < _integral_max(0)) { - _integral_error(0) = _integral_error(0) + e_R_v(0) * dt; - } - - if (fabsf(_integral_error(1)) < _integral_max(1)) { - _integral_error(1) = _integral_error(1) + e_R_v(1) * dt; - } - - } else { - _integral_error(0) = 0.0f; - _integral_error(1) = 0.0f; - } - - intError(0) = _integral_error(0); - intError(1) = _integral_error(1); - intError(2) = 0.0f; - } - - rates_des = (e_R_v * Kp + angular_rates * Kd + intError * Ki); -} diff --git a/src/modules/mc_att_control_vector/ecl_mc_att_control_vector.h b/src/modules/mc_att_control_vector/ecl_mc_att_control_vector.h deleted file mode 100644 index a3440a06b2..0000000000 --- a/src/modules/mc_att_control_vector/ecl_mc_att_control_vector.h +++ /dev/null @@ -1,76 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2013 PX4 Development Team. All rights reserved. - * Author: Tobias Naegeli - * - * 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 ecl_mc_att_control_vector.cpp - * - * Multirotor attitude controller based on concepts in: - * - * Minimum Snap Trajectory Generation and Control for Quadrotors - * http://www.seas.upenn.edu/~dmel/mellingerICRA11.pdf - * - * @author Tobias Naegeli - * @author Lorenz Meier - * - */ - -#include - -class ECL_MCAttControlVector { - -public: - ECL_MCAttControlVector(); - void control(float dt, const math::Dcm &R_nb, float yaw, const math::Vector &F_des, - float Kp, float Kd, float Ki, const math::Vector &angular_rates, - math::Vector &rates_des, float &thrust); - - void set_imax(float integral_max) { - _integral_max(0) = integral_max; - _integral_max(1) = integral_max; - } - - void reset_integral() { - _integral_error(0) = 0.0f; - _integral_error(1) = 0.0f; - } - - void lock_integrator(bool lock) { - _integral_lock = lock; - } - -protected: - math::Vector2f _integral_error; - math::Vector2f _integral_max; - bool _integral_lock; -}; diff --git a/src/modules/mc_att_control_vector/mc_att_control_vector_main.cpp b/src/modules/mc_att_control_vector/mc_att_control_vector_main.cpp index e0b36067c7..1783efc0e0 100644 --- a/src/modules/mc_att_control_vector/mc_att_control_vector_main.cpp +++ b/src/modules/mc_att_control_vector/mc_att_control_vector_main.cpp @@ -1,7 +1,9 @@ /**************************************************************************** * * Copyright (c) 2013 PX4 Development Team. All rights reserved. - * Author: Lorenz Meier + * Author: Tobias Naegeli + * Lorenz Meier + * Anton Babushkin * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -34,13 +36,16 @@ /** * @file mc_att_control_vector_main.c - * Implementation of a multicopter attitude controller based on desired thrust vector. + * Implementation of a multicopter attitude controller based on desired rotation matrix. + * + * References + * [1] Daniel Mellinger and Vijay Kumar, "Minimum Snap Trajectory Generation and Control for Quadrotors", + * http://www.seas.upenn.edu/~dmel/mellingerICRA11.pdf * * @author Tobias Naegeli * @author Lorenz Meier + * @author Anton Babushkin * - * Please refer to the library files for the authors and acknowledgements of - * the used control library functions. */ #include @@ -54,7 +59,6 @@ #include #include #include -#include #include #include #include @@ -72,8 +76,6 @@ #include #include -#include "ecl_mc_att_control_vector.h" - /** * Multicopter attitude control app start / stop handling function * @@ -107,7 +109,6 @@ private: int _control_task; /**< task handle for sensor task */ int _att_sub; /**< vehicle attitude subscription */ - int _accel_sub; /**< accelerometer subscription */ int _att_sp_sub; /**< vehicle attitude setpoint */ int _attitude_sub; /**< raw rc channels data subscription */ int _control_mode_sub; /**< vehicle control mode subscription */ @@ -119,52 +120,27 @@ private: orb_advert_t _actuators_0_pub; /**< actuator control group 0 setpoint */ struct vehicle_attitude_s _att; /**< vehicle attitude */ - struct accel_report _accel; /**< body frame accelerations */ struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */ struct manual_control_setpoint_s _manual; /**< r/c channel data */ struct vehicle_control_mode_s _control_mode; /**< vehicle control mode */ struct actuator_controls_s _actuators; /**< actuator control inputs */ struct actuator_armed_s _arming; /**< actuator arming status */ + struct vehicle_rates_setpoint_s _rates_setpoint; /**< vehicle rates setpoint */ perf_counter_t _loop_perf; /**< loop performance counter */ bool _setpoint_valid; /**< flag if the position control setpoint is valid */ - struct { - float yaw_p; - float yaw_i; - float yaw_d; - float yaw_imax; - - float att_p; - float att_i; - float att_d; - float att_imax; - - float att_rate_p; - - float yaw_rate_p; - } _parameters; /**< local copies of interesting parameters */ + math::Matrix _K; /**< diagonal gain matrix for position error */ + math::Matrix _K_rate; /**< diagonal gain matrix for angular rate error */ struct { - param_t yaw_p; - param_t yaw_i; - param_t yaw_d; - param_t yaw_imax; - param_t att_p; - param_t att_i; - param_t att_d; - param_t att_imax; - param_t att_rate_p; - + param_t yaw_p; param_t yaw_rate_p; } _parameter_handles; /**< handles for interesting parameters */ - - ECL_MCAttControlVector _att_control; - /** * Update our local parameter cache. */ @@ -186,11 +162,6 @@ private: */ void vehicle_manual_poll(); - /** - * Check for accel updates. - */ - void vehicle_accel_poll(); - /** * Check for set triplet updates. */ @@ -231,7 +202,6 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : /* subscriptions */ _att_sub(-1), - _accel_sub(-1), _control_mode_sub(-1), _params_sub(-1), _manual_sub(-1), @@ -244,19 +214,15 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "fw att control")), /* states */ - _setpoint_valid(false) + _setpoint_valid(false), +/* gain matrices */ + _K(3, 3), + _K_rate(3, 3) + { - _parameter_handles.yaw_p = param_find("MC_YAWPOS_P"); - _parameter_handles.yaw_i = param_find("MC_YAWPOS_I"); - _parameter_handles.yaw_d = param_find("MC_YAWPOS_D"); - _parameter_handles.yaw_imax = param_find("MC_YAWPOS_IMAX"); - _parameter_handles.att_p = param_find("MC_ATT_P"); - _parameter_handles.att_i = param_find("MC_ATT_I"); - _parameter_handles.att_d = param_find("MC_ATT_D"); - _parameter_handles.att_imax = param_find("MC_ATT_IMAX"); - _parameter_handles.att_rate_p = param_find("MC_ATTRATE_P"); + _parameter_handles.yaw_p = param_find("MC_YAWPOS_P"); _parameter_handles.yaw_rate_p = param_find("MC_YAWRATE_P"); /* fetch initial parameter values */ @@ -291,29 +257,22 @@ MulticopterAttitudeControl::~MulticopterAttitudeControl() int MulticopterAttitudeControl::parameters_update() { + float att_p; + float att_rate_p; + float yaw_p; + float yaw_rate_p; - //param_get(_parameter_handles.tconst, &(_parameters.tconst)); + param_get(_parameter_handles.att_p, &att_p); + param_get(_parameter_handles.att_rate_p, &att_rate_p); + param_get(_parameter_handles.yaw_p, &yaw_p); + param_get(_parameter_handles.yaw_rate_p, &yaw_rate_p); - param_get(_parameter_handles.yaw_p, &(_parameters.yaw_p)); - param_get(_parameter_handles.yaw_i, &(_parameters.yaw_i)); - param_get(_parameter_handles.yaw_d, &(_parameters.yaw_d)); - param_get(_parameter_handles.yaw_imax, &(_parameters.yaw_imax)); - - param_get(_parameter_handles.att_p, &(_parameters.att_p)); - param_get(_parameter_handles.att_i, &(_parameters.att_i)); - param_get(_parameter_handles.att_d, &(_parameters.att_d)); - param_get(_parameter_handles.att_imax, &(_parameters.att_imax)); - - param_get(_parameter_handles.yaw_rate_p, &(_parameters.yaw_rate_p)); - - param_get(_parameter_handles.att_rate_p, &(_parameters.att_rate_p)); - - /* att control parameters */ - _att_control.set_imax(_parameters.att_imax); - //_att_control.set_tau(_parameters.p_tconst); - // _att_control.set_k_p(math::radians(_parameters.p_p)); - // _att_control.set_k_i(math::radians(_parameters.p_i)); - // _att_control.set_k_d(math::radians(_parameters.p_d)); + _K(0, 0) = att_p; + _K(1, 1) = att_p; + _K(2, 2) = yaw_p; + _K_rate(0, 0) = att_rate_p; + _K_rate(1, 1) = att_rate_p; + _K_rate(2, 2) = yaw_rate_p; return OK; } @@ -346,18 +305,6 @@ MulticopterAttitudeControl::vehicle_manual_poll() } } -void -MulticopterAttitudeControl::vehicle_accel_poll() -{ - /* check if there is a new position */ - bool accel_updated; - orb_check(_accel_sub, &accel_updated); - - if (accel_updated) { - orb_copy(ORB_ID(sensor_accel), _accel_sub, &_accel); - } -} - void MulticopterAttitudeControl::vehicle_setpoint_poll() { @@ -402,7 +349,6 @@ MulticopterAttitudeControl::task_main() */ _att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); _att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); - _accel_sub = orb_subscribe(ORB_ID(sensor_accel)); _control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); _manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); @@ -418,7 +364,6 @@ MulticopterAttitudeControl::task_main() /* get an initial update for all sensor and status data */ vehicle_setpoint_poll(); - vehicle_accel_poll(); vehicle_control_mode_poll(); vehicle_manual_poll(); arming_status_poll(); @@ -451,11 +396,10 @@ MulticopterAttitudeControl::task_main() /* only update parameters if they changed */ if (fds[0].revents & POLLIN) { - /* read from param to clear updated flag */ + /* copy the topic to clear updated flag */ struct parameter_update_s update; orb_copy(ORB_ID(parameter_update), _params_sub, &update); - /* update parameters from storage */ parameters_update(); } @@ -469,29 +413,20 @@ MulticopterAttitudeControl::task_main() if (deltaT > 1.0f) deltaT = 0.01f; - /* load local copies */ + /* copy attitude topic */ orb_copy(ORB_ID(vehicle_attitude), _att_sub, &_att); vehicle_setpoint_poll(); - vehicle_accel_poll(); vehicle_control_mode_poll(); arming_status_poll(); vehicle_manual_poll(); - /* lock integrator until armed */ - if (_arming.armed) { - _att_control.lock_integrator(false); - } else { - _att_control.reset_integral(); - _att_control.lock_integrator(true); - } - - /* decide if in auto or full manual control */ float roll_sp = 0.0f; float pitch_sp = 0.0f; float throttle_sp = 0.0f; float yaw_sp = 0.0f; + /* decide if in auto or manual control */ if (_control_mode.flag_control_manual_enabled) { roll_sp = _manual.roll; pitch_sp = _manual.pitch; @@ -505,51 +440,42 @@ MulticopterAttitudeControl::task_main() throttle_sp = _att_sp.thrust; } - // XXX take rotation matrix directly from att_sp for auto mode - math::Vector3 F_des(roll_sp, pitch_sp, yaw_sp); + /* construct rotation matrix from euler angles */ + math::EulerAngles euler(roll_sp, pitch_sp, yaw_sp); + math::Dcm R_des(euler); - math::Vector3 rates_des; + /* rotation matrix for current state */ + math::Dcm R(_att.R); + /* current body angular rates */ + math::Vector3 rates(_att.rollspeed, _att.pitchspeed, _att.yawspeed); - math::Dcm R_nb(_att.R); - // XXX actually it's not euler angles rates, but body rates, rename it (?) - math::Vector3 angular_rates(_att.rollspeed, _att.pitchspeed, _att.yawspeed); + /* orientation error between R and R_des */ + math::Matrix e_R_m = (R_des.transpose() * R - R.transpose() * R_des).transpose() * 0.5f; + math::Vector3 e_R(-e_R_m(1, 2), e_R_m(0, 2), -e_R_m(0, 1)); - _att_control.control(deltaT, R_nb, _att.yaw, F_des, - _parameters.att_p, _parameters.att_d, _parameters.att_i, - angular_rates, rates_des, throttle_sp); + /* angular rates setpoint*/ + math::Vector3 rates_sp = K_p * e_R; + math::Vector3 control = K_r_p * (rates_sp - rates); - float roll_out = _parameters.att_rate_p * rates_des(0); - float pitch_out = _parameters.att_rate_p * rates_des(1); - float yaw_out = _parameters.yaw_rate_p * rates_des(2); - - _actuators.control[0] = (isfinite(roll_out)) ? roll_out : 0.0f; - _actuators.control[1] = (isfinite(pitch_out)) ? pitch_out : 0.0f; - _actuators.control[2] = (isfinite(yaw_out)) ? yaw_out : 0.0f; - - /* throttle passed through */ - _actuators.control[3] = (isfinite(throttle_sp)) ? throttle_sp : 0.0f; - - /* - * Lazily publish the rate setpoint (for analysis, the actuators are published below) - * only once available - */ - vehicle_rates_setpoint_s rates_sp; - rates_sp.roll = rates_des(0); - rates_sp.pitch = rates_des(1); - rates_sp.yaw = rates_des(2); - - rates_sp.timestamp = hrt_absolute_time(); + /* publish the attitude rates setpoint */ + _rates_setpoint.roll = rates_sp(0); + _rates_setpoint.pitch = rates_sp(1); + _rates_setpoint.yaw = rates_sp(2); + _rates_setpoint.thrust = throttle_sp; + _rates_setpoint.timestamp = hrt_absolute_time(); if (_rate_sp_pub > 0) { - /* publish the attitude setpoint */ - orb_publish(ORB_ID(vehicle_rates_setpoint), _actuators_0_pub, &rates_sp); + orb_publish(ORB_ID(vehicle_rates_setpoint), _actuators_0_pub, &_rates_setpoint); } else { - /* advertise and publish */ - _rate_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &rates_sp); + _rate_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &_rates_setpoint); } - /* lazily publish the setpoint only once available */ + /* publish the attitude controls */ + _actuators.control[0] = (isfinite(control(0))) ? control(0) : 0.0f; + _actuators.control[1] = (isfinite(control(1))) ? control(1) : 0.0f; + _actuators.control[2] = (isfinite(control(2))) ? control(2) : 0.0f; + _actuators.control[3] = (isfinite(throttle_sp)) ? throttle_sp : 0.0f; _actuators.timestamp = hrt_absolute_time(); if (_actuators_0_pub > 0) { @@ -566,7 +492,7 @@ MulticopterAttitudeControl::task_main() perf_end(_loop_perf); } - warnx("exiting.\n"); + warnx("exit"); _control_task = -1; _exit(0); diff --git a/src/modules/mc_att_control_vector/module.mk b/src/modules/mc_att_control_vector/module.mk index 61eb3b3fdf..8d380d719f 100644 --- a/src/modules/mc_att_control_vector/module.mk +++ b/src/modules/mc_att_control_vector/module.mk @@ -37,5 +37,4 @@ MODULE_COMMAND = mc_att_control_vector -SRCS = mc_att_control_vector_main.cpp \ - ecl_mc_att_control_vector.cpp +SRCS = mc_att_control_vector_main.cpp From 628b54a396d80e4755788e8a44a072d2d12e398f Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 17 Nov 2013 23:03:58 +0400 Subject: [PATCH 114/447] Use mc_att_control_vector as default attitude controller for multirotors --- ROMFS/px4fmu_common/init.d/rc.multirotor | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.multirotor b/ROMFS/px4fmu_common/init.d/rc.multirotor index bc550ac5a1..c996e3ff90 100644 --- a/ROMFS/px4fmu_common/init.d/rc.multirotor +++ b/ROMFS/px4fmu_common/init.d/rc.multirotor @@ -31,7 +31,7 @@ position_estimator_inav start # # Start attitude control # -multirotor_att_control start +mc_att_control_vector start # # Start position control From c8942d0b3402a88753d8d95da5535155c12845c2 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 18 Nov 2013 15:58:45 +0100 Subject: [PATCH 115/447] Makefiles: fix compilation for backside --- makefiles/config_px4fmu-v1_backside.mk | 1 + 1 file changed, 1 insertion(+) diff --git a/makefiles/config_px4fmu-v1_backside.mk b/makefiles/config_px4fmu-v1_backside.mk index e1a42530b1..c07f7ab1a7 100644 --- a/makefiles/config_px4fmu-v1_backside.mk +++ b/makefiles/config_px4fmu-v1_backside.mk @@ -106,6 +106,7 @@ MODULES += modules/systemlib MODULES += modules/systemlib/mixer MODULES += modules/controllib MODULES += modules/uORB +MODULES += modules/dataman # # Libraries From bc583838b75fd2e8aabb6f7174bdd11aa75419a6 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 18 Nov 2013 15:59:14 +0100 Subject: [PATCH 116/447] Navigator: only whitespace fixes --- makefiles/config_px4fmu-v1_default.mk | 2 +- makefiles/config_px4fmu-v2_default.mk | 2 +- src/modules/navigator/navigator_main.cpp | 133 ++++++++++++----------- 3 files changed, 69 insertions(+), 68 deletions(-) diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk index d116df1904..1e1d0f4191 100644 --- a/makefiles/config_px4fmu-v1_default.mk +++ b/makefiles/config_px4fmu-v1_default.mk @@ -105,7 +105,7 @@ MODULES += modules/systemlib MODULES += modules/systemlib/mixer MODULES += modules/controllib MODULES += modules/uORB -MODULES += modules/dataman +MODULES += modules/dataman # # Libraries diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index ec4bb0b0b4..b9aec79fec 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -102,7 +102,7 @@ MODULES += modules/systemlib MODULES += modules/systemlib/mixer MODULES += modules/controllib MODULES += modules/uORB -MODULES += modules/dataman +MODULES += modules/dataman # # Libraries diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 2181001c4a..60859534c2 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -2,7 +2,8 @@ * * Copyright (c) 2013 PX4 Development Team. All rights reserved. * Author: Lorenz Meier - * Jean Cyr + * Jean Cyr + * Julian Oes * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -114,7 +115,7 @@ public: /** * Specify fence vertex position. */ - void fence_point(int argc, char *argv[]); + void fence_point(int argc, char *argv[]); private: @@ -128,10 +129,10 @@ private: int _vstatus_sub; /**< vehicle status subscription */ int _params_sub; /**< notification of parameter updates */ int _manual_control_sub; /**< notification of manual control updates */ - int _mission_sub; + int _mission_sub; int _capabilities_sub; - int _fence_sub; - int _fence_pub; + int _fence_sub; + int _fence_pub; orb_advert_t _triplet_pub; /**< position setpoint */ @@ -145,9 +146,9 @@ private: perf_counter_t _loop_perf; /**< loop performance counter */ - unsigned _mission_items_maxcount; /**< maximum number of mission items supported */ - struct mission_item_s * _mission_items; /**< storage for mission items */ - bool _mission_valid; /**< flag if mission is valid */ + unsigned _mission_items_maxcount; /**< maximum number of mission items supported */ + struct mission_item_s * _mission_items; /**< storage for mission items */ + bool _mission_valid; /**< flag if mission is valid */ struct fence_s _fence; /**< storage for fence vertices */ @@ -228,11 +229,11 @@ private: */ void task_main() __attribute__((noreturn)); - void publish_fence(unsigned vertices); + void publish_fence(unsigned vertices); - void publish_mission(unsigned points); + void publish_mission(unsigned points); - void publish_safepoints(unsigned points); + void publish_safepoints(unsigned points); bool fence_valid(const struct fence_s &fence); }; @@ -261,9 +262,9 @@ Navigator::Navigator() : _vstatus_sub(-1), _params_sub(-1), _manual_control_sub(-1), - _fence_sub(-1), - _fence_pub(-1), - _mission_sub(-1), + _fence_sub(-1), + _fence_pub(-1), + _mission_sub(-1), _capabilities_sub(-1), /* publications */ @@ -357,8 +358,8 @@ Navigator::mission_poll() void Navigator::mission_update() { - struct mission_s mission; - if (orb_copy(ORB_ID(mission), _mission_sub, &mission) == OK) { + struct mission_s mission; + if (orb_copy(ORB_ID(mission), _mission_sub, &mission) == OK) { // XXX this is not optimal yet, but a first prototype / // test implementation @@ -398,7 +399,7 @@ Navigator::task_main() _global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); _mission_sub = orb_subscribe(ORB_ID(mission)); _capabilities_sub = orb_subscribe(ORB_ID(navigation_capabilities)); - _fence_sub = orb_subscribe(ORB_ID(fence)); + _fence_sub = orb_subscribe(ORB_ID(fence)); _att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); _airspeed_sub = orb_subscribe(ORB_ID(airspeed)); _vstatus_sub = orb_subscribe(ORB_ID(vehicle_status)); @@ -435,10 +436,10 @@ Navigator::task_main() fds[1].events = POLLIN; fds[2].fd = _fence_sub; fds[2].events = POLLIN; - fds[3].fd = _capabilities_sub; - fds[3].events = POLLIN; - fds[4].fd = _mission_sub; - fds[4].events = POLLIN; + fds[3].fd = _capabilities_sub; + fds[3].events = POLLIN; + fds[4].fd = _mission_sub; + fds[4].events = POLLIN; while (!_task_should_exit) { @@ -480,13 +481,13 @@ Navigator::task_main() } /* only update craft capabilities if they have changed */ - if (fds[3].revents & POLLIN) { - orb_copy(ORB_ID(navigation_capabilities), _capabilities_sub, &_nav_caps); - } + if (fds[3].revents & POLLIN) { + orb_copy(ORB_ID(navigation_capabilities), _capabilities_sub, &_nav_caps); + } - if (fds[4].revents & POLLIN) { - mission_update(); - } + if (fds[4].revents & POLLIN) { + mission_update(); + } /* only run controller if position changed */ if (fds[1].revents & POLLIN) { @@ -659,22 +660,22 @@ Navigator::status() (double)_global_pos.vx, (double)_global_pos.vy, (double)_global_pos.vz); warnx("Compass heading in degrees %5.5f", (double)_global_pos.yaw * 57.2957795); } - if (_fence_valid) { - warnx("Geofence is valid"); - warnx("Vertex longitude latitude"); - for (unsigned i = 0; i < _fence.count; i++) - warnx("%6u %9.5f %8.5f", i, (double)_fence.vertices[i].lon, (double)_fence.vertices[i].lat); - } else - warnx("Geofence not set"); + if (_fence_valid) { + warnx("Geofence is valid"); + warnx("Vertex longitude latitude"); + for (unsigned i = 0; i < _fence.count; i++) + warnx("%6u %9.5f %8.5f", i, (double)_fence.vertices[i].lon, (double)_fence.vertices[i].lat); + } else + warnx("Geofence not set"); } void Navigator::publish_fence(unsigned vertices) { - if (_fence_pub == -1) - _fence_pub = orb_advertise(ORB_ID(fence), &vertices); - else - orb_publish(ORB_ID(fence), _fence_pub, &vertices); + if (_fence_pub == -1) + _fence_pub = orb_advertise(ORB_ID(fence), &vertices); + else + orb_publish(ORB_ID(fence), _fence_pub, &vertices); } bool @@ -721,41 +722,41 @@ Navigator::load_fence(unsigned vertices) void Navigator::fence_point(int argc, char *argv[]) { - int ix, last; - double lon, lat; - struct fence_vertex_s vertex; - char *end; + int ix, last; + double lon, lat; + struct fence_vertex_s vertex; + char *end; - if ((argc == 1) && (strcmp("-clear", argv[0]) == 0)) { - dm_clear(DM_KEY_FENCE_POINTS); + if ((argc == 1) && (strcmp("-clear", argv[0]) == 0)) { + dm_clear(DM_KEY_FENCE_POINTS); publish_fence(0); - return; - } + return; + } - if (argc < 3) - errx(1, "Specify: -clear | sequence latitude longitude [-publish]"); + if (argc < 3) + errx(1, "Specify: -clear | sequence latitude longitude [-publish]"); - ix = atoi(argv[0]); - if (ix >= DM_KEY_FENCE_POINTS_MAX) - errx(1, "Sequence must be less than %d", DM_KEY_FENCE_POINTS_MAX); + ix = atoi(argv[0]); + if (ix >= DM_KEY_FENCE_POINTS_MAX) + errx(1, "Sequence must be less than %d", DM_KEY_FENCE_POINTS_MAX); - lat = strtod(argv[1], &end); - lon = strtod(argv[2], &end); + lat = strtod(argv[1], &end); + lon = strtod(argv[2], &end); - last = 0; - if ((argc > 3) && (strcmp(argv[3], "-publish") == 0)) - last = 1; + last = 0; + if ((argc > 3) && (strcmp(argv[3], "-publish") == 0)) + last = 1; - vertex.lat = (float)lat; - vertex.lon = (float)lon; + vertex.lat = (float)lat; + vertex.lon = (float)lon; - if (dm_write(DM_KEY_FENCE_POINTS, ix, DM_PERSIST_POWER_ON_RESET, &vertex, sizeof(vertex)) == sizeof(vertex)) { - if (last) - publish_fence((unsigned)ix + 1); - return; - } + if (dm_write(DM_KEY_FENCE_POINTS, ix, DM_PERSIST_POWER_ON_RESET, &vertex, sizeof(vertex)) == sizeof(vertex)) { + if (last) + publish_fence((unsigned)ix + 1); + return; + } - errx(1, "can't store fence point"); + errx(1, "can't store fence point"); } static void usage() @@ -800,8 +801,8 @@ int navigator_main(int argc, char *argv[]) } else if (!strcmp(argv[1], "status")) { navigator::g_navigator->status(); - } else if (!strcmp(argv[1], "fence")) { - navigator::g_navigator->fence_point(argc - 2, argv + 2); + } else if (!strcmp(argv[1], "fence")) { + navigator::g_navigator->fence_point(argc - 2, argv + 2); } else { usage(); From b33634bae424e1770b6cc4cd965e2accb63f5cdc Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 19 Nov 2013 10:05:38 +0100 Subject: [PATCH 117/447] Navigator: cleanup and getting rid of unnecessary subscriptions --- src/modules/navigator/navigator_main.cpp | 128 +++++------------------ 1 file changed, 27 insertions(+), 101 deletions(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 60859534c2..207b511542 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -1,7 +1,7 @@ /**************************************************************************** * * Copyright (c) 2013 PX4 Development Team. All rights reserved. - * Author: Lorenz Meier + * Authors: Lorenz Meier * Jean Cyr * Julian Oes * @@ -55,14 +55,8 @@ #include #include #include -#include #include #include -#include -#include -#include -#include -#include #include #include #include @@ -75,6 +69,7 @@ #include #include + /** * navigator app start / stop handling function * @@ -120,26 +115,17 @@ public: private: bool _task_should_exit; /**< if true, sensor task should exit */ - int _navigator_task; /**< task handle for sensor task */ + int _navigator_task; /**< task handle for sensor task */ int _global_pos_sub; - int _att_sub; /**< vehicle attitude subscription */ - int _attitude_sub; /**< raw rc channels data subscription */ - int _airspeed_sub; /**< airspeed subscription */ int _vstatus_sub; /**< vehicle status subscription */ int _params_sub; /**< notification of parameter updates */ - int _manual_control_sub; /**< notification of manual control updates */ - int _mission_sub; - int _capabilities_sub; - int _fence_sub; - int _fence_pub; + int _mission_sub; /**< notification of mission updates */ + int _capabilities_sub; /**< notification of vehicle capabilities updates */ - orb_advert_t _triplet_pub; /**< position setpoint */ + orb_advert_t _triplet_pub; /**< publish position setpoint triplet */ + orb_advert_t _fence_pub; /**< publish fence topic */ - struct vehicle_attitude_s _att; /**< vehicle attitude */ - struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */ - struct manual_control_setpoint_s _manual; /**< r/c channel data */ - struct airspeed_s _airspeed; /**< airspeed */ struct vehicle_status_s _vstatus; /**< vehicle status */ struct vehicle_global_position_s _global_pos; /**< global vehicle position */ struct vehicle_global_position_set_triplet_s _global_triplet; /**< triplet of global setpoints */ @@ -215,10 +201,6 @@ private: */ float control_pitch(float altitude_error); - void calculate_airspeed_errors(); - void calculate_gndspeed_undershoot(); - void calculate_altitude_error(); - /** * Shim for calling task_main from task_create. */ @@ -231,8 +213,6 @@ private: void publish_fence(unsigned vertices); - void publish_mission(unsigned points); - void publish_safepoints(unsigned points); bool fence_valid(const struct fence_s &fence); @@ -257,27 +237,23 @@ Navigator::Navigator() : /* subscriptions */ _global_pos_sub(-1), - _att_sub(-1), - _airspeed_sub(-1), _vstatus_sub(-1), _params_sub(-1), - _manual_control_sub(-1), - _fence_sub(-1), - _fence_pub(-1), _mission_sub(-1), _capabilities_sub(-1), /* publications */ _triplet_pub(-1), + _fence_pub(-1), /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "navigator")), /* states */ _mission_items_maxcount(20), _mission_valid(false), - _loiter_hold(false), _fence_valid(false), - _inside_fence(true) + _inside_fence(true), + _loiter_hold(false) { _global_pos.valid = false; memset(&_fence, 0, sizeof(_fence)); @@ -321,40 +297,6 @@ Navigator::parameters_update() return OK; } -void -Navigator::vehicle_status_poll() -{ - bool vstatus_updated; - - /* Check HIL state if vehicle status has changed */ - orb_check(_vstatus_sub, &vstatus_updated); - - if (vstatus_updated) - orb_copy(ORB_ID(vehicle_status), _vstatus_sub, &_vstatus); -} - -void -Navigator::vehicle_attitude_poll() -{ - /* check if there is a new position */ - bool att_updated; - orb_check(_att_sub, &att_updated); - - if (att_updated) - orb_copy(ORB_ID(vehicle_attitude), _att_sub, &_att); -} - -void -Navigator::mission_poll() -{ - /* check if there is a new setpoint */ - bool mission_updated; - orb_check(_mission_sub, &mission_updated); - - if (mission_updated) - mission_update(); -} - void Navigator::mission_update() { @@ -399,19 +341,14 @@ Navigator::task_main() _global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); _mission_sub = orb_subscribe(ORB_ID(mission)); _capabilities_sub = orb_subscribe(ORB_ID(navigation_capabilities)); - _fence_sub = orb_subscribe(ORB_ID(fence)); - _att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); - _airspeed_sub = orb_subscribe(ORB_ID(airspeed)); _vstatus_sub = orb_subscribe(ORB_ID(vehicle_status)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); - _manual_control_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); // Load initial states if (orb_copy(ORB_ID(vehicle_status), _vstatus_sub, &_vstatus) != OK) { _vstatus.arming_state = ARMING_STATE_STANDBY; // for testing... commander may not be running } - orb_copy(ORB_ID(vehicle_attitude), _att_sub, &_att); mission_update(); /* rate limit vehicle status updates to 5Hz */ @@ -434,11 +371,11 @@ Navigator::task_main() fds[0].events = POLLIN; fds[1].fd = _global_pos_sub; fds[1].events = POLLIN; - fds[2].fd = _fence_sub; + fds[2].fd = _capabilities_sub; fds[2].events = POLLIN; - fds[3].fd = _capabilities_sub; + fds[3].fd = _mission_sub; fds[3].events = POLLIN; - fds[4].fd = _mission_sub; + fds[4].fd = _vstatus_sub; fds[4].events = POLLIN; while (!_task_should_exit) { @@ -459,10 +396,13 @@ Navigator::task_main() perf_begin(_loop_perf); - /* check vehicle status for changes to publication state */ - vehicle_status_poll(); - /* only update parameters if they changed */ + if (fds[4].revents & POLLIN) { + /* read from param to clear updated flag */ + orb_copy(ORB_ID(vehicle_status), _vstatus_sub, &_vstatus); + } + + /* only update vehicle status if it changed */ if (fds[0].revents & POLLIN) { /* read from param to clear updated flag */ struct parameter_update_s update; @@ -472,26 +412,21 @@ Navigator::task_main() parameters_update(); } - /* only update fence if it has changed */ - if (fds[2].revents & POLLIN) { - /* read from fence to clear updated flag */ - unsigned vertices; - orb_copy(ORB_ID(fence), _fence_sub, &vertices); - _fence_valid = load_fence(vertices); - } - /* only update craft capabilities if they have changed */ - if (fds[3].revents & POLLIN) { + if (fds[2].revents & POLLIN) { orb_copy(ORB_ID(navigation_capabilities), _capabilities_sub, &_nav_caps); } - if (fds[4].revents & POLLIN) { + if (fds[3].revents & POLLIN) { mission_update(); } /* only run controller if position changed */ if (fds[1].revents & POLLIN) { + /* load local copies */ + orb_copy(ORB_ID(vehicle_global_position), _global_pos_sub, &_global_pos); + static uint64_t last_run = 0; float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f; last_run = hrt_absolute_time(); @@ -501,13 +436,6 @@ Navigator::task_main() deltaT = 0.01f; } - /* load local copies */ - orb_copy(ORB_ID(vehicle_global_position), _global_pos_sub, &_global_pos); - - vehicle_attitude_poll(); - - mission_poll(); - if (_fence_valid && _global_pos.valid) { _inside_fence = inside_geofence(&_global_pos, &_fence); } @@ -600,9 +528,9 @@ Navigator::task_main() } else if (_global_triplet.current.nav_cmd == NAV_CMD_TAKEOFF) { /* apply minimum pitch if altitude has not yet been reached */ - if (_global_pos.alt < _global_triplet.current.altitude) { - _att_sp.pitch_body = math::max(_att_sp.pitch_body, _global_triplet.current.param1); - } + // if (_global_pos.alt < _global_triplet.current.altitude) { + // _att_sp.pitch_body = math::max(_att_sp.pitch_body, _global_triplet.current.param1); + // } } /* lazily publish the setpoint only once available */ @@ -681,8 +609,6 @@ Navigator::publish_fence(unsigned vertices) bool Navigator::fence_valid(const struct fence_s &fence) { - struct vehicle_global_position_s pos; - // NULL fence is valid if (fence.count == 0) { return true; From a318d0cf40057ee22e792518c1011b6128e3535a Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 19 Nov 2013 11:44:41 +0100 Subject: [PATCH 118/447] fix off by one in missionlib --- src/modules/mavlink/missionlib.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mavlink/missionlib.c b/src/modules/mavlink/missionlib.c index fa23f996f3..bb857dc690 100644 --- a/src/modules/mavlink/missionlib.c +++ b/src/modules/mavlink/missionlib.c @@ -276,7 +276,7 @@ void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1, next_setpoint_index = index + 1; } - while (next_setpoint_index < wpm->size - 1) { + while (next_setpoint_index < wpm->size) { if (wpm->waypoints[next_setpoint_index].frame == (int)MAV_FRAME_GLOBAL && (wpm->waypoints[next_setpoint_index].command == (int)MAV_CMD_NAV_WAYPOINT || wpm->waypoints[next_setpoint_index].command == (int)MAV_CMD_NAV_LOITER_TURNS || From 4111cb0831cf4d8aacf427e2244a613183b2037f Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 19 Nov 2013 12:44:51 +0100 Subject: [PATCH 119/447] Mission topic: get rid of magic params --- src/modules/uORB/topics/mission.h | 10 ++++------ 1 file changed, 4 insertions(+), 6 deletions(-) diff --git a/src/modules/uORB/topics/mission.h b/src/modules/uORB/topics/mission.h index 978a3383a5..836d58faa2 100644 --- a/src/modules/uORB/topics/mission.h +++ b/src/modules/uORB/topics/mission.h @@ -35,8 +35,8 @@ ****************************************************************************/ /** - * @file mission_item.h - * Definition of one mission item. + * @file mission.h + * Definition of a mission consisting of mission items. */ #ifndef TOPIC_MISSION_H_ @@ -77,10 +77,8 @@ struct mission_item_s float loiter_radius; /**< loiter radius in meters, 0 for a VTOL to hover */ uint8_t loiter_direction; /**< 1: positive / clockwise, -1, negative. */ enum NAV_CMD nav_cmd; /**< true if loitering is enabled */ - float param1; - float param2; - float param3; - float param4; + float radius; /**< radius in which the mission is accepted as reached in meters */ + float time_inside; /**< time that the MAV should stay inside the radius before advancing in milliseconds */ }; struct mission_s From c57f6572307ed2b8f52fddfbba5d0fb7a7078ef9 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 19 Nov 2013 12:46:08 +0100 Subject: [PATCH 120/447] Startup scripts: start the navigator by default --- ROMFS/px4fmu_common/init.d/rcS | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index d8b5cb6087..5f52969d13 100755 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -111,6 +111,11 @@ then # commander start + # + # Start the Navigator + # + navigator start + if param compare SYS_AUTOSTART 1000 then sh /etc/init.d/1000_rc_fw_easystar.hil From 6f9a31c4013ba9b5ff5e12697e501688d708bb16 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 19 Nov 2013 14:23:29 +0100 Subject: [PATCH 121/447] Mission topic: please allow the sign to be negative as well --- src/modules/uORB/topics/mission.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/uORB/topics/mission.h b/src/modules/uORB/topics/mission.h index 836d58faa2..ec78352798 100644 --- a/src/modules/uORB/topics/mission.h +++ b/src/modules/uORB/topics/mission.h @@ -75,7 +75,7 @@ struct mission_item_s float altitude; /**< altitude in meters */ float yaw; /**< in radians NED -PI..+PI */ float loiter_radius; /**< loiter radius in meters, 0 for a VTOL to hover */ - uint8_t loiter_direction; /**< 1: positive / clockwise, -1, negative. */ + int8_t loiter_direction; /**< 1: positive / clockwise, -1, negative. */ enum NAV_CMD nav_cmd; /**< true if loitering is enabled */ float radius; /**< radius in which the mission is accepted as reached in meters */ float time_inside; /**< time that the MAV should stay inside the radius before advancing in milliseconds */ From a3b7ecb9235915caa98b21fc5e82645eed8f030f Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 19 Nov 2013 16:13:29 +0100 Subject: [PATCH 122/447] Navigator: more cleanup, prune unused functions --- src/modules/navigator/navigator_main.cpp | 15 --------------- 1 file changed, 15 deletions(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 207b511542..9e73fcb229 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -171,21 +171,6 @@ private: */ void control_update(); - /** - * Check for changes in vehicle status. - */ - void vehicle_status_poll(); - - /** - * Check for position updates. - */ - void vehicle_attitude_poll(); - - /** - * Check for set triplet updates. - */ - void mission_poll(); - /** * Retrieve mission. */ From 7c7b5e475d7971bda40cd2bd6ecd41d7a512b26e Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 19 Nov 2013 16:14:15 +0100 Subject: [PATCH 123/447] Waypoints: Store waypoints in mission topic (WIP, in parallel for now) --- src/modules/mavlink/waypoints.c | 78 +++++++++++++++++++++++++++++---- 1 file changed, 69 insertions(+), 9 deletions(-) diff --git a/src/modules/mavlink/waypoints.c b/src/modules/mavlink/waypoints.c index 7e4a2688f6..5fdbd57e14 100644 --- a/src/modules/mavlink/waypoints.c +++ b/src/modules/mavlink/waypoints.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. + * Copyright (C) 2008-2013 PX4 Development Team. All rights reserved. * Author: @author Petri Tanskanen * @author Lorenz Meier * @author Thomas Gubler @@ -49,6 +49,9 @@ #include "missionlib.h" #include "waypoints.h" #include "util.h" +#include +#include + #ifndef FM_PI #define FM_PI 3.1415926535897932384626433832795f @@ -58,10 +61,39 @@ bool debug = false; bool verbose = false; -#define MAVLINK_WPM_NO_PRINTF +//#define MAVLINK_WPM_NO_PRINTF +#define MAVLINK_WPM_VERBOSE 1 uint8_t mavlink_wpm_comp_id = MAV_COMP_ID_MISSIONPLANNER; +void map_mavlink_mission_item_to_mission_item(const mavlink_mission_item_t *mavlink_mission_item, struct mission_item_s *mission_item) +{ + mission_item->lat = (double)mavlink_mission_item->x; + mission_item->lon = (double)mavlink_mission_item->y; + mission_item->altitude = mavlink_mission_item->z; + mission_item->yaw = mavlink_mission_item->param4*M_DEG_TO_RAD_F; + mission_item->loiter_radius = fabsf(mavlink_mission_item->param3); + mission_item->loiter_direction = (mavlink_mission_item->param3 > 0) ? 1 : -1; /* 1 if positive CW, -1 if negative CCW */ + mission_item->nav_cmd = NAV_CMD_WAYPOINT; // TODO correct + mission_item->radius = mavlink_mission_item->param1; + mission_item->time_inside = mavlink_mission_item->param2; +} + +void map_mission_item_to_mavlink_mission_item(const struct mission_item_s *mission_item, const uint16_t seq, mavlink_mission_item_t *mavlink_mission_item) +{ + + mavlink_mission_item->x = (float)mission_item->lat; + mavlink_mission_item->y = (float)mission_item->lon; + mavlink_mission_item->z = mission_item->altitude; + mavlink_mission_item->param4 = mission_item->yaw*M_RAD_TO_DEG_F; + mavlink_mission_item->param3 = mission_item->loiter_radius*(float)mission_item->loiter_direction; + mavlink_mission_item->command = MAV_CMD_NAV_WAYPOINT; // TODO add + mavlink_mission_item->param1 = mission_item->radius; + mavlink_mission_item->param2 = mission_item->time_inside; + + mavlink_mission_item->seq = seq; +} + void mavlink_wpm_init(mavlink_wpm_storage *state) { // Set all waypoints to zero @@ -185,11 +217,12 @@ void mavlink_wpm_send_waypoint_count(uint8_t sysid, uint8_t compid, uint16_t cou // FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY")); } -void mavlink_wpm_send_waypoint(uint8_t sysid, uint8_t compid, uint16_t seq) +void mavlink_wpm_send_waypoint(uint8_t sysid, uint8_t compid, mavlink_mission_item_t *wp) { - if (seq < wpm->size) { + + // if (seq < wpm->size) { mavlink_message_t msg; - mavlink_mission_item_t *wp = &(wpm->waypoints[seq]); + // mavlink_mission_item_t *wp = &(wpm->waypoints[seq]); wp->target_system = wpm->current_partner_sysid; wp->target_component = wpm->current_partner_compid; mavlink_msg_mission_item_encode(mavlink_system.sysid, mavlink_wpm_comp_id, &msg, wp); @@ -199,9 +232,9 @@ void mavlink_wpm_send_waypoint(uint8_t sysid, uint8_t compid, uint16_t seq) // FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY")); - } else { - if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_missionlib_send_gcs_string("ERROR: Waypoint index out of bounds\n"); - } + // } else { + // if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_missionlib_send_gcs_string("ERROR: Waypoint index out of bounds\n"); + // } } void mavlink_wpm_send_waypoint_request(uint8_t sysid, uint8_t compid, uint16_t seq) @@ -523,6 +556,9 @@ int mavlink_waypoint_eventloop(uint64_t now, const struct vehicle_global_positio void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehicle_global_position_s *global_pos , struct vehicle_local_position_s *local_pos) { + static orb_advert_t mission_pub = -1; + static struct mission_s mission; + uint64_t now = mavlink_missionlib_get_system_timestamp(); switch (msg->msgid) { @@ -671,7 +707,10 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi wpm->current_state = MAVLINK_WPM_STATE_SENDLIST_SENDWPS; wpm->current_wp_id = wpr.seq; - mavlink_wpm_send_waypoint(wpm->current_partner_sysid, wpm->current_partner_compid, wpr.seq); + + mavlink_mission_item_t wp; + map_mission_item_to_mavlink_mission_item(&mission.items[wpr.seq], wpr.seq, &wp); + mavlink_wpm_send_waypoint(wpm->current_partner_sysid, wpm->current_partner_compid, &wp); } else { // if (verbose) @@ -794,6 +833,16 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi wpm->current_partner_compid = msg->compid; wpm->current_count = wpc.count; + /* prepare mission topic */ + mission.count = wpc.count; + mission.items = (struct mission_item_s*)malloc(sizeof(struct mission_item_s) * mission.count); + + if (!mission.items) { + mission.count = 0; + /* XXX reject waypoints if this fails */ + warnx("no free RAM to allocate mission, rejecting any waypoints"); + } + #ifdef MAVLINK_WPM_NO_PRINTF mavlink_missionlib_send_gcs_string("CLR RCV BUF: READY"); #else @@ -933,7 +982,18 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi for (i = 0; i < wpm->current_count; ++i) { wpm->waypoints[i] = wpm->rcv_waypoints[i]; + map_mavlink_mission_item_to_mission_item(&wpm->rcv_waypoints[i], &mission.items[i]); } + // TODO: update count? + + /* Initialize mission publication if necessary */ + if (mission_pub < 0) { + mission_pub = orb_advertise(ORB_ID(mission), &mission); + + } else { + orb_publish(ORB_ID(mission), mission_pub, &mission); + } + wpm->size = wpm->current_count; From a9e51105c81a4de0cf35a03af0be67fb49ba8870 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 19 Nov 2013 16:15:16 +0100 Subject: [PATCH 124/447] Missionlib: Don't let the missionlib publish the triplet, moving forward this should be done by the navigator --- src/modules/mavlink/missionlib.c | 80 ++++++++++++++++---------------- 1 file changed, 40 insertions(+), 40 deletions(-) diff --git a/src/modules/mavlink/missionlib.c b/src/modules/mavlink/missionlib.c index fa23f996f3..9b1d3c51a4 100644 --- a/src/modules/mavlink/missionlib.c +++ b/src/modules/mavlink/missionlib.c @@ -206,7 +206,7 @@ void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1, float param6_lon_y, float param7_alt_z, uint8_t frame, uint16_t command) { static orb_advert_t global_position_setpoint_pub = -1; - static orb_advert_t global_position_set_triplet_pub = -1; + // static orb_advert_t global_position_set_triplet_pub = -1; static orb_advert_t local_position_setpoint_pub = -1; static unsigned last_waypoint_index = -1; char buf[50] = {0}; @@ -234,10 +234,10 @@ void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1, /* fill triplet: previous, current, next waypoint */ - struct vehicle_global_position_set_triplet_s triplet; + // struct vehicle_global_position_set_triplet_s triplet; /* current waypoint is same as sp */ - memcpy(&(triplet.current), &sp, sizeof(sp)); + // memcpy(&(triplet.current), &sp, sizeof(sp)); /* * Check if previous WP (in mission, not in execution order) @@ -291,48 +291,48 @@ void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1, /* populate last and next */ - triplet.previous_valid = false; - triplet.next_valid = false; + // triplet.previous_valid = false; + // triplet.next_valid = false; - if (last_setpoint_valid) { - triplet.previous_valid = true; - struct vehicle_global_position_setpoint_s sp; - sp.lat = wpm->waypoints[last_setpoint_index].x * 1e7f; - sp.lon = wpm->waypoints[last_setpoint_index].y * 1e7f; - sp.altitude = wpm->waypoints[last_setpoint_index].z; - sp.altitude_is_relative = false; - sp.yaw = (wpm->waypoints[last_setpoint_index].param4 / 180.0f) * M_PI_F - M_PI_F; - set_special_fields(wpm->waypoints[last_setpoint_index].param1, - wpm->waypoints[last_setpoint_index].param2, - wpm->waypoints[last_setpoint_index].param3, - wpm->waypoints[last_setpoint_index].param4, - wpm->waypoints[last_setpoint_index].command, &sp); - memcpy(&(triplet.previous), &sp, sizeof(sp)); - } + // if (last_setpoint_valid) { + // triplet.previous_valid = true; + // struct vehicle_global_position_setpoint_s sp; + // sp.lat = wpm->waypoints[last_setpoint_index].x * 1e7f; + // sp.lon = wpm->waypoints[last_setpoint_index].y * 1e7f; + // sp.altitude = wpm->waypoints[last_setpoint_index].z; + // sp.altitude_is_relative = false; + // sp.yaw = (wpm->waypoints[last_setpoint_index].param4 / 180.0f) * M_PI_F - M_PI_F; + // set_special_fields(wpm->waypoints[last_setpoint_index].param1, + // wpm->waypoints[last_setpoint_index].param2, + // wpm->waypoints[last_setpoint_index].param3, + // wpm->waypoints[last_setpoint_index].param4, + // wpm->waypoints[last_setpoint_index].command, &sp); + // memcpy(&(triplet.previous), &sp, sizeof(sp)); + // } - if (next_setpoint_valid) { - triplet.next_valid = true; - struct vehicle_global_position_setpoint_s sp; - sp.lat = wpm->waypoints[next_setpoint_index].x * 1e7f; - sp.lon = wpm->waypoints[next_setpoint_index].y * 1e7f; - sp.altitude = wpm->waypoints[next_setpoint_index].z; - sp.altitude_is_relative = false; - sp.yaw = (wpm->waypoints[next_setpoint_index].param4 / 180.0f) * M_PI_F - M_PI_F; - set_special_fields(wpm->waypoints[next_setpoint_index].param1, - wpm->waypoints[next_setpoint_index].param2, - wpm->waypoints[next_setpoint_index].param3, - wpm->waypoints[next_setpoint_index].param4, - wpm->waypoints[next_setpoint_index].command, &sp); - memcpy(&(triplet.next), &sp, sizeof(sp)); - } + // if (next_setpoint_valid) { + // triplet.next_valid = true; + // struct vehicle_global_position_setpoint_s sp; + // sp.lat = wpm->waypoints[next_setpoint_index].x * 1e7f; + // sp.lon = wpm->waypoints[next_setpoint_index].y * 1e7f; + // sp.altitude = wpm->waypoints[next_setpoint_index].z; + // sp.altitude_is_relative = false; + // sp.yaw = (wpm->waypoints[next_setpoint_index].param4 / 180.0f) * M_PI_F - M_PI_F; + // set_special_fields(wpm->waypoints[next_setpoint_index].param1, + // wpm->waypoints[next_setpoint_index].param2, + // wpm->waypoints[next_setpoint_index].param3, + // wpm->waypoints[next_setpoint_index].param4, + // wpm->waypoints[next_setpoint_index].command, &sp); + // memcpy(&(triplet.next), &sp, sizeof(sp)); + // } /* Initialize triplet publication if necessary */ - if (global_position_set_triplet_pub < 0) { - global_position_set_triplet_pub = orb_advertise(ORB_ID(vehicle_global_position_set_triplet), &triplet); + // if (global_position_set_triplet_pub < 0) { + // global_position_set_triplet_pub = orb_advertise(ORB_ID(vehicle_global_position_set_triplet), &triplet); - } else { - orb_publish(ORB_ID(vehicle_global_position_set_triplet), global_position_set_triplet_pub, &triplet); - } + // } else { + // orb_publish(ORB_ID(vehicle_global_position_set_triplet), global_position_set_triplet_pub, &triplet); + // } sprintf(buf, "[mp] WP#%i lat: % 3.6f/lon % 3.6f/alt % 4.6f/hdg %3.4f\n", (int)index, (double)param5_lat_x, (double)param6_lon_y, (double)param7_alt_z, (double)param4); From cc96edfe014b0990fdd489ef1e38df2fad456189 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 19 Nov 2013 16:37:48 +0100 Subject: [PATCH 125/447] tecs: fix wrong != 0 check --- src/lib/external_lgpl/tecs/tecs.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/external_lgpl/tecs/tecs.cpp b/src/lib/external_lgpl/tecs/tecs.cpp index a94a06dda2..510b8ed9c1 100644 --- a/src/lib/external_lgpl/tecs/tecs.cpp +++ b/src/lib/external_lgpl/tecs/tecs.cpp @@ -314,7 +314,7 @@ void TECS::_update_throttle(float throttle_cruise, const math::Dcm &rotMat) // Rate limit PD + FF throttle // Calculate the throttle increment from the specified slew time - if (fabsf(_throttle_slewrate) < 0.01f) { + if (fabsf(_throttle_slewrate) > 0.01f) { float thrRateIncr = _DT * (_THRmaxf - _THRminf) * _throttle_slewrate; _throttle_dem = constrain(_throttle_dem, From 37ef10ceead77876108847e31f56ae68015f5784 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 20 Nov 2013 12:17:42 +0100 Subject: [PATCH 126/447] groundspeed undershoot: take into account altitude difference --- .../fw_pos_control_l1_main.cpp | 28 +++++++++++++------ 1 file changed, 20 insertions(+), 8 deletions(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 2606956208..84983785bf 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -318,7 +318,7 @@ private: const struct vehicle_global_position_set_triplet_s &global_triplet); float calculate_target_airspeed(float airspeed_demand); - void calculate_gndspeed_undershoot(); + void calculate_gndspeed_undershoot(const math::Vector2f ¤t_position, const math::Vector2f &ground_speed, const struct vehicle_global_position_set_triplet_s &global_triplet); /** * Shim for calling task_main from task_create. @@ -665,17 +665,29 @@ FixedwingPositionControl::calculate_target_airspeed(float airspeed_demand) } void -FixedwingPositionControl::calculate_gndspeed_undershoot() +FixedwingPositionControl::calculate_gndspeed_undershoot(const math::Vector2f ¤t_position, const math::Vector2f &ground_speed, const struct vehicle_global_position_set_triplet_s &global_triplet) { if (_global_pos_valid) { - /* get ground speed vector */ - math::Vector2f ground_speed_vector(_global_pos.vx, _global_pos.vy); - /* rotate with current attitude */ + /* rotate ground speed vector with current attitude */ math::Vector2f yaw_vector(_R_nb(0, 0), _R_nb(1, 0)); yaw_vector.normalize(); - float ground_speed_body = yaw_vector * ground_speed_vector; + float ground_speed_body = yaw_vector * ground_speed; + + /* The minimum desired ground speed is the minimum airspeed projected on to the ground using the altitude and horizontal difference between the waypoints if available*/ + float distance = 0.0f; + float delta_altitude = 0.0f; + if (global_triplet.previous_valid) { + distance = get_distance_to_next_waypoint(global_triplet.previous.lat * 1e-7f, global_triplet.previous.lon * 1e-7f, global_triplet.current.lat * 1e-7f, global_triplet.current.lon * 1e-7f); + delta_altitude = global_triplet.current.altitude - global_triplet.previous.altitude; + } else { + distance = get_distance_to_next_waypoint(current_position.getX(), current_position.getY(), global_triplet.current.lat * 1e-7f, global_triplet.current.lon * 1e-7f); + delta_altitude = global_triplet.current.altitude - _global_pos.alt; + } + + float ground_speed_desired = _parameters.airspeed_min * cosf(atan2f(delta_altitude, distance)); + /* * Ground speed undershoot is the amount of ground velocity not reached @@ -686,7 +698,7 @@ FixedwingPositionControl::calculate_gndspeed_undershoot() * not exceeded) travels towards a waypoint (and is not pushed more and more away * by wind). Not countering this would lead to a fly-away. */ - _groundspeed_undershoot = math::max(_parameters.airspeed_min - ground_speed_body, 0.0f); + _groundspeed_undershoot = math::max(ground_speed_desired - ground_speed_body, 0.0f); } else { _groundspeed_undershoot = 0; @@ -704,7 +716,7 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio { bool setpoint = true; - calculate_gndspeed_undershoot(); + calculate_gndspeed_undershoot(current_position, ground_speed, global_triplet); float eas2tas = 1.0f; // XXX calculate actual number based on current measurements From a6c5a1920639f3ff1b2d1a0cea6eeacf6676fc76 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 20 Nov 2013 12:46:25 +0100 Subject: [PATCH 127/447] Mission topic: Use mission topic instead of global position for triplet --- src/modules/controllib/uorb/blocks.cpp | 18 ++-- src/modules/controllib/uorb/blocks.hpp | 9 +- src/modules/fixedwing_backside/fixedwing.cpp | 10 +-- src/modules/fixedwing_backside/fixedwing.hpp | 2 +- .../fw_pos_control_l1_main.cpp | 85 ++++++++++--------- src/modules/mavlink/orb_topics.h | 2 +- src/modules/navigator/navigator_main.cpp | 42 ++++----- src/modules/uORB/objects_common.cpp | 4 +- ...n_set_triplet.h => mission_item_triplet.h} | 25 +++--- 9 files changed, 100 insertions(+), 97 deletions(-) rename src/modules/uORB/topics/{vehicle_global_position_set_triplet.h => mission_item_triplet.h} (77%) diff --git a/src/modules/controllib/uorb/blocks.cpp b/src/modules/controllib/uorb/blocks.cpp index 448a42a996..e213ac17fc 100644 --- a/src/modules/controllib/uorb/blocks.cpp +++ b/src/modules/controllib/uorb/blocks.cpp @@ -54,26 +54,26 @@ BlockWaypointGuidance::~BlockWaypointGuidance() {}; void BlockWaypointGuidance::update(vehicle_global_position_s &pos, vehicle_attitude_s &att, - vehicle_global_position_setpoint_s &posCmd, - vehicle_global_position_setpoint_s &lastPosCmd) + mission_item_s &missionCmd, + mission_item_s &lastMissionCmd) { // heading to waypoint float psiTrack = get_bearing_to_next_waypoint( (double)pos.lat / (double)1e7d, (double)pos.lon / (double)1e7d, - (double)posCmd.lat / (double)1e7d, - (double)posCmd.lon / (double)1e7d); + missionCmd.lat, + missionCmd.lon); // cross track struct crosstrack_error_s xtrackError; get_distance_to_line(&xtrackError, (double)pos.lat / (double)1e7d, (double)pos.lon / (double)1e7d, - (double)lastPosCmd.lat / (double)1e7d, - (double)lastPosCmd.lon / (double)1e7d, - (double)posCmd.lat / (double)1e7d, - (double)posCmd.lon / (double)1e7d); + lastMissionCmd.lat, + lastMissionCmd.lon, + missionCmd.lat, + missionCmd.lon); _psiCmd = _wrap_2pi(psiTrack - _xtYawLimit.update(_xt2Yaw.update(xtrackError.distance))); @@ -86,7 +86,7 @@ BlockUorbEnabledAutopilot::BlockUorbEnabledAutopilot(SuperBlock *parent, const c _attCmd(&getSubscriptions(), ORB_ID(vehicle_attitude_setpoint), 20), _ratesCmd(&getSubscriptions(), ORB_ID(vehicle_rates_setpoint), 20), _pos(&getSubscriptions() , ORB_ID(vehicle_global_position), 20), - _posCmd(&getSubscriptions(), ORB_ID(vehicle_global_position_set_triplet), 20), + _missionCmd(&getSubscriptions(), ORB_ID(mission_item_triplet), 20), _manual(&getSubscriptions(), ORB_ID(manual_control_setpoint), 20), _status(&getSubscriptions(), ORB_ID(vehicle_status), 20), _param_update(&getSubscriptions(), ORB_ID(parameter_update), 1000), // limit to 1 Hz diff --git a/src/modules/controllib/uorb/blocks.hpp b/src/modules/controllib/uorb/blocks.hpp index 46dc1bec25..335439fb77 100644 --- a/src/modules/controllib/uorb/blocks.hpp +++ b/src/modules/controllib/uorb/blocks.hpp @@ -43,7 +43,8 @@ #include #include #include -#include +#include +#include #include #include #include @@ -82,8 +83,8 @@ public: virtual ~BlockWaypointGuidance(); void update(vehicle_global_position_s &pos, vehicle_attitude_s &att, - vehicle_global_position_setpoint_s &posCmd, - vehicle_global_position_setpoint_s &lastPosCmd); + mission_item_s &missionCmd, + mission_item_s &lastMissionCmd); float getPsiCmd() { return _psiCmd; } }; @@ -98,7 +99,7 @@ protected: UOrbSubscription _attCmd; UOrbSubscription _ratesCmd; UOrbSubscription _pos; - UOrbSubscription _posCmd; + UOrbSubscription _missionCmd; UOrbSubscription _manual; UOrbSubscription _status; UOrbSubscription _param_update; diff --git a/src/modules/fixedwing_backside/fixedwing.cpp b/src/modules/fixedwing_backside/fixedwing.cpp index 6dc19df41f..108e9896d6 100644 --- a/src/modules/fixedwing_backside/fixedwing.cpp +++ b/src/modules/fixedwing_backside/fixedwing.cpp @@ -117,7 +117,7 @@ BlockMultiModeBacksideAutopilot::BlockMultiModeBacksideAutopilot(SuperBlock *par _vCmd(this, "V_CMD"), _crMax(this, "CR_MAX"), _attPoll(), - _lastPosCmd(), + _lastMissionCmd(), _timeStamp(0) { _attPoll.fd = _att.getHandle(); @@ -141,8 +141,8 @@ void BlockMultiModeBacksideAutopilot::update() setDt(dt); // store old position command before update if new command sent - if (_posCmd.updated()) { - _lastPosCmd = _posCmd.getData(); + if (_missionCmd.updated()) { + _lastMissionCmd = _missionCmd.getData(); } // check for new updates @@ -159,7 +159,7 @@ void BlockMultiModeBacksideAutopilot::update() if (_status.main_state == MAIN_STATE_AUTO) { // TODO use vehicle_control_mode here? // update guidance - _guide.update(_pos, _att, _posCmd.current, _lastPosCmd.current); + _guide.update(_pos, _att, _missionCmd.current, _lastMissionCmd.current); } // XXX handle STABILIZED (loiter on spot) as well @@ -182,7 +182,7 @@ void BlockMultiModeBacksideAutopilot::update() float vCmd = _vLimit.update(_vCmd.get()); // altitude hold - float dThrottle = _h2Thr.update(_posCmd.current.altitude - _pos.alt); + float dThrottle = _h2Thr.update(_missionCmd.current.altitude - _pos.alt); // heading hold float psiError = _wrap_pi(_guide.getPsiCmd() - _att.yaw); diff --git a/src/modules/fixedwing_backside/fixedwing.hpp b/src/modules/fixedwing_backside/fixedwing.hpp index 567efeb359..b4dbc36b09 100644 --- a/src/modules/fixedwing_backside/fixedwing.hpp +++ b/src/modules/fixedwing_backside/fixedwing.hpp @@ -264,7 +264,7 @@ private: BlockParamFloat _crMax; struct pollfd _attPoll; - vehicle_global_position_set_triplet_s _lastPosCmd; + mission_item_triplet_s _lastMissionCmd; enum {CH_AIL, CH_ELV, CH_RDR, CH_THR}; uint64_t _timeStamp; public: diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index a9648b207d..0b147f9032 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -67,7 +67,7 @@ #include #include #include -#include +#include #include #include #include @@ -120,7 +120,7 @@ private: int _control_task; /**< task handle for sensor task */ int _global_pos_sub; - int _global_set_triplet_sub; + int _mission_item_triplet_sub; int _att_sub; /**< vehicle attitude subscription */ int _attitude_sub; /**< raw rc channels data subscription */ int _airspeed_sub; /**< airspeed subscription */ @@ -139,7 +139,7 @@ private: struct airspeed_s _airspeed; /**< airspeed */ struct vehicle_control_mode_s _control_mode; /**< vehicle status */ struct vehicle_global_position_s _global_pos; /**< global vehicle position */ - struct vehicle_global_position_set_triplet_s _global_triplet; /**< triplet of global setpoints */ + struct mission_item_triplet_s _mission_item_triplet; /**< triplet of mission items */ struct accel_report _accel; /**< body frame accelerations */ perf_counter_t _loop_perf; /**< loop performance counter */ @@ -283,7 +283,7 @@ private: * Control position. */ bool control_position(const math::Vector2f &global_pos, const math::Vector2f &ground_speed, - const struct vehicle_global_position_set_triplet_s &global_triplet); + const struct mission_item_triplet_s &_mission_item_triplet); float calculate_target_airspeed(float airspeed_demand); void calculate_gndspeed_undershoot(); @@ -318,7 +318,7 @@ FixedwingPositionControl::FixedwingPositionControl() : /* subscriptions */ _global_pos_sub(-1), - _global_set_triplet_sub(-1), + _mission_item_triplet_sub(-1), _att_sub(-1), _airspeed_sub(-1), _control_mode_sub(-1), @@ -548,11 +548,11 @@ void FixedwingPositionControl::vehicle_setpoint_poll() { /* check if there is a new setpoint */ - bool global_sp_updated; - orb_check(_global_set_triplet_sub, &global_sp_updated); + bool mission_item_triplet_updated; + orb_check(_mission_item_triplet_sub, &mission_item_triplet_updated); - if (global_sp_updated) { - orb_copy(ORB_ID(vehicle_global_position_set_triplet), _global_set_triplet_sub, &_global_triplet); + if (mission_item_triplet_updated) { + orb_copy(ORB_ID(mission_item_triplet), _mission_item_triplet_sub, &_mission_item_triplet); _setpoint_valid = true; } } @@ -625,7 +625,7 @@ FixedwingPositionControl::calculate_gndspeed_undershoot() bool FixedwingPositionControl::control_position(const math::Vector2f ¤t_position, const math::Vector2f &ground_speed, - const struct vehicle_global_position_set_triplet_s &global_triplet) + const struct mission_item_triplet_s &mission_item_triplet) { bool setpoint = true; @@ -641,7 +641,7 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio math::Vector3 accel_earth = _R_nb.transpose() * accel_body; _tecs.update_50hz(baro_altitude, _airspeed.indicated_airspeed_m_s, _R_nb, accel_body, accel_earth); - float altitude_error = _global_triplet.current.altitude - _global_pos.alt; + float altitude_error = _mission_item_triplet.current.altitude - _global_pos.alt; /* no throttle limit as default */ float throttle_max = 1.0f; @@ -662,27 +662,27 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio if (_setpoint_valid) { /* current waypoint (the one currently heading for) */ - math::Vector2f next_wp(global_triplet.current.lat / 1e7f, global_triplet.current.lon / 1e7f); + math::Vector2f next_wp(mission_item_triplet.current.lat, mission_item_triplet.current.lon); /* previous waypoint */ math::Vector2f prev_wp; - if (global_triplet.previous_valid) { - prev_wp.setX(global_triplet.previous.lat / 1e7f); - prev_wp.setY(global_triplet.previous.lon / 1e7f); + if (mission_item_triplet.previous_valid) { + prev_wp.setX(mission_item_triplet.previous.lat); + prev_wp.setY(mission_item_triplet.previous.lon); } else { /* * No valid previous waypoint, go for the current wp. * This is automatically handled by the L1 library. */ - prev_wp.setX(global_triplet.current.lat / 1e7f); - prev_wp.setY(global_triplet.current.lon / 1e7f); + prev_wp.setX(mission_item_triplet.current.lat); + prev_wp.setY(mission_item_triplet.current.lon); } // XXX add RTL switch - if (global_triplet.current.nav_cmd == NAV_CMD_RETURN_TO_LAUNCH && _launch_valid) { + if (mission_item_triplet.current.nav_cmd == NAV_CMD_RETURN_TO_LAUNCH && _launch_valid) { math::Vector2f rtl_pos(_launch_lat, _launch_lon); @@ -698,35 +698,35 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio // XXX handle case when having arrived at home (loiter) - } else if (global_triplet.current.nav_cmd == NAV_CMD_WAYPOINT) { + } else if (mission_item_triplet.current.nav_cmd == NAV_CMD_WAYPOINT) { /* waypoint is a plain navigation waypoint */ _l1_control.navigate_waypoints(prev_wp, next_wp, current_position, ground_speed); _att_sp.roll_body = _l1_control.nav_roll(); _att_sp.yaw_body = _l1_control.nav_bearing(); - _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_trim), + _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _mission_item_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_trim), _airspeed.indicated_airspeed_m_s, eas2tas, false, math::radians(_parameters.pitch_limit_min), _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max)); - } else if (global_triplet.current.nav_cmd == NAV_CMD_LOITER_TURN_COUNT || - global_triplet.current.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || - global_triplet.current.nav_cmd == NAV_CMD_LOITER_UNLIMITED) { + } else if (mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_TURN_COUNT || + mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || + mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_UNLIMITED) { /* waypoint is a loiter waypoint */ - _l1_control.navigate_loiter(next_wp, current_position, global_triplet.current.loiter_radius, - global_triplet.current.loiter_direction, ground_speed); + _l1_control.navigate_loiter(next_wp, current_position, mission_item_triplet.current.loiter_radius, + mission_item_triplet.current.loiter_direction, ground_speed); _att_sp.roll_body = _l1_control.nav_roll(); _att_sp.yaw_body = _l1_control.nav_bearing(); - _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_trim), + _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _mission_item_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_trim), _airspeed.indicated_airspeed_m_s, eas2tas, false, math::radians(_parameters.pitch_limit_min), _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max)); - } else if (global_triplet.current.nav_cmd == NAV_CMD_LAND) { + } else if (mission_item_triplet.current.nav_cmd == NAV_CMD_LAND) { /* switch to heading hold for the last meters, continue heading hold after */ @@ -737,7 +737,7 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio /* heading hold, along the line connecting this and the last waypoint */ - // if (global_triplet.previous_valid) { + // if (mission_item_triplet.previous_valid) { // target_bearing = get_bearing_to_next_waypoint(prev_wp.getX(), prev_wp.getY(), next_wp.getX(), next_wp.getY()); // } else { @@ -760,7 +760,7 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio /* do not go down too early */ if (wp_distance > 50.0f) { - altitude_error = (_global_triplet.current.altitude + 25.0f) - _global_pos.alt; + altitude_error = (_mission_item_triplet.current.altitude + 25.0f) - _global_pos.alt; } @@ -770,7 +770,7 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio /* apply minimum pitch (flare) and limit roll if close to touch down, altitude error is negative (going down) */ // XXX this could make a great param - float flare_angle_rad = math::radians(10.0f);//math::radians(global_triplet.current.param1) + float flare_angle_rad = math::radians(10.0f);//math::radians(mission_item_triplet.current.param1) float land_pitch_min = math::radians(5.0f); float throttle_land = _parameters.throttle_min + (_parameters.throttle_max - _parameters.throttle_min) * 0.1f; float airspeed_land = _parameters.airspeed_min; @@ -783,7 +783,7 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio /* force TECS to only control speed with pitch, altitude is only implicitely controlled now */ _tecs.set_speed_weight(2.0f); - _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(airspeed_land), + _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _mission_item_triplet.current.altitude, calculate_target_airspeed(airspeed_land), _airspeed.indicated_airspeed_m_s, eas2tas, false, flare_angle_rad, 0.0f, _parameters.throttle_max, throttle_land, @@ -799,7 +799,7 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio /* minimize speed to approach speed */ - _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(airspeed_approach), + _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _mission_item_triplet.current.altitude, calculate_target_airspeed(airspeed_approach), _airspeed.indicated_airspeed_m_s, eas2tas, false, flare_angle_rad, _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, @@ -809,14 +809,14 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio /* normal cruise speed */ - _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_trim), + _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _mission_item_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_trim), _airspeed.indicated_airspeed_m_s, eas2tas, false, math::radians(_parameters.pitch_limit_min), _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max)); } - } else if (global_triplet.current.nav_cmd == NAV_CMD_TAKEOFF) { + } else if (mission_item_triplet.current.nav_cmd == NAV_CMD_TAKEOFF) { _l1_control.navigate_waypoints(prev_wp, next_wp, current_position, ground_speed); _att_sp.roll_body = _l1_control.nav_roll(); @@ -826,9 +826,9 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio if (altitude_error > 10.0f) { /* enforce a minimum of 10 degrees pitch up on takeoff, or take parameter */ - _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_min), + _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _mission_item_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_min), _airspeed.indicated_airspeed_m_s, eas2tas, - true, math::max(math::radians(global_triplet.current.param1), math::radians(10.0f)), + true, math::max(math::radians(mission_item_triplet.current.radius), math::radians(10.0f)), _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max)); @@ -837,7 +837,7 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio } else { - _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_trim), + _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _mission_item_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_trim), _airspeed.indicated_airspeed_m_s, eas2tas, false, math::radians(_parameters.pitch_limit_min), _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, @@ -848,7 +848,7 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio // warnx("nav bearing: %8.4f bearing err: %8.4f target bearing: %8.4f", (double)_l1_control.nav_bearing(), // (double)_l1_control.bearing_error(), (double)_l1_control.target_bearing()); // warnx("prev wp: %8.4f/%8.4f, next wp: %8.4f/%8.4f prev:%s", (double)prev_wp.getX(), (double)prev_wp.getY(), - // (double)next_wp.getX(), (double)next_wp.getY(), (global_triplet.previous_valid) ? "valid" : "invalid"); + // (double)next_wp.getX(), (double)next_wp.getY(), (mission_item_triplet.previous_valid) ? "valid" : "invalid"); // XXX at this point we always want no loiter hold if a // mission is active @@ -902,7 +902,7 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio } /* reset land state */ - if (global_triplet.current.nav_cmd != NAV_CMD_LAND) { + if (mission_item_triplet.current.nav_cmd != NAV_CMD_LAND) { land_noreturn = false; } @@ -1018,7 +1018,7 @@ FixedwingPositionControl::task_main() * do subscriptions */ _global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); - _global_set_triplet_sub = orb_subscribe(ORB_ID(vehicle_global_position_set_triplet)); + _mission_item_triplet_sub = orb_subscribe(ORB_ID(mission_item_triplet)); _att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); _accel_sub = orb_subscribe(ORB_ID(sensor_accel)); _control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); @@ -1108,7 +1108,7 @@ FixedwingPositionControl::task_main() * Attempt to control position, on success (= sensors present and not in manual mode), * publish setpoint. */ - if (control_position(current_position, ground_speed, _global_triplet)) { + if (control_position(current_position, ground_speed, _mission_item_triplet)) { _att_sp.timestamp = hrt_absolute_time(); /* lazily publish the setpoint only once available */ @@ -1121,7 +1121,8 @@ FixedwingPositionControl::task_main() _attitude_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_att_sp); } - float turn_distance = _l1_control.switch_distance(_global_triplet.current.turn_distance_xy); + /* XXX check if radius makes sense here */ + float turn_distance = _l1_control.switch_distance(_mission_item_triplet.current.radius); /* lazily publish navigation capabilities */ if (turn_distance != _nav_capabilities.turn_distance && turn_distance > 0) { diff --git a/src/modules/mavlink/orb_topics.h b/src/modules/mavlink/orb_topics.h index 91773843b6..2cba25338b 100644 --- a/src/modules/mavlink/orb_topics.h +++ b/src/modules/mavlink/orb_topics.h @@ -52,7 +52,7 @@ #include #include #include -#include +#include #include #include #include diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 9e73fcb229..5d4a6ef947 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -56,7 +56,7 @@ #include #include #include -#include +#include #include #include #include @@ -128,12 +128,12 @@ private: struct vehicle_status_s _vstatus; /**< vehicle status */ struct vehicle_global_position_s _global_pos; /**< global vehicle position */ - struct vehicle_global_position_set_triplet_s _global_triplet; /**< triplet of global setpoints */ + struct mission_item_triplet_s _mission_item_triplet; /**< triplet of mission items */ perf_counter_t _loop_perf; /**< loop performance counter */ - unsigned _mission_items_maxcount; /**< maximum number of mission items supported */ - struct mission_item_s * _mission_items; /**< storage for mission items */ + unsigned _mission_item_count; /**< maximum number of mission items supported */ + struct mission_item_s * _mission_items; /**< storage for mission */ bool _mission_valid; /**< flag if mission is valid */ @@ -427,7 +427,7 @@ Navigator::task_main() math::Vector2f ground_speed(_global_pos.vx, _global_pos.vy); // Current waypoint - math::Vector2f next_wp(_global_triplet.current.lat / 1e7f, _global_triplet.current.lon / 1e7f); + math::Vector2f next_wp(_mission_item_triplet.current.lat / 1e7f, _mission_item_triplet.current.lon / 1e7f); // Global position math::Vector2f current_position(_global_pos.lat / 1e7f, _global_pos.lon / 1e7f); @@ -441,17 +441,17 @@ Navigator::task_main() // Next waypoint math::Vector2f prev_wp; - if (_global_triplet.previous_valid) { - prev_wp.setX(_global_triplet.previous.lat / 1e7f); - prev_wp.setY(_global_triplet.previous.lon / 1e7f); + if (_mission_item_triplet.previous_valid) { + prev_wp.setX(_mission_item_triplet.previous.lat / 1e7f); + prev_wp.setY(_mission_item_triplet.previous.lon / 1e7f); } else { /* * No valid next waypoint, go for heading hold. * This is automatically handled by the L1 library. */ - prev_wp.setX(_global_triplet.current.lat / 1e7f); - prev_wp.setY(_global_triplet.current.lon / 1e7f); + prev_wp.setX(_mission_item_triplet.current.lat / 1e7f); + prev_wp.setY(_mission_item_triplet.current.lon / 1e7f); } @@ -461,13 +461,13 @@ Navigator::task_main() // XXX to be put in its own class - if (_global_triplet.current.nav_cmd == NAV_CMD_WAYPOINT) { + if (_mission_item_triplet.current.nav_cmd == NAV_CMD_WAYPOINT) { /* waypoint is a plain navigation waypoint */ - } else if (_global_triplet.current.nav_cmd == NAV_CMD_LOITER_TURN_COUNT || - _global_triplet.current.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || - _global_triplet.current.nav_cmd == NAV_CMD_LOITER_UNLIMITED) { + } else if (_mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_TURN_COUNT || + _mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || + _mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_UNLIMITED) { /* waypoint is a loiter waypoint */ @@ -504,28 +504,28 @@ Navigator::task_main() /******** MAIN NAVIGATION STATE MACHINE ********/ - if (_global_triplet.current.nav_cmd == NAV_CMD_RETURN_TO_LAUNCH) { + if (_mission_item_triplet.current.nav_cmd == NAV_CMD_RETURN_TO_LAUNCH) { // XXX define launch position and return - } else if (_global_triplet.current.nav_cmd == NAV_CMD_LAND) { + } else if (_mission_item_triplet.current.nav_cmd == NAV_CMD_LAND) { // XXX flared descent on final approach - } else if (_global_triplet.current.nav_cmd == NAV_CMD_TAKEOFF) { + } else if (_mission_item_triplet.current.nav_cmd == NAV_CMD_TAKEOFF) { /* apply minimum pitch if altitude has not yet been reached */ - // if (_global_pos.alt < _global_triplet.current.altitude) { - // _att_sp.pitch_body = math::max(_att_sp.pitch_body, _global_triplet.current.param1); + // if (_global_pos.alt < _mission_item_triplet.current.altitude) { + // _att_sp.pitch_body = math::max(_att_sp.pitch_body, _mission_item_triplet.current.param1); // } } /* lazily publish the setpoint only once available */ if (_triplet_pub > 0) { /* publish the attitude setpoint */ - orb_publish(ORB_ID(vehicle_global_position_set_triplet), _triplet_pub, &_global_triplet); + orb_publish(ORB_ID(mission_item_triplet), _triplet_pub, &_mission_item_triplet); } else { /* advertise and publish */ - _triplet_pub = orb_advertise(ORB_ID(vehicle_global_position_set_triplet), &_global_triplet); + _triplet_pub = orb_advertise(ORB_ID(mission_item_triplet), &_mission_item_triplet); } } diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index 769fe90459..ecc1a3b3a2 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -120,8 +120,8 @@ ORB_DEFINE(vehicle_bodyframe_speed_setpoint, struct vehicle_bodyframe_speed_setp #include "topics/vehicle_global_position_setpoint.h" ORB_DEFINE(vehicle_global_position_setpoint, struct vehicle_global_position_setpoint_s); -#include "topics/vehicle_global_position_set_triplet.h" -ORB_DEFINE(vehicle_global_position_set_triplet, struct vehicle_global_position_set_triplet_s); +#include "topics/mission_item_triplet.h" +ORB_DEFINE(mission_item_triplet, struct mission_item_triplet_s); #include "topics/vehicle_global_velocity_setpoint.h" ORB_DEFINE(vehicle_global_velocity_setpoint, struct vehicle_global_velocity_setpoint_s); diff --git a/src/modules/uORB/topics/vehicle_global_position_set_triplet.h b/src/modules/uORB/topics/mission_item_triplet.h similarity index 77% rename from src/modules/uORB/topics/vehicle_global_position_set_triplet.h rename to src/modules/uORB/topics/mission_item_triplet.h index 8516b263fc..48553b0ac5 100644 --- a/src/modules/uORB/topics/vehicle_global_position_set_triplet.h +++ b/src/modules/uORB/topics/mission_item_triplet.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (C) 2013 PX4 Development Team. All rights reserved. * Author: @author Thomas Gubler * @author Julian Oes * @author Lorenz Meier @@ -35,18 +35,18 @@ ****************************************************************************/ /** - * @file vehicle_global_position_setpoint.h + * @file mission_item_triplet.h * Definition of the global WGS84 position setpoint uORB topic. */ -#ifndef TOPIC_VEHICLE_GLOBAL_POSITION_SET_TRIPLET_H_ -#define TOPIC_VEHICLE_GLOBAL_POSITION_SET_TRIPLET_H_ +#ifndef TOPIC_MISSION_ITEM_TRIPLET_H_ +#define TOPIC_MISSION_ITEM_TRIPLET_H_ #include #include #include "../uORB.h" -#include "vehicle_global_position_setpoint.h" +#include "mission.h" /** * @addtogroup topics @@ -58,14 +58,15 @@ * * This are the three next waypoints (or just the next two or one). */ -struct vehicle_global_position_set_triplet_s +struct mission_item_triplet_s { - bool previous_valid; /**< flag indicating previous position is valid */ - bool next_valid; /**< flag indicating next position is valid */ + bool previous_valid; + bool current_valid; /**< flag indicating previous mission item is valid */ + bool next_valid; /**< flag indicating next mission item is valid */ - struct vehicle_global_position_setpoint_s previous; - struct vehicle_global_position_setpoint_s current; - struct vehicle_global_position_setpoint_s next; + struct mission_item_s previous; + struct mission_item_s current; + struct mission_item_s next; }; /** @@ -73,6 +74,6 @@ struct vehicle_global_position_set_triplet_s */ /* register this as object request broker structure */ -ORB_DECLARE(vehicle_global_position_set_triplet); +ORB_DECLARE(mission_item_triplet); #endif From a27c7e831945f0a6b95b50b9ac68364b28a49362 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 20 Nov 2013 12:53:05 +0100 Subject: [PATCH 128/447] Navigator: Added simple mission triplet publication on waypoint change --- src/modules/navigator/navigator_main.cpp | 111 ++++++++++++++++++++--- 1 file changed, 98 insertions(+), 13 deletions(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 5d4a6ef947..a255797c6a 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -201,6 +201,8 @@ private: void publish_safepoints(unsigned points); bool fence_valid(const struct fence_s &fence); + + void current_waypoint_changed(unsigned next_setpoint_index); }; namespace navigator @@ -234,7 +236,7 @@ Navigator::Navigator() : /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "navigator")), /* states */ - _mission_items_maxcount(20), + _mission_item_count(0), _mission_valid(false), _fence_valid(false), _inside_fence(true), @@ -290,19 +292,28 @@ Navigator::mission_update() // XXX this is not optimal yet, but a first prototype / // test implementation - if (mission.count <= _mission_items_maxcount) { - /* - * Perform an atomic copy & state update - */ - irqstate_t flags = irqsave(); - - memcpy(_mission_items, mission.items, mission.count * sizeof(struct mission_item_s)); - _mission_valid = true; - - irqrestore(flags); - } else { - warnx("mission larger than storage space"); + if (mission.count > _mission_item_count) { + _mission_items = (mission_item_s*)malloc(sizeof(mission_item_s) * _mission_item_count); + if (!_mission_items) { + _mission_item_count = 0; + warnx("no free RAM to allocate mission, rejecting any waypoints"); + } } + + /* + * Perform an atomic copy & state update + */ + irqstate_t flags = irqsave(); + + memcpy(_mission_items, mission.items, mission.count * sizeof(struct mission_item_s)); + _mission_valid = true; + _mission_item_count = mission.count; + + irqrestore(flags); + + /* Reset to 0 for now when a waypoint is changed */ + /* TODO add checks if and how the mission has changed */ + current_waypoint_changed(0); } } @@ -670,6 +681,80 @@ Navigator::fence_point(int argc, char *argv[]) errx(1, "can't store fence point"); } +void +Navigator::current_waypoint_changed(unsigned new_setpoint_index) +{ + /* TODO: extend this to different frames, global for now */ + + _mission_item_triplet.current_valid = false; + + if (_mission_item_count > 0 && new_setpoint_index < _mission_item_count) { + _mission_item_triplet.current_valid = true; + memcpy(&_mission_item_triplet.current, &_mission_items[new_setpoint_index], sizeof(mission_item_s)); + } + + int previous_setpoint_index = -1; + _mission_item_triplet.previous_valid = false; + + if (new_setpoint_index > 0) { + previous_setpoint_index = new_setpoint_index - 1; + } + + while (previous_setpoint_index >= 0) { + + if ((_mission_items[previous_setpoint_index].nav_cmd == (int)MAV_CMD_NAV_WAYPOINT || + _mission_items[previous_setpoint_index].nav_cmd == (int)MAV_CMD_NAV_LOITER_TURNS || + _mission_items[previous_setpoint_index].nav_cmd == (int)MAV_CMD_NAV_LOITER_TIME || + _mission_items[previous_setpoint_index].nav_cmd == (int)MAV_CMD_NAV_LOITER_UNLIM)) { + + _mission_item_triplet.previous_valid = true; + memcpy(&_mission_item_triplet.previous, &_mission_items[previous_setpoint_index], sizeof(mission_item_s)); + break; + } + + previous_setpoint_index--; + } + + /* + * Check if next WP (in mission, not in execution order) + * is available and identify correct index + */ + int next_setpoint_index = -1; + _mission_item_triplet.next_valid = false; + + /* next waypoint */ + if (_mission_item_count > 1) { + next_setpoint_index = new_setpoint_index + 1; + } + + while (next_setpoint_index < _mission_item_count - 1) { + + if ((_mission_items[next_setpoint_index].nav_cmd == (int)MAV_CMD_NAV_WAYPOINT || + _mission_items[next_setpoint_index].nav_cmd == (int)MAV_CMD_NAV_LOITER_TURNS || + _mission_items[next_setpoint_index].nav_cmd == (int)MAV_CMD_NAV_LOITER_TIME || + _mission_items[next_setpoint_index].nav_cmd == (int)MAV_CMD_NAV_LOITER_UNLIM)) { + + _mission_item_triplet.next_valid = true; + memcpy(&_mission_item_triplet.next, &_mission_items[next_setpoint_index], sizeof(mission_item_s)); + break; + } + + next_setpoint_index++; + } + + + /* lazily publish the setpoint only once available */ + if (_triplet_pub > 0) { + /* publish the attitude setpoint */ + orb_publish(ORB_ID(mission_item_triplet), _triplet_pub, &_mission_item_triplet); + + } else { + /* advertise and publish */ + _triplet_pub = orb_advertise(ORB_ID(mission_item_triplet), &_mission_item_triplet); + } + +} + static void usage() { errx(1, "usage: navigator {start|stop|status|fence}"); From 7c741073fedf878af6a65100f00cd554fd5e332c Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 20 Nov 2013 17:05:17 +0100 Subject: [PATCH 129/447] Navigator: Only support 10WPs for now --- src/modules/navigator/navigator_main.cpp | 73 ++++++++++++------------ 1 file changed, 37 insertions(+), 36 deletions(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index a255797c6a..075e1a26f4 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -132,10 +132,10 @@ private: perf_counter_t _loop_perf; /**< loop performance counter */ - unsigned _mission_item_count; /**< maximum number of mission items supported */ - struct mission_item_s * _mission_items; /**< storage for mission */ - bool _mission_valid; /**< flag if mission is valid */ - + unsigned _max_mission_item_count; /**< maximum number of mission items supported */ + + unsigned _mission_item_count; /** number of mission items copied */ + struct mission_item_s *_mission_item; /**< storage for mission */ struct fence_s _fence; /**< storage for fence vertices */ bool _fence_valid; /**< flag if fence is valid */ @@ -236,8 +236,7 @@ Navigator::Navigator() : /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "navigator")), /* states */ - _mission_item_count(0), - _mission_valid(false), + _max_mission_item_count(10), _fence_valid(false), _inside_fence(true), _loiter_hold(false) @@ -246,6 +245,8 @@ Navigator::Navigator() : memset(&_fence, 0, sizeof(_fence)); _parameter_handles.throttle_cruise = param_find("NAV_DUMMY"); + _mission_item = (mission_item_s*)malloc(sizeof(mission_item_s) * _max_mission_item_count); + /* fetch initial values */ parameters_update(); } @@ -292,28 +293,25 @@ Navigator::mission_update() // XXX this is not optimal yet, but a first prototype / // test implementation - if (mission.count > _mission_item_count) { - _mission_items = (mission_item_s*)malloc(sizeof(mission_item_s) * _mission_item_count); - if (!_mission_items) { - _mission_item_count = 0; - warnx("no free RAM to allocate mission, rejecting any waypoints"); - } + if (mission.count <= _max_mission_item_count) { + /* + * Perform an atomic copy & state update + */ + irqstate_t flags = irqsave(); + + memcpy(_mission_item, mission.items, mission.count * sizeof(struct mission_item_s)); + _mission_item_count = mission.count; + + irqrestore(flags); + + current_waypoint_changed(0); + } else { + warnx("ERROR: too many waypoints, not supported"); } - /* - * Perform an atomic copy & state update - */ - irqstate_t flags = irqsave(); - - memcpy(_mission_items, mission.items, mission.count * sizeof(struct mission_item_s)); - _mission_valid = true; - _mission_item_count = mission.count; - - irqrestore(flags); - /* Reset to 0 for now when a waypoint is changed */ /* TODO add checks if and how the mission has changed */ - current_waypoint_changed(0); + } } @@ -447,7 +445,7 @@ Navigator::task_main() if (1 /* autonomous flight */) { /* execute navigation once we have a setpoint */ - if (_mission_valid) { + if (_mission_item_count > 0) { // Next waypoint math::Vector2f prev_wp; @@ -690,7 +688,8 @@ Navigator::current_waypoint_changed(unsigned new_setpoint_index) if (_mission_item_count > 0 && new_setpoint_index < _mission_item_count) { _mission_item_triplet.current_valid = true; - memcpy(&_mission_item_triplet.current, &_mission_items[new_setpoint_index], sizeof(mission_item_s)); + memcpy(&_mission_item_triplet.current, &_mission_item[new_setpoint_index], sizeof(mission_item_s)); + warnx("current is valid"); } int previous_setpoint_index = -1; @@ -702,13 +701,14 @@ Navigator::current_waypoint_changed(unsigned new_setpoint_index) while (previous_setpoint_index >= 0) { - if ((_mission_items[previous_setpoint_index].nav_cmd == (int)MAV_CMD_NAV_WAYPOINT || - _mission_items[previous_setpoint_index].nav_cmd == (int)MAV_CMD_NAV_LOITER_TURNS || - _mission_items[previous_setpoint_index].nav_cmd == (int)MAV_CMD_NAV_LOITER_TIME || - _mission_items[previous_setpoint_index].nav_cmd == (int)MAV_CMD_NAV_LOITER_UNLIM)) { + if ((_mission_item[previous_setpoint_index].nav_cmd == (int)MAV_CMD_NAV_WAYPOINT || + _mission_item[previous_setpoint_index].nav_cmd == (int)MAV_CMD_NAV_LOITER_TURNS || + _mission_item[previous_setpoint_index].nav_cmd == (int)MAV_CMD_NAV_LOITER_TIME || + _mission_item[previous_setpoint_index].nav_cmd == (int)MAV_CMD_NAV_LOITER_UNLIM)) { _mission_item_triplet.previous_valid = true; - memcpy(&_mission_item_triplet.previous, &_mission_items[previous_setpoint_index], sizeof(mission_item_s)); + memcpy(&_mission_item_triplet.previous, &_mission_item[previous_setpoint_index], sizeof(mission_item_s)); + warnx("previous is valid"); break; } @@ -729,13 +729,14 @@ Navigator::current_waypoint_changed(unsigned new_setpoint_index) while (next_setpoint_index < _mission_item_count - 1) { - if ((_mission_items[next_setpoint_index].nav_cmd == (int)MAV_CMD_NAV_WAYPOINT || - _mission_items[next_setpoint_index].nav_cmd == (int)MAV_CMD_NAV_LOITER_TURNS || - _mission_items[next_setpoint_index].nav_cmd == (int)MAV_CMD_NAV_LOITER_TIME || - _mission_items[next_setpoint_index].nav_cmd == (int)MAV_CMD_NAV_LOITER_UNLIM)) { + if ((_mission_item[next_setpoint_index].nav_cmd == (int)MAV_CMD_NAV_WAYPOINT || + _mission_item[next_setpoint_index].nav_cmd == (int)MAV_CMD_NAV_LOITER_TURNS || + _mission_item[next_setpoint_index].nav_cmd == (int)MAV_CMD_NAV_LOITER_TIME || + _mission_item[next_setpoint_index].nav_cmd == (int)MAV_CMD_NAV_LOITER_UNLIM)) { _mission_item_triplet.next_valid = true; - memcpy(&_mission_item_triplet.next, &_mission_items[next_setpoint_index], sizeof(mission_item_s)); + memcpy(&_mission_item_triplet.next, &_mission_item[next_setpoint_index], sizeof(mission_item_s)); + warnx("next is valid"); break; } From 344705396d94602b5c3b3b34c61131602bc03fde Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 20 Nov 2013 22:27:28 +0100 Subject: [PATCH 130/447] TECS: I don't need this debug output --- src/lib/external_lgpl/tecs/tecs.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/external_lgpl/tecs/tecs.cpp b/src/lib/external_lgpl/tecs/tecs.cpp index 510b8ed9c1..cfb505194b 100644 --- a/src/lib/external_lgpl/tecs/tecs.cpp +++ b/src/lib/external_lgpl/tecs/tecs.cpp @@ -240,7 +240,7 @@ void TECS::_update_height_demand(float demand, float state) _hgt_rate_dem = -_maxSinkRate; } - warnx("_hgt_rate_dem: %.4f, _hgt_dem_adj %.4f", _hgt_rate_dem, _hgt_dem_adj); + // warnx("_hgt_rate_dem: %.4f, _hgt_dem_adj %.4f", _hgt_rate_dem, _hgt_dem_adj); } void TECS::_detect_underspeed(void) From f351afe3f69c81e4f1ee43f24531728e4b518cea Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 20 Nov 2013 22:28:05 +0100 Subject: [PATCH 131/447] Geo: Copy distance function over from mavlink --- src/lib/geo/geo.c | 24 ++++++++++++++++++++++++ src/lib/geo/geo.h | 4 ++++ 2 files changed, 28 insertions(+) diff --git a/src/lib/geo/geo.c b/src/lib/geo/geo.c index 8aeca5be75..614f001865 100644 --- a/src/lib/geo/geo.c +++ b/src/lib/geo/geo.c @@ -387,6 +387,30 @@ __EXPORT int get_distance_to_arc(struct crosstrack_error_s * crosstrack_error, d return return_value; } +__EXPORT float get_distance_to_point_global_wgs84(double lat_now, double lon_now, float alt_now, + double lat_next, double lon_next, float alt_next, + float *dist_xy, float *dist_z) +{ + double current_x_rad = lat_next / 180.0 * M_PI; + double current_y_rad = lon_next / 180.0 * M_PI; + double x_rad = lat_now / 180.0 * M_PI; + double y_rad = lon_now / 180.0 * M_PI; + + double d_lat = x_rad - current_x_rad; + double d_lon = y_rad - current_y_rad; + + double a = sin(d_lat / 2.0) * sin(d_lat / 2.0) + sin(d_lon / 2.0) * sin(d_lon / 2.0f) * cos(current_x_rad) * cos(x_rad); + double c = 2 * atan2(sqrt(a), sqrt(1 - a)); + + float dxy = CONSTANTS_RADIUS_OF_EARTH * c; + float dz = alt_now - alt_next; + + *dist_xy = fabsf(dxy); + *dist_z = fabsf(dz); + + return sqrtf(dxy * dxy + dz * dz); +} + __EXPORT float _wrap_pi(float bearing) { /* value is inf or NaN */ diff --git a/src/lib/geo/geo.h b/src/lib/geo/geo.h index 6bce9309b1..47f643593c 100644 --- a/src/lib/geo/geo.h +++ b/src/lib/geo/geo.h @@ -124,6 +124,10 @@ __EXPORT int get_distance_to_line(struct crosstrack_error_s * crosstrack_error, __EXPORT int get_distance_to_arc(struct crosstrack_error_s * crosstrack_error, double lat_now, double lon_now, double lat_center, double lon_center, float radius, float arc_start_bearing, float arc_sweep); +__EXPORT float get_distance_to_point_global_wgs84(double lat_now, double lon_now, float alt_now, + double lat_next, double lon_next, float alt_next, + float *dist_xy, float *dist_z); + __EXPORT float _wrap_180(float bearing); __EXPORT float _wrap_360(float bearing); __EXPORT float _wrap_pi(float bearing); From b3c657450056eab9ec1549b80a4cf4c002d1503b Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 20 Nov 2013 22:34:15 +0100 Subject: [PATCH 132/447] Waypoints: Get time inside WP radius right --- src/modules/mavlink/waypoints.c | 4 ++-- src/modules/uORB/topics/mission.h | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/modules/mavlink/waypoints.c b/src/modules/mavlink/waypoints.c index 5fdbd57e14..964adee1da 100644 --- a/src/modules/mavlink/waypoints.c +++ b/src/modules/mavlink/waypoints.c @@ -76,7 +76,7 @@ void map_mavlink_mission_item_to_mission_item(const mavlink_mission_item_t *mavl mission_item->loiter_direction = (mavlink_mission_item->param3 > 0) ? 1 : -1; /* 1 if positive CW, -1 if negative CCW */ mission_item->nav_cmd = NAV_CMD_WAYPOINT; // TODO correct mission_item->radius = mavlink_mission_item->param1; - mission_item->time_inside = mavlink_mission_item->param2; + mission_item->time_inside = mavlink_mission_item->param2 / 1e3f; /* from milliseconds to seconds */ } void map_mission_item_to_mavlink_mission_item(const struct mission_item_s *mission_item, const uint16_t seq, mavlink_mission_item_t *mavlink_mission_item) @@ -89,7 +89,7 @@ void map_mission_item_to_mavlink_mission_item(const struct mission_item_s *missi mavlink_mission_item->param3 = mission_item->loiter_radius*(float)mission_item->loiter_direction; mavlink_mission_item->command = MAV_CMD_NAV_WAYPOINT; // TODO add mavlink_mission_item->param1 = mission_item->radius; - mavlink_mission_item->param2 = mission_item->time_inside; + mavlink_mission_item->param2 = mission_item->time_inside * 1e3f; /* from seconds to milliseconds */ mavlink_mission_item->seq = seq; } diff --git a/src/modules/uORB/topics/mission.h b/src/modules/uORB/topics/mission.h index ec78352798..4c251269b3 100644 --- a/src/modules/uORB/topics/mission.h +++ b/src/modules/uORB/topics/mission.h @@ -78,7 +78,7 @@ struct mission_item_s int8_t loiter_direction; /**< 1: positive / clockwise, -1, negative. */ enum NAV_CMD nav_cmd; /**< true if loitering is enabled */ float radius; /**< radius in which the mission is accepted as reached in meters */ - float time_inside; /**< time that the MAV should stay inside the radius before advancing in milliseconds */ + float time_inside; /**< time that the MAV should stay inside the radius before advancing in seconds */ }; struct mission_s From 31f0edd6636e14d64fd9c18dcd62bfa7befac374 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 20 Nov 2013 22:36:53 +0100 Subject: [PATCH 133/447] Mission topic: the autocontinue option was missing --- src/modules/mavlink/waypoints.c | 2 ++ src/modules/uORB/topics/mission.h | 1 + 2 files changed, 3 insertions(+) diff --git a/src/modules/mavlink/waypoints.c b/src/modules/mavlink/waypoints.c index 964adee1da..a4e31bda65 100644 --- a/src/modules/mavlink/waypoints.c +++ b/src/modules/mavlink/waypoints.c @@ -77,6 +77,7 @@ void map_mavlink_mission_item_to_mission_item(const mavlink_mission_item_t *mavl mission_item->nav_cmd = NAV_CMD_WAYPOINT; // TODO correct mission_item->radius = mavlink_mission_item->param1; mission_item->time_inside = mavlink_mission_item->param2 / 1e3f; /* from milliseconds to seconds */ + mission_item->autocontinue = mavlink_mission_item->autocontinue; } void map_mission_item_to_mavlink_mission_item(const struct mission_item_s *mission_item, const uint16_t seq, mavlink_mission_item_t *mavlink_mission_item) @@ -90,6 +91,7 @@ void map_mission_item_to_mavlink_mission_item(const struct mission_item_s *missi mavlink_mission_item->command = MAV_CMD_NAV_WAYPOINT; // TODO add mavlink_mission_item->param1 = mission_item->radius; mavlink_mission_item->param2 = mission_item->time_inside * 1e3f; /* from seconds to milliseconds */ + mavlink_mission_item->autocontinue = mission_item->autocontinue; mavlink_mission_item->seq = seq; } diff --git a/src/modules/uORB/topics/mission.h b/src/modules/uORB/topics/mission.h index 4c251269b3..f97de94bc4 100644 --- a/src/modules/uORB/topics/mission.h +++ b/src/modules/uORB/topics/mission.h @@ -79,6 +79,7 @@ struct mission_item_s enum NAV_CMD nav_cmd; /**< true if loitering is enabled */ float radius; /**< radius in which the mission is accepted as reached in meters */ float time_inside; /**< time that the MAV should stay inside the radius before advancing in seconds */ + bool autocontinue; /**< true if next waypoint should follow after this one */ }; struct mission_s From 18941155b2f87ffd0d07f7a0a14a25dbdf05e8b4 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 20 Nov 2013 22:37:49 +0100 Subject: [PATCH 134/447] Navigator: Checking if a waypoint was reached and switching to next one works rudimentary --- src/modules/navigator/navigator_main.cpp | 140 +++++++++++++++++++++-- 1 file changed, 132 insertions(+), 8 deletions(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 075e1a26f4..ae7c1f252f 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -137,12 +137,18 @@ private: unsigned _mission_item_count; /** number of mission items copied */ struct mission_item_s *_mission_item; /**< storage for mission */ + int _current_mission_item_index; /** current active mission item , -1 for none */ + struct fence_s _fence; /**< storage for fence vertices */ bool _fence_valid; /**< flag if fence is valid */ bool _inside_fence; /**< vehicle is inside fence */ struct navigation_capabilities_s _nav_caps; + bool _waypoint_position_reached; + bool _waypoint_yaw_reached; + uint64_t _time_first_inside_orbit; + /** manual control states */ float _seatbelt_hold_heading; /**< heading the system should hold in seatbelt mode */ float _loiter_hold_lat; @@ -202,7 +208,9 @@ private: bool fence_valid(const struct fence_s &fence); - void current_waypoint_changed(unsigned next_setpoint_index); + void change_current_mission_item(int new_mission_item_index); + + bool mission_item_reached(); }; namespace navigator @@ -239,6 +247,10 @@ Navigator::Navigator() : _max_mission_item_count(10), _fence_valid(false), _inside_fence(true), + _current_mission_item_index(-1), + _waypoint_position_reached(false), + _waypoint_yaw_reached(false), + _time_first_inside_orbit(0), _loiter_hold(false) { _global_pos.valid = false; @@ -304,7 +316,8 @@ Navigator::mission_update() irqrestore(flags); - current_waypoint_changed(0); + /* start new mission at beginning */ + change_current_mission_item(0); } else { warnx("ERROR: too many waypoints, not supported"); } @@ -444,6 +457,11 @@ Navigator::task_main() if (1 /* autonomous flight */) { + /* proceed to next waypoint if we reach it */ + if (mission_item_reached()) { + change_current_mission_item(_current_mission_item_index + 1); + } + /* execute navigation once we have a setpoint */ if (_mission_item_count > 0) { @@ -680,23 +698,41 @@ Navigator::fence_point(int argc, char *argv[]) } void -Navigator::current_waypoint_changed(unsigned new_setpoint_index) +Navigator::change_current_mission_item(int new_mission_item_index) { + /* no change */ + if (new_mission_item_index == _current_mission_item_index) { + return; + } + /* or maybe there are no more missions */ + if (new_mission_item_index >= _mission_item_count) { + /* XXX switch to loiter here */ + return; + } + + /* accept new mission item */ + _current_mission_item_index = new_mission_item_index; + + /* reset all states */ + _waypoint_position_reached = false; + _waypoint_yaw_reached = false; + _time_first_inside_orbit = 0; + /* TODO: extend this to different frames, global for now */ _mission_item_triplet.current_valid = false; - if (_mission_item_count > 0 && new_setpoint_index < _mission_item_count) { + if (_mission_item_count > 0 && new_mission_item_index < _mission_item_count) { _mission_item_triplet.current_valid = true; - memcpy(&_mission_item_triplet.current, &_mission_item[new_setpoint_index], sizeof(mission_item_s)); + memcpy(&_mission_item_triplet.current, &_mission_item[new_mission_item_index], sizeof(mission_item_s)); warnx("current is valid"); } int previous_setpoint_index = -1; _mission_item_triplet.previous_valid = false; - if (new_setpoint_index > 0) { - previous_setpoint_index = new_setpoint_index - 1; + if (new_mission_item_index > 0) { + previous_setpoint_index = new_mission_item_index - 1; } while (previous_setpoint_index >= 0) { @@ -724,7 +760,7 @@ Navigator::current_waypoint_changed(unsigned new_setpoint_index) /* next waypoint */ if (_mission_item_count > 1) { - next_setpoint_index = new_setpoint_index + 1; + next_setpoint_index = new_mission_item_index + 1; } while (next_setpoint_index < _mission_item_count - 1) { @@ -756,6 +792,94 @@ Navigator::current_waypoint_changed(unsigned new_setpoint_index) } + +bool +Navigator::mission_item_reached() +{ + uint64_t now = hrt_absolute_time(); + float orbit; + + if (_mission_item_triplet.current.nav_cmd == (int)MAV_CMD_NAV_WAYPOINT) { + + orbit = _mission_item_triplet.current.radius; + + } else if (_mission_item_triplet.current.nav_cmd == (int)MAV_CMD_NAV_LOITER_TURNS || + _mission_item_triplet.current.nav_cmd == (int)MAV_CMD_NAV_LOITER_TIME || + _mission_item_triplet.current.nav_cmd == (int)MAV_CMD_NAV_LOITER_UNLIM) { + + orbit = _mission_item_triplet.current.loiter_radius; + } else { + + // XXX set default orbit via param + orbit = 15.0f; + } + + /* keep vertical orbit */ + float vertical_switch_distance = orbit; + + /* Take the larger turn distance - orbit or turn_distance */ + if (orbit < _nav_caps.turn_distance) + orbit = _nav_caps.turn_distance; + + // TODO add frame + // int coordinate_frame = wpm->waypoints[wpm->current_active_wp_id].frame; + + float dist = -1.0f; + float dist_xy = -1.0f; + float dist_z = -1.0f; + + // if (coordinate_frame == (int)MAV_FRAME_GLOBAL) { + dist = get_distance_to_point_global_wgs84(_mission_item_triplet.current.lat, _mission_item_triplet.current.lon, _mission_item_triplet.current.altitude, + (double)_global_pos.lat / 1e7, (double)_global_pos.lon / 1e7, _global_pos.alt, + &dist_xy, &dist_z); + + // warnx("1 lat: %2.2f, lon: %2.2f, alt: %2.2f", _mission_item_triplet.current.lat, _mission_item_triplet.current.lon, _mission_item_triplet.current.altitude); + // warnx("2 lat: %2.2f, lon: %2.2f, alt: %2.2f", (double)_global_pos.lat / 1e7, (double)_global_pos.lon / 1e7, _global_pos.alt); + + // warnx("Dist: %4.4f", dist); + + // } else if (coordinate_frame == (int)MAV_FRAME_GLOBAL_RELATIVE_ALT) { + // dist = mavlink_wpm_distance_to_point_global_wgs84(wpm->current_active_wp_id, (float)global_pos->lat * 1e-7f, (float)global_pos->lon * 1e-7f, global_pos->relative_alt, &dist_xy, &dist_z); + + // } else if (coordinate_frame == (int)MAV_FRAME_LOCAL_ENU || coordinate_frame == (int)MAV_FRAME_LOCAL_NED) { + // dist = mavlink_wpm_distance_to_point_local(wpm->current_active_wp_id, local_pos->x, local_pos->y, local_pos->z, &dist_xy, &dist_z); + + // } else if (coordinate_frame == (int)MAV_FRAME_MISSION) { + // /* Check if conditions of mission item are satisfied */ + // // XXX TODO + // } + + if (dist >= 0.0f && dist_xy <= orbit && dist_z >= 0.0f && dist_z <= vertical_switch_distance) { + _waypoint_position_reached = true; + } + + /* check if required yaw reached */ + float yaw_sp = _wrap_pi(_mission_item_triplet.current.yaw); + float yaw_err = _wrap_pi(yaw_sp - _global_pos.yaw); + + if (fabsf(yaw_err) < 0.05f) { /* XXX get rid of magic number */ + _waypoint_yaw_reached = true; + } + + /* check if the current waypoint was reached */ + if (_waypoint_position_reached /* && _waypoint_yaw_reached */) { /* XXX what about yaw? */ + + if (_time_first_inside_orbit == 0) { + /* XXX announcment? */ + _time_first_inside_orbit = now; + } + + /* check if the MAV was long enough inside the waypoint orbit */ + if ((now - _time_first_inside_orbit >= (uint64_t)_mission_item_triplet.current.time_inside * 1e6) + || _mission_item_triplet.current.nav_cmd == (int)MAV_CMD_NAV_TAKEOFF) { + + return true; + } + } + return false; + +} + static void usage() { errx(1, "usage: navigator {start|stop|status|fence}"); From dca72ba6eefbda45060cbfb4033dacd64fd14403 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 21 Nov 2013 10:47:02 +0100 Subject: [PATCH 135/447] L1_pos_controller: Fixed strange bitwise or --- src/lib/ecl/l1/ecl_l1_pos_controller.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/ecl/l1/ecl_l1_pos_controller.cpp b/src/lib/ecl/l1/ecl_l1_pos_controller.cpp index 31d7cecb7b..11def23710 100644 --- a/src/lib/ecl/l1/ecl_l1_pos_controller.cpp +++ b/src/lib/ecl/l1/ecl_l1_pos_controller.cpp @@ -280,7 +280,7 @@ void ECL_L1_Pos_Controller::navigate_loiter(const math::Vector2f &vector_A, cons */ // XXX check switch over - if ((lateral_accel_sp_center < lateral_accel_sp_circle && loiter_direction > 0 && xtrack_err_circle > 0.0f) | + 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; From 8535d12e60438784c90fc92ad676b5f49289ad1a Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 21 Nov 2013 10:48:26 +0100 Subject: [PATCH 136/447] Navigator: Switch to loiter after mission works --- src/modules/navigator/navigator_main.cpp | 156 ++++++++++++++--------- 1 file changed, 93 insertions(+), 63 deletions(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index ae7c1f252f..79e93f4907 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -704,81 +704,112 @@ Navigator::change_current_mission_item(int new_mission_item_index) if (new_mission_item_index == _current_mission_item_index) { return; } - /* or maybe there are no more missions */ + /* or maybe there are no more mission items */ if (new_mission_item_index >= _mission_item_count) { - /* XXX switch to loiter here */ - return; - } - - /* accept new mission item */ - _current_mission_item_index = new_mission_item_index; - - /* reset all states */ - _waypoint_position_reached = false; - _waypoint_yaw_reached = false; - _time_first_inside_orbit = 0; - - /* TODO: extend this to different frames, global for now */ - - _mission_item_triplet.current_valid = false; - - if (_mission_item_count > 0 && new_mission_item_index < _mission_item_count) { + + /* just keep the last mission item and set it to loiter */ + _mission_item_triplet.previous_valid = false; _mission_item_triplet.current_valid = true; - memcpy(&_mission_item_triplet.current, &_mission_item[new_mission_item_index], sizeof(mission_item_s)); - warnx("current is valid"); + _mission_item_triplet.next_valid = false; + + switch (_mission_item_triplet.current.nav_cmd) { + + /* if the last mission is not a loiter item, set it to one */ + case NAV_CMD_WAYPOINT: + case NAV_CMD_RETURN_TO_LAUNCH: + case NAV_CMD_TAKEOFF: + + _mission_item_triplet.current.nav_cmd = NAV_CMD_LOITER_UNLIMITED; + _mission_item_triplet.current.loiter_radius = 100.0f; + //_mission_item_triplet.current.loiter_radius = _nav_caps.turn_distance; // TODO: publish capabilities somewhere + _mission_item_triplet.current.loiter_direction = 1; + break; + + /* if the last mission item was to loiter, continue unlimited */ + case NAV_CMD_LOITER_TURN_COUNT: + case NAV_CMD_LOITER_TIME_LIMIT: + + _mission_item_triplet.current.nav_cmd = NAV_CMD_LOITER_UNLIMITED; + break; + /* if already in loiter, don't change anything */ + case NAV_CMD_LOITER_UNLIMITED: + break; + /* if landed, stay in land mode */ + case NAV_CMD_LAND: + break; + + default: + warnx("Unsupported nav_cmd"); + break; + } } + else { + /* accept new mission item */ + _current_mission_item_index = new_mission_item_index; - int previous_setpoint_index = -1; - _mission_item_triplet.previous_valid = false; + /* reset all states */ + _waypoint_position_reached = false; + _waypoint_yaw_reached = false; + _time_first_inside_orbit = 0; - if (new_mission_item_index > 0) { - previous_setpoint_index = new_mission_item_index - 1; - } + /* TODO: extend this to different frames, global for now */ - while (previous_setpoint_index >= 0) { + _mission_item_triplet.current_valid = false; - if ((_mission_item[previous_setpoint_index].nav_cmd == (int)MAV_CMD_NAV_WAYPOINT || - _mission_item[previous_setpoint_index].nav_cmd == (int)MAV_CMD_NAV_LOITER_TURNS || - _mission_item[previous_setpoint_index].nav_cmd == (int)MAV_CMD_NAV_LOITER_TIME || - _mission_item[previous_setpoint_index].nav_cmd == (int)MAV_CMD_NAV_LOITER_UNLIM)) { - - _mission_item_triplet.previous_valid = true; - memcpy(&_mission_item_triplet.previous, &_mission_item[previous_setpoint_index], sizeof(mission_item_s)); - warnx("previous is valid"); - break; + if (_mission_item_count > 0 && new_mission_item_index < _mission_item_count) { + _mission_item_triplet.current_valid = true; + memcpy(&_mission_item_triplet.current, &_mission_item[new_mission_item_index], sizeof(mission_item_s)); } - previous_setpoint_index--; - } + int previous_setpoint_index = -1; + _mission_item_triplet.previous_valid = false; - /* - * Check if next WP (in mission, not in execution order) - * is available and identify correct index - */ - int next_setpoint_index = -1; - _mission_item_triplet.next_valid = false; - - /* next waypoint */ - if (_mission_item_count > 1) { - next_setpoint_index = new_mission_item_index + 1; - } - - while (next_setpoint_index < _mission_item_count - 1) { - - if ((_mission_item[next_setpoint_index].nav_cmd == (int)MAV_CMD_NAV_WAYPOINT || - _mission_item[next_setpoint_index].nav_cmd == (int)MAV_CMD_NAV_LOITER_TURNS || - _mission_item[next_setpoint_index].nav_cmd == (int)MAV_CMD_NAV_LOITER_TIME || - _mission_item[next_setpoint_index].nav_cmd == (int)MAV_CMD_NAV_LOITER_UNLIM)) { - - _mission_item_triplet.next_valid = true; - memcpy(&_mission_item_triplet.next, &_mission_item[next_setpoint_index], sizeof(mission_item_s)); - warnx("next is valid"); - break; + if (new_mission_item_index > 0) { + previous_setpoint_index = new_mission_item_index - 1; } - next_setpoint_index++; - } + while (previous_setpoint_index >= 0) { + if ((_mission_item[previous_setpoint_index].nav_cmd == (int)MAV_CMD_NAV_WAYPOINT || + _mission_item[previous_setpoint_index].nav_cmd == (int)MAV_CMD_NAV_LOITER_TURNS || + _mission_item[previous_setpoint_index].nav_cmd == (int)MAV_CMD_NAV_LOITER_TIME || + _mission_item[previous_setpoint_index].nav_cmd == (int)MAV_CMD_NAV_LOITER_UNLIM)) { + + _mission_item_triplet.previous_valid = true; + memcpy(&_mission_item_triplet.previous, &_mission_item[previous_setpoint_index], sizeof(mission_item_s)); + break; + } + + previous_setpoint_index--; + } + + /* + * Check if next WP (in mission, not in execution order) + * is available and identify correct index + */ + int next_setpoint_index = -1; + _mission_item_triplet.next_valid = false; + + /* next waypoint */ + if (_mission_item_count > 1) { + next_setpoint_index = new_mission_item_index + 1; + } + + while (next_setpoint_index < _mission_item_count - 1) { + + if ((_mission_item[next_setpoint_index].nav_cmd == (int)MAV_CMD_NAV_WAYPOINT || + _mission_item[next_setpoint_index].nav_cmd == (int)MAV_CMD_NAV_LOITER_TURNS || + _mission_item[next_setpoint_index].nav_cmd == (int)MAV_CMD_NAV_LOITER_TIME || + _mission_item[next_setpoint_index].nav_cmd == (int)MAV_CMD_NAV_LOITER_UNLIM)) { + + _mission_item_triplet.next_valid = true; + memcpy(&_mission_item_triplet.next, &_mission_item[next_setpoint_index], sizeof(mission_item_s)); + break; + } + + next_setpoint_index++; + } + } /* lazily publish the setpoint only once available */ if (_triplet_pub > 0) { @@ -789,7 +820,6 @@ Navigator::change_current_mission_item(int new_mission_item_index) /* advertise and publish */ _triplet_pub = orb_advertise(ORB_ID(mission_item_triplet), &_mission_item_triplet); } - } From 4afb420bedf40de3b5b30929defd9a79b77aed32 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 21 Nov 2013 18:20:53 +0400 Subject: [PATCH 137/447] mc_att_control_vector: use rotation matrix for attitude setpoint, move yaw setpoint with stick, WIP --- .../mc_att_control_vector_main.cpp | 144 +++++++++++++----- 1 file changed, 109 insertions(+), 35 deletions(-) diff --git a/src/modules/mc_att_control_vector/mc_att_control_vector_main.cpp b/src/modules/mc_att_control_vector/mc_att_control_vector_main.cpp index 1783efc0e0..0faba307d1 100644 --- a/src/modules/mc_att_control_vector/mc_att_control_vector_main.cpp +++ b/src/modules/mc_att_control_vector/mc_att_control_vector_main.cpp @@ -75,6 +75,7 @@ #include #include #include +#include /** * Multicopter attitude control app start / stop handling function @@ -83,6 +84,9 @@ */ extern "C" __EXPORT int mc_att_control_vector_main(int argc, char *argv[]); +#define MIN_TAKEOFF_THROTTLE 0.3f +#define YAW_DEADZONE 0.01f + class MulticopterAttitudeControl { public: @@ -116,7 +120,8 @@ private: int _manual_sub; /**< notification of manual control updates */ int _arming_sub; /**< arming status of outputs */ - orb_advert_t _rate_sp_pub; /**< rate setpoint publication */ + orb_advert_t _att_sp_pub; /**< attitude setpoint publication */ + orb_advert_t _rates_sp_pub; /**< rate setpoint publication */ orb_advert_t _actuators_0_pub; /**< actuator control group 0 setpoint */ struct vehicle_attitude_s _att; /**< vehicle attitude */ @@ -125,7 +130,7 @@ private: struct vehicle_control_mode_s _control_mode; /**< vehicle control mode */ struct actuator_controls_s _actuators; /**< actuator control inputs */ struct actuator_armed_s _arming; /**< actuator arming status */ - struct vehicle_rates_setpoint_s _rates_setpoint; /**< vehicle rates setpoint */ + struct vehicle_rates_setpoint_s _rates_sp; /**< vehicle rates setpoint */ perf_counter_t _loop_perf; /**< loop performance counter */ @@ -208,7 +213,7 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : _arming_sub(-1), /* publications */ - _rate_sp_pub(-1), + _rates_sp_pub(-1), _actuators_0_pub(-1), /* performance counters */ @@ -368,6 +373,8 @@ MulticopterAttitudeControl::task_main() vehicle_manual_poll(); arming_status_poll(); + bool reset_yaw_sp = true; + /* wakeup source(s) */ struct pollfd fds[2]; @@ -406,12 +413,12 @@ MulticopterAttitudeControl::task_main() /* only run controller if attitude changed */ if (fds[1].revents & POLLIN) { static uint64_t last_run = 0; - float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f; + float dt = (hrt_absolute_time() - last_run) / 1000000.0f; last_run = hrt_absolute_time(); - /* guard against too large deltaT's */ - if (deltaT > 1.0f) - deltaT = 0.01f; + /* guard against too large dt's */ + if (dt > 0.02f) + dt = 0.02f; /* copy attitude topic */ orb_copy(ORB_ID(vehicle_attitude), _att_sub, &_att); @@ -421,28 +428,93 @@ MulticopterAttitudeControl::task_main() arming_status_poll(); vehicle_manual_poll(); - float roll_sp = 0.0f; - float pitch_sp = 0.0f; - float throttle_sp = 0.0f; - float yaw_sp = 0.0f; + float yaw_sp_move_rate = 0.0f; + bool publish_att_sp = false; - /* decide if in auto or manual control */ + /* define which input is the dominating control input */ if (_control_mode.flag_control_manual_enabled) { - roll_sp = _manual.roll; - pitch_sp = _manual.pitch; - yaw_sp = _manual.yaw; // XXX move yaw setpoint - throttle_sp = _manual.throttle; + /* manual input */ + if (!_control_mode.flag_control_climb_rate_enabled) { + /* pass throttle directly if not in altitude control mode */ + _att_sp.thrust = _manual.throttle; + } - } else if (_control_mode.flag_control_auto_enabled) { - roll_sp = _att_sp.roll_body; - pitch_sp = _att_sp.pitch_body; - yaw_sp = _att_sp.yaw_body; - throttle_sp = _att_sp.thrust; + if (_control_mode.flag_control_attitude_enabled) { + /* control attitude, update attitude setpoint depending on mode */ + + if (_att_sp.thrust < 0.1f) { + // TODO + //if (_status.condition_landed) { + /* reset yaw setpoint if on ground */ + // reset_yaw_sp = true; + //} + } else { + if (_manual.yaw < -YAW_DEADZONE || YAW_DEADZONE < _manual.yaw) { + /* move yaw setpoint */ + yaw_sp_move_rate = _manual.yaw; + _att_sp.yaw_body = _wrap_pi(_att_sp.yaw_body + yaw_sp_move_rate * dt); + publish_att_sp = true; + } + } + + /* reset yaw setpint to current position if needed */ + if (reset_yaw_sp) { + reset_yaw_sp = false; + _att_sp.yaw_body = _att.yaw; + publish_att_sp = true; + } + + if (!_control_mode.flag_control_velocity_enabled) { + /* update attitude setpoint if not in position control mode */ + _att_sp.roll_body = _manual.roll; + _att_sp.pitch_body = _manual.pitch; + publish_att_sp = true; + } + } else { + /* manual rate inputs (ACRO) */ + // TODO + /* reset yaw setpoint after ACRO */ + reset_yaw_sp = true; + } + + } else { + if (!_control_mode.flag_control_auto_enabled) { + /* no control, try to stay on place */ + if (!_control_mode.flag_control_velocity_enabled) { + /* no velocity control, reset attitude setpoint */ + _att_sp.roll_body = 0.0f; + _att_sp.pitch_body = 0.0f; + publish_att_sp = true; + } + } + + /* reset yaw setpoint after non-manual control */ + reset_yaw_sp = true; } - /* construct rotation matrix from euler angles */ - math::EulerAngles euler(roll_sp, pitch_sp, yaw_sp); - math::Dcm R_des(euler); + if (publish_att_sp || !_att_sp.R_valid) { + /* controller uses rotation matrix, not euler angles, convert if necessary */ + math::EulerAngles euler_sp(_att_sp.roll_body, _att_sp.pitch_body, _att_sp.yaw_body); + math::Dcm R_sp(euler_sp); + for (int i = 0; i < 9; i++) { + _att_sp.R_body[i] = R_sp(i / 3, i % 3); + } + _att_sp.R_valid = true; + } + + if (publish_att_sp) { + /* publish the attitude setpoint */ + _att_sp.timestamp = hrt_absolute_time(); + if (_att_sp_pub > 0) { + orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &_att_sp); + + } else { + _att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_att_sp); + } + } + + /* desired rotation matrix */ + math::Dcm R_des(&_att_sp.R_body[0]); /* rotation matrix for current state */ math::Dcm R(_att.R); @@ -454,28 +526,30 @@ MulticopterAttitudeControl::task_main() math::Vector3 e_R(-e_R_m(1, 2), e_R_m(0, 2), -e_R_m(0, 1)); /* angular rates setpoint*/ - math::Vector3 rates_sp = K_p * e_R; - math::Vector3 control = K_r_p * (rates_sp - rates); + math::Vector3 rates_sp = _K * e_R; + /* feed forward yaw setpoint rate */ + rates_sp(2) += yaw_sp_move_rate; + math::Vector3 control = _K_rate * (rates_sp - rates); /* publish the attitude rates setpoint */ - _rates_setpoint.roll = rates_sp(0); - _rates_setpoint.pitch = rates_sp(1); - _rates_setpoint.yaw = rates_sp(2); - _rates_setpoint.thrust = throttle_sp; - _rates_setpoint.timestamp = hrt_absolute_time(); + _rates_sp.roll = rates_sp(0); + _rates_sp.pitch = rates_sp(1); + _rates_sp.yaw = rates_sp(2); + _rates_sp.thrust = _att_sp.thrust; + _rates_sp.timestamp = hrt_absolute_time(); - if (_rate_sp_pub > 0) { - orb_publish(ORB_ID(vehicle_rates_setpoint), _actuators_0_pub, &_rates_setpoint); + if (_rates_sp_pub > 0) { + orb_publish(ORB_ID(vehicle_rates_setpoint), _rates_sp_pub, &_rates_sp); } else { - _rate_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &_rates_setpoint); + _rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &_rates_sp); } /* publish the attitude controls */ _actuators.control[0] = (isfinite(control(0))) ? control(0) : 0.0f; _actuators.control[1] = (isfinite(control(1))) ? control(1) : 0.0f; _actuators.control[2] = (isfinite(control(2))) ? control(2) : 0.0f; - _actuators.control[3] = (isfinite(throttle_sp)) ? throttle_sp : 0.0f; + _actuators.control[3] = (isfinite(_rates_sp.thrust)) ? _rates_sp.thrust : 0.0f; _actuators.timestamp = hrt_absolute_time(); if (_actuators_0_pub > 0) { From 42aefe838c2471c53c326347f0230c301689d96d Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 21 Nov 2013 17:01:54 +0100 Subject: [PATCH 138/447] Navigator: More improvements, loiter at the end works --- src/modules/navigator/navigator_main.cpp | 340 ++++++++++------------- 1 file changed, 141 insertions(+), 199 deletions(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 79e93f4907..fc2f6d3801 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -1,9 +1,9 @@ /**************************************************************************** * * Copyright (c) 2013 PX4 Development Team. All rights reserved. - * Authors: Lorenz Meier - * Jean Cyr - * Julian Oes + * Author: @author Lorenz Meier + * @author Jean Cyr + * @author Julian Oes * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -69,6 +69,11 @@ #include #include +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; /** * navigator app start / stop handling function @@ -208,9 +213,17 @@ private: bool fence_valid(const struct fence_s &fence); - void change_current_mission_item(int new_mission_item_index); + int add_mission_item(unsigned mission_item, struct mission_item_s *new_mission_item); - bool mission_item_reached(); + void add_last_mission_item(const struct mission_item_s *existing_mission_item, struct mission_item_s *new_mission_item); + + void advance_current_mission_item(); + + void restart_mission(); + + void reset_mission_item_reached(); + + bool check_mission_item_reached(); }; namespace navigator @@ -317,7 +330,7 @@ Navigator::mission_update() irqrestore(flags); /* start new mission at beginning */ - change_current_mission_item(0); + restart_mission(); } else { warnx("ERROR: too many waypoints, not supported"); } @@ -448,101 +461,18 @@ Navigator::task_main() } math::Vector2f ground_speed(_global_pos.vx, _global_pos.vy); - // Current waypoint + /* Current waypoint */ math::Vector2f next_wp(_mission_item_triplet.current.lat / 1e7f, _mission_item_triplet.current.lon / 1e7f); - // Global position + /* Global position */ math::Vector2f current_position(_global_pos.lat / 1e7f, _global_pos.lon / 1e7f); - /* AUTONOMOUS FLIGHT */ - + /* Autonomous flight */ if (1 /* autonomous flight */) { /* proceed to next waypoint if we reach it */ - if (mission_item_reached()) { - change_current_mission_item(_current_mission_item_index + 1); + if (check_mission_item_reached()) { + advance_current_mission_item(); } - - /* execute navigation once we have a setpoint */ - if (_mission_item_count > 0) { - - // Next waypoint - math::Vector2f prev_wp; - - if (_mission_item_triplet.previous_valid) { - prev_wp.setX(_mission_item_triplet.previous.lat / 1e7f); - prev_wp.setY(_mission_item_triplet.previous.lon / 1e7f); - - } else { - /* - * No valid next waypoint, go for heading hold. - * This is automatically handled by the L1 library. - */ - prev_wp.setX(_mission_item_triplet.current.lat / 1e7f); - prev_wp.setY(_mission_item_triplet.current.lon / 1e7f); - - } - - - - /******** MAIN NAVIGATION STATE MACHINE ********/ - - // XXX to be put in its own class - - if (_mission_item_triplet.current.nav_cmd == NAV_CMD_WAYPOINT) { - /* waypoint is a plain navigation waypoint */ - - - } else if (_mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_TURN_COUNT || - _mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || - _mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_UNLIMITED) { - - /* waypoint is a loiter waypoint */ - - } - - // XXX at this point we always want no loiter hold if a - // mission is active - _loiter_hold = false; - - } else { - - if (!_loiter_hold) { - _loiter_hold_lat = _global_pos.lat / 1e7f; - _loiter_hold_lon = _global_pos.lon / 1e7f; - _loiter_hold_alt = _global_pos.alt; - _loiter_hold = true; - } - - //_parameters.loiter_hold_radius - } - - } else if (0/* seatbelt mode enabled */) { - - /** SEATBELT FLIGHT **/ - continue; - - } else { - - /** MANUAL FLIGHT **/ - - /* no flight mode applies, do not publish an attitude setpoint */ - continue; - } - - /******** MAIN NAVIGATION STATE MACHINE ********/ - - if (_mission_item_triplet.current.nav_cmd == NAV_CMD_RETURN_TO_LAUNCH) { - // XXX define launch position and return - - } else if (_mission_item_triplet.current.nav_cmd == NAV_CMD_LAND) { - // XXX flared descent on final approach - - } else if (_mission_item_triplet.current.nav_cmd == NAV_CMD_TAKEOFF) { - - /* apply minimum pitch if altitude has not yet been reached */ - // if (_global_pos.alt < _mission_item_triplet.current.altitude) { - // _att_sp.pitch_body = math::max(_att_sp.pitch_body, _mission_item_triplet.current.param1); - // } } /* lazily publish the setpoint only once available */ @@ -554,7 +484,6 @@ Navigator::task_main() /* advertise and publish */ _triplet_pub = orb_advertise(ORB_ID(mission_item_triplet), &_mission_item_triplet); } - } perf_end(_loop_perf); @@ -697,39 +626,70 @@ Navigator::fence_point(int argc, char *argv[]) errx(1, "can't store fence point"); } -void -Navigator::change_current_mission_item(int new_mission_item_index) -{ - /* no change */ - if (new_mission_item_index == _current_mission_item_index) { - return; - } - /* or maybe there are no more mission items */ - if (new_mission_item_index >= _mission_item_count) { - - /* just keep the last mission item and set it to loiter */ - _mission_item_triplet.previous_valid = false; - _mission_item_triplet.current_valid = true; - _mission_item_triplet.next_valid = false; +int +Navigator::add_mission_item(unsigned mission_item_index, struct mission_item_s *new_mission_item) { - switch (_mission_item_triplet.current.nav_cmd) { + /* Check if there is a further mission as the new next item */ + while (mission_item_index < _mission_item_count) { + + if (1 /* TODO: check for correct frame */) { + + warnx("copying item number %d", mission_item_index); + memcpy(new_mission_item, &_mission_item[mission_item_index], sizeof(mission_item_s)); + return OK; + } + mission_item_index++; + } + + return ERROR; +} + +void +Navigator::add_last_mission_item(const struct mission_item_s *existing_mission_item, struct mission_item_s *new_mission_item) +{ + /* if no existing mission item exists, take curent location */ + if (existing_mission_item == nullptr) { + + new_mission_item->lat = (double)_global_pos.lat / 1e7; + new_mission_item->lon = (double)_global_pos.lon / 1e7; + new_mission_item->altitude = _global_pos.alt; + new_mission_item->yaw = _global_pos.yaw; + new_mission_item->nav_cmd = NAV_CMD_LOITER_UNLIMITED; + new_mission_item->loiter_direction = 1; + new_mission_item->loiter_radius = 100.0f; // TODO: get rid of magic number + new_mission_item->radius = 10.0f; // TODO: get rid of magic number + new_mission_item->autocontinue = false; + + } else { + + switch (existing_mission_item->nav_cmd) { /* if the last mission is not a loiter item, set it to one */ case NAV_CMD_WAYPOINT: case NAV_CMD_RETURN_TO_LAUNCH: case NAV_CMD_TAKEOFF: - _mission_item_triplet.current.nav_cmd = NAV_CMD_LOITER_UNLIMITED; - _mission_item_triplet.current.loiter_radius = 100.0f; + /* copy current mission to next item */ + memcpy(new_mission_item, existing_mission_item, sizeof(mission_item_s)); + + /* and set it to a loiter item */ + new_mission_item->nav_cmd = NAV_CMD_LOITER_UNLIMITED; + /* also adapt the loiter_radius */ + new_mission_item->loiter_radius = 100.0f; //_mission_item_triplet.current.loiter_radius = _nav_caps.turn_distance; // TODO: publish capabilities somewhere - _mission_item_triplet.current.loiter_direction = 1; + new_mission_item->loiter_direction = 1; + break; /* if the last mission item was to loiter, continue unlimited */ case NAV_CMD_LOITER_TURN_COUNT: case NAV_CMD_LOITER_TIME_LIMIT: - _mission_item_triplet.current.nav_cmd = NAV_CMD_LOITER_UNLIMITED; + /* copy current mission to next item */ + memcpy(new_mission_item, existing_mission_item, sizeof(mission_item_s)); + /* and set it to loiter */ + new_mission_item->nav_cmd = NAV_CMD_LOITER_UNLIMITED; + break; /* if already in loiter, don't change anything */ case NAV_CMD_LOITER_UNLIMITED: @@ -741,101 +701,86 @@ Navigator::change_current_mission_item(int new_mission_item_index) default: warnx("Unsupported nav_cmd"); break; - } - } - else { - /* accept new mission item */ - _current_mission_item_index = new_mission_item_index; - - /* reset all states */ - _waypoint_position_reached = false; - _waypoint_yaw_reached = false; - _time_first_inside_orbit = 0; - - /* TODO: extend this to different frames, global for now */ - - _mission_item_triplet.current_valid = false; - - if (_mission_item_count > 0 && new_mission_item_index < _mission_item_count) { - _mission_item_triplet.current_valid = true; - memcpy(&_mission_item_triplet.current, &_mission_item[new_mission_item_index], sizeof(mission_item_s)); } - - int previous_setpoint_index = -1; - _mission_item_triplet.previous_valid = false; - - if (new_mission_item_index > 0) { - previous_setpoint_index = new_mission_item_index - 1; - } - - while (previous_setpoint_index >= 0) { - - if ((_mission_item[previous_setpoint_index].nav_cmd == (int)MAV_CMD_NAV_WAYPOINT || - _mission_item[previous_setpoint_index].nav_cmd == (int)MAV_CMD_NAV_LOITER_TURNS || - _mission_item[previous_setpoint_index].nav_cmd == (int)MAV_CMD_NAV_LOITER_TIME || - _mission_item[previous_setpoint_index].nav_cmd == (int)MAV_CMD_NAV_LOITER_UNLIM)) { - - _mission_item_triplet.previous_valid = true; - memcpy(&_mission_item_triplet.previous, &_mission_item[previous_setpoint_index], sizeof(mission_item_s)); - break; - } - - previous_setpoint_index--; - } - - /* - * Check if next WP (in mission, not in execution order) - * is available and identify correct index - */ - int next_setpoint_index = -1; - _mission_item_triplet.next_valid = false; - - /* next waypoint */ - if (_mission_item_count > 1) { - next_setpoint_index = new_mission_item_index + 1; - } - - while (next_setpoint_index < _mission_item_count - 1) { - - if ((_mission_item[next_setpoint_index].nav_cmd == (int)MAV_CMD_NAV_WAYPOINT || - _mission_item[next_setpoint_index].nav_cmd == (int)MAV_CMD_NAV_LOITER_TURNS || - _mission_item[next_setpoint_index].nav_cmd == (int)MAV_CMD_NAV_LOITER_TIME || - _mission_item[next_setpoint_index].nav_cmd == (int)MAV_CMD_NAV_LOITER_UNLIM)) { - - _mission_item_triplet.next_valid = true; - memcpy(&_mission_item_triplet.next, &_mission_item[next_setpoint_index], sizeof(mission_item_s)); - break; - } - - next_setpoint_index++; - } - } - - /* lazily publish the setpoint only once available */ - if (_triplet_pub > 0) { - /* publish the attitude setpoint */ - orb_publish(ORB_ID(mission_item_triplet), _triplet_pub, &_mission_item_triplet); - - } else { - /* advertise and publish */ - _triplet_pub = orb_advertise(ORB_ID(mission_item_triplet), &_mission_item_triplet); } } +void +Navigator::advance_current_mission_item() +{ + /* if there is one more mission available we can just advance by one, otherwise return */ + if (_mission_item_triplet.next_valid) { + + reset_mission_item_reached(); + + /* copy current mission to previous item */ + memcpy(&_mission_item_triplet.previous, &_mission_item_triplet.current, sizeof(mission_item_s)); + _mission_item_triplet.previous_valid = _mission_item_triplet.current_valid; + + /* copy the next to current */ + memcpy(&_mission_item_triplet.current, &_mission_item_triplet.next, sizeof(mission_item_s)); + _mission_item_triplet.current_valid = _mission_item_triplet.next_valid; + + _current_mission_item_index++; + + + /* maybe there are no more mission item, in this case add a loiter mission item */ + if (add_mission_item(_current_mission_item_index + 1, &_mission_item_triplet.next) != OK) { + + add_last_mission_item(&_mission_item_triplet.current, &_mission_item_triplet.next); + _mission_item_triplet.next_valid = true; + } + } +} + +void +Navigator::restart_mission() +{ + reset_mission_item_reached(); + + _current_mission_item_index = 0; + + _mission_item_triplet.previous_valid = false; + + /* add a new current mission item */ + if (add_mission_item(_current_mission_item_index, &_mission_item_triplet.current) != OK) { + + add_last_mission_item(nullptr, &_mission_item_triplet.current); + } else { + /* if current succeeds, we can even add a next item */ + if (add_mission_item(_current_mission_item_index + 1, &_mission_item_triplet.next) != OK) { + add_last_mission_item(&_mission_item_triplet.current, &_mission_item_triplet.next); + } + _mission_item_triplet.next_valid = true; + } + _mission_item_triplet.current_valid = true; +} + + + + +void +Navigator::reset_mission_item_reached() +{ + /* reset all states */ + _waypoint_position_reached = false; + _waypoint_yaw_reached = false; + _time_first_inside_orbit = 0; +} bool -Navigator::mission_item_reached() +Navigator::check_mission_item_reached() { uint64_t now = hrt_absolute_time(); float orbit; - if (_mission_item_triplet.current.nav_cmd == (int)MAV_CMD_NAV_WAYPOINT) { + if (_mission_item_triplet.current.nav_cmd == NAV_CMD_WAYPOINT) { orbit = _mission_item_triplet.current.radius; - } else if (_mission_item_triplet.current.nav_cmd == (int)MAV_CMD_NAV_LOITER_TURNS || - _mission_item_triplet.current.nav_cmd == (int)MAV_CMD_NAV_LOITER_TIME || - _mission_item_triplet.current.nav_cmd == (int)MAV_CMD_NAV_LOITER_UNLIM) { + } else if (_mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_TURN_COUNT || + _mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || + _mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_UNLIMITED) { orbit = _mission_item_triplet.current.loiter_radius; } else { @@ -847,9 +792,6 @@ Navigator::mission_item_reached() /* keep vertical orbit */ float vertical_switch_distance = orbit; - /* Take the larger turn distance - orbit or turn_distance */ - if (orbit < _nav_caps.turn_distance) - orbit = _nav_caps.turn_distance; // TODO add frame // int coordinate_frame = wpm->waypoints[wpm->current_active_wp_id].frame; From 7892a72f90be76fc948a0fbefb2357d29bbdffcc Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 22 Nov 2013 14:03:09 +0100 Subject: [PATCH 139/447] Navigator, waypoints: save index in mission item and use this in navigator --- src/modules/mavlink/waypoints.c | 53 ++++++--- src/modules/navigator/navigator_main.cpp | 110 +++++++++--------- src/modules/uORB/topics/mission.h | 1 + .../uORB/topics/mission_item_triplet.h | 4 + 4 files changed, 100 insertions(+), 68 deletions(-) diff --git a/src/modules/mavlink/waypoints.c b/src/modules/mavlink/waypoints.c index a4e31bda65..a9ee26eacf 100644 --- a/src/modules/mavlink/waypoints.c +++ b/src/modules/mavlink/waypoints.c @@ -60,6 +60,10 @@ bool debug = false; bool verbose = false; +orb_advert_t mission_pub = -1; +struct mission_s mission; + +#define NUM_MISSIONS_SUPPORTED 10 //#define MAVLINK_WPM_NO_PRINTF #define MAVLINK_WPM_VERBOSE 1 @@ -78,9 +82,10 @@ void map_mavlink_mission_item_to_mission_item(const mavlink_mission_item_t *mavl mission_item->radius = mavlink_mission_item->param1; mission_item->time_inside = mavlink_mission_item->param2 / 1e3f; /* from milliseconds to seconds */ mission_item->autocontinue = mavlink_mission_item->autocontinue; + mission_item->index = mavlink_mission_item->seq; } -void map_mission_item_to_mavlink_mission_item(const struct mission_item_s *mission_item, const uint16_t seq, mavlink_mission_item_t *mavlink_mission_item) +void map_mission_item_to_mavlink_mission_item(const struct mission_item_s *mission_item, mavlink_mission_item_t *mavlink_mission_item) { mavlink_mission_item->x = (float)mission_item->lat; @@ -92,8 +97,7 @@ void map_mission_item_to_mavlink_mission_item(const struct mission_item_s *missi mavlink_mission_item->param1 = mission_item->radius; mavlink_mission_item->param2 = mission_item->time_inside * 1e3f; /* from seconds to milliseconds */ mavlink_mission_item->autocontinue = mission_item->autocontinue; - - mavlink_mission_item->seq = seq; + mavlink_mission_item->seq = mission_item->index; } void mavlink_wpm_init(mavlink_wpm_storage *state) @@ -117,6 +121,13 @@ void mavlink_wpm_init(mavlink_wpm_storage *state) state->timestamp_lastoutside_orbit = 0;///< timestamp when the MAV was last outside the orbit or had the wrong yaw value state->timestamp_firstinside_orbit = 0;///< timestamp when the MAV was the first time after a waypoint change inside the orbit and had the correct yaw value + mission.count = 0; + mission.items = (struct mission_item_s*)malloc(sizeof(struct mission_item_s) * NUM_MISSIONS_SUPPORTED); + if (!mission.items) { + mission.count = 0; + /* XXX reject waypoints if this fails */ + warnx("no free RAM to allocate mission, rejecting any waypoints"); + } } /* @@ -558,9 +569,6 @@ int mavlink_waypoint_eventloop(uint64_t now, const struct vehicle_global_positio void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehicle_global_position_s *global_pos , struct vehicle_local_position_s *local_pos) { - static orb_advert_t mission_pub = -1; - static struct mission_s mission; - uint64_t now = mavlink_missionlib_get_system_timestamp(); switch (msg->msgid) { @@ -711,7 +719,7 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi wpm->current_wp_id = wpr.seq; mavlink_mission_item_t wp; - map_mission_item_to_mavlink_mission_item(&mission.items[wpr.seq], wpr.seq, &wp); + map_mission_item_to_mavlink_mission_item(&mission.items[wpr.seq], &wp); mavlink_wpm_send_waypoint(wpm->current_partner_sysid, wpm->current_partner_compid, &wp); } else { @@ -835,14 +843,11 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi wpm->current_partner_compid = msg->compid; wpm->current_count = wpc.count; - /* prepare mission topic */ - mission.count = wpc.count; - mission.items = (struct mission_item_s*)malloc(sizeof(struct mission_item_s) * mission.count); - - if (!mission.items) { - mission.count = 0; - /* XXX reject waypoints if this fails */ - warnx("no free RAM to allocate mission, rejecting any waypoints"); + if (wpc.count > NUM_MISSIONS_SUPPORTED) { + warnx("Too many waypoints: %d, supported: %d", wpc.count, NUM_MISSIONS_SUPPORTED); + } else { + /* prepare mission topic */ + mission.count = wpc.count; } #ifdef MAVLINK_WPM_NO_PRINTF @@ -1087,8 +1092,26 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi wpm->yaw_reached = false; wpm->pos_reached = false; + /* prepare mission topic */ + mission.count = 0; + memset(mission.items, 0, sizeof(struct mission_item_s)*NUM_MISSIONS_SUPPORTED); + + /* Initialize mission publication if necessary */ + if (mission_pub < 0) { + mission_pub = orb_advertise(ORB_ID(mission), &mission); + + } else { + orb_publish(ORB_ID(mission), mission_pub, &mission); + } + + + warnx("Mission cleared"); + + mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_ACCEPTED); + } else if (wpca.target_system == mavlink_system.sysid /*&& wpca.target_component == mavlink_wpm_comp_id */ && wpm->current_state != MAVLINK_WPM_STATE_IDLE) { // if (verbose) // printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_CLEAR_LIST from %u because i'm doing something else already (state=%i).\n", msg->sysid, wpm->current_state); + warnx("not cleared"); } break; diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index fc2f6d3801..9066a3b424 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -69,11 +69,6 @@ #include #include -/* oddly, ERROR is not defined for c++ */ -#ifdef ERROR -# undef ERROR -#endif -static const int ERROR = -1; /** * navigator app start / stop handling function @@ -142,8 +137,6 @@ private: unsigned _mission_item_count; /** number of mission items copied */ struct mission_item_s *_mission_item; /**< storage for mission */ - int _current_mission_item_index; /** current active mission item , -1 for none */ - struct fence_s _fence; /**< storage for fence vertices */ bool _fence_valid; /**< flag if fence is valid */ bool _inside_fence; /**< vehicle is inside fence */ @@ -213,9 +206,11 @@ private: bool fence_valid(const struct fence_s &fence); - int add_mission_item(unsigned mission_item, struct mission_item_s *new_mission_item); + void add_mission_item(unsigned mission_item_index, + const struct mission_item_s *existing_mission_item, + struct mission_item_s *new_mission_item); - void add_last_mission_item(const struct mission_item_s *existing_mission_item, struct mission_item_s *new_mission_item); + void update_mission_item_triplet(); void advance_current_mission_item(); @@ -260,7 +255,6 @@ Navigator::Navigator() : _max_mission_item_count(10), _fence_valid(false), _inside_fence(true), - _current_mission_item_index(-1), _waypoint_position_reached(false), _waypoint_yaw_reached(false), _time_first_inside_orbit(0), @@ -626,27 +620,20 @@ Navigator::fence_point(int argc, char *argv[]) errx(1, "can't store fence point"); } -int -Navigator::add_mission_item(unsigned mission_item_index, struct mission_item_s *new_mission_item) { +void +Navigator::add_mission_item(unsigned mission_item_index, const struct mission_item_s *existing_mission_item, struct mission_item_s *new_mission_item) { /* Check if there is a further mission as the new next item */ while (mission_item_index < _mission_item_count) { if (1 /* TODO: check for correct frame */) { - warnx("copying item number %d", mission_item_index); memcpy(new_mission_item, &_mission_item[mission_item_index], sizeof(mission_item_s)); - return OK; + return; } mission_item_index++; } - return ERROR; -} - -void -Navigator::add_last_mission_item(const struct mission_item_s *existing_mission_item, struct mission_item_s *new_mission_item) -{ /* if no existing mission item exists, take curent location */ if (existing_mission_item == nullptr) { @@ -706,54 +693,71 @@ Navigator::add_last_mission_item(const struct mission_item_s *existing_mission_i } void -Navigator::advance_current_mission_item() +Navigator::update_mission_item_triplet() { - /* if there is one more mission available we can just advance by one, otherwise return */ - if (_mission_item_triplet.next_valid) { - - reset_mission_item_reached(); - - /* copy current mission to previous item */ - memcpy(&_mission_item_triplet.previous, &_mission_item_triplet.current, sizeof(mission_item_s)); - _mission_item_triplet.previous_valid = _mission_item_triplet.current_valid; - - /* copy the next to current */ - memcpy(&_mission_item_triplet.current, &_mission_item_triplet.next, sizeof(mission_item_s)); - _mission_item_triplet.current_valid = _mission_item_triplet.next_valid; - - _current_mission_item_index++; - + if (!_mission_item_triplet.current_valid) { - /* maybe there are no more mission item, in this case add a loiter mission item */ - if (add_mission_item(_current_mission_item_index + 1, &_mission_item_triplet.next) != OK) { + /* the current mission item is missing, add one */ + if (_mission_item_triplet.previous_valid) { + /* if we know the last one, proceed to succeeding one */ + add_mission_item(_mission_item_triplet.previous.index + 1, &_mission_item_triplet.previous, &_mission_item_triplet.current); + } + else { + /* if we don't remember the last one, start new */ + add_mission_item(0, nullptr, &_mission_item_triplet.current); + } + _mission_item_triplet.current_valid = true; + } - add_last_mission_item(&_mission_item_triplet.current, &_mission_item_triplet.next); + if (_mission_item_triplet.current_valid && !_mission_item_triplet.next_valid) { + + if (_mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_UNLIMITED) { + /* if we are already loitering, don't bother about a next mission item */ + + _mission_item_triplet.next_valid = false; + } else { + + add_mission_item(_mission_item_triplet.current.index + 1, &_mission_item_triplet.current, &_mission_item_triplet.next); _mission_item_triplet.next_valid = true; } } } +void +Navigator::advance_current_mission_item() +{ + /* if there is no more mission available, don't advance and return */ + if (!_mission_item_triplet.next_valid) { + return; + } + + reset_mission_item_reached(); + + /* copy current mission to previous item */ + memcpy(&_mission_item_triplet.previous, &_mission_item_triplet.current, sizeof(mission_item_s)); + _mission_item_triplet.previous_valid = _mission_item_triplet.current_valid; + + /* copy the next to current */ + memcpy(&_mission_item_triplet.current, &_mission_item_triplet.next, sizeof(mission_item_s)); + _mission_item_triplet.current_valid = _mission_item_triplet.next_valid; + + /* flag the next mission as invalid */ + _mission_item_triplet.next_valid = false; + + update_mission_item_triplet(); +} + void Navigator::restart_mission() { reset_mission_item_reached(); - _current_mission_item_index = 0; - + /* forget about the all mission items */ _mission_item_triplet.previous_valid = false; + _mission_item_triplet.current_valid = false; + _mission_item_triplet.next_valid = false; - /* add a new current mission item */ - if (add_mission_item(_current_mission_item_index, &_mission_item_triplet.current) != OK) { - - add_last_mission_item(nullptr, &_mission_item_triplet.current); - } else { - /* if current succeeds, we can even add a next item */ - if (add_mission_item(_current_mission_item_index + 1, &_mission_item_triplet.next) != OK) { - add_last_mission_item(&_mission_item_triplet.current, &_mission_item_triplet.next); - } - _mission_item_triplet.next_valid = true; - } - _mission_item_triplet.current_valid = true; + update_mission_item_triplet(); } diff --git a/src/modules/uORB/topics/mission.h b/src/modules/uORB/topics/mission.h index f97de94bc4..a39a1e4d7e 100644 --- a/src/modules/uORB/topics/mission.h +++ b/src/modules/uORB/topics/mission.h @@ -80,6 +80,7 @@ struct mission_item_s float radius; /**< radius in which the mission is accepted as reached in meters */ float time_inside; /**< time that the MAV should stay inside the radius before advancing in seconds */ bool autocontinue; /**< true if next waypoint should follow after this one */ + int index; /**< index matching the mavlink waypoint */ }; struct mission_s diff --git a/src/modules/uORB/topics/mission_item_triplet.h b/src/modules/uORB/topics/mission_item_triplet.h index 48553b0ac5..b35eae6070 100644 --- a/src/modules/uORB/topics/mission_item_triplet.h +++ b/src/modules/uORB/topics/mission_item_triplet.h @@ -67,6 +67,10 @@ struct mission_item_triplet_s struct mission_item_s previous; struct mission_item_s current; struct mission_item_s next; + + int previous_index; + int current_index; + int next_index; }; /** From 5748a9c964b2c7d5b682a8d539e3c52d34a34d25 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 22 Nov 2013 14:07:21 +0100 Subject: [PATCH 140/447] Navigator: Publish mission triplet only when actually updated --- src/modules/navigator/navigator_main.cpp | 20 ++++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 9066a3b424..e58b9ae115 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -468,16 +468,6 @@ Navigator::task_main() advance_current_mission_item(); } } - - /* lazily publish the setpoint only once available */ - if (_triplet_pub > 0) { - /* publish the attitude setpoint */ - orb_publish(ORB_ID(mission_item_triplet), _triplet_pub, &_mission_item_triplet); - - } else { - /* advertise and publish */ - _triplet_pub = orb_advertise(ORB_ID(mission_item_triplet), &_mission_item_triplet); - } } perf_end(_loop_perf); @@ -721,6 +711,16 @@ Navigator::update_mission_item_triplet() _mission_item_triplet.next_valid = true; } } + + /* lazily publish the mission triplet only once available */ + if (_triplet_pub > 0) { + /* publish the mission triplet */ + orb_publish(ORB_ID(mission_item_triplet), _triplet_pub, &_mission_item_triplet); + + } else { + /* advertise and publish */ + _triplet_pub = orb_advertise(ORB_ID(mission_item_triplet), &_mission_item_triplet); + } } void From 1da7599541d33b8ffb0c8764bb505f1d5e6018b9 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 22 Nov 2013 14:10:09 +0100 Subject: [PATCH 141/447] Navigator: Energy control doesn't belong in navigator --- src/modules/navigator/navigator_main.cpp | 10 ---------- 1 file changed, 10 deletions(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index e58b9ae115..d213310654 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -180,16 +180,6 @@ private: */ void mission_update(); - /** - * Control throttle. - */ - float control_throttle(float energy_error); - - /** - * Control pitch. - */ - float control_pitch(float altitude_error); - /** * Shim for calling task_main from task_create. */ From dada0b85998ac46a5b447e13a0c2f54819d07360 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 22 Nov 2013 21:44:37 +0400 Subject: [PATCH 142/447] attitude_estimator_ekf: mag declination parameter implemented --- .../attitude_estimator_ekf_main.cpp | 22 ++++++++++++++----- .../attitude_estimator_ekf_params.c | 7 ++++++ .../attitude_estimator_ekf_params.h | 2 ++ 3 files changed, 25 insertions(+), 6 deletions(-) diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp index a70a14fe4b..983ac7c8d8 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp @@ -61,6 +61,8 @@ #include #include +#include + #include #include #include @@ -433,10 +435,9 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds /* send out */ att.timestamp = raw.timestamp; - // XXX Apply the same transformation to the rotation matrix - att.roll = euler[0] - ekf_params.roll_off; - att.pitch = euler[1] - ekf_params.pitch_off; - att.yaw = euler[2] - ekf_params.yaw_off; + att.roll = euler[0]; + att.pitch = euler[1]; + att.yaw = euler[2] + ekf_params.mag_decl; att.rollspeed = x_aposteriori[0]; att.pitchspeed = x_aposteriori[1]; @@ -445,12 +446,21 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds att.pitchacc = x_aposteriori[4]; att.yawacc = x_aposteriori[5]; - //att.yawspeed =z_k[2] ; /* copy offsets */ memcpy(&att.rate_offsets, &(x_aposteriori[3]), sizeof(att.rate_offsets)); + /* magnetic declination */ + math::EulerAngles eulerMagDecl(0.0f, 0.0f, ekf_params.mag_decl); + math::Dcm R(eulerMagDecl); + math::Dcm R_body(&Rot_matrix[0]); + R = R * R_body; + /* copy rotation matrix */ - memcpy(&att.R, Rot_matrix, sizeof(Rot_matrix)); + for (int i = 0; i < 3; i++) { + for (int j = 0; j < 3; j++) { + att.R[i][j] = R(i, j); + } + } att.R_valid = true; if (isfinite(att.roll) && isfinite(att.pitch) && isfinite(att.yaw)) { diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c index 52dac652be..a6a40b4a10 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c @@ -63,6 +63,9 @@ PARAM_DEFINE_FLOAT(ATT_ROLL_OFF3, 0.0f); PARAM_DEFINE_FLOAT(ATT_PITCH_OFF3, 0.0f); PARAM_DEFINE_FLOAT(ATT_YAW_OFF3, 0.0f); +/* magnetic declination, in degrees */ +PARAM_DEFINE_FLOAT(ATT_MAG_DECL, 0.0f); + int parameters_init(struct attitude_estimator_ekf_param_handles *h) { /* PID parameters */ @@ -81,6 +84,8 @@ int parameters_init(struct attitude_estimator_ekf_param_handles *h) h->pitch_off = param_find("ATT_PITCH_OFF3"); h->yaw_off = param_find("ATT_YAW_OFF3"); + h->mag_decl = param_find("ATT_MAG_DECL"); + return OK; } @@ -101,5 +106,7 @@ int parameters_update(const struct attitude_estimator_ekf_param_handles *h, stru param_get(h->pitch_off, &(p->pitch_off)); param_get(h->yaw_off, &(p->yaw_off)); + param_get(h->mag_decl, &(p->mag_decl)); + return OK; } diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.h b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.h index 09817d58e6..b4b3ca50d7 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.h +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.h @@ -47,12 +47,14 @@ struct attitude_estimator_ekf_params { float roll_off; float pitch_off; float yaw_off; + float mag_decl; }; struct attitude_estimator_ekf_param_handles { param_t r0, r1, r2, r3; param_t q0, q1, q2, q3, q4; param_t roll_off, pitch_off, yaw_off; + param_t mag_decl; }; /** From bae2171edbb082190898bc8d5afbc9b0d991712b Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 22 Nov 2013 21:49:30 +0400 Subject: [PATCH 143/447] mc_att_control_vector: bugfixes, use float[3][3] type for R in vehicle_attitude_setpoint topic --- .../mc_att_control_vector_main.cpp | 62 ++++++++++++------- .../uORB/topics/vehicle_attitude_setpoint.h | 2 +- 2 files changed, 41 insertions(+), 23 deletions(-) diff --git a/src/modules/mc_att_control_vector/mc_att_control_vector_main.cpp b/src/modules/mc_att_control_vector/mc_att_control_vector_main.cpp index 0faba307d1..834189a546 100644 --- a/src/modules/mc_att_control_vector/mc_att_control_vector_main.cpp +++ b/src/modules/mc_att_control_vector/mc_att_control_vector_main.cpp @@ -109,15 +109,14 @@ public: private: - bool _task_should_exit; /**< if true, sensor task should exit */ + bool _task_should_exit; /**< if true, sensor task should exit */ int _control_task; /**< task handle for sensor task */ - int _att_sub; /**< vehicle attitude subscription */ + int _att_sub; /**< vehicle attitude subscription */ int _att_sp_sub; /**< vehicle attitude setpoint */ - int _attitude_sub; /**< raw rc channels data subscription */ - int _control_mode_sub; /**< vehicle control mode subscription */ - int _params_sub; /**< notification of parameter updates */ - int _manual_sub; /**< notification of manual control updates */ + int _control_mode_sub; /**< vehicle control mode subscription */ + int _params_sub; /**< notification of parameter updates */ + int _manual_sub; /**< notification of manual control updates */ int _arming_sub; /**< arming status of outputs */ orb_advert_t _att_sp_pub; /**< attitude setpoint publication */ @@ -125,19 +124,19 @@ private: orb_advert_t _actuators_0_pub; /**< actuator control group 0 setpoint */ struct vehicle_attitude_s _att; /**< vehicle attitude */ - struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */ - struct manual_control_setpoint_s _manual; /**< r/c channel data */ - struct vehicle_control_mode_s _control_mode; /**< vehicle control mode */ + struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */ + struct manual_control_setpoint_s _manual; /**< r/c channel data */ + struct vehicle_control_mode_s _control_mode; /**< vehicle control mode */ struct actuator_controls_s _actuators; /**< actuator control inputs */ struct actuator_armed_s _arming; /**< actuator arming status */ struct vehicle_rates_setpoint_s _rates_sp; /**< vehicle rates setpoint */ perf_counter_t _loop_perf; /**< loop performance counter */ - bool _setpoint_valid; /**< flag if the position control setpoint is valid */ + bool _att_sp_valid; /**< flag if the attitude setpoint is valid */ math::Matrix _K; /**< diagonal gain matrix for position error */ - math::Matrix _K_rate; /**< diagonal gain matrix for angular rate error */ + math::Matrix _K_rate; /**< diagonal gain matrix for angular rate error */ struct { param_t att_p; @@ -149,11 +148,10 @@ private: /** * Update our local parameter cache. */ - int parameters_update(); + int parameters_update(); /** * Update control outputs - * */ void control_update(); @@ -207,27 +205,37 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : /* subscriptions */ _att_sub(-1), + _att_sp_sub(-1), _control_mode_sub(-1), _params_sub(-1), _manual_sub(-1), _arming_sub(-1), /* publications */ + _att_sp_pub(-1), _rates_sp_pub(-1), _actuators_0_pub(-1), /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "fw att control")), + /* states */ - _setpoint_valid(false), + _att_sp_valid(false), + /* gain matrices */ _K(3, 3), _K_rate(3, 3) { - _parameter_handles.att_p = param_find("MC_ATT_P"); + memset(&_att, 0, sizeof(_att)); + memset(&_att_sp, 0, sizeof(_att_sp)); + memset(&_manual, 0, sizeof(_manual)); + memset(&_control_mode, 0, sizeof(_control_mode)); + memset(&_arming, 0, sizeof(_arming)); + + _parameter_handles.att_p = param_find("MC_ATT_P"); _parameter_handles.att_rate_p = param_find("MC_ATTRATE_P"); - _parameter_handles.yaw_p = param_find("MC_YAWPOS_P"); + _parameter_handles.yaw_p = param_find("MC_YAWPOS_P"); _parameter_handles.yaw_rate_p = param_find("MC_YAWRATE_P"); /* fetch initial parameter values */ @@ -237,7 +245,6 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : MulticopterAttitudeControl::~MulticopterAttitudeControl() { if (_control_task != -1) { - /* task wakes up every 100ms or so at the longest */ _task_should_exit = true; @@ -272,9 +279,11 @@ MulticopterAttitudeControl::parameters_update() param_get(_parameter_handles.yaw_p, &yaw_p); param_get(_parameter_handles.yaw_rate_p, &yaw_rate_p); + _K.setAll(0.0f); _K(0, 0) = att_p; _K(1, 1) = att_p; _K(2, 2) = yaw_p; + _K_rate.setAll(0.0f); _K_rate(0, 0) = att_rate_p; _K_rate(1, 1) = att_rate_p; _K_rate(2, 2) = yaw_rate_p; @@ -319,7 +328,7 @@ MulticopterAttitudeControl::vehicle_setpoint_poll() if (att_sp_updated) { orb_copy(ORB_ID(vehicle_attitude_setpoint), _att_sp_sub, &_att_sp); - _setpoint_valid = true; + _att_sp_valid = true; } } @@ -439,6 +448,11 @@ MulticopterAttitudeControl::task_main() _att_sp.thrust = _manual.throttle; } + if (!_arming.armed) { + /* reset yaw setpoint when disarmed */ + reset_yaw_sp = true; + } + if (_control_mode.flag_control_attitude_enabled) { /* control attitude, update attitude setpoint depending on mode */ @@ -470,6 +484,8 @@ MulticopterAttitudeControl::task_main() _att_sp.pitch_body = _manual.pitch; publish_att_sp = true; } + + _att_sp_valid = true; } else { /* manual rate inputs (ACRO) */ // TODO @@ -492,12 +508,14 @@ MulticopterAttitudeControl::task_main() reset_yaw_sp = true; } - if (publish_att_sp || !_att_sp.R_valid) { + if (publish_att_sp || (_att_sp_valid && !_att_sp.R_valid)) { /* controller uses rotation matrix, not euler angles, convert if necessary */ math::EulerAngles euler_sp(_att_sp.roll_body, _att_sp.pitch_body, _att_sp.yaw_body); math::Dcm R_sp(euler_sp); - for (int i = 0; i < 9; i++) { - _att_sp.R_body[i] = R_sp(i / 3, i % 3); + for (int i = 0; i < 3; i++) { + for (int j = 0; j < 3; j++) { + _att_sp.R_body[i][j] = R_sp(i, j); + } } _att_sp.R_valid = true; } @@ -514,7 +532,7 @@ MulticopterAttitudeControl::task_main() } /* desired rotation matrix */ - math::Dcm R_des(&_att_sp.R_body[0]); + math::Dcm R_des(_att_sp.R_body); /* rotation matrix for current state */ math::Dcm R(_att.R); diff --git a/src/modules/uORB/topics/vehicle_attitude_setpoint.h b/src/modules/uORB/topics/vehicle_attitude_setpoint.h index 1a245132a8..7596f944f3 100644 --- a/src/modules/uORB/topics/vehicle_attitude_setpoint.h +++ b/src/modules/uORB/topics/vehicle_attitude_setpoint.h @@ -61,7 +61,7 @@ struct vehicle_attitude_setpoint_s float yaw_body; /**< body angle in NED frame */ //float body_valid; /**< Set to true if body angles are valid */ - float R_body[9]; /**< Rotation matrix describing the setpoint as rotation from the current body frame */ + float R_body[3][3]; /**< Rotation matrix describing the setpoint as rotation from the current body frame */ bool R_valid; /**< Set to true if rotation matrix is valid */ //! For quaternion-based attitude control From 8c3e67993e2aa5e434ad1273889ce8f321fb1908 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 22 Nov 2013 22:23:27 +0400 Subject: [PATCH 144/447] =?UTF-8?q?position=5Festimator=5Finav:=20don?= =?UTF-8?q?=E2=80=99t=20use=20GPS=20vertical=20speed?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../position_estimator_inav/position_estimator_inav_main.c | 3 --- .../position_estimator_inav/position_estimator_inav_params.c | 3 --- .../position_estimator_inav/position_estimator_inav_params.h | 2 -- 3 files changed, 8 deletions(-) diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index 5be59f9652..d95de11c4a 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -656,7 +656,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) float w_xy_gps_p = params.w_xy_gps_p * w_gps_xy; float w_xy_gps_v = params.w_xy_gps_v * w_gps_xy; float w_z_gps_p = params.w_z_gps_p * w_gps_z; - float w_z_gps_v = params.w_z_gps_v * w_gps_z; /* reduce GPS weight if optical flow is good */ if (use_flow && flow_accurate) { @@ -682,7 +681,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } if (use_gps_z) { accel_bias_corr[2] -= corr_gps[2][0] * w_z_gps_p * w_z_gps_p; - accel_bias_corr[2] -= corr_gps[2][1] * w_z_gps_v; } if (use_flow) { @@ -709,7 +707,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) /* inertial filter correction for altitude */ inertial_filter_correct(corr_baro, dt, z_est, 0, params.w_z_baro); inertial_filter_correct(corr_gps[2][0], dt, z_est, 0, w_z_gps_p); - inertial_filter_correct(corr_gps[2][1], dt, z_est, 1, w_z_gps_v); inertial_filter_correct(corr_acc[2], dt, z_est, 2, params.w_z_acc); if (can_estimate_xy) { diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.c b/src/modules/position_estimator_inav/position_estimator_inav_params.c index 77299bd711..da4c9482c0 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.c @@ -42,7 +42,6 @@ PARAM_DEFINE_FLOAT(INAV_W_Z_BARO, 1.0f); PARAM_DEFINE_FLOAT(INAV_W_Z_GPS_P, 0.5f); -PARAM_DEFINE_FLOAT(INAV_W_Z_GPS_V, 1.0f); PARAM_DEFINE_FLOAT(INAV_W_Z_ACC, 20.0f); PARAM_DEFINE_FLOAT(INAV_W_Z_SONAR, 3.0f); PARAM_DEFINE_FLOAT(INAV_W_XY_GPS_P, 1.0f); @@ -63,7 +62,6 @@ int parameters_init(struct position_estimator_inav_param_handles *h) { h->w_z_baro = param_find("INAV_W_Z_BARO"); h->w_z_gps_p = param_find("INAV_W_Z_GPS_P"); - h->w_z_gps_v = param_find("INAV_W_Z_GPS_V"); h->w_z_acc = param_find("INAV_W_Z_ACC"); h->w_z_sonar = param_find("INAV_W_Z_SONAR"); h->w_xy_gps_p = param_find("INAV_W_XY_GPS_P"); @@ -87,7 +85,6 @@ int parameters_update(const struct position_estimator_inav_param_handles *h, str { param_get(h->w_z_baro, &(p->w_z_baro)); param_get(h->w_z_gps_p, &(p->w_z_gps_p)); - param_get(h->w_z_gps_v, &(p->w_z_gps_v)); param_get(h->w_z_acc, &(p->w_z_acc)); param_get(h->w_z_sonar, &(p->w_z_sonar)); param_get(h->w_xy_gps_p, &(p->w_xy_gps_p)); diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.h b/src/modules/position_estimator_inav/position_estimator_inav_params.h index f583f4b7d0..e2be079d35 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.h +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.h @@ -43,7 +43,6 @@ struct position_estimator_inav_params { float w_z_baro; float w_z_gps_p; - float w_z_gps_v; float w_z_acc; float w_z_sonar; float w_xy_gps_p; @@ -64,7 +63,6 @@ struct position_estimator_inav_params { struct position_estimator_inav_param_handles { param_t w_z_baro; param_t w_z_gps_p; - param_t w_z_gps_v; param_t w_z_acc; param_t w_z_sonar; param_t w_xy_gps_p; From 01df715f946afc1cec79d33cba970ad15c62ec73 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 22 Nov 2013 21:21:30 +0100 Subject: [PATCH 145/447] Mission topic: make nav_cmd compatible to the mavlink command --- src/modules/mavlink/waypoints.c | 4 ++-- src/modules/uORB/topics/mission.h | 19 +++++++++++-------- 2 files changed, 13 insertions(+), 10 deletions(-) diff --git a/src/modules/mavlink/waypoints.c b/src/modules/mavlink/waypoints.c index a9ee26eacf..8e4bbce36f 100644 --- a/src/modules/mavlink/waypoints.c +++ b/src/modules/mavlink/waypoints.c @@ -78,7 +78,7 @@ void map_mavlink_mission_item_to_mission_item(const mavlink_mission_item_t *mavl mission_item->yaw = mavlink_mission_item->param4*M_DEG_TO_RAD_F; mission_item->loiter_radius = fabsf(mavlink_mission_item->param3); mission_item->loiter_direction = (mavlink_mission_item->param3 > 0) ? 1 : -1; /* 1 if positive CW, -1 if negative CCW */ - mission_item->nav_cmd = NAV_CMD_WAYPOINT; // TODO correct + mission_item->nav_cmd = mavlink_mission_item->command; mission_item->radius = mavlink_mission_item->param1; mission_item->time_inside = mavlink_mission_item->param2 / 1e3f; /* from milliseconds to seconds */ mission_item->autocontinue = mavlink_mission_item->autocontinue; @@ -93,7 +93,7 @@ void map_mission_item_to_mavlink_mission_item(const struct mission_item_s *missi mavlink_mission_item->z = mission_item->altitude; mavlink_mission_item->param4 = mission_item->yaw*M_RAD_TO_DEG_F; mavlink_mission_item->param3 = mission_item->loiter_radius*(float)mission_item->loiter_direction; - mavlink_mission_item->command = MAV_CMD_NAV_WAYPOINT; // TODO add + mavlink_mission_item->command = mission_item->nav_cmd; mavlink_mission_item->param1 = mission_item->radius; mavlink_mission_item->param2 = mission_item->time_inside * 1e3f; /* from seconds to milliseconds */ mavlink_mission_item->autocontinue = mission_item->autocontinue; diff --git a/src/modules/uORB/topics/mission.h b/src/modules/uORB/topics/mission.h index a39a1e4d7e..441efe4587 100644 --- a/src/modules/uORB/topics/mission.h +++ b/src/modules/uORB/topics/mission.h @@ -46,14 +46,17 @@ #include #include "../uORB.h" +/* compatible to mavlink MAV_CMD */ enum NAV_CMD { - NAV_CMD_WAYPOINT = 0, - NAV_CMD_LOITER_TURN_COUNT, - NAV_CMD_LOITER_TIME_LIMIT, - NAV_CMD_LOITER_UNLIMITED, - NAV_CMD_RETURN_TO_LAUNCH, - NAV_CMD_LAND, - NAV_CMD_TAKEOFF + NAV_CMD_WAYPOINT=16, + NAV_CMD_LOITER_UNLIMITED=17, + NAV_CMD_LOITER_TURN_COUNT=18, + NAV_CMD_LOITER_TIME_LIMIT=19, + NAV_CMD_RETURN_TO_LAUNCH=20, + NAV_CMD_LAND=21, + NAV_CMD_TAKEOFF=22, + NAV_CMD_ROI=80, + NAV_CMD_PATHPLANNING=81 }; /** @@ -76,7 +79,7 @@ struct mission_item_s float yaw; /**< in radians NED -PI..+PI */ float loiter_radius; /**< loiter radius in meters, 0 for a VTOL to hover */ int8_t loiter_direction; /**< 1: positive / clockwise, -1, negative. */ - enum NAV_CMD nav_cmd; /**< true if loitering is enabled */ + enum NAV_CMD nav_cmd; /**< navigation command */ float radius; /**< radius in which the mission is accepted as reached in meters */ float time_inside; /**< time that the MAV should stay inside the radius before advancing in seconds */ bool autocontinue; /**< true if next waypoint should follow after this one */ From ae5b20eb7ff90fbfd43817808c4806a0d993d167 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 22 Nov 2013 21:26:17 +0100 Subject: [PATCH 146/447] FW_pos_control_l1: Remove RTL logic which will be in navigator --- .../fw_pos_control_l1_main.cpp | 425 ++++++++---------- 1 file changed, 179 insertions(+), 246 deletions(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index cfa98e573d..975ac85ff2 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -745,312 +745,245 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio /* restore speed weight, in case changed intermittently (e.g. in landing handling) */ _tecs.set_speed_weight(_parameters.speed_weight); - /* execute navigation once we have a setpoint */ - if (_setpoint_valid && _control_mode.flag_control_auto_enabled) { + /* current waypoint (the one currently heading for) */ + math::Vector2f next_wp(mission_item_triplet.current.lat, mission_item_triplet.current.lon); - /* current waypoint (the one currently heading for) */ - math::Vector2f next_wp(mission_item_triplet.current.lat, mission_item_triplet.current.lon); - - /* current waypoint (the one currently heading for) */ - math::Vector2f curr_wp(mission_item_triplet.current.lat, mission_item_triplet.current.lon); + /* current waypoint (the one currently heading for) */ + math::Vector2f curr_wp(mission_item_triplet.current.lat, mission_item_triplet.current.lon); - /* previous waypoint */ - math::Vector2f prev_wp; + /* previous waypoint */ + math::Vector2f prev_wp; - if (mission_item_triplet.previous_valid) { - prev_wp.setX(mission_item_triplet.previous.lat); - prev_wp.setY(mission_item_triplet.previous.lon); + if (mission_item_triplet.previous_valid) { + prev_wp.setX(mission_item_triplet.previous.lat); + prev_wp.setY(mission_item_triplet.previous.lon); - } else { - /* - * No valid previous waypoint, go for the current wp. - * This is automatically handled by the L1 library. - */ - prev_wp.setX(mission_item_triplet.current.lat); - prev_wp.setY(mission_item_triplet.current.lon); + } else { + /* + * No valid previous waypoint, go for the current wp. + * This is automatically handled by the L1 library. + */ + prev_wp.setX(mission_item_triplet.current.lat); + prev_wp.setY(mission_item_triplet.current.lon); - } + } - // XXX add RTL switch - if (mission_item_triplet.current.nav_cmd == NAV_CMD_RETURN_TO_LAUNCH && _launch_valid) { + if (mission_item_triplet.current.nav_cmd == NAV_CMD_WAYPOINT || mission_item_triplet.current.nav_cmd == NAV_CMD_RETURN_TO_LAUNCH) { + /* waypoint is a plain navigation waypoint */ + _l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed); + _att_sp.roll_body = _l1_control.nav_roll(); + _att_sp.yaw_body = _l1_control.nav_bearing(); - math::Vector2f rtl_pos(_launch_lat, _launch_lon); + _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _mission_item_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_trim), + _airspeed.indicated_airspeed_m_s, eas2tas, + false, math::radians(_parameters.pitch_limit_min), + _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, + math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max)); - _l1_control.navigate_waypoints(rtl_pos, rtl_pos, current_position, ground_speed); - _att_sp.roll_body = _l1_control.nav_roll(); - _att_sp.yaw_body = _l1_control.nav_bearing(); + } else if (mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_TURN_COUNT || + mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || + mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_UNLIMITED) { - _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _launch_alt, calculate_target_airspeed(_parameters.airspeed_trim), - _airspeed.indicated_airspeed_m_s, eas2tas, - false, math::radians(_parameters.pitch_limit_min), - _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, - math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max)); + /* waypoint is a loiter waypoint */ + _l1_control.navigate_loiter(curr_wp, current_position, mission_item_triplet.current.loiter_radius, + mission_item_triplet.current.loiter_direction, ground_speed); + _att_sp.roll_body = _l1_control.nav_roll(); + _att_sp.yaw_body = _l1_control.nav_bearing(); - // XXX handle case when having arrived at home (loiter) + _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _mission_item_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_trim), + _airspeed.indicated_airspeed_m_s, eas2tas, + false, math::radians(_parameters.pitch_limit_min), + _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, + math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max)); - } else if (mission_item_triplet.current.nav_cmd == NAV_CMD_WAYPOINT) { - /* waypoint is a plain navigation waypoint */ - _l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed); - _att_sp.roll_body = _l1_control.nav_roll(); - _att_sp.yaw_body = _l1_control.nav_bearing(); + } else if (mission_item_triplet.current.nav_cmd == NAV_CMD_LAND) { - _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _mission_item_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_trim), - _airspeed.indicated_airspeed_m_s, eas2tas, - false, math::radians(_parameters.pitch_limit_min), - _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, - math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max)); + /* Horizontal landing control */ + /* switch to heading hold for the last meters, continue heading hold after */ + float wp_distance = get_distance_to_next_waypoint(current_position.getX(), current_position.getY(), curr_wp.getX(), curr_wp.getY()); + //warnx("wp dist: %d, alt err: %d, noret: %s", (int)wp_distance, (int)altitude_error, (land_noreturn) ? "YES" : "NO"); + const float heading_hold_distance = 15.0f; + if (wp_distance < heading_hold_distance || land_noreturn_horizontal) { - } else if (mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_TURN_COUNT || - mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || - mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_UNLIMITED) { + /* heading hold, along the line connecting this and the last waypoint */ + - /* waypoint is a loiter waypoint */ - _l1_control.navigate_loiter(curr_wp, current_position, mission_item_triplet.current.loiter_radius, - mission_item_triplet.current.loiter_direction, ground_speed); - _att_sp.roll_body = _l1_control.nav_roll(); - _att_sp.yaw_body = _l1_control.nav_bearing(); + // if (mission_item_triplet.previous_valid) { + // target_bearing = get_bearing_to_next_waypoint(prev_wp.getX(), prev_wp.getY(), next_wp.getX(), next_wp.getY()); + // } else { - _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _mission_item_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_trim), - _airspeed.indicated_airspeed_m_s, eas2tas, - false, math::radians(_parameters.pitch_limit_min), - _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, - math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max)); - - } else if (mission_item_triplet.current.nav_cmd == NAV_CMD_LAND) { - - /* Horizontal landing control */ - /* switch to heading hold for the last meters, continue heading hold after */ - float wp_distance = get_distance_to_next_waypoint(current_position.getX(), current_position.getY(), curr_wp.getX(), curr_wp.getY()); - //warnx("wp dist: %d, alt err: %d, noret: %s", (int)wp_distance, (int)altitude_error, (land_noreturn) ? "YES" : "NO"); - const float heading_hold_distance = 15.0f; - if (wp_distance < heading_hold_distance || land_noreturn_horizontal) { - - /* heading hold, along the line connecting this and the last waypoint */ - - - // if (mission_item_triplet.previous_valid) { - // target_bearing = get_bearing_to_next_waypoint(prev_wp.getX(), prev_wp.getY(), next_wp.getX(), next_wp.getY()); - // } else { - - if (!land_noreturn_horizontal) //set target_bearing in first occurrence - target_bearing = _att.yaw; - //} + if (!land_noreturn_horizontal) //set target_bearing in first occurrence + target_bearing = _att.yaw; + //} // warnx("NORET: %d, target_bearing: %d, yaw: %d", (int)land_noreturn_horizontal, (int)math::degrees(target_bearing), (int)math::degrees(_att.yaw)); - _l1_control.navigate_heading(target_bearing, _att.yaw, ground_speed); + _l1_control.navigate_heading(target_bearing, _att.yaw, ground_speed); - land_noreturn_horizontal = true; + land_noreturn_horizontal = true; - } else { + } else { - /* normal navigation */ - _l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed); - } + /* normal navigation */ + _l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed); + } - _att_sp.roll_body = _l1_control.nav_roll(); - _att_sp.yaw_body = _l1_control.nav_bearing(); + _att_sp.roll_body = _l1_control.nav_roll(); + _att_sp.yaw_body = _l1_control.nav_bearing(); - /* Vertical landing control */ - //xxx: using the tecs altitude controller for slope control for now + /* Vertical landing control */ + //xxx: using the tecs altitude controller for slope control for now // /* do not go down too early */ // if (wp_distance > 50.0f) { // altitude_error = (_global_triplet.current.altitude + 25.0f) - _global_pos.alt; // } - /* apply minimum pitch (flare) and limit roll if close to touch down, altitude error is negative (going down) */ - // XXX this could make a great param + /* apply minimum pitch (flare) and limit roll if close to touch down, altitude error is negative (going down) */ + // XXX this could make a great param - float flare_angle_rad = -math::radians(5.0f);//math::radians(mission_item_triplet.current.param1) - float land_pitch_min = math::radians(5.0f); - float throttle_land = _parameters.throttle_min + (_parameters.throttle_max - _parameters.throttle_min) * 0.1f; - float airspeed_land = 1.3f * _parameters.airspeed_min; - float airspeed_approach = 1.3f * _parameters.airspeed_min; + float flare_angle_rad = -math::radians(5.0f);//math::radians(mission_item_triplet.current.param1) + float land_pitch_min = math::radians(5.0f); + float throttle_land = _parameters.throttle_min + (_parameters.throttle_max - _parameters.throttle_min) * 0.1f; + float airspeed_land = 1.3f * _parameters.airspeed_min; + float airspeed_approach = 1.3f * _parameters.airspeed_min; - float landing_slope_angle_rad = math::radians(_parameters.land_slope_angle); - float flare_relative_alt = _parameters.land_flare_alt_relative; - float motor_lim_horizontal_distance = _parameters.land_thrust_lim_horizontal_distance;//be generous here as we currently have to rely on the user input for the waypoint - float L_wp_distance = get_distance_to_next_waypoint(prev_wp.getX(), prev_wp.getY(), curr_wp.getX(), curr_wp.getY()) * _parameters.land_slope_length; - float H1 = _parameters.land_H1_virt; - float H0 = flare_relative_alt + H1; - float d1 = flare_relative_alt/tanf(landing_slope_angle_rad); - float flare_constant = (H0 * d1)/flare_relative_alt;//-flare_length/(logf(H1/H0)); - float flare_length = - logf(H1/H0) * flare_constant;//d1+20.0f;//-logf(0.01f/flare_relative_alt); - float horizontal_slope_displacement = (flare_length - d1); - float L_altitude = getLandingSlopeAbsoluteAltitude(L_wp_distance, _mission_item_triplet.current.altitude, landing_slope_angle_rad, horizontal_slope_displacement); - float landing_slope_alt_desired = getLandingSlopeAbsoluteAltitude(wp_distance, _mission_item_triplet.current.altitude, landing_slope_angle_rad, horizontal_slope_displacement); + float landing_slope_angle_rad = math::radians(_parameters.land_slope_angle); + float flare_relative_alt = _parameters.land_flare_alt_relative; + float motor_lim_horizontal_distance = _parameters.land_thrust_lim_horizontal_distance;//be generous here as we currently have to rely on the user input for the waypoint + float L_wp_distance = get_distance_to_next_waypoint(prev_wp.getX(), prev_wp.getY(), curr_wp.getX(), curr_wp.getY()) * _parameters.land_slope_length; + float H1 = _parameters.land_H1_virt; + float H0 = flare_relative_alt + H1; + float d1 = flare_relative_alt/tanf(landing_slope_angle_rad); + float flare_constant = (H0 * d1)/flare_relative_alt;//-flare_length/(logf(H1/H0)); + float flare_length = - logf(H1/H0) * flare_constant;//d1+20.0f;//-logf(0.01f/flare_relative_alt); + float horizontal_slope_displacement = (flare_length - d1); + float L_altitude = getLandingSlopeAbsoluteAltitude(L_wp_distance, _mission_item_triplet.current.altitude, landing_slope_angle_rad, horizontal_slope_displacement); + float landing_slope_alt_desired = getLandingSlopeAbsoluteAltitude(wp_distance, _mission_item_triplet.current.altitude, landing_slope_angle_rad, horizontal_slope_displacement); - if ( (_global_pos.alt < _mission_item_triplet.current.altitude + flare_relative_alt) || land_noreturn_vertical) { //checking for land_noreturn to avoid unwanted climb out + if ( (_global_pos.alt < _mission_item_triplet.current.altitude + flare_relative_alt) || land_noreturn_vertical) { //checking for land_noreturn to avoid unwanted climb out - /* land with minimal speed */ + /* land with minimal speed */ // /* force TECS to only control speed with pitch, altitude is only implicitely controlled now */ // _tecs.set_speed_weight(2.0f); - /* kill the throttle if param requests it */ - throttle_max = _parameters.throttle_max; + /* kill the throttle if param requests it */ + throttle_max = _parameters.throttle_max; - if (wp_distance < motor_lim_horizontal_distance || land_motor_lim) { - throttle_max = math::min(throttle_max, _parameters.throttle_land_max); - if (!land_motor_lim) { - land_motor_lim = true; - mavlink_log_info(mavlink_fd, "[POSCTRL] Landing, limit throttle"); - } - - } - - float flare_curve_alt = _mission_item_triplet.current.altitude + H0 * expf(-math::max(0.0f, flare_length - wp_distance)/flare_constant) - H1; - - /* avoid climbout */ - if (flare_curve_alt_last < flare_curve_alt && land_noreturn_vertical || land_stayonground) - { - flare_curve_alt = mission_item_triplet.current.altitude; - land_stayonground = true; + if (wp_distance < motor_lim_horizontal_distance || land_motor_lim) { + throttle_max = math::min(throttle_max, _parameters.throttle_land_max); + if (!land_motor_lim) { + land_motor_lim = true; + mavlink_log_info(mavlink_fd, "[POSCTRL] Landing, limit throttle"); } - _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, flare_curve_alt, calculate_target_airspeed(airspeed_land), - _airspeed.indicated_airspeed_m_s, eas2tas, - false, flare_angle_rad, - 0.0f, throttle_max, throttle_land, - flare_angle_rad, math::radians(15.0f)); + } - /* limit roll motion to prevent wings from touching the ground first */ - _att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-10.0f), math::radians(10.0f)); - if (!land_noreturn_vertical) { - mavlink_log_info(mavlink_fd, "[POSCTRL] Landing, flare"); - land_noreturn_vertical = true; - } - //warnx("Landing: flare, _global_pos.alt %.1f, flare_curve_alt %.1f, flare_curve_alt_last %.1f, flare_length %.1f, wp_distance %.1f", _global_pos.alt, flare_curve_alt, flare_curve_alt_last, flare_length, wp_distance); + float flare_curve_alt = _mission_item_triplet.current.altitude + H0 * expf(-math::max(0.0f, flare_length - wp_distance)/flare_constant) - H1; - flare_curve_alt_last = flare_curve_alt; - - } else if (wp_distance < L_wp_distance) { - - /* minimize speed to approach speed, stay on landing slope */ - _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, landing_slope_alt_desired, calculate_target_airspeed(airspeed_approach), - _airspeed.indicated_airspeed_m_s, eas2tas, - false, flare_angle_rad, - _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, - math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max)); - //warnx("Landing: stay on slope, alt_desired: %.1f (wp_distance: %.1f), calculate_target_airspeed(airspeed_land) %.1f, horizontal_slope_displacement %.1f, d1 %.1f, flare_length %.1f", landing_slope_alt_desired, wp_distance, calculate_target_airspeed(airspeed_land), horizontal_slope_displacement, d1, flare_length); - - if (!land_onslope) { - - mavlink_log_info(mavlink_fd, "[POSCTRL] Landing, on slope"); - land_onslope = true; - } - - } else { - - /* intersect glide slope: - * if current position is higher or within 10m of slope follow the glide slope - * if current position is below slope -10m continue on maximum of previous wp altitude or L_altitude until the intersection with the slope - * */ - float altitude_desired = _global_pos.alt; - if (_global_pos.alt > landing_slope_alt_desired - 10.0f) { - /* stay on slope */ - altitude_desired = landing_slope_alt_desired; - //warnx("Landing: before L, stay on landing slope, alt_desired: %.1f (wp_distance: %.1f, L_wp_distance %.1f), calculate_target_airspeed(airspeed_land) %.1f, horizontal_slope_displacement %.1f", altitude_desired, wp_distance, L_wp_distance, calculate_target_airspeed(airspeed_land), horizontal_slope_displacement); - } else { - /* continue horizontally */ - altitude_desired = math::max(_global_pos.alt, L_altitude); - //warnx("Landing: before L,continue at: %.4f, (landing_slope_alt_desired %.4f, wp_distance: %.4f, L_altitude: %.4f L_wp_distance: %.4f)", altitude_desired, landing_slope_alt_desired, wp_distance, L_altitude, L_wp_distance); - } - - _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, altitude_desired, calculate_target_airspeed(airspeed_approach), - _airspeed.indicated_airspeed_m_s, eas2tas, - false, math::radians(_parameters.pitch_limit_min), - _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, - math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max)); + /* avoid climbout */ + if (flare_curve_alt_last < flare_curve_alt && land_noreturn_vertical || land_stayonground) + { + flare_curve_alt = mission_item_triplet.current.altitude; + land_stayonground = true; } - } else if (mission_item_triplet.current.nav_cmd == NAV_CMD_TAKEOFF) { + _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, flare_curve_alt, calculate_target_airspeed(airspeed_land), + _airspeed.indicated_airspeed_m_s, eas2tas, + false, flare_angle_rad, + 0.0f, throttle_max, throttle_land, + flare_angle_rad, math::radians(15.0f)); - _l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed); - _att_sp.roll_body = _l1_control.nav_roll(); - _att_sp.yaw_body = _l1_control.nav_bearing(); - - /* apply minimum pitch and limit roll if target altitude is not within 10 meters */ - if (altitude_error > 15.0f) { - - /* enforce a minimum of 10 degrees pitch up on takeoff, or take parameter */ - _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _mission_item_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_min), - _airspeed.indicated_airspeed_m_s, eas2tas, - true, math::max(math::radians(mission_item_triplet.current.radius), math::radians(10.0f)), - _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, - math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max)); - - /* limit roll motion to ensure enough lift */ - _att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-15.0f), math::radians(15.0f)); - - } else { - - _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _mission_item_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_trim), - _airspeed.indicated_airspeed_m_s, eas2tas, - false, math::radians(_parameters.pitch_limit_min), - _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, - math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max)); + /* limit roll motion to prevent wings from touching the ground first */ + _att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-10.0f), math::radians(10.0f)); + if (!land_noreturn_vertical) { + mavlink_log_info(mavlink_fd, "[POSCTRL] Landing, flare"); + land_noreturn_vertical = true; } + //warnx("Landing: flare, _global_pos.alt %.1f, flare_curve_alt %.1f, flare_curve_alt_last %.1f, flare_length %.1f, wp_distance %.1f", _global_pos.alt, flare_curve_alt, flare_curve_alt_last, flare_length, wp_distance); + + flare_curve_alt_last = flare_curve_alt; + + } else if (wp_distance < L_wp_distance) { + + /* minimize speed to approach speed, stay on landing slope */ + _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, landing_slope_alt_desired, calculate_target_airspeed(airspeed_approach), + _airspeed.indicated_airspeed_m_s, eas2tas, + false, flare_angle_rad, + _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, + math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max)); + //warnx("Landing: stay on slope, alt_desired: %.1f (wp_distance: %.1f), calculate_target_airspeed(airspeed_land) %.1f, horizontal_slope_displacement %.1f, d1 %.1f, flare_length %.1f", landing_slope_alt_desired, wp_distance, calculate_target_airspeed(airspeed_land), horizontal_slope_displacement, d1, flare_length); + + if (!land_onslope) { + + mavlink_log_info(mavlink_fd, "[POSCTRL] Landing, on slope"); + land_onslope = true; + } + + } else { + + /* intersect glide slope: + * if current position is higher or within 10m of slope follow the glide slope + * if current position is below slope -10m continue on maximum of previous wp altitude or L_altitude until the intersection with the slope + * */ + float altitude_desired = _global_pos.alt; + if (_global_pos.alt > landing_slope_alt_desired - 10.0f) { + /* stay on slope */ + altitude_desired = landing_slope_alt_desired; + //warnx("Landing: before L, stay on landing slope, alt_desired: %.1f (wp_distance: %.1f, L_wp_distance %.1f), calculate_target_airspeed(airspeed_land) %.1f, horizontal_slope_displacement %.1f", altitude_desired, wp_distance, L_wp_distance, calculate_target_airspeed(airspeed_land), horizontal_slope_displacement); + } else { + /* continue horizontally */ + altitude_desired = math::max(_global_pos.alt, L_altitude); + //warnx("Landing: before L,continue at: %.4f, (landing_slope_alt_desired %.4f, wp_distance: %.4f, L_altitude: %.4f L_wp_distance: %.4f)", altitude_desired, landing_slope_alt_desired, wp_distance, L_altitude, L_wp_distance); + } + + _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, altitude_desired, calculate_target_airspeed(airspeed_approach), + _airspeed.indicated_airspeed_m_s, eas2tas, + false, math::radians(_parameters.pitch_limit_min), + _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, + math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max)); } - // warnx("nav bearing: %8.4f bearing err: %8.4f target bearing: %8.4f", (double)_l1_control.nav_bearing(), - // (double)_l1_control.bearing_error(), (double)_l1_control.target_bearing()); - // warnx("prev wp: %8.4f/%8.4f, next wp: %8.4f/%8.4f prev:%s", (double)prev_wp.getX(), (double)prev_wp.getY(), - // (double)next_wp.getX(), (double)next_wp.getY(), (mission_item_triplet.previous_valid) ? "valid" : "invalid"); + } else if (mission_item_triplet.current.nav_cmd == NAV_CMD_TAKEOFF) { - // XXX at this point we always want no loiter hold if a - // mission is active - _loiter_hold = false; - - } else if (_control_mode.flag_armed) { - - /* hold position, but only if armed, climb 20m in case this is engaged on ground level */ - - // XXX rework with smarter state machine - - if (!_loiter_hold) { - _loiter_hold_lat = _global_pos.lat / 1e7f; - _loiter_hold_lon = _global_pos.lon / 1e7f; - _loiter_hold_alt = _global_pos.alt + 25.0f; - _loiter_hold = true; - } - - altitude_error = _loiter_hold_alt - _global_pos.alt; - - math::Vector2f loiter_hold_pos(_loiter_hold_lat, _loiter_hold_lon); - - /* loiter around current position */ - _l1_control.navigate_loiter(loiter_hold_pos, current_position, _parameters.loiter_hold_radius, - 1, ground_speed); + _l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed); _att_sp.roll_body = _l1_control.nav_roll(); _att_sp.yaw_body = _l1_control.nav_bearing(); - /* climb with full throttle if the altitude error is bigger than 5 meters */ - bool climb_out = (altitude_error > 3); + /* apply minimum pitch and limit roll if target altitude is not within 10 meters */ + if (altitude_error > 15.0f) { - float min_pitch; + /* enforce a minimum of 10 degrees pitch up on takeoff, or take parameter */ + _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _mission_item_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_min), + _airspeed.indicated_airspeed_m_s, eas2tas, + true, math::max(math::radians(mission_item_triplet.current.radius), math::radians(10.0f)), + _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, + math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max)); - if (climb_out) { - min_pitch = math::radians(20.0f); - - } else { - min_pitch = math::radians(_parameters.pitch_limit_min); - } - - _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _loiter_hold_alt, calculate_target_airspeed(_parameters.airspeed_trim), - _airspeed.indicated_airspeed_m_s, eas2tas, - climb_out, min_pitch, - _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, - math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max)); - - if (climb_out) { /* limit roll motion to ensure enough lift */ _att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-15.0f), math::radians(15.0f)); + + } else { + + _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _mission_item_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_trim), + _airspeed.indicated_airspeed_m_s, eas2tas, + false, math::radians(_parameters.pitch_limit_min), + _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, + math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max)); } } + // warnx("nav bearing: %8.4f bearing err: %8.4f target bearing: %8.4f", (double)_l1_control.nav_bearing(), + // (double)_l1_control.bearing_error(), (double)_l1_control.target_bearing()); + // warnx("prev wp: %8.4f/%8.4f, next wp: %8.4f/%8.4f prev:%s", (double)prev_wp.getX(), (double)prev_wp.getY(), + // (double)next_wp.getX(), (double)next_wp.getY(), (mission_item_triplet.previous_valid) ? "valid" : "invalid"); + + // XXX at this point we always want no loiter hold if a + // mission is active + _loiter_hold = false; + /* reset land state */ if (mission_item_triplet.current.nav_cmd != NAV_CMD_LAND) { land_noreturn_horizontal = false; From 6b53c7e841308e112832c6af2368ae3536a91061 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 22 Nov 2013 21:26:52 +0100 Subject: [PATCH 147/447] Navigator: Missions (including RTL), Loiter and RTL working --- src/modules/navigator/navigator_main.cpp | 232 ++++++++++++++++++----- 1 file changed, 189 insertions(+), 43 deletions(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index d213310654..5b6b2f8215 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -56,6 +56,7 @@ #include #include #include +#include #include #include #include @@ -69,6 +70,12 @@ #include #include +typedef enum { + NAVIGATION_MODE_NONE, + NAVIGATION_MODE_LOITER, + NAVIGATION_MODE_WAYPOINT, + NAVIGATION_MODE_RTL +} navigation_mode_t; /** * navigator app start / stop handling function @@ -117,9 +124,10 @@ private: bool _task_should_exit; /**< if true, sensor task should exit */ int _navigator_task; /**< task handle for sensor task */ - int _global_pos_sub; + int _global_pos_sub; /**< global position subscription */ + int _home_pos_sub; /**< home position subscription */ int _vstatus_sub; /**< vehicle status subscription */ - int _params_sub; /**< notification of parameter updates */ + int _params_sub; /**< notification of parameter updates */ int _mission_sub; /**< notification of mission updates */ int _capabilities_sub; /**< notification of vehicle capabilities updates */ @@ -128,6 +136,7 @@ private: struct vehicle_status_s _vstatus; /**< vehicle status */ struct vehicle_global_position_s _global_pos; /**< global vehicle position */ + struct home_position_s _home_pos; /**< home position for RTL */ struct mission_item_triplet_s _mission_item_triplet; /**< triplet of mission items */ perf_counter_t _loop_perf; /**< loop performance counter */ @@ -147,12 +156,8 @@ private: bool _waypoint_yaw_reached; uint64_t _time_first_inside_orbit; - /** manual control states */ - float _seatbelt_hold_heading; /**< heading the system should hold in seatbelt mode */ - float _loiter_hold_lat; - float _loiter_hold_lon; - float _loiter_hold_alt; - bool _loiter_hold; + navigation_mode_t _mode; + bool _restart_mission_wanted; struct { float throttle_cruise; @@ -169,12 +174,6 @@ private: */ int parameters_update(); - /** - * Update control outputs - * - */ - void control_update(); - /** * Retrieve mission. */ @@ -196,6 +195,8 @@ private: bool fence_valid(const struct fence_s &fence); + void set_loiter_mission_item(struct mission_item_s *new_mission_item); + void add_mission_item(unsigned mission_item_index, const struct mission_item_s *existing_mission_item, struct mission_item_s *new_mission_item); @@ -209,6 +210,10 @@ private: void reset_mission_item_reached(); bool check_mission_item_reached(); + + void start_loiter(); + + void start_rtl(); }; namespace navigator @@ -230,6 +235,7 @@ Navigator::Navigator() : /* subscriptions */ _global_pos_sub(-1), + _home_pos_sub(-1), _vstatus_sub(-1), _params_sub(-1), _mission_sub(-1), @@ -248,7 +254,8 @@ Navigator::Navigator() : _waypoint_position_reached(false), _waypoint_yaw_reached(false), _time_first_inside_orbit(0), - _loiter_hold(false) + _mode(NAVIGATION_MODE_NONE), + _restart_mission_wanted(true) { _global_pos.valid = false; memset(&_fence, 0, sizeof(_fence)); @@ -256,6 +263,13 @@ Navigator::Navigator() : _mission_item = (mission_item_s*)malloc(sizeof(mission_item_s) * _max_mission_item_count); + _mission_item_triplet.previous_valid = false; + _mission_item_triplet.current_valid = false; + _mission_item_triplet.next_valid = false; + memset(&_mission_item_triplet.previous, 0, sizeof(struct mission_item_s)); + memset(&_mission_item_triplet.current, 0, sizeof(struct mission_item_s)); + memset(&_mission_item_triplet.next, 0, sizeof(struct mission_item_s)); + /* fetch initial values */ parameters_update(); } @@ -313,8 +327,9 @@ Navigator::mission_update() irqrestore(flags); - /* start new mission at beginning */ - restart_mission(); + /* set flag to restart mission next we're in auto */ + _restart_mission_wanted = true; + } else { warnx("ERROR: too many waypoints, not supported"); } @@ -347,6 +362,7 @@ Navigator::task_main() _capabilities_sub = orb_subscribe(ORB_ID(navigation_capabilities)); _vstatus_sub = orb_subscribe(ORB_ID(vehicle_status)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); + _home_pos_sub = orb_subscribe(ORB_ID(home_position)); // Load initial states if (orb_copy(ORB_ID(vehicle_status), _vstatus_sub, &_vstatus) != OK) { @@ -368,19 +384,22 @@ Navigator::task_main() orb_copy(ORB_ID(navigation_capabilities), _capabilities_sub, &_nav_caps); /* wakeup source(s) */ - struct pollfd fds[5]; + struct pollfd fds[6]; /* Setup of loop */ fds[0].fd = _params_sub; fds[0].events = POLLIN; fds[1].fd = _global_pos_sub; fds[1].events = POLLIN; - fds[2].fd = _capabilities_sub; + fds[2].fd = _home_pos_sub; fds[2].events = POLLIN; - fds[3].fd = _mission_sub; + fds[3].fd = _capabilities_sub; fds[3].events = POLLIN; - fds[4].fd = _vstatus_sub; + fds[4].fd = _mission_sub; fds[4].events = POLLIN; + fds[5].fd = _vstatus_sub; + fds[5].events = POLLIN; + while (!_task_should_exit) { @@ -400,13 +419,13 @@ Navigator::task_main() perf_begin(_loop_perf); - /* only update parameters if they changed */ - if (fds[4].revents & POLLIN) { + /* only update vehicle status if it changed */ + if (fds[5].revents & POLLIN) { /* read from param to clear updated flag */ orb_copy(ORB_ID(vehicle_status), _vstatus_sub, &_vstatus); } - /* only update vehicle status if it changed */ + /* only update parameters if it changed */ if (fds[0].revents & POLLIN) { /* read from param to clear updated flag */ struct parameter_update_s update; @@ -417,14 +436,18 @@ Navigator::task_main() } /* only update craft capabilities if they have changed */ - if (fds[2].revents & POLLIN) { + if (fds[3].revents & POLLIN) { orb_copy(ORB_ID(navigation_capabilities), _capabilities_sub, &_nav_caps); } - if (fds[3].revents & POLLIN) { + if (fds[4].revents & POLLIN) { mission_update(); } + if (fds[2].revents & POLLIN) { + orb_copy(ORB_ID(home_position), _home_pos_sub, &_home_pos); + } + /* only run controller if position changed */ if (fds[1].revents & POLLIN) { @@ -451,12 +474,65 @@ Navigator::task_main() math::Vector2f current_position(_global_pos.lat / 1e7f, _global_pos.lon / 1e7f); /* Autonomous flight */ - if (1 /* autonomous flight */) { + if (_vstatus.main_state == MAIN_STATE_AUTO) { + + switch (_vstatus.navigation_state) { + case NAVIGATION_STATE_AUTO_MISSION: + + if (_mission_item_count == 0) { + + if (_mode != NAVIGATION_MODE_LOITER) { + + start_loiter(); + _mode = NAVIGATION_MODE_LOITER; + } + } else { + + if (_restart_mission_wanted) { + restart_mission(); + _restart_mission_wanted = false; + } + + /* after RTL, start mission new */ + if (_mode == NAVIGATION_MODE_RTL) { + restart_mission(); + } + + /* proceed to next waypoint if we reach it */ + if (check_mission_item_reached()) { + advance_current_mission_item(); + } + _mode = NAVIGATION_MODE_WAYPOINT; + } + + break; + + case NAVIGATION_STATE_AUTO_LOITER: + + if (_mode != NAVIGATION_MODE_LOITER) { + + start_loiter(); + _mode = NAVIGATION_MODE_LOITER; + } + break; + + case NAVIGATION_STATE_AUTO_RTL: + + if (_mode != NAVIGATION_MODE_RTL) { + + start_rtl(); + _mode = NAVIGATION_MODE_RTL; + } + + if (check_mission_item_reached()) { + advance_current_mission_item(); + } + break; - /* proceed to next waypoint if we reach it */ - if (check_mission_item_reached()) { - advance_current_mission_item(); } + + } else { + _mode = NAVIGATION_MODE_NONE; } } @@ -600,15 +676,39 @@ Navigator::fence_point(int argc, char *argv[]) errx(1, "can't store fence point"); } +void +Navigator::set_loiter_mission_item(struct mission_item_s *new_mission_item) { + + new_mission_item->lat = (double)_global_pos.lat / 1e7; + new_mission_item->lon = (double)_global_pos.lon / 1e7; + new_mission_item->altitude = _global_pos.alt; + new_mission_item->yaw = _global_pos.yaw; + new_mission_item->nav_cmd = NAV_CMD_LOITER_UNLIMITED; + new_mission_item->loiter_direction = 1; + new_mission_item->loiter_radius = 100.0f; // TODO: get rid of magic number + new_mission_item->radius = 50.0f; // TODO: get rid of magic number + new_mission_item->autocontinue = false; +} + void Navigator::add_mission_item(unsigned mission_item_index, const struct mission_item_s *existing_mission_item, struct mission_item_s *new_mission_item) { - /* Check if there is a further mission as the new next item */ + /* Check if there is a further mission as the new item */ while (mission_item_index < _mission_item_count) { if (1 /* TODO: check for correct frame */) { memcpy(new_mission_item, &_mission_item[mission_item_index], sizeof(mission_item_s)); + + if (new_mission_item->nav_cmd == NAV_CMD_RETURN_TO_LAUNCH) { + /* if it is a RTL waypoint, append the home position */ + new_mission_item->lat = (double)_home_pos.lat / 1e7; + new_mission_item->lon = (double)_home_pos.lon / 1e7; + new_mission_item->altitude = (float)_home_pos.alt / 1e3 + 50.0f; // TODO: add parameter for RTL altitude + new_mission_item->loiter_radius = 100.0f; // TODO: get rid of magic number + new_mission_item->radius = 50.0f; // TODO: get rid of magic number + } + return; } mission_item_index++; @@ -617,15 +717,7 @@ Navigator::add_mission_item(unsigned mission_item_index, const struct mission_it /* if no existing mission item exists, take curent location */ if (existing_mission_item == nullptr) { - new_mission_item->lat = (double)_global_pos.lat / 1e7; - new_mission_item->lon = (double)_global_pos.lon / 1e7; - new_mission_item->altitude = _global_pos.alt; - new_mission_item->yaw = _global_pos.yaw; - new_mission_item->nav_cmd = NAV_CMD_LOITER_UNLIMITED; - new_mission_item->loiter_direction = 1; - new_mission_item->loiter_radius = 100.0f; // TODO: get rid of magic number - new_mission_item->radius = 10.0f; // TODO: get rid of magic number - new_mission_item->autocontinue = false; + set_loiter_mission_item(new_mission_item); } else { @@ -636,7 +728,7 @@ Navigator::add_mission_item(unsigned mission_item_index, const struct mission_it case NAV_CMD_RETURN_TO_LAUNCH: case NAV_CMD_TAKEOFF: - /* copy current mission to next item */ + /* copy current mission to new item */ memcpy(new_mission_item, existing_mission_item, sizeof(mission_item_s)); /* and set it to a loiter item */ @@ -751,8 +843,6 @@ Navigator::restart_mission() } - - void Navigator::reset_mission_item_reached() { @@ -846,6 +936,62 @@ Navigator::check_mission_item_reached() } +void +Navigator::start_loiter() +{ + if (_mission_item_triplet.current_valid && _mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_UNLIMITED) { + /* already loitering, set again for latest lat/lon/alt */ + set_loiter_mission_item(&_mission_item_triplet.current); + + } else if (_mission_item_triplet.current_valid && _mission_item_triplet.current.nav_cmd != NAV_CMD_LOITER_UNLIMITED) { + /* move current waypoint back to next and switch to loiter now */ + memcpy(&_mission_item_triplet.next, &_mission_item_triplet.current, sizeof(mission_item_s)); + _mission_item_triplet.next_valid = _mission_item_triplet.current_valid; + + set_loiter_mission_item(&_mission_item_triplet.current); + } else { + /* if the current mission item is invalid, it will be added now */ + set_loiter_mission_item(&_mission_item_triplet.current); + _mission_item_triplet.current_valid = true; + } + + update_mission_item_triplet(); +} + +void +Navigator::start_rtl() +{ + reset_mission_item_reached(); + + /* discard all mission item and insert RTL item */ + _mission_item_triplet.previous_valid = false; + + _mission_item_triplet.current.lat = (double)_home_pos.lat / 1e7; + _mission_item_triplet.current.lon = (double)_home_pos.lon / 1e7; + _mission_item_triplet.current.altitude = (float)_home_pos.alt / 1e3 + 50.0f; // TODO: add parameter for RTL altitude + _mission_item_triplet.current.yaw = 0.0f; + _mission_item_triplet.current.nav_cmd = NAV_CMD_RETURN_TO_LAUNCH; + _mission_item_triplet.current.loiter_direction = 1; + _mission_item_triplet.current.loiter_radius = 100.0f; // TODO: get rid of magic number + _mission_item_triplet.current.radius = 50.0f; // TODO: get rid of magic number + _mission_item_triplet.current.autocontinue = false; + _mission_item_triplet.current_valid = true; + + _mission_item_triplet.next.lat = (double)_home_pos.lat / 1e7; + _mission_item_triplet.next.lon = (double)_home_pos.lon / 1e7; + _mission_item_triplet.next.altitude = (float)_home_pos.alt / 1e3 + 50.0f; // TODO: add parameter for RTL altitude + _mission_item_triplet.next.yaw = _global_pos.yaw; + _mission_item_triplet.next.nav_cmd = NAV_CMD_LOITER_UNLIMITED; + _mission_item_triplet.next.loiter_direction = 1; + _mission_item_triplet.next.loiter_radius = 100.0f; // TODO: get rid of magic number + _mission_item_triplet.next.radius = 50.0f; // TODO: get rid of magic number + _mission_item_triplet.next.autocontinue = false; + _mission_item_triplet.next_valid; + + update_mission_item_triplet(); +} + + static void usage() { errx(1, "usage: navigator {start|stop|status|fence}"); From 20ac3c815547716ea7ad201c56e3e64ba742555f Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 23 Nov 2013 12:48:26 +0400 Subject: [PATCH 148/447] multirotor_pos_control: fixed wrong merging --- .../multirotor_pos_control.c | 104 ++++++++++++++---- 1 file changed, 81 insertions(+), 23 deletions(-) diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c index 3d23d0c096..129e639ae4 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c @@ -70,11 +70,13 @@ #include "multirotor_pos_control_params.h" #include "thrust_pid.h" - static bool thread_should_exit = false; /**< Deamon exit flag */ static bool thread_running = false; /**< Deamon status flag */ static int deamon_task; /**< Handle of deamon task / thread */ +static const float alt_ctl_dz = 0.2f; +static const float pos_ctl_dz = 0.05f; + __EXPORT int multirotor_pos_control_main(int argc, char *argv[]); /** @@ -133,8 +135,14 @@ int multirotor_pos_control_main(int argc, char *argv[]) } if (!strcmp(argv[1], "stop")) { - warnx("stop"); - thread_should_exit = true; + if (thread_running) { + warnx("stop"); + thread_should_exit = true; + + } else { + warnx("app not started"); + } + exit(0); } @@ -223,14 +231,17 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) bool reset_auto_sp_xy = true; bool reset_auto_sp_z = true; bool reset_takeoff_sp = true; - - hrt_abstime t_prev = 0; - const float alt_ctl_dz = 0.2f; - const float pos_ctl_dz = 0.05f; + bool reset_z_sp_dist = false; + float surface_offset = 0.0f; // Z of ground level + float sp_z_dist = 0.0f; // distance to ground setpoint (positive) float ref_alt = 0.0f; - hrt_abstime ref_alt_t = 0; + hrt_abstime ref_timestamp = 0; + + hrt_abstime t_prev = 0; + uint64_t local_ref_timestamp = 0; + uint64_t surface_bottom_timestamp = 0; PID_t xy_pos_pids[2]; PID_t xy_vel_pids[2]; @@ -244,7 +255,6 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) parameters_init(¶ms_h); parameters_update(¶ms_h, ¶ms); - for (int i = 0; i < 2; i++) { pid_init(&(xy_pos_pids[i]), params.xy_p, 0.0f, params.xy_d, 1.0f, 0.0f, PID_MODE_DERIVATIV_SET, 0.02f); pid_init(&(xy_vel_pids[i]), params.xy_vel_p, params.xy_vel_i, params.xy_vel_d, 1.0f, params.tilt_max, PID_MODE_DERIVATIV_CALC_NO_SP, 0.02f); @@ -325,45 +335,71 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) t_prev = t; + orb_copy(ORB_ID(vehicle_local_position), local_pos_sub, &local_pos); + if (control_mode.flag_control_altitude_enabled || control_mode.flag_control_velocity_enabled || control_mode.flag_control_position_enabled) { orb_copy(ORB_ID(manual_control_setpoint), manual_sub, &manual); orb_copy(ORB_ID(vehicle_attitude), att_sub, &att); orb_copy(ORB_ID(vehicle_attitude_setpoint), att_sp_sub, &att_sp); - orb_copy(ORB_ID(vehicle_local_position), local_pos_sub, &local_pos); float z_sp_offs_max = params.z_vel_max / params.z_p * 2.0f; float xy_sp_offs_max = params.xy_vel_max / params.xy_p * 2.0f; float sp_move_rate[3] = { 0.0f, 0.0f, 0.0f }; + float sp_z = local_pos_sp.z; + float z = local_pos.z; + float vz = local_pos.vz; + if (control_mode.flag_control_manual_enabled) { /* manual control */ /* check for reference point updates and correct setpoint */ - if (local_pos.ref_timestamp != ref_alt_t) { - if (ref_alt_t != 0) { - /* home alt changed, don't follow large ground level changes in manual flight */ - local_pos_sp.z += local_pos.ref_alt - ref_alt; - } - - ref_alt_t = local_pos.ref_timestamp; - ref_alt = local_pos.ref_alt; + if (local_pos.ref_timestamp != ref_timestamp) { + /* reference alt changed, don't follow large ground level changes in manual flight */ + local_pos_sp.z += local_pos.ref_alt - ref_alt; // TODO also correct XY setpoint } - /* reset setpoints to current position if needed */ if (control_mode.flag_control_altitude_enabled) { + /* reset setpoints to current position if needed */ if (reset_man_sp_z) { reset_man_sp_z = false; local_pos_sp.z = local_pos.z; + reset_z_sp_dist = true; mavlink_log_info(mavlink_fd, "[mpc] reset alt sp: %.2f", (double) - local_pos_sp.z); } + /* if distance to surface is available, use it */ + if (control_mode.flag_use_dist_bottom && local_pos.dist_bottom_valid) { + if (reset_z_sp_dist) { + reset_z_sp_dist = false; + surface_offset = local_pos.z + local_pos.dist_bottom; + sp_z_dist = -local_pos_sp.z + surface_offset; + mavlink_log_info(mavlink_fd, "[mpc] surface offset: %.2f", surface_offset); + + } else { + if (local_pos.surface_bottom_timestamp != surface_bottom_timestamp) { + /* new surface level */ + sp_z_dist += local_pos.z + local_pos.dist_bottom - surface_offset; + } + + surface_offset = local_pos.z + local_pos.dist_bottom; + } + + z = -local_pos.dist_bottom; + vz = -local_pos.dist_bottom_rate; + /* move altitude setpoint according to ground distance */ + local_pos_sp.z = surface_offset - sp_z_dist; + } + /* move altitude setpoint with throttle stick */ float z_sp_ctl = scale_control(manual.throttle - 0.5f, 0.5f, alt_ctl_dz); if (z_sp_ctl != 0.0f) { sp_move_rate[2] = -z_sp_ctl * params.z_vel_max; local_pos_sp.z += sp_move_rate[2] * dt; + sp_z_dist -= sp_move_rate[2] * dt; + // TODO add z_sp_dist correction here if (local_pos_sp.z > local_pos.z + z_sp_offs_max) { local_pos_sp.z = local_pos.z + z_sp_offs_max; @@ -371,6 +407,12 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) local_pos_sp.z = local_pos.z - z_sp_offs_max; } } + + if (control_mode.flag_use_dist_bottom && local_pos.dist_bottom_valid) { + sp_z = -sp_z_dist; + } else { + sp_z = local_pos_sp.z; + } } if (control_mode.flag_control_position_enabled) { @@ -471,6 +513,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) } else { local_pos_sp.z = local_pos.ref_alt - global_pos_sp.altitude; } + /* update yaw setpoint only if value is valid */ if (isfinite(global_pos_sp.yaw) && fabsf(global_pos_sp.yaw) < M_TWOPI) { att_sp.yaw_body = global_pos_sp.yaw; @@ -516,6 +559,8 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) reset_man_sp_xy = true; reset_man_sp_z = true; + sp_z = local_pos_sp.z; + } else { /* no control (failsafe), loiter or stay on ground */ if (local_pos.landed) { @@ -553,6 +598,8 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) reset_auto_sp_xy = false; } + + sp_z = local_pos_sp.z; } /* publish local position setpoint */ @@ -560,7 +607,8 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) /* run position & altitude controllers, calculate velocity setpoint */ if (control_mode.flag_control_altitude_enabled) { - global_vel_sp.vz = pid_calculate(&z_pos_pid, local_pos_sp.z, local_pos.z, local_pos.vz - sp_move_rate[2], dt) + sp_move_rate[2]; + // TODO add feed-forward to PID library to allow limiting resulting value + global_vel_sp.vz = pid_calculate(&z_pos_pid, sp_z, z, vz - sp_move_rate[2], dt) + sp_move_rate[2]; } else { reset_man_sp_z = true; @@ -614,7 +662,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) mavlink_log_info(mavlink_fd, "[mpc] reset hovering thrust: %.2f", (double)i); } - thrust_sp[2] = thrust_pid_calculate(&z_vel_pid, global_vel_sp.vz, local_pos.vz, dt, att.R[2][2]); + thrust_sp[2] = thrust_pid_calculate(&z_vel_pid, global_vel_sp.vz, vz, dt, att.R[2][2]); att_sp.thrust = -thrust_sp[2]; } else { @@ -675,8 +723,18 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) /* reset altitude controller integral (hovering throttle) to manual throttle after manual throttle control */ reset_int_z_manual = control_mode.flag_armed && control_mode.flag_control_manual_enabled && !control_mode.flag_control_climb_rate_enabled; - /* run at approximately 50 Hz */ - usleep(20000); + /* reset distance setpoint if distance is not available */ + if (!local_pos.dist_bottom_valid || !control_mode.flag_use_dist_bottom) { + reset_z_sp_dist = true; + } + + surface_bottom_timestamp = local_pos.surface_bottom_timestamp; + + ref_alt = local_pos.ref_alt; + ref_timestamp = local_pos.ref_timestamp; + + /* run at approximately 100 Hz */ + usleep(10000); } warnx("stopped"); From 94745fa0af93f8e58eaf2d100c482030a838bc46 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 24 Nov 2013 08:32:53 +0100 Subject: [PATCH 149/447] fw autoland: move constrain of roll to horizontal landing navigation --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 975ac85ff2..7495a39e07 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -821,6 +821,9 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio _l1_control.navigate_heading(target_bearing, _att.yaw, ground_speed); + /* limit roll motion to prevent wings from touching the ground first */ + _att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-10.0f), math::radians(10.0f)); + land_noreturn_horizontal = true; } else { @@ -896,8 +899,6 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio 0.0f, throttle_max, throttle_land, flare_angle_rad, math::radians(15.0f)); - /* limit roll motion to prevent wings from touching the ground first */ - _att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-10.0f), math::radians(10.0f)); if (!land_noreturn_vertical) { mavlink_log_info(mavlink_fd, "[POSCTRL] Landing, flare"); land_noreturn_vertical = true; From 0611b26eeace29a366247026534872c714abd76d Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 24 Nov 2013 08:32:53 +0100 Subject: [PATCH 150/447] fw autoland: move constrain of roll to horizontal landing navigation Conflicts: src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 84983785bf..bfdca9cce3 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -837,6 +837,9 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio _l1_control.navigate_heading(target_bearing, _att.yaw, ground_speed); + /* limit roll motion to prevent wings from touching the ground first */ + _att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-10.0f), math::radians(10.0f)); + land_noreturn_horizontal = true; } else { @@ -911,8 +914,6 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio 0.0f, throttle_max, throttle_land, flare_angle_rad, math::radians(15.0f)); - /* limit roll motion to prevent wings from touching the ground first */ - _att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-10.0f), math::radians(10.0f)); if (!land_noreturn_vertical) { mavlink_log_info(mavlink_fd, "[POSCTRL] Landing, flare"); land_noreturn_vertical = true; From 881c89dd1b55f5e2dbb355562665a94dcc618217 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 24 Nov 2013 11:02:41 +0100 Subject: [PATCH 151/447] increase safety margin for takeoff speed --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index bfdca9cce3..2b1c01c839 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -972,7 +972,7 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio if (altitude_error > 15.0f) { /* enforce a minimum of 10 degrees pitch up on takeoff, or take parameter */ - _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_min), + _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(1.3f*_parameters.airspeed_min), _airspeed.indicated_airspeed_m_s, eas2tas, true, math::max(math::radians(global_triplet.current.param1), math::radians(10.0f)), _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, From b6f2c286e9774194f75d25d272ad66e9dc63bcd5 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 24 Nov 2013 11:09:14 +0100 Subject: [PATCH 152/447] disabling bad descent detection because of to many false positives --- src/lib/external_lgpl/tecs/tecs.cpp | 18 +++++++++++------- 1 file changed, 11 insertions(+), 7 deletions(-) diff --git a/src/lib/external_lgpl/tecs/tecs.cpp b/src/lib/external_lgpl/tecs/tecs.cpp index 510b8ed9c1..1ae6e76531 100644 --- a/src/lib/external_lgpl/tecs/tecs.cpp +++ b/src/lib/external_lgpl/tecs/tecs.cpp @@ -368,14 +368,18 @@ void TECS::_detect_bad_descent(void) // 1) Underspeed protection not active // 2) Specific total energy error > 0 // This mode will produce an undulating speed and height response as it cuts in and out but will prevent the aircraft from descending into the ground if an unachievable speed demand is set - float STEdot = _SPEdot + _SKEdot; +// float STEdot = _SPEdot + _SKEdot; +// +// if ((!_underspeed && (_STE_error > 200.0f) && (STEdot < 0.0f) && (_throttle_dem >= _THRmaxf * 0.9f)) || (_badDescent && !_underspeed && (_STE_error > 0.0f))) { +// +// warnx("bad descent detected: _STE_error %.1f, STEdot %.1f, _throttle_dem %.1f", _STE_error, STEdot, _throttle_dem); +// _badDescent = true; +// +// } else { +// _badDescent = false; +// } - if ((!_underspeed && (_STE_error > 200.0f) && (STEdot < 0.0f) && (_throttle_dem >= _THRmaxf * 0.9f)) || (_badDescent && !_underspeed && (_STE_error > 0.0f))) { - _badDescent = true; - - } else { - _badDescent = false; - } + _badDescent = false; } void TECS::_update_pitch(void) From f267fcf4ff2da214560d3be7b9c6583cefac4475 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 24 Nov 2013 11:11:06 +0100 Subject: [PATCH 153/447] disable a printf --- src/lib/external_lgpl/tecs/tecs.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/external_lgpl/tecs/tecs.cpp b/src/lib/external_lgpl/tecs/tecs.cpp index 1ae6e76531..fb7ffccda7 100644 --- a/src/lib/external_lgpl/tecs/tecs.cpp +++ b/src/lib/external_lgpl/tecs/tecs.cpp @@ -240,7 +240,7 @@ void TECS::_update_height_demand(float demand, float state) _hgt_rate_dem = -_maxSinkRate; } - warnx("_hgt_rate_dem: %.4f, _hgt_dem_adj %.4f", _hgt_rate_dem, _hgt_dem_adj); + //warnx("_hgt_rate_dem: %.4f, _hgt_dem_adj %.4f", _hgt_rate_dem, _hgt_dem_adj); } void TECS::_detect_underspeed(void) From 85a76a32c5be43e8f1a4d82041e1b860dc21e217 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 24 Nov 2013 12:29:48 +0100 Subject: [PATCH 154/447] tecs: roll2thr: fix to be consistent with comment --- src/lib/external_lgpl/tecs/tecs.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/external_lgpl/tecs/tecs.cpp b/src/lib/external_lgpl/tecs/tecs.cpp index fb7ffccda7..a2ac6507e2 100644 --- a/src/lib/external_lgpl/tecs/tecs.cpp +++ b/src/lib/external_lgpl/tecs/tecs.cpp @@ -300,7 +300,7 @@ void TECS::_update_throttle(float throttle_cruise, const math::Dcm &rotMat) // additional component which scales with (1/cos(bank angle) - 1) to compensate for induced // drag increase during turns. float cosPhi = sqrtf((rotMat(0, 1) * rotMat(0, 1)) + (rotMat(1, 1) * rotMat(1, 1))); - STEdot_dem = STEdot_dem + _rollComp * (1.0f / constrain(cosPhi * cosPhi , 0.1f, 1.0f) - 1.0f); + STEdot_dem = STEdot_dem + _rollComp * (1.0f / constrain(cosPhi , 0.1f, 1.0f) - 1.0f); if (STEdot_dem >= 0) { ff_throttle = nomThr + STEdot_dem / _STEdot_max * (1.0f - nomThr); From 3c6f01bea8a65e2c347d1b893b3fe0d152bff69c Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 24 Nov 2013 12:48:44 +0100 Subject: [PATCH 155/447] tecs: speedrate: use p loop instead of pre calculated speed rate for now --- src/lib/external_lgpl/tecs/tecs.cpp | 60 +++++++++++-------- src/lib/external_lgpl/tecs/tecs.h | 5 ++ .../fw_pos_control_l1_main.cpp | 5 ++ .../fw_pos_control_l1_params.c | 1 + 4 files changed, 46 insertions(+), 25 deletions(-) diff --git a/src/lib/external_lgpl/tecs/tecs.cpp b/src/lib/external_lgpl/tecs/tecs.cpp index a2ac6507e2..d089be080b 100644 --- a/src/lib/external_lgpl/tecs/tecs.cpp +++ b/src/lib/external_lgpl/tecs/tecs.cpp @@ -169,35 +169,45 @@ void TECS::_update_speed_demand(void) // calculate velocity rate limits based on physical performance limits // provision to use a different rate limit if bad descent or underspeed condition exists // Use 50% of maximum energy rate to allow margin for total energy contgroller - float velRateMax; - float velRateMin; +// float velRateMax; +// float velRateMin; +// +// if ((_badDescent) || (_underspeed)) { +// velRateMax = 0.5f * _STEdot_max / _integ5_state; +// velRateMin = 0.5f * _STEdot_min / _integ5_state; +// +// } else { +// velRateMax = 0.5f * _STEdot_max / _integ5_state; +// velRateMin = 0.5f * _STEdot_min / _integ5_state; +// } +// +// // Apply rate limit +// if ((_TAS_dem - _TAS_dem_adj) > (velRateMax * 0.1f)) { +// _TAS_dem_adj = _TAS_dem_adj + velRateMax * 0.1f; +// _TAS_rate_dem = velRateMax; +// +// } else if ((_TAS_dem - _TAS_dem_adj) < (velRateMin * 0.1f)) { +// _TAS_dem_adj = _TAS_dem_adj + velRateMin * 0.1f; +// _TAS_rate_dem = velRateMin; +// +// } else { +// _TAS_dem_adj = _TAS_dem; +// +// +// _TAS_rate_dem = (_TAS_dem - _TAS_dem_last) / 0.1f; +// } - if ((_badDescent) || (_underspeed)) { - velRateMax = 0.5f * _STEdot_max / _integ5_state; - velRateMin = 0.5f * _STEdot_min / _integ5_state; - - } else { - velRateMax = 0.5f * _STEdot_max / _integ5_state; - velRateMin = 0.5f * _STEdot_min / _integ5_state; - } - - // Apply rate limit - if ((_TAS_dem - _TAS_dem_adj) > (velRateMax * 0.1f)) { - _TAS_dem_adj = _TAS_dem_adj + velRateMax * 0.1f; - _TAS_rate_dem = velRateMax; - - } else if ((_TAS_dem - _TAS_dem_adj) < (velRateMin * 0.1f)) { - _TAS_dem_adj = _TAS_dem_adj + velRateMin * 0.1f; - _TAS_rate_dem = velRateMin; - - } else { - _TAS_dem_adj = _TAS_dem; - _TAS_rate_dem = (_TAS_dem - _TAS_dem_last) / 0.1f; - } + _TAS_dem_adj = _TAS_dem; + _TAS_rate_dem = (_TAS_dem_adj-_integ5_state)*_speedrate_p; //xxx: using a p loop for now // Constrain speed demand again to protect against bad values on initialisation. _TAS_dem_adj = constrain(_TAS_dem_adj, _TASmin, _TASmax); - _TAS_dem_last = _TAS_dem; +// _TAS_dem_last = _TAS_dem; + +// warnx("_TAS_rate_dem: %.1f, _TAS_dem_adj %.1f, _integ5_state %.1f, _badDescent %u , _underspeed %u, velRateMin %.1f", +// (double)_TAS_rate_dem, (double)_TAS_dem_adj, (double)_integ5_state, _badDescent, _underspeed, velRateMin); +// warnx("_TAS_rate_dem: %.1f, _TAS_dem_adj %.1f, _integ5_state %.1f, _badDescent %u , _underspeed %u", +// (double)_TAS_rate_dem, (double)_TAS_dem_adj, (double)_integ5_state, _badDescent , _underspeed); } void TECS::_update_height_demand(float demand, float state) diff --git a/src/lib/external_lgpl/tecs/tecs.h b/src/lib/external_lgpl/tecs/tecs.h index 06e1c8ad32..4fc009da9e 100644 --- a/src/lib/external_lgpl/tecs/tecs.h +++ b/src/lib/external_lgpl/tecs/tecs.h @@ -184,6 +184,10 @@ public: _heightrate_p = heightrate_p; } + void set_speedrate_p(float speedrate_p) { + _speedrate_p = speedrate_p; + } + private: // Last time update_50Hz was called uint64_t _update_50hz_last_usec; @@ -208,6 +212,7 @@ private: float _rollComp; float _spdWeight; float _heightrate_p; + float _speedrate_p; // throttle demand in the range from 0.0 to 1.0 float _throttle_dem; diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 2b1c01c839..8b78bfbc4a 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -217,6 +217,7 @@ private: float loiter_hold_radius; float heightrate_p; + float speedrate_p; float land_slope_angle; float land_slope_length; @@ -260,6 +261,7 @@ private: param_t loiter_hold_radius; param_t heightrate_p; + param_t speedrate_p; param_t land_slope_angle; param_t land_slope_length; @@ -431,6 +433,7 @@ FixedwingPositionControl::FixedwingPositionControl() : _parameter_handles.speed_weight = param_find("FW_T_SPDWEIGHT"); _parameter_handles.pitch_damping = param_find("FW_T_PTCH_DAMP"); _parameter_handles.heightrate_p = param_find("FW_T_HRATE_P"); + _parameter_handles.speedrate_p = param_find("FW_T_SRATE_P"); /* fetch initial parameter values */ parameters_update(); @@ -497,6 +500,7 @@ FixedwingPositionControl::parameters_update() param_get(_parameter_handles.max_climb_rate, &(_parameters.max_climb_rate)); param_get(_parameter_handles.heightrate_p, &(_parameters.heightrate_p)); + param_get(_parameter_handles.speedrate_p, &(_parameters.speedrate_p)); param_get(_parameter_handles.land_slope_angle, &(_parameters.land_slope_angle)); param_get(_parameter_handles.land_slope_length, &(_parameters.land_slope_length)); @@ -523,6 +527,7 @@ FixedwingPositionControl::parameters_update() _tecs.set_indicated_airspeed_max(_parameters.airspeed_max); _tecs.set_max_climb_rate(_parameters.max_climb_rate); _tecs.set_heightrate_p(_parameters.heightrate_p); + _tecs.set_speedrate_p(_parameters.speedrate_p); /* sanity check parameters */ if (_parameters.airspeed_max < _parameters.airspeed_min || 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 f206d808e4..10a0c00fc9 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 @@ -113,6 +113,7 @@ PARAM_DEFINE_FLOAT(FW_T_PTCH_DAMP, 0.0f); PARAM_DEFINE_FLOAT(FW_T_SINK_MAX, 5.0f); PARAM_DEFINE_FLOAT(FW_T_HRATE_P, 0.05f); +PARAM_DEFINE_FLOAT(FW_T_SRATE_P, 0.05f); PARAM_DEFINE_FLOAT(FW_LND_ANG, 10.0f); PARAM_DEFINE_FLOAT(FW_LND_SLLR, 0.9f); From 98ebcb626d4a8f84b61fbbedaa594a0a3b8df17d Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 25 Nov 2013 19:47:33 +0400 Subject: [PATCH 156/447] position_estimator_inav: minor comments and code style fixes --- .../position_estimator_inav_main.c | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index d95de11c4a..d99895a648 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -388,7 +388,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (sensor.accelerometer_counter > accel_counter) { if (att.R_valid) { - /* correct accel bias, now only for Z */ + /* correct accel bias */ sensor.accelerometer_m_s2[0] -= acc_bias[0]; sensor.accelerometer_m_s2[1] -= acc_bias[1]; sensor.accelerometer_m_s2[2] -= acc_bias[2]; @@ -664,11 +664,11 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } /* baro offset correction */ - if (use_gps_z) { - float offs_corr = corr_gps[2][0] * w_z_gps_p * dt; - baro_offset += offs_corr; - baro_counter += offs_corr; - } + if (use_gps_z) { + float offs_corr = corr_gps[2][0] * w_z_gps_p * dt; + baro_offset += offs_corr; + baro_counter += offs_corr; + } /* accelerometer bias correction */ float accel_bias_corr[3] = { 0.0f, 0.0f, 0.0f }; @@ -679,6 +679,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) accel_bias_corr[1] -= corr_gps[1][0] * w_xy_gps_p * w_xy_gps_p; accel_bias_corr[1] -= corr_gps[1][1] * w_xy_gps_v; } + if (use_gps_z) { accel_bias_corr[2] -= corr_gps[2][0] * w_z_gps_p * w_z_gps_p; } From 036d1a40175ab7e851257ba703526cb3d380d25d Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 25 Nov 2013 19:04:22 +0100 Subject: [PATCH 157/447] Navigator: Yet another rewrite of the logic --- src/modules/navigator/navigator_main.cpp | 574 +++++++++++++---------- 1 file changed, 324 insertions(+), 250 deletions(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 5b6b2f8215..a91c694e1c 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -74,9 +74,17 @@ typedef enum { NAVIGATION_MODE_NONE, NAVIGATION_MODE_LOITER, NAVIGATION_MODE_WAYPOINT, - NAVIGATION_MODE_RTL + NAVIGATION_MODE_LOITER_WAYPOINT, + NAVIGATION_MODE_RTL, + NAVIGATION_MODE_LOITER_RTL } navigation_mode_t; +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; + /** * navigator app start / stop handling function * @@ -125,7 +133,7 @@ private: int _navigator_task; /**< task handle for sensor task */ int _global_pos_sub; /**< global position subscription */ - int _home_pos_sub; /**< home position subscription */ + int _home_pos_sub; /**< home position subscription */ int _vstatus_sub; /**< vehicle status subscription */ int _params_sub; /**< notification of parameter updates */ int _mission_sub; /**< notification of mission updates */ @@ -155,9 +163,10 @@ private: bool _waypoint_position_reached; bool _waypoint_yaw_reached; uint64_t _time_first_inside_orbit; + bool _mission_item_reached; navigation_mode_t _mode; - bool _restart_mission_wanted; + unsigned _current_mission_index; struct { float throttle_cruise; @@ -195,23 +204,21 @@ private: bool fence_valid(const struct fence_s &fence); - void set_loiter_mission_item(struct mission_item_s *new_mission_item); + void set_mode(navigation_mode_t new_nav_mode); - void add_mission_item(unsigned mission_item_index, - const struct mission_item_s *existing_mission_item, - struct mission_item_s *new_mission_item); + int set_waypoint_mission_item(unsigned mission_item_index, struct mission_item_s *new_mission_item); - void update_mission_item_triplet(); + void publish_mission_item_triplet(); - void advance_current_mission_item(); - - void restart_mission(); + int advance_current_mission_item(); void reset_mission_item_reached(); - bool check_mission_item_reached(); + void check_mission_item_reached(); - void start_loiter(); + void start_waypoint(); + + void start_loiter(mission_item_s *new_loiter_position); void start_rtl(); }; @@ -249,13 +256,15 @@ Navigator::Navigator() : _loop_perf(perf_alloc(PC_ELAPSED, "navigator")), /* states */ _max_mission_item_count(10), + _mission_item_count(0), _fence_valid(false), _inside_fence(true), _waypoint_position_reached(false), _waypoint_yaw_reached(false), _time_first_inside_orbit(0), + _mission_item_reached(false), _mode(NAVIGATION_MODE_NONE), - _restart_mission_wanted(true) + _current_mission_index(0) { _global_pos.valid = false; memset(&_fence, 0, sizeof(_fence)); @@ -317,6 +326,7 @@ Navigator::mission_update() // test implementation if (mission.count <= _max_mission_item_count) { + /* * Perform an atomic copy & state update */ @@ -327,16 +337,24 @@ Navigator::mission_update() irqrestore(flags); - /* set flag to restart mission next we're in auto */ - _restart_mission_wanted = true; + } else { warnx("ERROR: too many waypoints, not supported"); + _mission_item_count = 0; + } + + /* set flag to restart mission next we're in auto */ + _current_mission_index = 0; + + if (_mode == NAVIGATION_MODE_WAYPOINT) { + start_waypoint(); } - /* Reset to 0 for now when a waypoint is changed */ /* TODO add checks if and how the mission has changed */ - + } else { + _mission_item_count = 0; + _current_mission_index = 0; } } @@ -423,6 +441,61 @@ Navigator::task_main() if (fds[5].revents & POLLIN) { /* read from param to clear updated flag */ orb_copy(ORB_ID(vehicle_status), _vstatus_sub, &_vstatus); + + /* Evaluate state machine from commander and set the navigator mode accordingly */ + if (_vstatus.main_state == MAIN_STATE_AUTO) { + + switch (_vstatus.navigation_state) { + case NAVIGATION_STATE_DIRECT: + case NAVIGATION_STATE_STABILIZE: + case NAVIGATION_STATE_ALTHOLD: + case NAVIGATION_STATE_VECTOR: + + set_mode(NAVIGATION_MODE_NONE); + break; + + case NAVIGATION_STATE_AUTO_READY: + case NAVIGATION_STATE_AUTO_TAKEOFF: + + /* TODO add this */ + //set_mode(NAVIGATION_MODE_TAKEOFF); + break; + + case NAVIGATION_STATE_AUTO_LOITER: + + set_mode(NAVIGATION_MODE_LOITER); + break; + + case NAVIGATION_STATE_AUTO_MISSION: + + if (_mission_item_count > 0 && !(_current_mission_index >= _mission_item_count)) { + /* Start mission if there is a mission available and the last waypoint has not been reached */ + set_mode(NAVIGATION_MODE_WAYPOINT); + } else { + /* else fallback to loiter */ + set_mode(NAVIGATION_MODE_LOITER); + } + break; + + case NAVIGATION_STATE_AUTO_RTL: + + set_mode(NAVIGATION_MODE_RTL); + break; + + case NAVIGATION_STATE_AUTO_LAND: + + /* TODO add this */ + //set_mode(NAVIGATION_MODE_LAND); + break; + + default: + warnx("Navigation state not supported"); + break; + } + + } else { + set_mode(NAVIGATION_MODE_NONE); + } } /* only update parameters if it changed */ @@ -453,91 +526,41 @@ Navigator::task_main() /* load local copies */ orb_copy(ORB_ID(vehicle_global_position), _global_pos_sub, &_global_pos); + + /* do stuff according to mode */ + switch (_mode) { + case NAVIGATION_MODE_NONE: + case NAVIGATION_MODE_LOITER: + case NAVIGATION_MODE_LOITER_WAYPOINT: + case NAVIGATION_MODE_LOITER_RTL: + break; - static uint64_t last_run = 0; - float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f; - last_run = hrt_absolute_time(); + case NAVIGATION_MODE_WAYPOINT: - /* guard against too large deltaT's */ - if (deltaT > 1.0f) { - deltaT = 0.01f; - } + check_mission_item_reached(); - if (_fence_valid && _global_pos.valid) { - _inside_fence = inside_geofence(&_global_pos, &_fence); - } - - math::Vector2f ground_speed(_global_pos.vx, _global_pos.vy); - /* Current waypoint */ - math::Vector2f next_wp(_mission_item_triplet.current.lat / 1e7f, _mission_item_triplet.current.lon / 1e7f); - /* Global position */ - math::Vector2f current_position(_global_pos.lat / 1e7f, _global_pos.lon / 1e7f); - - /* Autonomous flight */ - if (_vstatus.main_state == MAIN_STATE_AUTO) { - - switch (_vstatus.navigation_state) { - case NAVIGATION_STATE_AUTO_MISSION: - - if (_mission_item_count == 0) { - - if (_mode != NAVIGATION_MODE_LOITER) { - - start_loiter(); - _mode = NAVIGATION_MODE_LOITER; - } - } else { - - if (_restart_mission_wanted) { - restart_mission(); - _restart_mission_wanted = false; - } - - /* after RTL, start mission new */ - if (_mode == NAVIGATION_MODE_RTL) { - restart_mission(); - } - - /* proceed to next waypoint if we reach it */ - if (check_mission_item_reached()) { - advance_current_mission_item(); - } - _mode = NAVIGATION_MODE_WAYPOINT; + if (_mission_item_reached) { + // warnx("Mission already reached"); + if (advance_current_mission_item() != OK) { + set_mode(NAVIGATION_MODE_LOITER_WAYPOINT); } - - break; + } + break; - case NAVIGATION_STATE_AUTO_LOITER: + case NAVIGATION_MODE_RTL: - if (_mode != NAVIGATION_MODE_LOITER) { + check_mission_item_reached(); - start_loiter(); - _mode = NAVIGATION_MODE_LOITER; - } - break; - - case NAVIGATION_STATE_AUTO_RTL: - - if (_mode != NAVIGATION_MODE_RTL) { - - start_rtl(); - _mode = NAVIGATION_MODE_RTL; - } - - if (check_mission_item_reached()) { - advance_current_mission_item(); - } - break; - - } - - } else { - _mode = NAVIGATION_MODE_NONE; + if (_mission_item_reached) { + set_mode(NAVIGATION_MODE_LOITER_RTL); + } + break; + default: + warnx("navigation mode not supported"); + break; } } - perf_end(_loop_perf); - } warnx("exiting."); @@ -677,123 +700,134 @@ Navigator::fence_point(int argc, char *argv[]) } void -Navigator::set_loiter_mission_item(struct mission_item_s *new_mission_item) { - - new_mission_item->lat = (double)_global_pos.lat / 1e7; - new_mission_item->lon = (double)_global_pos.lon / 1e7; - new_mission_item->altitude = _global_pos.alt; - new_mission_item->yaw = _global_pos.yaw; - new_mission_item->nav_cmd = NAV_CMD_LOITER_UNLIMITED; - new_mission_item->loiter_direction = 1; - new_mission_item->loiter_radius = 100.0f; // TODO: get rid of magic number - new_mission_item->radius = 50.0f; // TODO: get rid of magic number - new_mission_item->autocontinue = false; -} - -void -Navigator::add_mission_item(unsigned mission_item_index, const struct mission_item_s *existing_mission_item, struct mission_item_s *new_mission_item) { - - /* Check if there is a further mission as the new item */ - while (mission_item_index < _mission_item_count) { - - if (1 /* TODO: check for correct frame */) { - - memcpy(new_mission_item, &_mission_item[mission_item_index], sizeof(mission_item_s)); - - if (new_mission_item->nav_cmd == NAV_CMD_RETURN_TO_LAUNCH) { - /* if it is a RTL waypoint, append the home position */ - new_mission_item->lat = (double)_home_pos.lat / 1e7; - new_mission_item->lon = (double)_home_pos.lon / 1e7; - new_mission_item->altitude = (float)_home_pos.alt / 1e3 + 50.0f; // TODO: add parameter for RTL altitude - new_mission_item->loiter_radius = 100.0f; // TODO: get rid of magic number - new_mission_item->radius = 50.0f; // TODO: get rid of magic number - } - - return; - } - mission_item_index++; - } - - /* if no existing mission item exists, take curent location */ - if (existing_mission_item == nullptr) { - - set_loiter_mission_item(new_mission_item); - - } else { - - switch (existing_mission_item->nav_cmd) { - - /* if the last mission is not a loiter item, set it to one */ - case NAV_CMD_WAYPOINT: - case NAV_CMD_RETURN_TO_LAUNCH: - case NAV_CMD_TAKEOFF: - - /* copy current mission to new item */ - memcpy(new_mission_item, existing_mission_item, sizeof(mission_item_s)); - - /* and set it to a loiter item */ - new_mission_item->nav_cmd = NAV_CMD_LOITER_UNLIMITED; - /* also adapt the loiter_radius */ - new_mission_item->loiter_radius = 100.0f; - //_mission_item_triplet.current.loiter_radius = _nav_caps.turn_distance; // TODO: publish capabilities somewhere - new_mission_item->loiter_direction = 1; - - break; - - /* if the last mission item was to loiter, continue unlimited */ - case NAV_CMD_LOITER_TURN_COUNT: - case NAV_CMD_LOITER_TIME_LIMIT: - - /* copy current mission to next item */ - memcpy(new_mission_item, existing_mission_item, sizeof(mission_item_s)); - /* and set it to loiter */ - new_mission_item->nav_cmd = NAV_CMD_LOITER_UNLIMITED; - - break; - /* if already in loiter, don't change anything */ - case NAV_CMD_LOITER_UNLIMITED: - break; - /* if landed, stay in land mode */ - case NAV_CMD_LAND: - break; - - default: - warnx("Unsupported nav_cmd"); - break; - } - } -} - -void -Navigator::update_mission_item_triplet() +Navigator::set_mode(navigation_mode_t new_nav_mode) { - if (!_mission_item_triplet.current_valid) { - - /* the current mission item is missing, add one */ - if (_mission_item_triplet.previous_valid) { - /* if we know the last one, proceed to succeeding one */ - add_mission_item(_mission_item_triplet.previous.index + 1, &_mission_item_triplet.previous, &_mission_item_triplet.current); - } - else { - /* if we don't remember the last one, start new */ - add_mission_item(0, nullptr, &_mission_item_triplet.current); - } - _mission_item_triplet.current_valid = true; + if (new_nav_mode == _mode) { + /* no change, return */ + return; } - if (_mission_item_triplet.current_valid && !_mission_item_triplet.next_valid) { - - if (_mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_UNLIMITED) { - /* if we are already loitering, don't bother about a next mission item */ + switch (new_nav_mode) { + case NAVIGATION_MODE_NONE: - _mission_item_triplet.next_valid = false; - } else { + // warnx("Set mode NONE"); + _mode = new_nav_mode; + break; - add_mission_item(_mission_item_triplet.current.index + 1, &_mission_item_triplet.current, &_mission_item_triplet.next); - _mission_item_triplet.next_valid = true; - } + case NAVIGATION_MODE_LOITER: + + /* decide depending on previous navigation mode */ + switch (_mode) { + case NAVIGATION_MODE_NONE: + + case NAVIGATION_MODE_WAYPOINT: + case NAVIGATION_MODE_RTL: + + /* use current position and loiter around it */ + mission_item_s global_position_mission_item; + global_position_mission_item.lat = (double)_global_pos.lat / 1e7; + global_position_mission_item.lon = (double)_global_pos.lon / 1e7; + global_position_mission_item.altitude = _global_pos.alt; + start_loiter(&global_position_mission_item); + _mode = new_nav_mode; + // warnx("start loiter here"); + break; + + case NAVIGATION_MODE_LOITER_WAYPOINT: + case NAVIGATION_MODE_LOITER_RTL: + /* already loitering, just continue */ + _mode = new_nav_mode; + // warnx("continue loiter"); + break; + + case NAVIGATION_MODE_LOITER: + default: + // warnx("previous navigation mode not supported"); + break; + } + break; + + case NAVIGATION_MODE_WAYPOINT: + + /* Start mission if there is one available */ + start_waypoint(); + // warnx("Set mode WAYPOINT"); + _mode = new_nav_mode; + break; + + case NAVIGATION_MODE_LOITER_WAYPOINT: + + start_loiter(&_mission_item_triplet.current); + // warnx("Set mode LOITER from current waypoint"); + _mode = new_nav_mode; + break; + + case NAVIGATION_MODE_RTL: + + /* decide depending on previous navigation mode */ + switch (_mode) { + case NAVIGATION_MODE_NONE: + case NAVIGATION_MODE_LOITER: + case NAVIGATION_MODE_WAYPOINT: + case NAVIGATION_MODE_LOITER_WAYPOINT: + + /* start RTL here */ + start_rtl(); + _mode = new_nav_mode; + // warnx("start RTL"); + break; + + case NAVIGATION_MODE_LOITER_RTL: + /* already loitering after RTL, just continue */ + /* TODO: get rid of this conversion */ + // warnx("stay in loiter after RTL"); + break; + + case NAVIGATION_MODE_RTL: + default: + warnx("previous navigation mode not supported"); + break; + } + break; + + case NAVIGATION_MODE_LOITER_RTL: + + /* TODO: get rid of this conversion */ + mission_item_s home_position_mission_item; + home_position_mission_item.lat = (double)_home_pos.lat / 1e7; + home_position_mission_item.lon = (double)_home_pos.lon / 1e7; + home_position_mission_item.altitude = _home_pos.alt / 1e3f + 50.0f; + start_loiter(&home_position_mission_item); + // warnx("Set mode LOITER from home position"); + _mode = new_nav_mode; + break; } +} +int +Navigator::set_waypoint_mission_item(unsigned mission_item_index, struct mission_item_s *new_mission_item) +{ + if (mission_item_index < _mission_item_count) { + memcpy(new_mission_item, &_mission_item[mission_item_index], sizeof(mission_item_s)); + + if (new_mission_item->nav_cmd == NAV_CMD_RETURN_TO_LAUNCH) { + /* if it is a RTL waypoint, append the home position */ + new_mission_item->lat = (double)_home_pos.lat / 1e7; + new_mission_item->lon = (double)_home_pos.lon / 1e7; + new_mission_item->altitude = (float)_home_pos.alt / 1e3f + 50.0f; // TODO: add parameter for RTL altitude + new_mission_item->loiter_radius = 100.0f; // TODO: get rid of magic number + new_mission_item->radius = 50.0f; // TODO: get rid of magic number + } + // warnx("added mission item: %d", mission_item_index); + return OK; + } + // warnx("could not add mission item: %d", mission_item_index); + return ERROR; +} + +void +Navigator::publish_mission_item_triplet() +{ /* lazily publish the mission triplet only once available */ if (_triplet_pub > 0) { /* publish the mission triplet */ @@ -805,12 +839,20 @@ Navigator::update_mission_item_triplet() } } -void +int Navigator::advance_current_mission_item() { + reset_mission_item_reached(); + + // warnx("advancing from %d to %d", _current_mission_index, _current_mission_index+1); + + /* ultimately this index will be == _mission_item_count and this flags the mission as completed */ + _current_mission_index++; + /* if there is no more mission available, don't advance and return */ if (!_mission_item_triplet.next_valid) { - return; + // warnx("no next available"); + return ERROR; } reset_mission_item_reached(); @@ -822,24 +864,18 @@ Navigator::advance_current_mission_item() /* copy the next to current */ memcpy(&_mission_item_triplet.current, &_mission_item_triplet.next, sizeof(mission_item_s)); _mission_item_triplet.current_valid = _mission_item_triplet.next_valid; - - /* flag the next mission as invalid */ - _mission_item_triplet.next_valid = false; - update_mission_item_triplet(); -} + + if(set_waypoint_mission_item(_current_mission_index + 1, &_mission_item_triplet.next) == OK) { + _mission_item_triplet.next_valid = true; + } + else { + _mission_item_triplet.next_valid = false; + } -void -Navigator::restart_mission() -{ - reset_mission_item_reached(); + publish_mission_item_triplet(); - /* forget about the all mission items */ - _mission_item_triplet.previous_valid = false; - _mission_item_triplet.current_valid = false; - _mission_item_triplet.next_valid = false; - - update_mission_item_triplet(); + return OK; } @@ -850,11 +886,18 @@ Navigator::reset_mission_item_reached() _waypoint_position_reached = false; _waypoint_yaw_reached = false; _time_first_inside_orbit = 0; + + _mission_item_reached = false; } -bool +void Navigator::check_mission_item_reached() { + // warnx("checking mission item reached"); + if (_mission_item_reached) { + return; + } + uint64_t now = hrt_absolute_time(); float orbit; @@ -929,33 +972,73 @@ Navigator::check_mission_item_reached() if ((now - _time_first_inside_orbit >= (uint64_t)_mission_item_triplet.current.time_inside * 1e6) || _mission_item_triplet.current.nav_cmd == (int)MAV_CMD_NAV_TAKEOFF) { - return true; + _mission_item_reached = true; } } - return false; } void -Navigator::start_loiter() +Navigator::start_waypoint() { - if (_mission_item_triplet.current_valid && _mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_UNLIMITED) { - /* already loitering, set again for latest lat/lon/alt */ - set_loiter_mission_item(&_mission_item_triplet.current); - - } else if (_mission_item_triplet.current_valid && _mission_item_triplet.current.nav_cmd != NAV_CMD_LOITER_UNLIMITED) { - /* move current waypoint back to next and switch to loiter now */ - memcpy(&_mission_item_triplet.next, &_mission_item_triplet.current, sizeof(mission_item_s)); - _mission_item_triplet.next_valid = _mission_item_triplet.current_valid; + reset_mission_item_reached(); + + /* this means we should start fresh */ + if (_current_mission_index == 0) { + + /* Reset the index to start with the first mission item */ + _current_mission_index = 0; + _mission_item_triplet.previous_valid = false; - set_loiter_mission_item(&_mission_item_triplet.current); } else { - /* if the current mission item is invalid, it will be added now */ - set_loiter_mission_item(&_mission_item_triplet.current); - _mission_item_triplet.current_valid = true; + set_waypoint_mission_item(_current_mission_index - 1, &_mission_item_triplet.previous); + _mission_item_triplet.previous_valid = true; + } + + set_waypoint_mission_item(_current_mission_index, &_mission_item_triplet.current); + _mission_item_triplet.current_valid = true; + + // if (_mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_UNLIMITED) { + + // if the current mission is to loiter unlimited, don't bother about a next mission item + // _mission_item_triplet.next_valid = false; + // } else { + /* if we are not loitering yet, try to add the next mission item */ + // set_waypoint_mission_item(_current_mission_index + 1, &_mission_item_triplet.next); + // _mission_item_triplet.next_valid = true; + // } + + if(set_waypoint_mission_item(_current_mission_index + 1, &_mission_item_triplet.next) == OK) { + _mission_item_triplet.next_valid = true; + } + else { + _mission_item_triplet.next_valid = false; } - update_mission_item_triplet(); + publish_mission_item_triplet(); +} + +void +Navigator::start_loiter(mission_item_s *new_loiter_position) +{ + //reset_mission_item_reached(); + + _mission_item_triplet.previous_valid = false; + _mission_item_triplet.current_valid = true; + _mission_item_triplet.next_valid = false; + + _mission_item_triplet.current.nav_cmd = NAV_CMD_LOITER_UNLIMITED; + _mission_item_triplet.current.loiter_direction = 1; + _mission_item_triplet.current.loiter_radius = 100.0f; // TODO: get rid of magic number + _mission_item_triplet.current.radius = 50.0f; // TODO: get rid of magic number + _mission_item_triplet.current.autocontinue = false; + + _mission_item_triplet.current.lat = new_loiter_position->lat; + _mission_item_triplet.current.lon = new_loiter_position->lon; + _mission_item_triplet.current.altitude = new_loiter_position->altitude; + _mission_item_triplet.current.yaw = new_loiter_position->yaw; + + publish_mission_item_triplet(); } void @@ -965,10 +1048,12 @@ Navigator::start_rtl() /* discard all mission item and insert RTL item */ _mission_item_triplet.previous_valid = false; + _mission_item_triplet.current_valid = true; + _mission_item_triplet.next_valid = false; _mission_item_triplet.current.lat = (double)_home_pos.lat / 1e7; _mission_item_triplet.current.lon = (double)_home_pos.lon / 1e7; - _mission_item_triplet.current.altitude = (float)_home_pos.alt / 1e3 + 50.0f; // TODO: add parameter for RTL altitude + _mission_item_triplet.current.altitude = (float)_home_pos.alt / 1e3f + 50.0f; // TODO: add parameter for RTL altitude _mission_item_triplet.current.yaw = 0.0f; _mission_item_triplet.current.nav_cmd = NAV_CMD_RETURN_TO_LAUNCH; _mission_item_triplet.current.loiter_direction = 1; @@ -977,18 +1062,7 @@ Navigator::start_rtl() _mission_item_triplet.current.autocontinue = false; _mission_item_triplet.current_valid = true; - _mission_item_triplet.next.lat = (double)_home_pos.lat / 1e7; - _mission_item_triplet.next.lon = (double)_home_pos.lon / 1e7; - _mission_item_triplet.next.altitude = (float)_home_pos.alt / 1e3 + 50.0f; // TODO: add parameter for RTL altitude - _mission_item_triplet.next.yaw = _global_pos.yaw; - _mission_item_triplet.next.nav_cmd = NAV_CMD_LOITER_UNLIMITED; - _mission_item_triplet.next.loiter_direction = 1; - _mission_item_triplet.next.loiter_radius = 100.0f; // TODO: get rid of magic number - _mission_item_triplet.next.radius = 50.0f; // TODO: get rid of magic number - _mission_item_triplet.next.autocontinue = false; - _mission_item_triplet.next_valid; - - update_mission_item_triplet(); + publish_mission_item_triplet(); } From 310de1df963143da40ff6551d5823d40df282af4 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 25 Nov 2013 23:19:27 +0400 Subject: [PATCH 158/447] pid lib update --- .../multirotor_attitude_control.c | 4 +- .../multirotor_rate_control.c | 6 +- .../multirotor_pos_control.c | 6 +- src/modules/systemlib/pid/pid.c | 115 +++++++----------- src/modules/systemlib/pid/pid.h | 41 +++---- 5 files changed, 71 insertions(+), 101 deletions(-) diff --git a/src/modules/multirotor_att_control/multirotor_attitude_control.c b/src/modules/multirotor_att_control/multirotor_attitude_control.c index 8245aa5609..b7df0433af 100644 --- a/src/modules/multirotor_att_control/multirotor_attitude_control.c +++ b/src/modules/multirotor_att_control/multirotor_attitude_control.c @@ -194,8 +194,8 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s parameters_init(&h); parameters_update(&h, &p); - pid_init(&pitch_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, PID_MODE_DERIVATIV_SET, 0.0f); - pid_init(&roll_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, PID_MODE_DERIVATIV_SET, 0.0f); + pid_init(&pitch_controller, PID_MODE_DERIVATIV_SET, 0.0f); + pid_init(&roll_controller, PID_MODE_DERIVATIV_SET, 0.0f); initialized = true; } diff --git a/src/modules/multirotor_att_control/multirotor_rate_control.c b/src/modules/multirotor_att_control/multirotor_rate_control.c index 86ac0e4ff0..eb41bf93e0 100644 --- a/src/modules/multirotor_att_control/multirotor_rate_control.c +++ b/src/modules/multirotor_att_control/multirotor_rate_control.c @@ -161,9 +161,9 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, parameters_update(&h, &p); initialized = true; - pid_init(&pitch_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f, PID_MODE_DERIVATIV_CALC_NO_SP, 0.003f); - pid_init(&roll_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f, PID_MODE_DERIVATIV_CALC_NO_SP, 0.003f); - pid_init(&yaw_rate_controller, p.yawrate_p, p.yawrate_i, p.yawrate_d, 1.0f, 1.0f, PID_MODE_DERIVATIV_CALC_NO_SP, 0.003f); + pid_init(&pitch_rate_controller, PID_MODE_DERIVATIV_CALC_NO_SP, 0.003f); + pid_init(&roll_rate_controller, PID_MODE_DERIVATIV_CALC_NO_SP, 0.003f); + pid_init(&yaw_rate_controller, PID_MODE_DERIVATIV_CALC_NO_SP, 0.003f); } /* load new parameters with lower rate */ diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c index 3d23d0c096..c0e1eb5232 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c @@ -246,11 +246,11 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) for (int i = 0; i < 2; i++) { - pid_init(&(xy_pos_pids[i]), params.xy_p, 0.0f, params.xy_d, 1.0f, 0.0f, PID_MODE_DERIVATIV_SET, 0.02f); - pid_init(&(xy_vel_pids[i]), params.xy_vel_p, params.xy_vel_i, params.xy_vel_d, 1.0f, params.tilt_max, PID_MODE_DERIVATIV_CALC_NO_SP, 0.02f); + pid_init(&(xy_pos_pids[i]), PID_MODE_DERIVATIV_SET, 0.02f); + pid_init(&(xy_vel_pids[i]), PID_MODE_DERIVATIV_CALC_NO_SP, 0.02f); } - pid_init(&z_pos_pid, params.z_p, 0.0f, params.z_d, 1.0f, params.z_vel_max, PID_MODE_DERIVATIV_SET, 0.02f); + pid_init(&z_pos_pid, PID_MODE_DERIVATIV_SET, 0.02f); thrust_pid_init(&z_vel_pid, params.z_vel_p, params.z_vel_i, params.z_vel_d, -params.thr_max, -params.thr_min, PID_MODE_DERIVATIV_CALC_NO_SP, 0.02f); while (!thread_should_exit) { diff --git a/src/modules/systemlib/pid/pid.c b/src/modules/systemlib/pid/pid.c index 77c952f52c..cab33282cc 100644 --- a/src/modules/systemlib/pid/pid.c +++ b/src/modules/systemlib/pid/pid.c @@ -39,7 +39,7 @@ /** * @file pid.c * - * Implementation of generic PID control interface. + * Implementation of generic PID controller. * * @author Laurens Mackay * @author Tobias Naegeli @@ -53,24 +53,21 @@ #define SIGMA 0.000001f -__EXPORT void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax, - float limit, uint8_t mode, float dt_min) +__EXPORT void pid_init(PID_t *pid, uint8_t mode, float dt_min) { - pid->kp = kp; - pid->ki = ki; - pid->kd = kd; - pid->intmax = intmax; - pid->limit = limit; pid->mode = mode; pid->dt_min = dt_min; - pid->count = 0.0f; - pid->saturated = 0.0f; - pid->last_output = 0.0f; - pid->sp = 0.0f; - pid->error_previous = 0.0f; + pid->kp = 0.0f; + pid->ki = 0.0f; + pid->kd = 0.0f; pid->integral = 0.0f; + pid->integral_limit = 0.0f; + pid->output_limit = 0.0f; + pid->error_previous = 0.0f; + pid->last_output = 0.0f; } -__EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float intmax, float limit) + +__EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float integral_limit, float output_limit) { int ret = 0; @@ -95,15 +92,15 @@ __EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float ret = 1; } - if (isfinite(intmax)) { - pid->intmax = intmax; + if (isfinite(integral_limit)) { + pid->integral_limit = integral_limit; } else { ret = 1; } - if (isfinite(limit)) { - pid->limit = limit; + if (isfinite(output_limit)) { + pid->output_limit = output_limit; } else { ret = 1; @@ -112,42 +109,18 @@ __EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float return ret; } -//void pid_set(PID_t *pid, float sp) -//{ -// pid->sp = sp; -// pid->error_previous = 0; -// pid->integral = 0; -//} - -/** - * - * @param pid - * @param val - * @param dt - * @return - */ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, float dt) { - /* error = setpoint - actual_position - integral = integral + (error*dt) - derivative = (error - previous_error)/dt - output = (Kp*error) + (Ki*integral) + (Kd*derivative) - previous_error = error - wait(dt) - goto start - */ - if (!isfinite(sp) || !isfinite(val) || !isfinite(val_dot) || !isfinite(dt)) { return pid->last_output; } float i, d; - pid->sp = sp; - // Calculated current error value - float error = pid->sp - val; + /* current error value */ + float error = sp - val; - // Calculate or measured current error derivative + /* current error derivative */ if (pid->mode == PID_MODE_DERIVATIV_CALC) { d = (error - pid->error_previous) / fmaxf(dt, pid->dt_min); pid->error_previous = error; @@ -167,39 +140,39 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo d = 0.0f; } - if (pid->ki > 0.0f) { - // Calculate the error integral and check for saturation - i = pid->integral + (error * dt); + /* calculate PD output */ + float output = (error * pid->kp) + (d * pid->kd); - if ((pid->limit > SIGMA && (fabsf((error * pid->kp) + (i * pid->ki) + (d * pid->kd)) > pid->limit)) || - fabsf(i) > pid->intmax) { - i = pid->integral; // If saturated then do not update integral value - pid->saturated = 1; + if (pid->ki > SIGMA) { + /* calculate error integral and check for saturation */ + float i = pid->integral + (error * dt); - } else { - if (!isfinite(i)) { - i = 0.0f; - } - - pid->integral = i; - pid->saturated = 0; + /* fail-safe */ + if (!isfinite(i)) { + i = 0.0f; } - } else { - i = 0.0f; - pid->saturated = 0; + if ((pid->output_limit > SIGMA && (fabsf(output + (i * pid->ki)) > pid->output_limit)) || + fabsf(i) > pid->integral_limit) { + /* saturated, do not update integral value */ + i = pid->integral; + + } else { + pid->integral = i; + } + + /* add I component to output */ + output += i * pid->ki; } - // Calculate the output. Limit output magnitude to pid->limit - float output = (error * pid->kp) + (i * pid->ki) + (d * pid->kd); - + /* limit output */ if (isfinite(output)) { - if (pid->limit > SIGMA) { - if (output > pid->limit) { - output = pid->limit; + if (pid->output_limit > SIGMA) { + if (output > pid->output_limit) { + output = pid->output_limit; - } else if (output < -pid->limit) { - output = -pid->limit; + } else if (output < -pid->output_limit) { + output = -pid->output_limit; } } @@ -212,5 +185,5 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo __EXPORT void pid_reset_integral(PID_t *pid) { - pid->integral = 0; + pid->integral = 0.0f; } diff --git a/src/modules/systemlib/pid/pid.h b/src/modules/systemlib/pid/pid.h index eca228464f..e8b1aac4fd 100644 --- a/src/modules/systemlib/pid/pid.h +++ b/src/modules/systemlib/pid/pid.h @@ -39,7 +39,7 @@ /** * @file pid.h * - * Definition of generic PID control interface. + * Definition of generic PID controller. * * @author Laurens Mackay * @author Tobias Naegeli @@ -55,38 +55,35 @@ __BEGIN_DECLS -/* PID_MODE_DERIVATIV_CALC calculates discrete derivative from previous error - * val_dot in pid_calculate() will be ignored */ -#define PID_MODE_DERIVATIV_CALC 0 -/* PID_MODE_DERIVATIV_CALC_NO_SP calculates discrete derivative from previous value, setpoint derivative is ignored - * val_dot in pid_calculate() will be ignored */ -#define PID_MODE_DERIVATIV_CALC_NO_SP 1 -/* Use PID_MODE_DERIVATIV_SET if you have the derivative already (Gyros, Kalman) */ -#define PID_MODE_DERIVATIV_SET 2 -// Use PID_MODE_DERIVATIV_NONE for a PI controller (vs PID) -#define PID_MODE_DERIVATIV_NONE 9 +typedef enum PID_MODE { + /* Use PID_MODE_DERIVATIV_NONE for a PI controller (vs PID) */ + PID_MODE_DERIVATIV_NONE = 0, + /* PID_MODE_DERIVATIV_CALC calculates discrete derivative from previous error, + * val_dot in pid_calculate() will be ignored */ + PID_MODE_DERIVATIV_CALC, + /* PID_MODE_DERIVATIV_CALC_NO_SP calculates discrete derivative from previous value, + * setpoint derivative will be ignored, val_dot in pid_calculate() will be ignored */ + PID_MODE_DERIVATIV_CALC_NO_SP, + /* Use PID_MODE_DERIVATIV_SET if you have the derivative already (Gyros, Kalman) */ + PID_MODE_DERIVATIV_SET +} pid_mode_t; typedef struct { + pid_mode_t mode; + float dt_min; float kp; float ki; float kd; - float intmax; - float sp; float integral; + float integral_limit; + float output_limit; float error_previous; float last_output; - float limit; - float dt_min; - uint8_t mode; - uint8_t count; - uint8_t saturated; } PID_t; -__EXPORT void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax, float limit, uint8_t mode, float dt_min); -__EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float intmax, float limit); -//void pid_set(PID_t *pid, float sp); +__EXPORT void pid_init(PID_t *pid, pid_mode_t mode, float dt_min); +__EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float integral_limit, float output_limit); __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, float dt); - __EXPORT void pid_reset_integral(PID_t *pid); __END_DECLS From 8fb22a1df7f8c783a764d85a2daf798b1d2039d7 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 25 Nov 2013 22:15:34 +0100 Subject: [PATCH 159/447] navigator: remove redudant line --- src/modules/navigator/navigator_main.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index a91c694e1c..b809d8fdb6 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -986,8 +986,6 @@ Navigator::start_waypoint() /* this means we should start fresh */ if (_current_mission_index == 0) { - /* Reset the index to start with the first mission item */ - _current_mission_index = 0; _mission_item_triplet.previous_valid = false; } else { From 5428eab23b1e188ce4ca2203e08820cde7d23990 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 26 Nov 2013 01:19:37 +0100 Subject: [PATCH 160/447] navigator: do not restart mission (continue with changed second part of mission) if no items prior to the current item have been changed --- src/modules/navigator/navigator_main.cpp | 52 ++++++++++++++++++++++-- 1 file changed, 49 insertions(+), 3 deletions(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index b809d8fdb6..e52331bcf6 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -221,6 +221,14 @@ private: void start_loiter(mission_item_s *new_loiter_position); void start_rtl(); + + /** + * Compare two mission items if they are equivalent + * Two mission items can be considered equivalent for the purpose of the navigator even if some fields differ. + * + * @return true if equivalent, false otherwise + */ + bool cmp_mission_item_equivalent(const struct mission_item_s a, const struct mission_item_s b); }; namespace navigator @@ -327,6 +335,27 @@ Navigator::mission_update() if (mission.count <= _max_mission_item_count) { + /* Check if first part of mission (up to _current_mission_index - 1) changed: + * if the first part changed: start again at first waypoint + * if the first part remained unchanged: continue with the (possibly changed second part) + */ + if (_current_mission_index < _mission_item_count && _current_mission_index < mission.count) { //check if not finished and if the new mission is not a shorter mission + for (int i = 0; i < (int)_current_mission_index; i++) { + if (!cmp_mission_item_equivalent(_mission_item[i], mission.items[i])) { + /* set flag to restart mission next we're in auto */ + _current_mission_index = 0; + //warnx("First part of mission differs i=%d", i); + break; + } +// else { +// warnx("Mission item is equivalent i=%d", i); +// } + } + } else { + /* set flag to restart mission next we're in auto */ + _current_mission_index = 0; + } + /* * Perform an atomic copy & state update */ @@ -344,9 +373,6 @@ Navigator::mission_update() _mission_item_count = 0; } - /* set flag to restart mission next we're in auto */ - _current_mission_index = 0; - if (_mode == NAVIGATION_MODE_WAYPOINT) { start_waypoint(); } @@ -1115,3 +1141,23 @@ int navigator_main(int argc, char *argv[]) return 0; } + +bool Navigator::cmp_mission_item_equivalent(const struct mission_item_s a, const struct mission_item_s b) { + if (a.altitude_is_relative == b.altitude_is_relative && + a.lat == b.lat && + a.lon == b.lon && + a.altitude == b.altitude && + a.yaw == b.yaw && + a.loiter_radius == b.loiter_radius && + a.loiter_direction == b.loiter_direction && + a.nav_cmd == b.nav_cmd && + a.radius == b.radius && + a.time_inside == b.time_inside && + a.autocontinue == b.autocontinue && + a.index == b.index) { + return true; + } else { + warnx("a.index %d, b.index %d", a.index, b.index); + return false; + } +} From d8e95de9bf3249290524ae21204e87e2b75a3bd9 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 26 Nov 2013 10:28:30 +0400 Subject: [PATCH 161/447] pid lib fixes --- src/modules/systemlib/pid/pid.c | 26 ++++++++------------------ 1 file changed, 8 insertions(+), 18 deletions(-) diff --git a/src/modules/systemlib/pid/pid.c b/src/modules/systemlib/pid/pid.c index cab33282cc..0bec724951 100644 --- a/src/modules/systemlib/pid/pid.c +++ b/src/modules/systemlib/pid/pid.c @@ -143,28 +143,18 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo /* calculate PD output */ float output = (error * pid->kp) + (d * pid->kd); - if (pid->ki > SIGMA) { - /* calculate error integral and check for saturation */ - float i = pid->integral + (error * dt); - - /* fail-safe */ - if (!isfinite(i)) { - i = 0.0f; - } - - if ((pid->output_limit > SIGMA && (fabsf(output + (i * pid->ki)) > pid->output_limit)) || - fabsf(i) > pid->integral_limit) { - /* saturated, do not update integral value */ - i = pid->integral; - - } else { + /* check for saturation */ + if (isfinite(i)) { + if ((pid->output_limit < SIGMA || (fabsf(output + (i * pid->ki)) <= pid->output_limit)) && + fabsf(i) <= pid->integral_limit) { + /* not saturated, use new integral value */ pid->integral = i; } - - /* add I component to output */ - output += i * pid->ki; } + /* add I component to output */ + output += pid->integral * pid-> ki; + /* limit output */ if (isfinite(output)) { if (pid->output_limit > SIGMA) { From 3a6be42265921de536f950e2fb5ea25b06945549 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 26 Nov 2013 10:33:10 +0400 Subject: [PATCH 162/447] multirotor_pos_control: cleanup and fixes --- .../multirotor_pos_control.c | 25 +++-- .../multirotor_pos_control/thrust_pid.c | 105 ++++++++---------- .../multirotor_pos_control/thrust_pid.h | 17 +-- 3 files changed, 62 insertions(+), 85 deletions(-) diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c index c0e1eb5232..ebee0b7a8b 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c @@ -251,7 +251,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) } pid_init(&z_pos_pid, PID_MODE_DERIVATIV_SET, 0.02f); - thrust_pid_init(&z_vel_pid, params.z_vel_p, params.z_vel_i, params.z_vel_d, -params.thr_max, -params.thr_min, PID_MODE_DERIVATIV_CALC_NO_SP, 0.02f); + thrust_pid_init(&z_vel_pid, 0.02f); while (!thread_should_exit) { @@ -265,18 +265,18 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) /* update params */ parameters_update(¶ms_h, ¶ms); + /* integral_limit * ki = tilt_max / 2 */ + float i_limit; + + if (params.xy_vel_i > 0.0f) { + i_limit = params.tilt_max / params.xy_vel_i / 2.0f; + + } else { + i_limit = 0.0f; // not used + } + for (int i = 0; i < 2; i++) { - pid_set_parameters(&(xy_pos_pids[i]), params.xy_p, 0.0f, params.xy_d, 1.0f, 0.0f); - /* use integral_limit_out = tilt_max / 2 */ - float i_limit; - - if (params.xy_vel_i > 0.0f) { - i_limit = params.tilt_max / params.xy_vel_i / 2.0f; - - } else { - i_limit = 0.0f; // not used - } - + pid_set_parameters(&(xy_pos_pids[i]), params.xy_p, 0.0f, params.xy_d, 0.0f, 0.0f); pid_set_parameters(&(xy_vel_pids[i]), params.xy_vel_p, params.xy_vel_i, params.xy_vel_d, i_limit, params.tilt_max); } @@ -471,6 +471,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) } else { local_pos_sp.z = local_pos.ref_alt - global_pos_sp.altitude; } + /* update yaw setpoint only if value is valid */ if (isfinite(global_pos_sp.yaw) && fabsf(global_pos_sp.yaw) < M_TWOPI) { att_sp.yaw_body = global_pos_sp.yaw; diff --git a/src/modules/multirotor_pos_control/thrust_pid.c b/src/modules/multirotor_pos_control/thrust_pid.c index b985630aec..39a7153229 100644 --- a/src/modules/multirotor_pos_control/thrust_pid.c +++ b/src/modules/multirotor_pos_control/thrust_pid.c @@ -43,22 +43,22 @@ #include "thrust_pid.h" #include -__EXPORT void thrust_pid_init(thrust_pid_t *pid, float kp, float ki, float kd, float limit_min, float limit_max, uint8_t mode, float dt_min) +#define COS_TILT_MAX 0.7f + +__EXPORT void thrust_pid_init(thrust_pid_t *pid, float dt_min) { - pid->kp = kp; - pid->ki = ki; - pid->kd = kd; - pid->limit_min = limit_min; - pid->limit_max = limit_max; - pid->mode = mode; pid->dt_min = dt_min; - pid->last_output = 0.0f; - pid->sp = 0.0f; - pid->error_previous = 0.0f; + pid->kp = 0.0f; + pid->ki = 0.0f; + pid->kd = 0.0f; pid->integral = 0.0f; + pid->output_min = 0.0f; + pid->output_max = 0.0f; + pid->error_previous = 0.0f; + pid->last_output = 0.0f; } -__EXPORT int thrust_pid_set_parameters(thrust_pid_t *pid, float kp, float ki, float kd, float limit_min, float limit_max) +__EXPORT int thrust_pid_set_parameters(thrust_pid_t *pid, float kp, float ki, float kd, float output_min, float output_max) { int ret = 0; @@ -83,15 +83,15 @@ __EXPORT int thrust_pid_set_parameters(thrust_pid_t *pid, float kp, float ki, fl ret = 1; } - if (isfinite(limit_min)) { - pid->limit_min = limit_min; + if (isfinite(output_min)) { + pid->output_min = output_min; } else { ret = 1; } - if (isfinite(limit_max)) { - pid->limit_max = limit_max; + if (isfinite(output_max)) { + pid->output_max = output_max; } else { ret = 1; @@ -102,40 +102,18 @@ __EXPORT int thrust_pid_set_parameters(thrust_pid_t *pid, float kp, float ki, fl __EXPORT float thrust_pid_calculate(thrust_pid_t *pid, float sp, float val, float dt, float r22) { - /* Alternative integral component calculation - * - * start: - * error = setpoint - current_value - * integral = integral + (Ki * error * dt) - * derivative = (error - previous_error) / dt - * previous_error = error - * output = (Kp * error) + integral + (Kd * derivative) - * wait(dt) - * goto start - */ - if (!isfinite(sp) || !isfinite(val) || !isfinite(dt)) { return pid->last_output; } float i, d; - pid->sp = sp; - // Calculated current error value - float error = pid->sp - val; + /* error value */ + float error = sp - val; - // Calculate or measured current error derivative - if (pid->mode == THRUST_PID_MODE_DERIVATIV_CALC) { - d = (error - pid->error_previous) / fmaxf(dt, pid->dt_min); - pid->error_previous = error; - - } else if (pid->mode == THRUST_PID_MODE_DERIVATIV_CALC_NO_SP) { - d = (-val - pid->error_previous) / fmaxf(dt, pid->dt_min); - pid->error_previous = -val; - - } else { - d = 0.0f; - } + /* error derivative */ + d = (-val - pid->error_previous) / fmaxf(dt, pid->dt_min); + pid->error_previous = -val; if (!isfinite(d)) { d = 0.0f; @@ -145,36 +123,41 @@ __EXPORT float thrust_pid_calculate(thrust_pid_t *pid, float sp, float val, floa i = pid->integral + (pid->ki * error * dt); /* attitude-thrust compensation - * r22 is (2, 2) componet of rotation matrix for current attitude */ + * r22 is (2, 2) component of rotation matrix for current attitude */ float att_comp; - if (r22 > 0.8f) + if (r22 > COS_TILT_MAX) { att_comp = 1.0f / r22; - else if (r22 > 0.0f) - att_comp = ((1.0f / 0.8f - 1.0f) / 0.8f) * r22 + 1.0f; - else - att_comp = 1.0f; - /* calculate output */ - float output = ((error * pid->kp) + i + (d * pid->kd)) * att_comp; - - /* check for saturation */ - if (output < pid->limit_min || output > pid->limit_max) { - /* saturated, recalculate output with old integral */ - output = (error * pid->kp) + pid->integral + (d * pid->kd); + } else if (r22 > 0.0f) { + att_comp = ((1.0f / COS_TILT_MAX - 1.0f) / COS_TILT_MAX) * r22 + 1.0f; } else { - if (isfinite(i)) { + att_comp = 1.0f; + } + + /* calculate PD output */ + float output = ((error * pid->kp) + (d * pid->kd)) * att_comp; + + /* check for saturation */ + if (isfinite(i)) { + float i_comp = i * att_comp; + if ((output + i_comp) >= pid->output_min || (output + i_comp) <= pid->output_max) { + /* not saturated, use new integral value */ pid->integral = i; } } - if (isfinite(output)) { - if (output > pid->limit_max) { - output = pid->limit_max; + /* add I component to output */ + output += pid->integral * att_comp; - } else if (output < pid->limit_min) { - output = pid->limit_min; + /* limit output */ + if (isfinite(output)) { + if (output > pid->output_max) { + output = pid->output_max; + + } else if (output < pid->output_min) { + output = pid->output_min; } pid->last_output = output; diff --git a/src/modules/multirotor_pos_control/thrust_pid.h b/src/modules/multirotor_pos_control/thrust_pid.h index 5e169c1ba5..71c3704d01 100644 --- a/src/modules/multirotor_pos_control/thrust_pid.h +++ b/src/modules/multirotor_pos_control/thrust_pid.h @@ -47,27 +47,20 @@ __BEGIN_DECLS -/* PID_MODE_DERIVATIV_CALC calculates discrete derivative from previous error */ -#define THRUST_PID_MODE_DERIVATIV_CALC 0 -/* PID_MODE_DERIVATIV_CALC_NO_SP calculates discrete derivative from previous value, setpoint derivative is ignored */ -#define THRUST_PID_MODE_DERIVATIV_CALC_NO_SP 1 - typedef struct { + float dt_min; float kp; float ki; float kd; - float sp; float integral; + float output_min; + float output_max; float error_previous; float last_output; - float limit_min; - float limit_max; - float dt_min; - uint8_t mode; } thrust_pid_t; -__EXPORT void thrust_pid_init(thrust_pid_t *pid, float kp, float ki, float kd, float limit_min, float limit_max, uint8_t mode, float dt_min); -__EXPORT int thrust_pid_set_parameters(thrust_pid_t *pid, float kp, float ki, float kd, float limit_min, float limit_max); +__EXPORT void thrust_pid_init(thrust_pid_t *pid, float dt_min); +__EXPORT int thrust_pid_set_parameters(thrust_pid_t *pid, float kp, float ki, float kd, float output_min, float output_max); __EXPORT float thrust_pid_calculate(thrust_pid_t *pid, float sp, float val, float dt, float r22); __EXPORT void thrust_pid_set_integral(thrust_pid_t *pid, float i); From a94ed67b3fefa5437d0322948190c02d69f82fea Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 26 Nov 2013 13:47:37 +0100 Subject: [PATCH 163/447] update the commander to only use local pos for landing detection when on rotary wing --- src/modules/commander/commander.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 905d5dee55..dfd4d2f733 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -871,7 +871,7 @@ int commander_thread_main(int argc, char *argv[]) check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.xy_valid, &(status.condition_local_position_valid), &status_changed); check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.z_valid, &(status.condition_local_altitude_valid), &status_changed); - if (status.condition_local_altitude_valid) { + if (status.is_rotary_wing && status.condition_local_altitude_valid) { if (status.condition_landed != local_position.landed) { status.condition_landed = local_position.landed; status_changed = true; @@ -1540,7 +1540,7 @@ check_navigation_state_machine(struct vehicle_status_s *status, struct vehicle_c if (status->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) { /* don't switch to other states until takeoff not completed */ // XXX: only respect the condition_landed when the local position is actually valid - if (status->condition_local_altitude_valid && (local_pos->z > -takeoff_alt || status->condition_landed)) { + if (status->is_rotary_wing && status->condition_local_altitude_valid && (local_pos->z > -takeoff_alt || status->condition_landed)) { return TRANSITION_NOT_CHANGED; } } @@ -1550,7 +1550,7 @@ check_navigation_state_machine(struct vehicle_status_s *status, struct vehicle_c status->navigation_state != NAVIGATION_STATE_AUTO_MISSION && status->navigation_state != NAVIGATION_STATE_AUTO_RTL) { /* possibly on ground, switch to TAKEOFF if needed */ - if (local_pos->z > -takeoff_alt || status->condition_landed) { + if (status->is_rotary_wing && status->condition_local_altitude_valid && (local_pos->z > -takeoff_alt || status->condition_landed)) { res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_TAKEOFF, control_mode); return res; } From 92f7fb2732a25bcdfe1afa358c3a5a2715d53a1b Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 26 Nov 2013 14:17:41 +0100 Subject: [PATCH 164/447] Navigator: Added some mavlink debug output --- src/modules/navigator/navigator_main.cpp | 30 ++++++++++++++++++------ 1 file changed, 23 insertions(+), 7 deletions(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index e52331bcf6..4a239f901b 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -69,6 +69,7 @@ #include #include #include +#include typedef enum { NAVIGATION_MODE_NONE, @@ -132,6 +133,8 @@ private: bool _task_should_exit; /**< if true, sensor task should exit */ int _navigator_task; /**< task handle for sensor task */ + int _mavlink_fd; + int _global_pos_sub; /**< global position subscription */ int _home_pos_sub; /**< home position subscription */ int _vstatus_sub; /**< vehicle status subscription */ @@ -248,6 +251,8 @@ Navigator::Navigator() : _task_should_exit(false), _navigator_task(-1), + _mavlink_fd(-1), + /* subscriptions */ _global_pos_sub(-1), _home_pos_sub(-1), @@ -344,6 +349,7 @@ Navigator::mission_update() if (!cmp_mission_item_equivalent(_mission_item[i], mission.items[i])) { /* set flag to restart mission next we're in auto */ _current_mission_index = 0; + mavlink_log_info(_mavlink_fd, "[navigator] Reset to WP %d", _current_mission_index); //warnx("First part of mission differs i=%d", i); break; } @@ -354,6 +360,7 @@ Navigator::mission_update() } else { /* set flag to restart mission next we're in auto */ _current_mission_index = 0; + mavlink_log_info(_mavlink_fd, "[navigator] Reset to WP %d", _current_mission_index); } /* @@ -370,6 +377,7 @@ Navigator::mission_update() } else { warnx("ERROR: too many waypoints, not supported"); + mavlink_log_critical(_mavlink_fd, "[navigator] too many waypoints, not supported"); _mission_item_count = 0; } @@ -550,6 +558,12 @@ Navigator::task_main() /* only run controller if position changed */ if (fds[1].revents & POLLIN) { + /* XXX Hack to get mavlink output going */ + if (_mavlink_fd < 0) { + /* try to open the mavlink log device every once in a while */ + _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); + } + /* load local copies */ orb_copy(ORB_ID(vehicle_global_position), _global_pos_sub, &_global_pos); @@ -566,7 +580,7 @@ Navigator::task_main() check_mission_item_reached(); if (_mission_item_reached) { - // warnx("Mission already reached"); + mavlink_log_info(_mavlink_fd, "[navigator] reached WP %d", _current_mission_index); if (advance_current_mission_item() != OK) { set_mode(NAVIGATION_MODE_LOITER_WAYPOINT); } @@ -578,6 +592,7 @@ Navigator::task_main() check_mission_item_reached(); if (_mission_item_reached) { + mavlink_log_info(_mavlink_fd, "[navigator] reached RTL position"); set_mode(NAVIGATION_MODE_LOITER_RTL); } break; @@ -756,7 +771,7 @@ Navigator::set_mode(navigation_mode_t new_nav_mode) global_position_mission_item.altitude = _global_pos.alt; start_loiter(&global_position_mission_item); _mode = new_nav_mode; - // warnx("start loiter here"); + mavlink_log_info(_mavlink_fd, "[navigator] loiter here"); break; case NAVIGATION_MODE_LOITER_WAYPOINT: @@ -777,15 +792,15 @@ Navigator::set_mode(navigation_mode_t new_nav_mode) /* Start mission if there is one available */ start_waypoint(); - // warnx("Set mode WAYPOINT"); _mode = new_nav_mode; + mavlink_log_info(_mavlink_fd, "[navigator] start waypoint mission"); break; case NAVIGATION_MODE_LOITER_WAYPOINT: start_loiter(&_mission_item_triplet.current); - // warnx("Set mode LOITER from current waypoint"); _mode = new_nav_mode; + mavlink_log_info(_mavlink_fd, "[navigator] loiter at WP %d", _current_mission_index-1); break; case NAVIGATION_MODE_RTL: @@ -800,12 +815,11 @@ Navigator::set_mode(navigation_mode_t new_nav_mode) /* start RTL here */ start_rtl(); _mode = new_nav_mode; - // warnx("start RTL"); + mavlink_log_info(_mavlink_fd, "[navigator] start RTL"); break; case NAVIGATION_MODE_LOITER_RTL: /* already loitering after RTL, just continue */ - /* TODO: get rid of this conversion */ // warnx("stay in loiter after RTL"); break; @@ -824,7 +838,7 @@ Navigator::set_mode(navigation_mode_t new_nav_mode) home_position_mission_item.lon = (double)_home_pos.lon / 1e7; home_position_mission_item.altitude = _home_pos.alt / 1e3f + 50.0f; start_loiter(&home_position_mission_item); - // warnx("Set mode LOITER from home position"); + mavlink_log_info(_mavlink_fd, "[navigator] loiter after RTL"); _mode = new_nav_mode; break; } @@ -1022,6 +1036,8 @@ Navigator::start_waypoint() set_waypoint_mission_item(_current_mission_index, &_mission_item_triplet.current); _mission_item_triplet.current_valid = true; + mavlink_log_info(_mavlink_fd, "[navigator] heading to WP %d", _current_mission_index); + // if (_mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_UNLIMITED) { // if the current mission is to loiter unlimited, don't bother about a next mission item From 9a79ad4cdb72bfa8a878eb522d17b51ff640b002 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 26 Nov 2013 15:08:43 +0100 Subject: [PATCH 165/447] Navigator: Use parameter for minium altitude when starting loiter --- src/modules/navigator/navigator_main.cpp | 39 +++++++++++++++--------- src/modules/navigator/navigator_params.c | 6 ++-- 2 files changed, 29 insertions(+), 16 deletions(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 4a239f901b..399985442b 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -172,13 +172,13 @@ private: unsigned _current_mission_index; struct { - float throttle_cruise; - } _parameters; /**< local copies of interesting parameters */ + float min_altitude; + } _parameters; /**< local copies of parameters */ struct { - param_t throttle_cruise; + param_t min_altitude; - } _parameter_handles; /**< handles for interesting parameters */ + } _parameter_handles; /**< handles for parameters */ /** @@ -281,7 +281,7 @@ Navigator::Navigator() : { _global_pos.valid = false; memset(&_fence, 0, sizeof(_fence)); - _parameter_handles.throttle_cruise = param_find("NAV_DUMMY"); + _parameter_handles.min_altitude = param_find("NAV_MIN_ALT"); _mission_item = (mission_item_s*)malloc(sizeof(mission_item_s) * _max_mission_item_count); @@ -325,7 +325,7 @@ int Navigator::parameters_update() { - //param_get(_parameter_handles.throttle_cruise, &(_parameters.throttle_cruise)); + param_get(_parameter_handles.min_altitude, &(_parameters.min_altitude)); return OK; } @@ -491,7 +491,7 @@ Navigator::task_main() case NAVIGATION_STATE_AUTO_READY: case NAVIGATION_STATE_AUTO_TAKEOFF: - /* TODO add this */ + /* TODO probably not needed since takeoff WPs will just be passed on */ //set_mode(NAVIGATION_MODE_TAKEOFF); break; @@ -762,18 +762,29 @@ Navigator::set_mode(navigation_mode_t new_nav_mode) case NAVIGATION_MODE_NONE: case NAVIGATION_MODE_WAYPOINT: - case NAVIGATION_MODE_RTL: + case NAVIGATION_MODE_RTL: { /* use current position and loiter around it */ mission_item_s global_position_mission_item; global_position_mission_item.lat = (double)_global_pos.lat / 1e7; global_position_mission_item.lon = (double)_global_pos.lon / 1e7; - global_position_mission_item.altitude = _global_pos.alt; + + /* XXX get rid of ugly conversion for home position altitude */ + float global_min_alt = _parameters.min_altitude + (float)_home_pos.alt/1e3f; + + /* Use current altitude if above min altitude set by parameter */ + if (_global_pos.alt < global_min_alt) { + global_position_mission_item.altitude = global_min_alt; + mavlink_log_info(_mavlink_fd, "[navigator] loiter %.1fm higher", global_min_alt - _global_pos.alt); + } else { + global_position_mission_item.altitude = _global_pos.alt; + mavlink_log_info(_mavlink_fd, "[navigator] loiter here"); + } start_loiter(&global_position_mission_item); _mode = new_nav_mode; - mavlink_log_info(_mavlink_fd, "[navigator] loiter here"); + break; - + } case NAVIGATION_MODE_LOITER_WAYPOINT: case NAVIGATION_MODE_LOITER_RTL: /* already loitering, just continue */ @@ -836,7 +847,7 @@ Navigator::set_mode(navigation_mode_t new_nav_mode) mission_item_s home_position_mission_item; home_position_mission_item.lat = (double)_home_pos.lat / 1e7; home_position_mission_item.lon = (double)_home_pos.lon / 1e7; - home_position_mission_item.altitude = _home_pos.alt / 1e3f + 50.0f; + home_position_mission_item.altitude = _home_pos.alt / 1e3f + _parameters.min_altitude; start_loiter(&home_position_mission_item); mavlink_log_info(_mavlink_fd, "[navigator] loiter after RTL"); _mode = new_nav_mode; @@ -854,7 +865,7 @@ Navigator::set_waypoint_mission_item(unsigned mission_item_index, struct mission /* if it is a RTL waypoint, append the home position */ new_mission_item->lat = (double)_home_pos.lat / 1e7; new_mission_item->lon = (double)_home_pos.lon / 1e7; - new_mission_item->altitude = (float)_home_pos.alt / 1e3f + 50.0f; // TODO: add parameter for RTL altitude + new_mission_item->altitude = (float)_home_pos.alt / 1e3f + _parameters.min_altitude; new_mission_item->loiter_radius = 100.0f; // TODO: get rid of magic number new_mission_item->radius = 50.0f; // TODO: get rid of magic number } @@ -1093,7 +1104,7 @@ Navigator::start_rtl() _mission_item_triplet.current.lat = (double)_home_pos.lat / 1e7; _mission_item_triplet.current.lon = (double)_home_pos.lon / 1e7; - _mission_item_triplet.current.altitude = (float)_home_pos.alt / 1e3f + 50.0f; // TODO: add parameter for RTL altitude + _mission_item_triplet.current.altitude = (float)_home_pos.alt / 1e3f + _parameters.min_altitude; _mission_item_triplet.current.yaw = 0.0f; _mission_item_triplet.current.nav_cmd = NAV_CMD_RETURN_TO_LAUNCH; _mission_item_triplet.current.loiter_direction = 1; diff --git a/src/modules/navigator/navigator_params.c b/src/modules/navigator/navigator_params.c index 06df9a4526..df542de5dd 100644 --- a/src/modules/navigator/navigator_params.c +++ b/src/modules/navigator/navigator_params.c @@ -1,7 +1,8 @@ /**************************************************************************** * * Copyright (c) 2013 PX4 Development Team. All rights reserved. - * Author: Lorenz Meier + * Author: @autho Lorenz Meier + * @author Julian Oes * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -38,6 +39,7 @@ * Parameters defined by the navigator task. * * @author Lorenz Meier + * @author Julian Oes */ #include @@ -49,5 +51,5 @@ * */ -PARAM_DEFINE_FLOAT(NAV_DUMMY, 0.0f); +PARAM_DEFINE_FLOAT(NAV_MIN_ALT, 50.0f); From 126b0567feb7384ce30997f01a5e9eb873e018aa Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 26 Nov 2013 15:24:16 +0100 Subject: [PATCH 166/447] add safety check for orbit in navigator. Prevents issues with old qgc versions --- src/modules/navigator/navigator_main.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 399985442b..936e0589cf 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -952,13 +952,14 @@ Navigator::check_mission_item_reached() uint64_t now = hrt_absolute_time(); float orbit; - if (_mission_item_triplet.current.nav_cmd == NAV_CMD_WAYPOINT) { + if (_mission_item_triplet.current.nav_cmd == NAV_CMD_WAYPOINT && _mission_item_triplet.current.radius > 0.01f) { orbit = _mission_item_triplet.current.radius; } else if (_mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_TURN_COUNT || _mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || - _mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_UNLIMITED) { + _mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_UNLIMITED && + _mission_item_triplet.current.loiter_radius > 0.01f) { orbit = _mission_item_triplet.current.loiter_radius; } else { From a989e796638256caf5acba403760673384a24d64 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 26 Nov 2013 15:25:27 +0100 Subject: [PATCH 167/447] Navigator: Don't try to go to next WP or loiter after landing, just stay in landing mode --- src/modules/navigator/navigator_main.cpp | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 399985442b..8bd786a7be 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -944,11 +944,16 @@ Navigator::reset_mission_item_reached() void Navigator::check_mission_item_reached() { - // warnx("checking mission item reached"); + /* don't check if mission item is already reached */ if (_mission_item_reached) { return; } + /* don't try to reach the landing mission, just stay in that mode */ + if (_mission_item_triplet.current.nav_cmd == MAV_CMD_NAV_LAND) { + return; + } + uint64_t now = hrt_absolute_time(); float orbit; From b66730b5a9011e349d25655f777dccf5803d90c8 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 26 Nov 2013 15:38:53 +0100 Subject: [PATCH 168/447] making sure the mavlink fd is open in fw pos ctrl --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 7da28cbfa0..d12a1750ab 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -1173,6 +1173,11 @@ FixedwingPositionControl::task_main() /* only run controller if position changed */ if (fds[1].revents & POLLIN) { + /* XXX Hack to get mavlink output going */ + if (mavlink_fd < 0) { + /* try to open the mavlink log device every once in a while */ + mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); + } static uint64_t last_run = 0; float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f; From 254777d38ae0ab30d7f8f75e49a3619aae592460 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 26 Nov 2013 15:49:59 +0100 Subject: [PATCH 169/447] more fixes for the mavlink_fd in fw pos ctrl, this now enables mavlink output for landing --- .../fw_pos_control_l1_main.cpp | 19 ++++++++----------- 1 file changed, 8 insertions(+), 11 deletions(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index d12a1750ab..d60983bce2 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -95,8 +95,6 @@ */ extern "C" __EXPORT int fw_pos_control_l1_main(int argc, char *argv[]); -static int mavlink_fd; - class FixedwingPositionControl { public: @@ -118,6 +116,7 @@ public: int start(); private: + int _mavlink_fd; bool _task_should_exit; /**< if true, sensor task should exit */ int _control_task; /**< task handle for sensor task */ @@ -377,11 +376,9 @@ FixedwingPositionControl::FixedwingPositionControl() : land_stayonground(false), land_motor_lim(false), land_onslope(false), - flare_curve_alt_last(0.0f) + flare_curve_alt_last(0.0f), + _mavlink_fd(-1) { - - mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); - /* safely initialize structs */ vehicle_attitude_s _att = {0}; vehicle_attitude_setpoint_s _att_sp = {0}; @@ -884,7 +881,7 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio throttle_max = math::min(throttle_max, _parameters.throttle_land_max); if (!land_motor_lim) { land_motor_lim = true; - mavlink_log_info(mavlink_fd, "[POSCTRL] Landing, limit throttle"); + mavlink_log_info(_mavlink_fd, "[POSCTRL] Landing, limit throttle"); } } @@ -905,7 +902,7 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio flare_angle_rad, math::radians(15.0f)); if (!land_noreturn_vertical) { - mavlink_log_info(mavlink_fd, "[POSCTRL] Landing, flare"); + mavlink_log_info(_mavlink_fd, "[POSCTRL] Landing, flare"); land_noreturn_vertical = true; } //warnx("Landing: flare, _global_pos.alt %.1f, flare_curve_alt %.1f, flare_curve_alt_last %.1f, flare_length %.1f, wp_distance %.1f", _global_pos.alt, flare_curve_alt, flare_curve_alt_last, flare_length, wp_distance); @@ -924,7 +921,7 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio if (!land_onslope) { - mavlink_log_info(mavlink_fd, "[POSCTRL] Landing, on slope"); + mavlink_log_info(_mavlink_fd, "[POSCTRL] Landing, on slope"); land_onslope = true; } @@ -1174,9 +1171,9 @@ FixedwingPositionControl::task_main() if (fds[1].revents & POLLIN) { /* XXX Hack to get mavlink output going */ - if (mavlink_fd < 0) { + if (_mavlink_fd < 0) { /* try to open the mavlink log device every once in a while */ - mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); + _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); } static uint64_t last_run = 0; From 8e1af68bfd5db6a97289f4406beed7854b56d5bc Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 26 Nov 2013 15:54:19 +0100 Subject: [PATCH 170/447] Navigator: Added parameter for loiter radius --- src/modules/navigator/navigator_main.cpp | 12 +++++++++--- src/modules/navigator/navigator_params.c | 2 +- 2 files changed, 10 insertions(+), 4 deletions(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 8bd786a7be..45bb832ea4 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -173,10 +173,12 @@ private: struct { float min_altitude; + float loiter_radius; } _parameters; /**< local copies of parameters */ struct { param_t min_altitude; + param_t loiter_radius; } _parameter_handles; /**< handles for parameters */ @@ -282,6 +284,7 @@ Navigator::Navigator() : _global_pos.valid = false; memset(&_fence, 0, sizeof(_fence)); _parameter_handles.min_altitude = param_find("NAV_MIN_ALT"); + _parameter_handles.loiter_radius = param_find("NAV_LOITER_RAD"); _mission_item = (mission_item_s*)malloc(sizeof(mission_item_s) * _max_mission_item_count); @@ -326,6 +329,7 @@ Navigator::parameters_update() { param_get(_parameter_handles.min_altitude, &(_parameters.min_altitude)); + param_get(_parameter_handles.loiter_radius, &(_parameters.loiter_radius)); return OK; } @@ -540,6 +544,8 @@ Navigator::task_main() /* update parameters from storage */ parameters_update(); + + /* note that these new parameters won't be in effect until a mission triplet is published again */ } /* only update craft capabilities if they have changed */ @@ -866,7 +872,7 @@ Navigator::set_waypoint_mission_item(unsigned mission_item_index, struct mission new_mission_item->lat = (double)_home_pos.lat / 1e7; new_mission_item->lon = (double)_home_pos.lon / 1e7; new_mission_item->altitude = (float)_home_pos.alt / 1e3f + _parameters.min_altitude; - new_mission_item->loiter_radius = 100.0f; // TODO: get rid of magic number + new_mission_item->loiter_radius = _parameters.loiter_radius; // TODO: get rid of magic number new_mission_item->radius = 50.0f; // TODO: get rid of magic number } // warnx("added mission item: %d", mission_item_index); @@ -1085,7 +1091,7 @@ Navigator::start_loiter(mission_item_s *new_loiter_position) _mission_item_triplet.current.nav_cmd = NAV_CMD_LOITER_UNLIMITED; _mission_item_triplet.current.loiter_direction = 1; - _mission_item_triplet.current.loiter_radius = 100.0f; // TODO: get rid of magic number + _mission_item_triplet.current.loiter_radius = _parameters.loiter_radius; // TODO: get rid of magic number _mission_item_triplet.current.radius = 50.0f; // TODO: get rid of magic number _mission_item_triplet.current.autocontinue = false; @@ -1113,7 +1119,7 @@ Navigator::start_rtl() _mission_item_triplet.current.yaw = 0.0f; _mission_item_triplet.current.nav_cmd = NAV_CMD_RETURN_TO_LAUNCH; _mission_item_triplet.current.loiter_direction = 1; - _mission_item_triplet.current.loiter_radius = 100.0f; // TODO: get rid of magic number + _mission_item_triplet.current.loiter_radius = _parameters.loiter_radius; // TODO: get rid of magic number _mission_item_triplet.current.radius = 50.0f; // TODO: get rid of magic number _mission_item_triplet.current.autocontinue = false; _mission_item_triplet.current_valid = true; diff --git a/src/modules/navigator/navigator_params.c b/src/modules/navigator/navigator_params.c index df542de5dd..817e2f0093 100644 --- a/src/modules/navigator/navigator_params.c +++ b/src/modules/navigator/navigator_params.c @@ -52,4 +52,4 @@ */ PARAM_DEFINE_FLOAT(NAV_MIN_ALT, 50.0f); - +PARAM_DEFINE_FLOAT(NAV_LOITER_RAD, 100.0f); \ No newline at end of file From 9c1a5be8e16d18612c8e318355fef15e53961da7 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 26 Nov 2013 16:43:51 +0100 Subject: [PATCH 171/447] Navigator: Gotten rid of some warnings --- src/modules/navigator/navigator_main.cpp | 38 ++++++++++++------------ 1 file changed, 19 insertions(+), 19 deletions(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index b85c4bef99..2e88448015 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -349,7 +349,7 @@ Navigator::mission_update() * if the first part remained unchanged: continue with the (possibly changed second part) */ if (_current_mission_index < _mission_item_count && _current_mission_index < mission.count) { //check if not finished and if the new mission is not a shorter mission - for (int i = 0; i < (int)_current_mission_index; i++) { + for (unsigned i = 0; i < _current_mission_index; i++) { if (!cmp_mission_item_equivalent(_mission_item[i], mission.items[i])) { /* set flag to restart mission next we're in auto */ _current_mission_index = 0; @@ -781,7 +781,7 @@ Navigator::set_mode(navigation_mode_t new_nav_mode) /* Use current altitude if above min altitude set by parameter */ if (_global_pos.alt < global_min_alt) { global_position_mission_item.altitude = global_min_alt; - mavlink_log_info(_mavlink_fd, "[navigator] loiter %.1fm higher", global_min_alt - _global_pos.alt); + mavlink_log_info(_mavlink_fd, "[navigator] loiter %.1fm higher", (double)(global_min_alt - _global_pos.alt)); } else { global_position_mission_item.altitude = _global_pos.alt; mavlink_log_info(_mavlink_fd, "[navigator] loiter here"); @@ -956,7 +956,7 @@ Navigator::check_mission_item_reached() } /* don't try to reach the landing mission, just stay in that mode */ - if (_mission_item_triplet.current.nav_cmd == MAV_CMD_NAV_LAND) { + if (_mission_item_triplet.current.nav_cmd == NAV_CMD_LAND) { return; } @@ -967,9 +967,9 @@ Navigator::check_mission_item_reached() orbit = _mission_item_triplet.current.radius; - } else if (_mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_TURN_COUNT || + } else if ((_mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_TURN_COUNT || _mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || - _mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_UNLIMITED && + _mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_UNLIMITED) && _mission_item_triplet.current.loiter_radius > 0.01f) { orbit = _mission_item_triplet.current.loiter_radius; @@ -1030,10 +1030,10 @@ Navigator::check_mission_item_reached() /* XXX announcment? */ _time_first_inside_orbit = now; } - + /* check if the MAV was long enough inside the waypoint orbit */ if ((now - _time_first_inside_orbit >= (uint64_t)_mission_item_triplet.current.time_inside * 1e6) - || _mission_item_triplet.current.nav_cmd == (int)MAV_CMD_NAV_TAKEOFF) { + || _mission_item_triplet.current.nav_cmd == NAV_CMD_TAKEOFF) { _mission_item_reached = true; } @@ -1182,18 +1182,18 @@ int navigator_main(int argc, char *argv[]) } bool Navigator::cmp_mission_item_equivalent(const struct mission_item_s a, const struct mission_item_s b) { - if (a.altitude_is_relative == b.altitude_is_relative && - a.lat == b.lat && - a.lon == b.lon && - a.altitude == b.altitude && - a.yaw == b.yaw && - a.loiter_radius == b.loiter_radius && - a.loiter_direction == b.loiter_direction && - a.nav_cmd == b.nav_cmd && - a.radius == b.radius && - a.time_inside == b.time_inside && - a.autocontinue == b.autocontinue && - a.index == b.index) { + if (fabsf(a.altitude_is_relative - b.altitude_is_relative) < FLT_EPSILON && + fabsf(a.lat - b.lat) < FLT_EPSILON && + fabsf(a.lon - b.lon) < FLT_EPSILON && + fabsf(a.altitude - b.altitude) < FLT_EPSILON && + fabsf(a.yaw - b.yaw) < FLT_EPSILON && + fabsf(a.loiter_radius - b.loiter_radius) < FLT_EPSILON && + fabsf(a.loiter_direction - b.loiter_direction) < FLT_EPSILON && + fabsf(a.nav_cmd - b.nav_cmd) < FLT_EPSILON && + fabsf(a.radius - b.radius) < FLT_EPSILON && + fabsf(a.time_inside - b.time_inside) < FLT_EPSILON && + fabsf(a.autocontinue - b.autocontinue) < FLT_EPSILON && + fabsf(a.index - b.index) < FLT_EPSILON) { return true; } else { warnx("a.index %d, b.index %d", a.index, b.index); From b2ee78c21806f017da87e3d1322815149b7770b4 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 26 Nov 2013 17:02:52 +0100 Subject: [PATCH 172/447] fw_pos_ctrl: landing: audio output --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index d60983bce2..cdd41d16a2 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -881,7 +881,7 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio throttle_max = math::min(throttle_max, _parameters.throttle_land_max); if (!land_motor_lim) { land_motor_lim = true; - mavlink_log_info(_mavlink_fd, "[POSCTRL] Landing, limit throttle"); + mavlink_log_info(_mavlink_fd, "#audio: Landing, limiting throttle"); } } @@ -902,7 +902,7 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio flare_angle_rad, math::radians(15.0f)); if (!land_noreturn_vertical) { - mavlink_log_info(_mavlink_fd, "[POSCTRL] Landing, flare"); + mavlink_log_info(_mavlink_fd, "#audio: Landing, flaring"); land_noreturn_vertical = true; } //warnx("Landing: flare, _global_pos.alt %.1f, flare_curve_alt %.1f, flare_curve_alt_last %.1f, flare_length %.1f, wp_distance %.1f", _global_pos.alt, flare_curve_alt, flare_curve_alt_last, flare_length, wp_distance); @@ -921,7 +921,7 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio if (!land_onslope) { - mavlink_log_info(_mavlink_fd, "[POSCTRL] Landing, on slope"); + mavlink_log_info(_mavlink_fd, "#audio: Landing, on slope"); land_onslope = true; } From 3f252987988738d9246eec9716b780d23cb8b0f7 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 27 Nov 2013 09:27:08 +0100 Subject: [PATCH 173/447] Mavlink and navigator: Disable some functions in mavlink that are taken over by navigator, introduce topic to report mission status from commander back to mavlink --- src/lib/geo/geo.c | 15 + src/lib/geo/geo.h | 10 + src/modules/mavlink/mavlink.c | 17 + src/modules/mavlink/waypoints.c | 472 ++++++++++------------- src/modules/mavlink/waypoints.h | 25 +- src/modules/navigator/navigator_main.cpp | 39 ++ src/modules/uORB/objects_common.cpp | 3 + src/modules/uORB/topics/mission_result.h | 67 ++++ 8 files changed, 372 insertions(+), 276 deletions(-) create mode 100644 src/modules/uORB/topics/mission_result.h diff --git a/src/lib/geo/geo.c b/src/lib/geo/geo.c index 614f001865..85b17f9ae7 100644 --- a/src/lib/geo/geo.c +++ b/src/lib/geo/geo.c @@ -411,6 +411,21 @@ __EXPORT float get_distance_to_point_global_wgs84(double lat_now, double lon_now return sqrtf(dxy * dxy + dz * dz); } + +__EXPORT float mavlink_wpm_distance_to_point_local(float x_now, float y_now, float z_now, + float x_next, float y_next, float z_next, + float *dist_xy, float *dist_z) +{ + float dx = x_now - x_next; + float dy = y_now - y_next; + float dz = z_now - z_next; + + *dist_xy = sqrtf(dx * dx + dy * dy); + *dist_z = fabsf(dz); + + return sqrtf(dx * dx + dy * dy + dz * dz); +} + __EXPORT float _wrap_pi(float bearing) { /* value is inf or NaN */ diff --git a/src/lib/geo/geo.h b/src/lib/geo/geo.h index 47f643593c..5f92e14cf3 100644 --- a/src/lib/geo/geo.h +++ b/src/lib/geo/geo.h @@ -124,10 +124,20 @@ __EXPORT int get_distance_to_line(struct crosstrack_error_s * crosstrack_error, __EXPORT int get_distance_to_arc(struct crosstrack_error_s * crosstrack_error, double lat_now, double lon_now, double lat_center, double lon_center, float radius, float arc_start_bearing, float arc_sweep); +/* + * Calculate distance in global frame + */ __EXPORT float get_distance_to_point_global_wgs84(double lat_now, double lon_now, float alt_now, double lat_next, double lon_next, float alt_next, float *dist_xy, float *dist_z); +/* + * Calculate distance in local frame (NED) + */ +__EXPORT float mavlink_wpm_distance_to_point_local(float x_now, float y_now, float z_now, + float x_next, float y_next, float z_next, + float *dist_xy, float *dist_z); + __EXPORT float _wrap_180(float bearing); __EXPORT float _wrap_360(float bearing); __EXPORT float _wrap_pi(float bearing); diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c index 7c10e297b9..2ec00a9bc6 100644 --- a/src/modules/mavlink/mavlink.c +++ b/src/modules/mavlink/mavlink.c @@ -74,6 +74,8 @@ #include "waypoints.h" #include "mavlink_parameters.h" +#include + /* define MAVLink specific parameters */ PARAM_DEFINE_INT32(MAV_SYS_ID, 1); PARAM_DEFINE_INT32(MAV_COMP_ID, 50); @@ -644,6 +646,10 @@ int mavlink_thread_main(int argc, char *argv[]) set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 10000); } + int mission_result_sub = orb_subscribe(ORB_ID(mission_result)); + struct mission_result_s mission_result; + memset(&mission_result, 0, sizeof(mission_result)); + thread_running = true; /* arm counter to go off immediately */ @@ -690,6 +696,17 @@ int mavlink_thread_main(int argc, char *argv[]) lowspeed_counter++; + bool updated; + orb_check(mission_result_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(mission_result), mission_result_sub, &mission_result); + + if (mission_result.mission_reached) { + mavlink_wpm_send_waypoint_reached((uint16_t)mission_result.mission_index); + } + } + mavlink_waypoint_eventloop(mavlink_missionlib_get_system_timestamp(), &global_pos, &local_pos, &nav_cap); /* sleep quarter the time */ diff --git a/src/modules/mavlink/waypoints.c b/src/modules/mavlink/waypoints.c index 8e4bbce36f..95772f5a0c 100644 --- a/src/modules/mavlink/waypoints.c +++ b/src/modules/mavlink/waypoints.c @@ -111,15 +111,15 @@ void mavlink_wpm_init(mavlink_wpm_storage *state) state->current_partner_sysid = 0; state->current_partner_compid = 0; state->timestamp_lastaction = 0; - state->timestamp_last_send_setpoint = 0; + // state->timestamp_last_send_setpoint = 0; state->timeout = MAVLINK_WPM_PROTOCOL_TIMEOUT_DEFAULT; - state->delay_setpoint = MAVLINK_WPM_SETPOINT_DELAY_DEFAULT; - state->idle = false; ///< indicates if the system is following the waypoints or is waiting - state->current_active_wp_id = -1; ///< id of current waypoint - state->yaw_reached = false; ///< boolean for yaw attitude reached - state->pos_reached = false; ///< boolean for position reached - state->timestamp_lastoutside_orbit = 0;///< timestamp when the MAV was last outside the orbit or had the wrong yaw value - state->timestamp_firstinside_orbit = 0;///< timestamp when the MAV was the first time after a waypoint change inside the orbit and had the correct yaw value + // state->delay_setpoint = MAVLINK_WPM_SETPOINT_DELAY_DEFAULT; + // state->idle = false; ///< indicates if the system is following the waypoints or is waiting + // state->current_active_wp_id = -1; ///< id of current waypoint + // state->yaw_reached = false; ///< boolean for yaw attitude reached + // state->pos_reached = false; ///< boolean for position reached + // state->timestamp_lastoutside_orbit = 0;///< timestamp when the MAV was last outside the orbit or had the wrong yaw value + // state->timestamp_firstinside_orbit = 0;///< timestamp when the MAV was the first time after a waypoint change inside the orbit and had the correct yaw value mission.count = 0; mission.items = (struct mission_item_s*)malloc(sizeof(struct mission_item_s) * NUM_MISSIONS_SUPPORTED); @@ -155,7 +155,6 @@ void mavlink_wpm_send_waypoint_ack(uint8_t sysid, uint8_t compid, uint8_t type) if (MAVLINK_WPM_VERBOSE) printf("Sent waypoint ack (%u) to ID %u\n", wpa.type, wpa.target_system); #endif - mavlink_missionlib_send_gcs_string("Sent waypoint ACK"); } } @@ -206,7 +205,7 @@ void mavlink_wpm_send_setpoint(uint16_t seq) cur->param2, cur->param3, cur->param4, cur->x, cur->y, cur->z, cur->frame, cur->command); - wpm->timestamp_last_send_setpoint = mavlink_missionlib_get_system_timestamp(); + // wpm->timestamp_last_send_setpoint = mavlink_missionlib_get_system_timestamp(); } else { if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_missionlib_send_gcs_string("ERROR: Waypoint index out of bounds\n"); //// if (verbose) // printf("ERROR: index out of bounds\n"); @@ -220,7 +219,7 @@ void mavlink_wpm_send_waypoint_count(uint8_t sysid, uint8_t compid, uint16_t cou wpc.target_system = wpm->current_partner_sysid; wpc.target_component = wpm->current_partner_compid; - wpc.count = count; + wpc.count = mission.count; mavlink_msg_mission_count_encode(mavlink_system.sysid, mavlink_wpm_comp_id, &msg, &wpc); mavlink_missionlib_send_message(&msg); @@ -292,250 +291,191 @@ void mavlink_wpm_send_waypoint_reached(uint16_t seq) // FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY")); } -/* - * Calculate distance in global frame. - * - * The distance calculation is based on the WGS84 geoid (GPS) - */ -float mavlink_wpm_distance_to_point_global_wgs84(uint16_t seq, float lat, float lon, float alt, float *dist_xy, float *dist_z) -{ - if (seq < wpm->size) { - mavlink_mission_item_t *wp = &(wpm->waypoints[seq]); +// void check_waypoints_reached(uint64_t now, const struct vehicle_global_position_s *global_pos, struct vehicle_local_position_s *local_pos, float turn_distance) +// { +// static uint16_t counter; - double current_x_rad = wp->x / 180.0 * M_PI; - double current_y_rad = wp->y / 180.0 * M_PI; - double x_rad = lat / 180.0 * M_PI; - double y_rad = lon / 180.0 * M_PI; +// if ((!global_pos->valid && !local_pos->xy_valid) || +// /* no waypoint */ +// wpm->size == 0) { +// /* nothing to check here, return */ +// return; +// } - double d_lat = x_rad - current_x_rad; - double d_lon = y_rad - current_y_rad; +// if (wpm->current_active_wp_id < wpm->size) { - double a = sin(d_lat / 2.0) * sin(d_lat / 2.0) + sin(d_lon / 2.0) * sin(d_lon / 2.0f) * cos(current_x_rad) * cos(x_rad); - double c = 2 * atan2(sqrt(a), sqrt(1 - a)); +// float orbit; +// if (wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_WAYPOINT) { - const double radius_earth = 6371000.0; +// orbit = wpm->waypoints[wpm->current_active_wp_id].param2; - float dxy = radius_earth * c; - float dz = alt - wp->z; +// } else if (wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_TURNS || +// wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_TIME || +// wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_UNLIM) { - *dist_xy = fabsf(dxy); - *dist_z = fabsf(dz); +// orbit = wpm->waypoints[wpm->current_active_wp_id].param3; +// } else { - return sqrtf(dxy * dxy + dz * dz); +// // XXX set default orbit via param +// orbit = 15.0f; +// } - } else { - return -1.0f; - } +// /* keep vertical orbit */ +// float vertical_switch_distance = orbit; -} +// /* Take the larger turn distance - orbit or turn_distance */ +// if (orbit < turn_distance) +// orbit = turn_distance; -/* - * Calculate distance in local frame (NED) - */ -float mavlink_wpm_distance_to_point_local(uint16_t seq, float x, float y, float z, float *dist_xy, float *dist_z) -{ - if (seq < wpm->size) { - mavlink_mission_item_t *cur = &(wpm->waypoints[seq]); +// int coordinate_frame = wpm->waypoints[wpm->current_active_wp_id].frame; +// float dist = -1.0f; - float dx = (cur->x - x); - float dy = (cur->y - y); - float dz = (cur->z - z); +// float dist_xy = -1.0f; +// float dist_z = -1.0f; - *dist_xy = sqrtf(dx * dx + dy * dy); - *dist_z = fabsf(dz); +// if (coordinate_frame == (int)MAV_FRAME_GLOBAL) { +// dist = mavlink_wpm_distance_to_point_global_wgs84(wpm->current_active_wp_id, (float)global_pos->lat * 1e-7f, (float)global_pos->lon * 1e-7f, global_pos->alt, &dist_xy, &dist_z); - return sqrtf(dx * dx + dy * dy + dz * dz); +// } else if (coordinate_frame == (int)MAV_FRAME_GLOBAL_RELATIVE_ALT) { +// dist = mavlink_wpm_distance_to_point_global_wgs84(wpm->current_active_wp_id, (float)global_pos->lat * 1e-7f, (float)global_pos->lon * 1e-7f, global_pos->relative_alt, &dist_xy, &dist_z); - } else { - return -1.0f; - } -} +// } else if (coordinate_frame == (int)MAV_FRAME_LOCAL_ENU || coordinate_frame == (int)MAV_FRAME_LOCAL_NED) { +// dist = mavlink_wpm_distance_to_point_local(wpm->current_active_wp_id, local_pos->x, local_pos->y, local_pos->z, &dist_xy, &dist_z); -void check_waypoints_reached(uint64_t now, const struct vehicle_global_position_s *global_pos, struct vehicle_local_position_s *local_pos, float turn_distance) -{ - static uint16_t counter; +// } else if (coordinate_frame == (int)MAV_FRAME_MISSION) { +// /* Check if conditions of mission item are satisfied */ +// // XXX TODO +// } - if ((!global_pos->valid && !local_pos->xy_valid) || - /* no waypoint */ - wpm->size == 0) { - /* nothing to check here, return */ - return; - } +// if (dist >= 0.f && dist_xy <= orbit && dist_z >= 0.0f && dist_z <= vertical_switch_distance) { +// wpm->pos_reached = true; +// } - if (wpm->current_active_wp_id < wpm->size) { +// // check if required yaw reached +// float yaw_sp = _wrap_pi(wpm->waypoints[wpm->current_active_wp_id].param4 / 180.0f * FM_PI); +// float yaw_err = _wrap_pi(yaw_sp - local_pos->yaw); +// if (fabsf(yaw_err) < 0.05f) { +// wpm->yaw_reached = true; +// } +// } - float orbit; - if (wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_WAYPOINT) { +// //check if the current waypoint was reached +// if (wpm->pos_reached && /*wpm->yaw_reached &&*/ !wpm->idle) { +// if (wpm->current_active_wp_id < wpm->size) { +// mavlink_mission_item_t *cur_wp = &(wpm->waypoints[wpm->current_active_wp_id]); - orbit = wpm->waypoints[wpm->current_active_wp_id].param2; +// if (wpm->timestamp_firstinside_orbit == 0) { +// // Announce that last waypoint was reached +// mavlink_wpm_send_waypoint_reached(cur_wp->seq); +// wpm->timestamp_firstinside_orbit = now; +// } - } else if (wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_TURNS || - wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_TIME || - wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_UNLIM) { +// // check if the MAV was long enough inside the waypoint orbit +// //if (now-timestamp_lastoutside_orbit > (cur_wp->hold_time*1000)) - orbit = wpm->waypoints[wpm->current_active_wp_id].param3; - } else { +// bool time_elapsed = false; - // XXX set default orbit via param - orbit = 15.0f; - } +// if (now - wpm->timestamp_firstinside_orbit >= cur_wp->param1 * 1000 * 1000) { +// time_elapsed = true; +// } else if (cur_wp->command == (int)MAV_CMD_NAV_TAKEOFF) { +// time_elapsed = true; +// } - /* keep vertical orbit */ - float vertical_switch_distance = orbit; +// if (time_elapsed) { - /* Take the larger turn distance - orbit or turn_distance */ - if (orbit < turn_distance) - orbit = turn_distance; +// /* safeguard against invalid missions with last wp autocontinue on */ +// if (wpm->current_active_wp_id == wpm->size - 1) { +// /* stop handling missions here */ +// cur_wp->autocontinue = false; +// } - int coordinate_frame = wpm->waypoints[wpm->current_active_wp_id].frame; - float dist = -1.0f; +// if (cur_wp->autocontinue) { - float dist_xy = -1.0f; - float dist_z = -1.0f; +// cur_wp->current = 0; - if (coordinate_frame == (int)MAV_FRAME_GLOBAL) { - dist = mavlink_wpm_distance_to_point_global_wgs84(wpm->current_active_wp_id, (float)global_pos->lat * 1e-7f, (float)global_pos->lon * 1e-7f, global_pos->alt, &dist_xy, &dist_z); +// float navigation_lat = -1.0f; +// float navigation_lon = -1.0f; +// float navigation_alt = -1.0f; +// int navigation_frame = -1; - } else if (coordinate_frame == (int)MAV_FRAME_GLOBAL_RELATIVE_ALT) { - dist = mavlink_wpm_distance_to_point_global_wgs84(wpm->current_active_wp_id, (float)global_pos->lat * 1e-7f, (float)global_pos->lon * 1e-7f, global_pos->relative_alt, &dist_xy, &dist_z); +// /* initialize to current position in case we don't find a suitable navigation waypoint */ +// if (global_pos->valid) { +// navigation_lat = global_pos->lat/1e7; +// navigation_lon = global_pos->lon/1e7; +// navigation_alt = global_pos->alt; +// navigation_frame = MAV_FRAME_GLOBAL; +// } else if (local_pos->xy_valid && local_pos->z_valid) { +// navigation_lat = local_pos->x; +// navigation_lon = local_pos->y; +// navigation_alt = local_pos->z; +// navigation_frame = MAV_FRAME_LOCAL_NED; +// } - } else if (coordinate_frame == (int)MAV_FRAME_LOCAL_ENU || coordinate_frame == (int)MAV_FRAME_LOCAL_NED) { - dist = mavlink_wpm_distance_to_point_local(wpm->current_active_wp_id, local_pos->x, local_pos->y, local_pos->z, &dist_xy, &dist_z); +// /* guard against missions without final land waypoint */ +// /* only accept supported navigation waypoints, skip unknown ones */ +// do { - } else if (coordinate_frame == (int)MAV_FRAME_MISSION) { - /* Check if conditions of mission item are satisfied */ - // XXX TODO - } +// /* pick up the last valid navigation waypoint, this will be one we hold on to after the mission */ +// if (wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_WAYPOINT || +// wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_TURNS || +// wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_TIME || +// wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_UNLIM || +// wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_TAKEOFF) { - if (dist >= 0.f && dist_xy <= orbit && dist_z >= 0.0f && dist_z <= vertical_switch_distance) { - wpm->pos_reached = true; - } +// /* this is a navigation waypoint */ +// navigation_frame = cur_wp->frame; +// navigation_lat = cur_wp->x; +// navigation_lon = cur_wp->y; +// navigation_alt = cur_wp->z; +// } - // check if required yaw reached - float yaw_sp = _wrap_pi(wpm->waypoints[wpm->current_active_wp_id].param4 / 180.0f * FM_PI); - float yaw_err = _wrap_pi(yaw_sp - local_pos->yaw); - if (fabsf(yaw_err) < 0.05f) { - wpm->yaw_reached = true; - } - } +// if (wpm->current_active_wp_id == wpm->size - 1) { - //check if the current waypoint was reached - if (wpm->pos_reached && /*wpm->yaw_reached &&*/ !wpm->idle) { - if (wpm->current_active_wp_id < wpm->size) { - mavlink_mission_item_t *cur_wp = &(wpm->waypoints[wpm->current_active_wp_id]); +// /* if we're not landing at the last nav waypoint, we're falling back to loiter */ +// if (wpm->waypoints[wpm->current_active_wp_id].command != (int)MAV_CMD_NAV_LAND) { +// /* the last waypoint was reached, if auto continue is +// * activated AND it is NOT a land waypoint, keep the system loitering there. +// */ +// cur_wp->command = MAV_CMD_NAV_LOITER_UNLIM; +// cur_wp->param3 = 20.0f; // XXX magic number 20 m loiter radius +// cur_wp->frame = navigation_frame; +// cur_wp->x = navigation_lat; +// cur_wp->y = navigation_lon; +// cur_wp->z = navigation_alt; +// } - if (wpm->timestamp_firstinside_orbit == 0) { - // Announce that last waypoint was reached - mavlink_wpm_send_waypoint_reached(cur_wp->seq); - wpm->timestamp_firstinside_orbit = now; - } +// /* we risk an endless loop for missions without navigation waypoints, abort. */ +// break; - // check if the MAV was long enough inside the waypoint orbit - //if (now-timestamp_lastoutside_orbit > (cur_wp->hold_time*1000)) - - bool time_elapsed = false; - - if (now - wpm->timestamp_firstinside_orbit >= cur_wp->param1 * 1000 * 1000) { - time_elapsed = true; - } else if (cur_wp->command == (int)MAV_CMD_NAV_TAKEOFF) { - time_elapsed = true; - } - - if (time_elapsed) { - - /* safeguard against invalid missions with last wp autocontinue on */ - if (wpm->current_active_wp_id == wpm->size - 1) { - /* stop handling missions here */ - cur_wp->autocontinue = false; - } - - if (cur_wp->autocontinue) { - - cur_wp->current = 0; - - float navigation_lat = -1.0f; - float navigation_lon = -1.0f; - float navigation_alt = -1.0f; - int navigation_frame = -1; - - /* initialize to current position in case we don't find a suitable navigation waypoint */ - if (global_pos->valid) { - navigation_lat = global_pos->lat/1e7; - navigation_lon = global_pos->lon/1e7; - navigation_alt = global_pos->alt; - navigation_frame = MAV_FRAME_GLOBAL; - } else if (local_pos->xy_valid && local_pos->z_valid) { - navigation_lat = local_pos->x; - navigation_lon = local_pos->y; - navigation_alt = local_pos->z; - navigation_frame = MAV_FRAME_LOCAL_NED; - } - - /* guard against missions without final land waypoint */ - /* only accept supported navigation waypoints, skip unknown ones */ - do { - - /* pick up the last valid navigation waypoint, this will be one we hold on to after the mission */ - if (wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_WAYPOINT || - wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_TURNS || - wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_TIME || - wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_UNLIM || - wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_TAKEOFF) { - - /* this is a navigation waypoint */ - navigation_frame = cur_wp->frame; - navigation_lat = cur_wp->x; - navigation_lon = cur_wp->y; - navigation_alt = cur_wp->z; - } - - if (wpm->current_active_wp_id == wpm->size - 1) { - - /* if we're not landing at the last nav waypoint, we're falling back to loiter */ - if (wpm->waypoints[wpm->current_active_wp_id].command != (int)MAV_CMD_NAV_LAND) { - /* the last waypoint was reached, if auto continue is - * activated AND it is NOT a land waypoint, keep the system loitering there. - */ - cur_wp->command = MAV_CMD_NAV_LOITER_UNLIM; - cur_wp->param3 = 20.0f; // XXX magic number 20 m loiter radius - cur_wp->frame = navigation_frame; - cur_wp->x = navigation_lat; - cur_wp->y = navigation_lon; - cur_wp->z = navigation_alt; - } - - /* we risk an endless loop for missions without navigation waypoints, abort. */ - break; - - } else { - if ((uint16_t)(wpm->current_active_wp_id + 1) < wpm->size) - wpm->current_active_wp_id++; - } +// } else { +// if ((uint16_t)(wpm->current_active_wp_id + 1) < wpm->size) +// wpm->current_active_wp_id++; +// } - } while (!(wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_WAYPOINT || - wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_TURNS || - wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_TIME || - wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_UNLIM)); +// } while (!(wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_WAYPOINT || +// wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_TURNS || +// wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_TIME || +// wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_UNLIM)); - // Fly to next waypoint - wpm->timestamp_firstinside_orbit = 0; - mavlink_wpm_send_waypoint_current(wpm->current_active_wp_id); - mavlink_wpm_send_setpoint(wpm->current_active_wp_id); - wpm->waypoints[wpm->current_active_wp_id].current = true; - wpm->pos_reached = false; - wpm->yaw_reached = false; - printf("Set new waypoint (%u)\n", wpm->current_active_wp_id); - } - } - } +// // Fly to next waypoint +// wpm->timestamp_firstinside_orbit = 0; +// mavlink_wpm_send_waypoint_current(wpm->current_active_wp_id); +// mavlink_wpm_send_setpoint(wpm->current_active_wp_id); +// wpm->waypoints[wpm->current_active_wp_id].current = true; +// wpm->pos_reached = false; +// wpm->yaw_reached = false; +// printf("Set new waypoint (%u)\n", wpm->current_active_wp_id); +// } +// } +// } - } else { - wpm->timestamp_lastoutside_orbit = now; - } +// } else { +// wpm->timestamp_lastoutside_orbit = now; +// } - counter++; -} +// counter++; +// } int mavlink_waypoint_eventloop(uint64_t now, const struct vehicle_global_position_s *global_position, struct vehicle_local_position_s *local_position, struct navigation_capabilities_s *nav_cap) @@ -551,17 +491,17 @@ int mavlink_waypoint_eventloop(uint64_t now, const struct vehicle_global_positio #endif wpm->current_state = MAVLINK_WPM_STATE_IDLE; - wpm->current_count = 0; + // wpm->current_count = 0; wpm->current_partner_sysid = 0; wpm->current_partner_compid = 0; - wpm->current_wp_id = -1; + // wpm->current_wp_id = -1; - if (wpm->size == 0) { - wpm->current_active_wp_id = -1; - } + // if (wpm->size == 0) { + // wpm->current_active_wp_id = -1; + // } } - check_waypoints_reached(now, global_position, local_position, nav_cap->turn_distance); + // check_waypoints_reached(now, global_position, local_position, nav_cap->turn_distance); return OK; } @@ -583,7 +523,7 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi if (wpm->current_state == MAVLINK_WPM_STATE_SENDLIST || wpm->current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS) { if (wpm->current_wp_id == wpm->size - 1) { - mavlink_missionlib_send_gcs_string("Got last WP ACK state -> IDLE"); + // mavlink_missionlib_send_gcs_string("Got last WP ACK state -> IDLE"); wpm->current_state = MAVLINK_WPM_STATE_IDLE; wpm->current_wp_id = 0; @@ -607,25 +547,25 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi if (wpm->current_state == MAVLINK_WPM_STATE_IDLE) { if (wpc.seq < wpm->size) { // if (verbose) // printf("Received MAVLINK_MSG_ID_MISSION_ITEM_SET_CURRENT\n"); - wpm->current_active_wp_id = wpc.seq; - uint32_t i; + // wpm->current_active_wp_id = wpc.seq; + // uint32_t i; - for (i = 0; i < wpm->size; i++) { - if (i == wpm->current_active_wp_id) { - wpm->waypoints[i].current = true; + // for (i = 0; i < wpm->size; i++) { + // if (i == wpm->current_active_wp_id) { + // wpm->waypoints[i].current = true; - } else { - wpm->waypoints[i].current = false; - } - } + // } else { + // wpm->waypoints[i].current = false; + // } + // } - mavlink_missionlib_send_gcs_string("NEW WP SET"); + // mavlink_missionlib_send_gcs_string("NEW WP SET"); - wpm->yaw_reached = false; - wpm->pos_reached = false; - mavlink_wpm_send_waypoint_current(wpm->current_active_wp_id); - mavlink_wpm_send_setpoint(wpm->current_active_wp_id); - wpm->timestamp_firstinside_orbit = 0; + // wpm->yaw_reached = false; + // wpm->pos_reached = false; + mavlink_wpm_send_waypoint_current(wpc.seq); + // mavlink_wpm_send_setpoint(wpm->current_active_wp_id); + // wpm->timestamp_firstinside_orbit = 0; } else { mavlink_missionlib_send_gcs_string("IGN WP CURR CMD: Not in list"); @@ -880,9 +820,9 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi // delete waypoints->back(); // waypoints->pop_back(); // } - wpm->current_active_wp_id = -1; - wpm->yaw_reached = false; - wpm->pos_reached = false; + // wpm->current_active_wp_id = -1; + // wpm->yaw_reached = false; + // wpm->pos_reached = false; break; } else { @@ -942,7 +882,7 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi mavlink_mission_item_t wp; mavlink_msg_mission_item_decode(msg, &wp); - mavlink_missionlib_send_gcs_string("GOT WP"); + // mavlink_missionlib_send_gcs_string("GOT WP"); // printf("sysid=%d, current_partner_sysid=%d\n", msg->sysid, wpm->current_partner_sysid); // printf("compid=%d, current_partner_compid=%d\n", msg->compid, wpm->current_partner_compid); @@ -974,14 +914,14 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi // printf ("wpm->current_wp_id =%d, wpm->current_count=%d\n\n", wpm->current_wp_id, wpm->current_count); if (wpm->current_wp_id == wpm->current_count && wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS) { - mavlink_missionlib_send_gcs_string("GOT ALL WPS"); + // mavlink_missionlib_send_gcs_string("GOT ALL WPS"); // if (verbose) // printf("Got all %u waypoints, changing state to MAVLINK_WPM_STATE_IDLE\n", wpm->current_count); mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, 0); - if (wpm->current_active_wp_id > wpm->rcv_size - 1) { - wpm->current_active_wp_id = wpm->rcv_size - 1; - } + // if (wpm->current_active_wp_id > wpm->rcv_size - 1) { + // wpm->current_active_wp_id = wpm->rcv_size - 1; + // } // switch the waypoints list // FIXME CHECK!!! @@ -1006,25 +946,25 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi //get the new current waypoint - for (i = 0; i < wpm->size; i++) { - if (wpm->waypoints[i].current == 1) { - wpm->current_active_wp_id = i; - //// if (verbose) // printf("New current waypoint %u\n", current_active_wp_id); - wpm->yaw_reached = false; - wpm->pos_reached = false; - mavlink_wpm_send_waypoint_current(wpm->current_active_wp_id); - mavlink_wpm_send_setpoint(wpm->current_active_wp_id); - wpm->timestamp_firstinside_orbit = 0; - break; - } - } + // for (i = 0; i < wpm->size; i++) { + // if (wpm->waypoints[i].current == 1) { + // wpm->current_active_wp_id = i; + // //// if (verbose) // printf("New current waypoint %u\n", current_active_wp_id); + // // wpm->yaw_reached = false; + // // wpm->pos_reached = false; + // mavlink_wpm_send_waypoint_current(wpm->current_active_wp_id); + // // mavlink_wpm_send_setpoint(wpm->current_active_wp_id); + // // wpm->timestamp_firstinside_orbit = 0; + // break; + // } + // } - if (i == wpm->size) { - wpm->current_active_wp_id = -1; - wpm->yaw_reached = false; - wpm->pos_reached = false; - wpm->timestamp_firstinside_orbit = 0; - } + // if (i == wpm->size) { + // wpm->current_active_wp_id = -1; + // wpm->yaw_reached = false; + // wpm->pos_reached = false; + // wpm->timestamp_firstinside_orbit = 0; + // } wpm->current_state = MAVLINK_WPM_STATE_IDLE; @@ -1088,9 +1028,9 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi // if (verbose) // printf("Got MAVLINK_MSG_ID_MISSION_ITEM_CLEAR_LIST from %u deleting all waypoints\n", msg->sysid); // Delete all waypoints wpm->size = 0; - wpm->current_active_wp_id = -1; - wpm->yaw_reached = false; - wpm->pos_reached = false; + // wpm->current_active_wp_id = -1; + // wpm->yaw_reached = false; + // wpm->pos_reached = false; /* prepare mission topic */ mission.count = 0; diff --git a/src/modules/mavlink/waypoints.h b/src/modules/mavlink/waypoints.h index d7d6b31dc3..04759ec2da 100644 --- a/src/modules/mavlink/waypoints.h +++ b/src/modules/mavlink/waypoints.h @@ -56,6 +56,7 @@ #include #include #include +#include // FIXME XXX - TO BE MOVED TO XML enum MAVLINK_WPM_STATES { @@ -100,25 +101,29 @@ struct mavlink_wpm_storage { uint16_t rcv_size; enum MAVLINK_WPM_STATES current_state; int16_t current_wp_id; ///< Waypoint in current transmission - int16_t current_active_wp_id; ///< Waypoint the system is currently heading towards + // int16_t current_active_wp_id; ///< Waypoint the system is currently heading towards uint16_t current_count; uint8_t current_partner_sysid; uint8_t current_partner_compid; uint64_t timestamp_lastaction; - uint64_t timestamp_last_send_setpoint; - uint64_t timestamp_firstinside_orbit; - uint64_t timestamp_lastoutside_orbit; + // uint64_t timestamp_last_send_setpoint; + // uint64_t timestamp_firstinside_orbit; + // uint64_t timestamp_lastoutside_orbit; uint32_t timeout; - uint32_t delay_setpoint; - float accept_range_yaw; - float accept_range_distance; - bool yaw_reached; - bool pos_reached; - bool idle; + // uint32_t delay_setpoint; + // float accept_range_yaw; + // float accept_range_distance; + // bool yaw_reached; + // bool pos_reached; + // bool idle; }; typedef struct mavlink_wpm_storage mavlink_wpm_storage; +void map_mavlink_mission_item_to_mission_item(const mavlink_mission_item_t *mavlink_mission_item, struct mission_item_s *mission_item); +void map_mission_item_to_mavlink_mission_item(const struct mission_item_s *mission_item, mavlink_mission_item_t *mavlink_mission_item); + + void mavlink_wpm_init(mavlink_wpm_storage *state); int mavlink_waypoint_eventloop(uint64_t now, const struct vehicle_global_position_s *global_position, struct vehicle_local_position_s *local_pos, struct navigation_capabilities_s *nav_cap); diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 2e88448015..353629962f 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -58,6 +58,7 @@ #include #include #include +#include #include #include #include @@ -144,11 +145,13 @@ private: orb_advert_t _triplet_pub; /**< publish position setpoint triplet */ orb_advert_t _fence_pub; /**< publish fence topic */ + orb_advert_t _mission_result_pub; /**< publish mission result topic */ struct vehicle_status_s _vstatus; /**< vehicle status */ struct vehicle_global_position_s _global_pos; /**< global vehicle position */ struct home_position_s _home_pos; /**< home position for RTL */ struct mission_item_triplet_s _mission_item_triplet; /**< triplet of mission items */ + struct mission_result_s _mission_result; /**< mission result for commander/mavlink */ perf_counter_t _loop_perf; /**< loop performance counter */ @@ -215,12 +218,16 @@ private: void publish_mission_item_triplet(); + void publish_mission_result(); + int advance_current_mission_item(); void reset_mission_item_reached(); void check_mission_item_reached(); + void report_mission_reached(); + void start_waypoint(); void start_loiter(mission_item_s *new_loiter_position); @@ -266,6 +273,7 @@ Navigator::Navigator() : /* publications */ _triplet_pub(-1), _fence_pub(-1), + _mission_result_pub(-1), /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "navigator")), @@ -295,6 +303,8 @@ Navigator::Navigator() : memset(&_mission_item_triplet.current, 0, sizeof(struct mission_item_s)); memset(&_mission_item_triplet.next, 0, sizeof(struct mission_item_s)); + memset(&_mission_result, 0, sizeof(struct mission_result_s)); + /* fetch initial values */ parameters_update(); } @@ -586,6 +596,9 @@ Navigator::task_main() check_mission_item_reached(); if (_mission_item_reached) { + + report_mission_reached(); + mavlink_log_info(_mavlink_fd, "[navigator] reached WP %d", _current_mission_index); if (advance_current_mission_item() != OK) { set_mode(NAVIGATION_MODE_LOITER_WAYPOINT); @@ -896,6 +909,20 @@ Navigator::publish_mission_item_triplet() } } +void +Navigator::publish_mission_result() +{ + /* lazily publish the mission result only once available */ + if (_mission_result_pub > 0) { + /* publish the mission result */ + orb_publish(ORB_ID(mission_result), _mission_result_pub, &_mission_result); + + } else { + /* advertise and publish */ + _mission_result_pub = orb_advertise(ORB_ID(mission_result), &_mission_result); + } +} + int Navigator::advance_current_mission_item() { @@ -945,6 +972,9 @@ Navigator::reset_mission_item_reached() _time_first_inside_orbit = 0; _mission_item_reached = false; + + _mission_result.mission_reached = false; + _mission_result.mission_index = 0; } void @@ -1041,6 +1071,15 @@ Navigator::check_mission_item_reached() } +void +Navigator::report_mission_reached() +{ + _mission_result.mission_reached = true; + _mission_result.mission_index = _current_mission_index; + + publish_mission_result(); +} + void Navigator::start_waypoint() { diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index ecc1a3b3a2..e73e7ff8dd 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -129,6 +129,9 @@ ORB_DEFINE(vehicle_global_velocity_setpoint, struct vehicle_global_velocity_setp #include "topics/mission.h" ORB_DEFINE(mission, struct mission_s); +#include "topics/mission_result.h" +ORB_DEFINE(mission_result, struct mission_result_s); + #include "topics/fence.h" ORB_DEFINE(fence, unsigned); diff --git a/src/modules/uORB/topics/mission_result.h b/src/modules/uORB/topics/mission_result.h new file mode 100644 index 0000000000..c99706b97a --- /dev/null +++ b/src/modules/uORB/topics/mission_result.h @@ -0,0 +1,67 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: @author Thomas Gubler + * @author Julian Oes + * @author Lorenz Meier + * + * 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 mission_result.h + * Mission results that navigator needs to pass on to commander and mavlink. + */ + +#ifndef TOPIC_MISSION_RESULT_H +#define TOPIC_MISSION_RESULT_H + +#include +#include +#include "../uORB.h" + +/** + * @addtogroup topics + * @{ + */ + +struct mission_result_s +{ + bool mission_reached; /**< true if mission has been reached */ + unsigned mission_index; /**< index of the mission which has been reached */ +}; + +/** + * @} + */ + +/* register this as object request broker structure */ +ORB_DECLARE(mission_result); + +#endif From 81023fe5aafc33477a9e16d044d2b5a1420ade76 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 27 Nov 2013 16:17:44 +0100 Subject: [PATCH 174/447] Navigator and mavlink: more mavlink cleanup and set current waypoint option support added --- src/modules/mavlink/waypoints.c | 180 ++++++++++++++++------- src/modules/mavlink/waypoints.h | 12 +- src/modules/navigator/navigator_main.cpp | 7 +- src/modules/uORB/topics/mission.h | 1 + 4 files changed, 138 insertions(+), 62 deletions(-) diff --git a/src/modules/mavlink/waypoints.c b/src/modules/mavlink/waypoints.c index 95772f5a0c..8991f3a59d 100644 --- a/src/modules/mavlink/waypoints.c +++ b/src/modules/mavlink/waypoints.c @@ -70,11 +70,32 @@ struct mission_s mission; uint8_t mavlink_wpm_comp_id = MAV_COMP_ID_MISSIONPLANNER; -void map_mavlink_mission_item_to_mission_item(const mavlink_mission_item_t *mavlink_mission_item, struct mission_item_s *mission_item) +int map_mavlink_mission_item_to_mission_item(const mavlink_mission_item_t *mavlink_mission_item, struct mission_item_s *mission_item) { - mission_item->lat = (double)mavlink_mission_item->x; - mission_item->lon = (double)mavlink_mission_item->y; - mission_item->altitude = mavlink_mission_item->z; + /* only support global waypoints for now */ + switch (mavlink_mission_item->frame) { + case MAV_FRAME_GLOBAL: + mission_item->lat = (double)mavlink_mission_item->x; + mission_item->lon = (double)mavlink_mission_item->y; + mission_item->altitude = mavlink_mission_item->z; + mission_item->altitude_is_relative = false; + break; + + case MAV_FRAME_GLOBAL_RELATIVE_ALT: + mission_item->lat = (double)mavlink_mission_item->x; + mission_item->lon = (double)mavlink_mission_item->y; + mission_item->altitude = mavlink_mission_item->z; + mission_item->altitude_is_relative = true; + break; + + case MAV_FRAME_LOCAL_NED: + case MAV_FRAME_LOCAL_ENU: + return MAV_MISSION_UNSUPPORTED_FRAME; + case MAV_FRAME_MISSION: + default: + return MAV_MISSION_ERROR; + } + mission_item->yaw = mavlink_mission_item->param4*M_DEG_TO_RAD_F; mission_item->loiter_radius = fabsf(mavlink_mission_item->param3); mission_item->loiter_direction = (mavlink_mission_item->param3 > 0) ? 1 : -1; /* 1 if positive CW, -1 if negative CCW */ @@ -83,14 +104,22 @@ void map_mavlink_mission_item_to_mission_item(const mavlink_mission_item_t *mavl mission_item->time_inside = mavlink_mission_item->param2 / 1e3f; /* from milliseconds to seconds */ mission_item->autocontinue = mavlink_mission_item->autocontinue; mission_item->index = mavlink_mission_item->seq; + + return OK; } -void map_mission_item_to_mavlink_mission_item(const struct mission_item_s *mission_item, mavlink_mission_item_t *mavlink_mission_item) +int map_mission_item_to_mavlink_mission_item(const struct mission_item_s *mission_item, mavlink_mission_item_t *mavlink_mission_item) { - + if (mission_item->altitude_is_relative) { + mavlink_mission_item->frame = MAV_FRAME_GLOBAL; + } else { + mavlink_mission_item->frame = MAV_FRAME_GLOBAL_RELATIVE_ALT; + } + mavlink_mission_item->x = (float)mission_item->lat; mavlink_mission_item->y = (float)mission_item->lon; mavlink_mission_item->z = mission_item->altitude; + mavlink_mission_item->param4 = mission_item->yaw*M_RAD_TO_DEG_F; mavlink_mission_item->param3 = mission_item->loiter_radius*(float)mission_item->loiter_direction; mavlink_mission_item->command = mission_item->nav_cmd; @@ -98,6 +127,8 @@ void map_mission_item_to_mavlink_mission_item(const struct mission_item_s *missi mavlink_mission_item->param2 = mission_item->time_inside * 1e3f; /* from seconds to milliseconds */ mavlink_mission_item->autocontinue = mission_item->autocontinue; mavlink_mission_item->seq = mission_item->index; + + return OK; } void mavlink_wpm_init(mavlink_wpm_storage *state) @@ -147,15 +178,15 @@ void mavlink_wpm_send_waypoint_ack(uint8_t sysid, uint8_t compid, uint8_t type) // FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY")); - if (MAVLINK_WPM_TEXT_FEEDBACK) { -#ifdef MAVLINK_WPM_NO_PRINTF - mavlink_missionlib_send_gcs_string("Sent waypoint ACK"); -#else +// if (MAVLINK_WPM_TEXT_FEEDBACK) { +// #ifdef MAVLINK_WPM_NO_PRINTF +// mavlink_missionlib_send_gcs_string("Sent waypoint ACK"); +// #else - if (MAVLINK_WPM_VERBOSE) printf("Sent waypoint ack (%u) to ID %u\n", wpa.type, wpa.target_system); +// if (MAVLINK_WPM_VERBOSE) printf("Sent waypoint ack (%u) to ID %u\n", wpa.type, wpa.target_system); -#endif - } +// #endif +// } } /* @@ -563,7 +594,20 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi // wpm->yaw_reached = false; // wpm->pos_reached = false; - mavlink_wpm_send_waypoint_current(wpc.seq); + + mission.current_index = wpc.seq; + + /* Initialize mission publication if necessary */ + if (mission_pub < 0) { + mission_pub = orb_advertise(ORB_ID(mission), &mission); + + } else { + orb_publish(ORB_ID(mission), mission_pub, &mission); + } + + mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_ACCEPTED); + + //mavlink_wpm_send_waypoint_current(wpc.seq); // mavlink_wpm_send_setpoint(wpm->current_active_wp_id); // wpm->timestamp_firstinside_orbit = 0; @@ -788,6 +832,8 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi } else { /* prepare mission topic */ mission.count = wpc.count; + /* reset current index */ + mission.current_index = -1; } #ifdef MAVLINK_WPM_NO_PRINTF @@ -896,7 +942,9 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi // wpm->current_state = MAVLINK_WPM_STATE_GETLIST;//removeme debug XXX TODO //ensure that we are in the correct state and that the first waypoint has id 0 and the following waypoints have the correct ids - if ((wpm->current_state == MAVLINK_WPM_STATE_GETLIST && wp.seq == 0) || (wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS && wp.seq == wpm->current_wp_id && wp.seq < wpm->current_count)) { + if ((wpm->current_state == MAVLINK_WPM_STATE_GETLIST && wp.seq == 0) || + (wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS && wp.seq == wpm->current_wp_id && + wp.seq < wpm->current_count)) { //mavlink_missionlib_send_gcs_string("DEBUG 2"); // if (verbose && wpm->current_state == MAVLINK_WPM_STATE_GETLIST) // printf("Got MAVLINK_MSG_ID_MISSION_ITEM %u from %u changing state to MAVLINK_WPM_STATE_GETLIST_GETWPS\n", wp.seq, msg->sysid); @@ -904,9 +952,25 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi // if (verbose && wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS && wp.seq-1 == wpm->current_wp_id) // printf("Got MAVLINK_MSG_ID_MISSION_ITEM %u (again) from %u\n", wp.seq, msg->sysid); // wpm->current_state = MAVLINK_WPM_STATE_GETLIST_GETWPS; - mavlink_mission_item_t *newwp = &(wpm->rcv_waypoints[wp.seq]); - memcpy(newwp, &wp, sizeof(mavlink_mission_item_t)); -// printf("WP seq: %d\n",wp.seq); + // mavlink_mission_item_t *newwp = &(wpm->rcv_waypoints[wp.seq]); + // memcpy(newwp, &wp, sizeof(mavlink_mission_item_t)); + + int ret = map_mavlink_mission_item_to_mission_item(&wp, &mission.items[wp.seq]); + if (ret != OK) { + + + mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, ret); + wpm->current_state = MAVLINK_WPM_STATE_IDLE; + break; + } + + if (wp.current) { + mission.current_index = wp.seq; + warnx("current is: %d", wp.seq); + } else { + warnx("not current"); + } + wpm->current_wp_id = wp.seq + 1; // if (verbose) // printf ("Added new waypoint to list. X= %f\t Y= %f\t Z= %f\t Yaw= %f\n", newwp->x, newwp->y, newwp->z, newwp->param4); @@ -923,14 +987,19 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi // wpm->current_active_wp_id = wpm->rcv_size - 1; // } - // switch the waypoints list - // FIXME CHECK!!! - uint32_t i; + // bool copy_error = false; - for (i = 0; i < wpm->current_count; ++i) { - wpm->waypoints[i] = wpm->rcv_waypoints[i]; - map_mavlink_mission_item_to_mission_item(&wpm->rcv_waypoints[i], &mission.items[i]); - } + // // switch the waypoints list + // // FIXME CHECK!!! + // uint32_t i; + + // for (i = 0; i < wpm->current_count; ++i) { + // wpm->waypoints[i] = wpm->rcv_waypoints[i]; + // if (map_mavlink_mission_item_to_mission_item(&wpm->rcv_waypoints[i], &mission.items[i]) != OK) { + // copy_error = true; + // } + + // } // TODO: update count? /* Initialize mission publication if necessary */ @@ -973,38 +1042,41 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi } } else { - if (wpm->current_state == MAVLINK_WPM_STATE_IDLE) { - //we're done receiving waypoints, answer with ack. - mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, 0); - printf("Received MAVLINK_MSG_ID_MISSION_ITEM while state=MAVLINK_WPM_STATE_IDLE, answered with WAYPOINT_ACK.\n"); - } - // if (verbose) - { - if (!(wpm->current_state == MAVLINK_WPM_STATE_GETLIST || wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS)) { -// printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM %u because i'm doing something else already (state=%i).\n", wp.seq, wpm->current_state); - break; + mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_INVALID_SEQUENCE); - } else if (wpm->current_state == MAVLINK_WPM_STATE_GETLIST) { - if (!(wp.seq == 0)) { -// printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM because the first waypoint ID (%u) was not 0.\n", wp.seq); - } else { -// printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM %u - FIXME: missed error description\n", wp.seq); - } - } else if (wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS) { - if (!(wp.seq == wpm->current_wp_id)) { -// printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM because the waypoint ID (%u) was not the expected %u.\n", wp.seq, wpm->current_wp_id); - mavlink_wpm_send_waypoint_request(wpm->current_partner_sysid, wpm->current_partner_compid, wpm->current_wp_id); + // if (wpm->current_state == MAVLINK_WPM_STATE_IDLE) { + // //we're done receiving waypoints, answer with ack. + // mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, 0); + // printf("Received MAVLINK_MSG_ID_MISSION_ITEM while state=MAVLINK_WPM_STATE_IDLE, answered with WAYPOINT_ACK.\n"); + // } - } else if (!(wp.seq < wpm->current_count)) { -// printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM because the waypoint ID (%u) was out of bounds.\n", wp.seq); - } else { -// printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM %u - FIXME: missed error description\n", wp.seq); - } - } else { -// printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM %u - FIXME: missed error description\n", wp.seq); - } - } +// // if (verbose) +// { +// if (!(wpm->current_state == MAVLINK_WPM_STATE_GETLIST || wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS)) { +// // printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM %u because i'm doing something else already (state=%i).\n", wp.seq, wpm->current_state); +// break; + +// } else if (wpm->current_state == MAVLINK_WPM_STATE_GETLIST) { +// if (!(wp.seq == 0)) { +// // printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM because the first waypoint ID (%u) was not 0.\n", wp.seq); +// } else { +// // printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM %u - FIXME: missed error description\n", wp.seq); +// } +// } else if (wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS) { +// if (!(wp.seq == wpm->current_wp_id)) { +// // printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM because the waypoint ID (%u) was not the expected %u.\n", wp.seq, wpm->current_wp_id); +// mavlink_wpm_send_waypoint_request(wpm->current_partner_sysid, wpm->current_partner_compid, wpm->current_wp_id); + +// } else if (!(wp.seq < wpm->current_count)) { +// // printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM because the waypoint ID (%u) was out of bounds.\n", wp.seq); +// } else { +// // printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM %u - FIXME: missed error description\n", wp.seq); +// } +// } else { +// // printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM %u - FIXME: missed error description\n", wp.seq); +// } +// } } } else { //we we're target but already communicating with someone else diff --git a/src/modules/mavlink/waypoints.h b/src/modules/mavlink/waypoints.h index 04759ec2da..fc68285e93 100644 --- a/src/modules/mavlink/waypoints.h +++ b/src/modules/mavlink/waypoints.h @@ -82,7 +82,7 @@ enum MAVLINK_WPM_CODES { /* WAYPOINT MANAGER - MISSION LIB */ #define MAVLINK_WPM_MAX_WP_COUNT 15 -#define MAVLINK_WPM_CONFIG_IN_FLIGHT_UPDATE ///< Enable double buffer and in-flight updates +// #define MAVLINK_WPM_CONFIG_IN_FLIGHT_UPDATE ///< Enable double buffer and in-flight updates #ifndef MAVLINK_WPM_TEXT_FEEDBACK #define MAVLINK_WPM_TEXT_FEEDBACK 0 ///< Report back status information as text #endif @@ -93,9 +93,9 @@ enum MAVLINK_WPM_CODES { struct mavlink_wpm_storage { mavlink_mission_item_t waypoints[MAVLINK_WPM_MAX_WP_COUNT]; ///< Currently active waypoints -#ifdef MAVLINK_WPM_CONFIG_IN_FLIGHT_UPDATE - mavlink_mission_item_t rcv_waypoints[MAVLINK_WPM_MAX_WP_COUNT]; ///< Receive buffer for next waypoints -#endif +// #ifdef MAVLINK_WPM_CONFIG_IN_FLIGHT_UPDATE + // mavlink_mission_item_t rcv_waypoints[MAVLINK_WPM_MAX_WP_COUNT]; ///< Receive buffer for next waypoints +// #endif uint16_t size; uint16_t max_size; uint16_t rcv_size; @@ -120,8 +120,8 @@ struct mavlink_wpm_storage { typedef struct mavlink_wpm_storage mavlink_wpm_storage; -void map_mavlink_mission_item_to_mission_item(const mavlink_mission_item_t *mavlink_mission_item, struct mission_item_s *mission_item); -void map_mission_item_to_mavlink_mission_item(const struct mission_item_s *mission_item, mavlink_mission_item_t *mavlink_mission_item); +int map_mavlink_mission_item_to_mission_item(const mavlink_mission_item_t *mavlink_mission_item, struct mission_item_s *mission_item); +int map_mission_item_to_mavlink_mission_item(const struct mission_item_s *mission_item, mavlink_mission_item_t *mavlink_mission_item); void mavlink_wpm_init(mavlink_wpm_storage *state); diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 353629962f..b88fc804c4 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -358,7 +358,7 @@ Navigator::mission_update() * if the first part changed: start again at first waypoint * if the first part remained unchanged: continue with the (possibly changed second part) */ - if (_current_mission_index < _mission_item_count && _current_mission_index < mission.count) { //check if not finished and if the new mission is not a shorter mission + if (mission.current_index == -1 && _current_mission_index < _mission_item_count && _current_mission_index < mission.count) { //check if not finished and if the new mission is not a shorter mission for (unsigned i = 0; i < _current_mission_index; i++) { if (!cmp_mission_item_equivalent(_mission_item[i], mission.items[i])) { /* set flag to restart mission next we're in auto */ @@ -371,8 +371,11 @@ Navigator::mission_update() // warnx("Mission item is equivalent i=%d", i); // } } - } else { + } else if (mission.current_index >= 0 && mission.current_index < mission.count) { /* set flag to restart mission next we're in auto */ + _current_mission_index = mission.current_index; + mavlink_log_info(_mavlink_fd, "[navigator] Reset to WP %d", _current_mission_index); + } else { _current_mission_index = 0; mavlink_log_info(_mavlink_fd, "[navigator] Reset to WP %d", _current_mission_index); } diff --git a/src/modules/uORB/topics/mission.h b/src/modules/uORB/topics/mission.h index 441efe4587..56350d3497 100644 --- a/src/modules/uORB/topics/mission.h +++ b/src/modules/uORB/topics/mission.h @@ -90,6 +90,7 @@ struct mission_s { struct mission_item_s *items; unsigned count; + int current_index; /**< default -1, start at the one changed latest */ }; /** From de36ccfff5c9b1311f2b5457e374be048c0989ba Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 27 Nov 2013 17:00:27 +0100 Subject: [PATCH 175/447] Navigator: report the current waypoint back --- src/modules/mavlink/waypoints.c | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/src/modules/mavlink/waypoints.c b/src/modules/mavlink/waypoints.c index 8991f3a59d..809900d7d4 100644 --- a/src/modules/mavlink/waypoints.c +++ b/src/modules/mavlink/waypoints.c @@ -704,6 +704,13 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi mavlink_mission_item_t wp; map_mission_item_to_mavlink_mission_item(&mission.items[wpr.seq], &wp); + + if (mission.current_index == wpr.seq) { + wp.current = true; + } else { + wp.current = false; + } + mavlink_wpm_send_waypoint(wpm->current_partner_sysid, wpm->current_partner_compid, &wp); } else { From b9ea91513687332a1b005690005f0eba16d5c522 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 27 Nov 2013 23:09:05 +0400 Subject: [PATCH 176/447] multirotor_pos_control: set PID parameters on init --- .../multirotor_pos_control/multirotor_pos_control.c | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c index ebee0b7a8b..834df9655c 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c @@ -244,7 +244,6 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) parameters_init(¶ms_h); parameters_update(¶ms_h, ¶ms); - for (int i = 0; i < 2; i++) { pid_init(&(xy_pos_pids[i]), PID_MODE_DERIVATIV_SET, 0.02f); pid_init(&(xy_vel_pids[i]), PID_MODE_DERIVATIV_CALC_NO_SP, 0.02f); @@ -253,15 +252,18 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) pid_init(&z_pos_pid, PID_MODE_DERIVATIV_SET, 0.02f); thrust_pid_init(&z_vel_pid, 0.02f); + bool param_updated = true; + while (!thread_should_exit) { - bool param_updated; - orb_check(param_sub, ¶m_updated); + if (!param_updated) + orb_check(param_sub, ¶m_updated); if (param_updated) { /* clear updated flag */ struct parameter_update_s ps; orb_copy(ORB_ID(parameter_update), param_sub, &ps); + /* update params */ parameters_update(¶ms_h, ¶ms); From 69888d28a5bbb5ba86e3976e694b51356d1c5ecf Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 29 Nov 2013 10:06:01 +0100 Subject: [PATCH 177/447] Navigator: Added onboard mission (not usable yet) --- src/modules/navigator/navigator_main.cpp | 102 ++++++++++++++++++++++- src/modules/navigator/navigator_params.c | 3 +- src/modules/uORB/objects_common.cpp | 1 + src/modules/uORB/topics/mission.h | 1 + 4 files changed, 102 insertions(+), 5 deletions(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index b88fc804c4..1c81245e0c 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -141,6 +141,7 @@ private: int _vstatus_sub; /**< vehicle status subscription */ int _params_sub; /**< notification of parameter updates */ int _mission_sub; /**< notification of mission updates */ + int _onboard_mission_sub; /**< notification of onboard mission updates */ int _capabilities_sub; /**< notification of vehicle capabilities updates */ orb_advert_t _triplet_pub; /**< publish position setpoint triplet */ @@ -156,9 +157,12 @@ private: perf_counter_t _loop_perf; /**< loop performance counter */ unsigned _max_mission_item_count; /**< maximum number of mission items supported */ + unsigned _max_onboard_mission_item_count;/**< maximum number of onboard mission items supported */ unsigned _mission_item_count; /** number of mission items copied */ + unsigned _onboard_mission_item_count; /** number of onboard mission items copied */ struct mission_item_s *_mission_item; /**< storage for mission */ + struct mission_item_s *_onboard_mission_item; /**< storage for onboard mission */ struct fence_s _fence; /**< storage for fence vertices */ bool _fence_valid; /**< flag if fence is valid */ @@ -170,18 +174,22 @@ private: bool _waypoint_yaw_reached; uint64_t _time_first_inside_orbit; bool _mission_item_reached; + bool _onboard_mission_item_reached; navigation_mode_t _mode; unsigned _current_mission_index; + unsigned _current_onboard_mission_index; struct { float min_altitude; float loiter_radius; + int onboard_mission_enabled; } _parameters; /**< local copies of parameters */ struct { param_t min_altitude; param_t loiter_radius; + param_t onboard_mission_enabled; } _parameter_handles; /**< handles for parameters */ @@ -196,6 +204,11 @@ private: */ void mission_update(); + /** + * Retrieve onboard mission. + */ + void onboard_mission_update(); + /** * Shim for calling task_main from task_create. */ @@ -268,6 +281,7 @@ Navigator::Navigator() : _vstatus_sub(-1), _params_sub(-1), _mission_sub(-1), + _onboard_mission_sub(-1), _capabilities_sub(-1), /* publications */ @@ -279,22 +293,28 @@ Navigator::Navigator() : _loop_perf(perf_alloc(PC_ELAPSED, "navigator")), /* states */ _max_mission_item_count(10), + _max_onboard_mission_item_count(10), _mission_item_count(0), + _onboard_mission_item_count(0), _fence_valid(false), _inside_fence(true), _waypoint_position_reached(false), _waypoint_yaw_reached(false), _time_first_inside_orbit(0), _mission_item_reached(false), + _onboard_mission_item_reached(false), _mode(NAVIGATION_MODE_NONE), - _current_mission_index(0) + _current_mission_index(0), + _current_onboard_mission_index(0) { _global_pos.valid = false; memset(&_fence, 0, sizeof(_fence)); _parameter_handles.min_altitude = param_find("NAV_MIN_ALT"); _parameter_handles.loiter_radius = param_find("NAV_LOITER_RAD"); + _parameter_handles.onboard_mission_enabled = param_find("NAV_ONB_MIS_EN"); _mission_item = (mission_item_s*)malloc(sizeof(mission_item_s) * _max_mission_item_count); + _onboard_mission_item = (mission_item_s*)malloc(sizeof(mission_item_s) * _max_onboard_mission_item_count); _mission_item_triplet.previous_valid = false; _mission_item_triplet.current_valid = false; @@ -340,6 +360,7 @@ Navigator::parameters_update() param_get(_parameter_handles.min_altitude, &(_parameters.min_altitude)); param_get(_parameter_handles.loiter_radius, &(_parameters.loiter_radius)); + param_get(_parameter_handles.onboard_mission_enabled, &(_parameters.onboard_mission_enabled)); return OK; } @@ -409,6 +430,71 @@ Navigator::mission_update() } } +void +Navigator::onboard_mission_update() +{ + struct mission_s onboard_mission; + if (orb_copy(ORB_ID(onboard_mission), _onboard_mission_sub, &onboard_mission) == OK) { + // XXX this is not optimal yet, but a first prototype / + // test implementation + + if (onboard_mission.count <= _max_onboard_mission_item_count) { + + /* Check if first part of mission (up to _current_onboard_mission_index - 1) changed: + * if the first part changed: start again at first waypoint + * if the first part remained unchanged: continue with the (possibly changed second part) + */ + if (onboard_mission.current_index == -1 && _current_onboard_mission_index < _onboard_mission_item_count && _current_onboard_mission_index < onboard_mission.count) { //check if not finished and if the new mission is not a shorter mission + for (unsigned i = 0; i < _current_onboard_mission_index; i++) { + if (!cmp_mission_item_equivalent(_onboard_mission_item[i], onboard_mission.items[i])) { + /* set flag to restart mission next we're in auto */ + _current_onboard_mission_index = 0; + mavlink_log_info(_mavlink_fd, "[navigator] Reset to onboard WP %d", _current_onboard_mission_index); + //warnx("First part of onboard mission differs i=%d", i); + break; + } +// else { +// warnx("Onboard mission item is equivalent i=%d", i); +// } + } + } else if (onboard_mission.current_index >= 0 && onboard_mission.current_index < onboard_mission.count) { + /* set flag to restart mission next we're in auto */ + _current_onboard_mission_index = onboard_mission.current_index; + mavlink_log_info(_mavlink_fd, "[navigator] Reset to onboard WP %d", _current_onboard_mission_index); + } else { + _current_onboard_mission_index = 0; + mavlink_log_info(_mavlink_fd, "[navigator] Reset to onboard WP %d", _current_onboard_mission_index); + } + + /* + * Perform an atomic copy & state update + */ + irqstate_t flags = irqsave(); + + memcpy(_onboard_mission_item, onboard_mission.items, onboard_mission.count * sizeof(struct mission_item_s)); + _onboard_mission_item_count = onboard_mission.count; + + irqrestore(flags); + + + + } else { + warnx("ERROR: too many waypoints, not supported"); + mavlink_log_critical(_mavlink_fd, "[navigator] too many waypoints, not supported"); + _onboard_mission_item_count = 0; + } + + if (_mode == NAVIGATION_MODE_WAYPOINT) { + start_waypoint(); + } + + /* TODO add checks if and how the mission has changed */ + } else { + _onboard_mission_item_count = 0; + _current_onboard_mission_index = 0; + } +} + void Navigator::task_main_trampoline(int argc, char *argv[]) { @@ -428,6 +514,7 @@ Navigator::task_main() */ _global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); _mission_sub = orb_subscribe(ORB_ID(mission)); + _onboard_mission_sub = orb_subscribe(ORB_ID(onboard_mission)); _capabilities_sub = orb_subscribe(ORB_ID(navigation_capabilities)); _vstatus_sub = orb_subscribe(ORB_ID(vehicle_status)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); @@ -439,6 +526,7 @@ Navigator::task_main() } mission_update(); + onboard_mission_update(); /* rate limit vehicle status updates to 5Hz */ orb_set_interval(_vstatus_sub, 200); @@ -453,7 +541,7 @@ Navigator::task_main() orb_copy(ORB_ID(navigation_capabilities), _capabilities_sub, &_nav_caps); /* wakeup source(s) */ - struct pollfd fds[6]; + struct pollfd fds[7]; /* Setup of loop */ fds[0].fd = _params_sub; @@ -466,8 +554,10 @@ Navigator::task_main() fds[3].events = POLLIN; fds[4].fd = _mission_sub; fds[4].events = POLLIN; - fds[5].fd = _vstatus_sub; + fds[5].fd = _onboard_mission_sub; fds[5].events = POLLIN; + fds[6].fd = _vstatus_sub; + fds[6].events = POLLIN; while (!_task_should_exit) { @@ -489,7 +579,7 @@ Navigator::task_main() perf_begin(_loop_perf); /* only update vehicle status if it changed */ - if (fds[5].revents & POLLIN) { + if (fds[6].revents & POLLIN) { /* read from param to clear updated flag */ orb_copy(ORB_ID(vehicle_status), _vstatus_sub, &_vstatus); @@ -570,6 +660,10 @@ Navigator::task_main() mission_update(); } + if (fds[5].revents & POLLIN) { + onboard_mission_update(); + } + if (fds[2].revents & POLLIN) { orb_copy(ORB_ID(home_position), _home_pos_sub, &_home_pos); } diff --git a/src/modules/navigator/navigator_params.c b/src/modules/navigator/navigator_params.c index 817e2f0093..b9d887379b 100644 --- a/src/modules/navigator/navigator_params.c +++ b/src/modules/navigator/navigator_params.c @@ -52,4 +52,5 @@ */ PARAM_DEFINE_FLOAT(NAV_MIN_ALT, 50.0f); -PARAM_DEFINE_FLOAT(NAV_LOITER_RAD, 100.0f); \ No newline at end of file +PARAM_DEFINE_FLOAT(NAV_LOITER_RAD, 100.0f); +PARAM_DEFINE_INT32(NAV_ONB_MIS_EN, 0); \ No newline at end of file diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index e73e7ff8dd..9489550c6f 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -128,6 +128,7 @@ ORB_DEFINE(vehicle_global_velocity_setpoint, struct vehicle_global_velocity_setp #include "topics/mission.h" ORB_DEFINE(mission, struct mission_s); +ORB_DEFINE(onboard_mission, struct mission_s); #include "topics/mission_result.h" ORB_DEFINE(mission_result, struct mission_result_s); diff --git a/src/modules/uORB/topics/mission.h b/src/modules/uORB/topics/mission.h index 56350d3497..2427a1d579 100644 --- a/src/modules/uORB/topics/mission.h +++ b/src/modules/uORB/topics/mission.h @@ -99,5 +99,6 @@ struct mission_s /* register this as object request broker structure */ ORB_DECLARE(mission); +ORB_DECLARE(onboard_mission); #endif From 5c83af3868aaaed20f31f28cbc296fb249dca566 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 29 Nov 2013 10:54:29 +0100 Subject: [PATCH 178/447] Navigator: Onboard missions supported in theory --- src/modules/navigator/navigator_main.cpp | 72 ++++++++++++++++++------ 1 file changed, 56 insertions(+), 16 deletions(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 1c81245e0c..cd4e048831 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -227,6 +227,10 @@ private: void set_mode(navigation_mode_t new_nav_mode); + bool mission_possible(); + + bool onboard_mission_possible(); + int set_waypoint_mission_item(unsigned mission_item_index, struct mission_item_s *new_mission_item); void publish_mission_item_triplet(); @@ -609,8 +613,8 @@ Navigator::task_main() case NAVIGATION_STATE_AUTO_MISSION: - if (_mission_item_count > 0 && !(_current_mission_index >= _mission_item_count)) { - /* Start mission if there is a mission available and the last waypoint has not been reached */ + if (onboard_mission_possible() || mission_possible()) { + /* Start mission or onboard mission if available */ set_mode(NAVIGATION_MODE_WAYPOINT); } else { /* else fallback to loiter */ @@ -971,24 +975,60 @@ Navigator::set_mode(navigation_mode_t new_nav_mode) } } -int -Navigator::set_waypoint_mission_item(unsigned mission_item_index, struct mission_item_s *new_mission_item) +bool +Navigator::mission_possible() { - if (mission_item_index < _mission_item_count) { - memcpy(new_mission_item, &_mission_item[mission_item_index], sizeof(mission_item_s)); + return _mission_item_count > 0 && + !(_current_mission_index >= _mission_item_count); +} - if (new_mission_item->nav_cmd == NAV_CMD_RETURN_TO_LAUNCH) { - /* if it is a RTL waypoint, append the home position */ - new_mission_item->lat = (double)_home_pos.lat / 1e7; - new_mission_item->lon = (double)_home_pos.lon / 1e7; - new_mission_item->altitude = (float)_home_pos.alt / 1e3f + _parameters.min_altitude; - new_mission_item->loiter_radius = _parameters.loiter_radius; // TODO: get rid of magic number - new_mission_item->radius = 50.0f; // TODO: get rid of magic number +bool +Navigator::onboard_mission_possible() +{ + return _onboard_mission_item_count > 0 && + !(_current_onboard_mission_index >= _onboard_mission_item_count) && + _parameters.onboard_mission_enabled; +} + +int +Navigator::set_waypoint_mission_item(unsigned index, struct mission_item_s *new_item) +{ + if (onboard_mission_possible()) { + + if (index < _onboard_mission_item_count) { + memcpy(new_item, &_onboard_mission_item[index], sizeof(mission_item_s)); + + if (new_item->nav_cmd == NAV_CMD_RETURN_TO_LAUNCH) { + /* if it is a RTL waypoint, append the home position */ + new_item->lat = (double)_home_pos.lat / 1e7; + new_item->lon = (double)_home_pos.lon / 1e7; + new_item->altitude = (float)_home_pos.alt / 1e3f + _parameters.min_altitude; + new_item->loiter_radius = _parameters.loiter_radius; // TODO: get rid of magic number + new_item->radius = 50.0f; // TODO: get rid of magic number + } + // warnx("added mission item: %d", index); + return OK; + } + + } else if (mission_possible()) { + + if (index < _mission_item_count) { + memcpy(new_item, &_mission_item[index], sizeof(mission_item_s)); + + if (new_item->nav_cmd == NAV_CMD_RETURN_TO_LAUNCH) { + /* if it is a RTL waypoint, append the home position */ + new_item->lat = (double)_home_pos.lat / 1e7; + new_item->lon = (double)_home_pos.lon / 1e7; + new_item->altitude = (float)_home_pos.alt / 1e3f + _parameters.min_altitude; + new_item->loiter_radius = _parameters.loiter_radius; // TODO: get rid of magic number + new_item->radius = 50.0f; // TODO: get rid of magic number + } + // warnx("added mission item: %d", index); + return OK; } - // warnx("added mission item: %d", mission_item_index); - return OK; } - // warnx("could not add mission item: %d", mission_item_index); + + // warnx("could not add mission item: %d", index); return ERROR; } From 83b09614e71c7ea68db1081a673e53bca2d9422f Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 3 Dec 2013 16:25:11 +0100 Subject: [PATCH 179/447] Dataman: Start dataman and use it in waypoints and navigator instead of mission items in mission topic --- ROMFS/px4fmu_common/init.d/rcS | 5 + src/modules/mavlink/waypoints.c | 108 ++++++----- src/modules/navigator/navigator_main.cpp | 233 ++++++----------------- src/modules/uORB/topics/mission.h | 10 +- 4 files changed, 129 insertions(+), 227 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 5f52969d13..9445e963be 100755 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -111,6 +111,11 @@ then # commander start + # + # Start the datamanager + # + dataman start + # # Start the Navigator # diff --git a/src/modules/mavlink/waypoints.c b/src/modules/mavlink/waypoints.c index 809900d7d4..7aad5038d2 100644 --- a/src/modules/mavlink/waypoints.c +++ b/src/modules/mavlink/waypoints.c @@ -52,10 +52,8 @@ #include #include - -#ifndef FM_PI -#define FM_PI 3.1415926535897932384626433832795f -#endif +#include +#include bool debug = false; bool verbose = false; @@ -63,13 +61,22 @@ bool verbose = false; orb_advert_t mission_pub = -1; struct mission_s mission; -#define NUM_MISSIONS_SUPPORTED 10 - //#define MAVLINK_WPM_NO_PRINTF #define MAVLINK_WPM_VERBOSE 1 uint8_t mavlink_wpm_comp_id = MAV_COMP_ID_MISSIONPLANNER; +void publish_mission() +{ + /* Initialize mission publication if necessary */ + if (mission_pub < 0) { + mission_pub = orb_advertise(ORB_ID(mission), &mission); + + } else { + orb_publish(ORB_ID(mission), mission_pub, &mission); + } +} + int map_mavlink_mission_item_to_mission_item(const mavlink_mission_item_t *mavlink_mission_item, struct mission_item_s *mission_item) { /* only support global waypoints for now */ @@ -96,7 +103,7 @@ int map_mavlink_mission_item_to_mission_item(const mavlink_mission_item_t *mavli return MAV_MISSION_ERROR; } - mission_item->yaw = mavlink_mission_item->param4*M_DEG_TO_RAD_F; + mission_item->yaw = _wrap_pi(mavlink_mission_item->param4*M_DEG_TO_RAD_F); mission_item->loiter_radius = fabsf(mavlink_mission_item->param3); mission_item->loiter_direction = (mavlink_mission_item->param3 > 0) ? 1 : -1; /* 1 if positive CW, -1 if negative CCW */ mission_item->nav_cmd = mavlink_mission_item->command; @@ -104,6 +111,7 @@ int map_mavlink_mission_item_to_mission_item(const mavlink_mission_item_t *mavli mission_item->time_inside = mavlink_mission_item->param2 / 1e3f; /* from milliseconds to seconds */ mission_item->autocontinue = mavlink_mission_item->autocontinue; mission_item->index = mavlink_mission_item->seq; + mission_item->origin = ORIGIN_MAVLINK; return OK; } @@ -151,14 +159,6 @@ void mavlink_wpm_init(mavlink_wpm_storage *state) // state->pos_reached = false; ///< boolean for position reached // state->timestamp_lastoutside_orbit = 0;///< timestamp when the MAV was last outside the orbit or had the wrong yaw value // state->timestamp_firstinside_orbit = 0;///< timestamp when the MAV was the first time after a waypoint change inside the orbit and had the correct yaw value - - mission.count = 0; - mission.items = (struct mission_item_s*)malloc(sizeof(struct mission_item_s) * NUM_MISSIONS_SUPPORTED); - if (!mission.items) { - mission.count = 0; - /* XXX reject waypoints if this fails */ - warnx("no free RAM to allocate mission, rejecting any waypoints"); - } } /* @@ -597,13 +597,7 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi mission.current_index = wpc.seq; - /* Initialize mission publication if necessary */ - if (mission_pub < 0) { - mission_pub = orb_advertise(ORB_ID(mission), &mission); - - } else { - orb_publish(ORB_ID(mission), mission_pub, &mission); - } + publish_mission(); mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_ACCEPTED); @@ -703,16 +697,24 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi wpm->current_wp_id = wpr.seq; mavlink_mission_item_t wp; - map_mission_item_to_mavlink_mission_item(&mission.items[wpr.seq], &wp); - if (mission.current_index == wpr.seq) { - wp.current = true; + struct mission_item_s mission_item; + ssize_t len = sizeof(struct mission_item_s); + + if (dm_read(DM_KEY_WAYPOINTS, wpr.seq, &mission_item, len) == len) { + + if (mission.current_index == wpr.seq) { + wp.current = true; + } else { + wp.current = false; + } + + map_mission_item_to_mavlink_mission_item(&mission_item, &wp); + mavlink_wpm_send_waypoint(wpm->current_partner_sysid, wpm->current_partner_compid, &wp); } else { - wp.current = false; + mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_ERROR); } - mavlink_wpm_send_waypoint(wpm->current_partner_sysid, wpm->current_partner_compid, &wp); - } else { // if (verbose) { @@ -815,7 +817,7 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi if (MAVLINK_WPM_VERBOSE) printf("Got MAVLINK_MSG_ID_MISSION_ITEM_COUNT (%u) from %u changing state to MAVLINK_WPM_STATE_GETLIST\n", wpc.count, msg->sysid); -#endif +#endif } if (wpm->current_state == MAVLINK_WPM_STATE_GETLIST) { @@ -837,10 +839,9 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi if (wpc.count > NUM_MISSIONS_SUPPORTED) { warnx("Too many waypoints: %d, supported: %d", wpc.count, NUM_MISSIONS_SUPPORTED); } else { - /* prepare mission topic */ - mission.count = wpc.count; - /* reset current index */ - mission.current_index = -1; + /* set count to 0 while copying */ + mission.count = 0; + publish_mission(); } #ifdef MAVLINK_WPM_NO_PRINTF @@ -962,20 +963,26 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi // mavlink_mission_item_t *newwp = &(wpm->rcv_waypoints[wp.seq]); // memcpy(newwp, &wp, sizeof(mavlink_mission_item_t)); - int ret = map_mavlink_mission_item_to_mission_item(&wp, &mission.items[wp.seq]); + struct mission_item_s mission_item; + + int ret = map_mavlink_mission_item_to_mission_item(&wp, &mission_item); + if (ret != OK) { - - mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, ret); wpm->current_state = MAVLINK_WPM_STATE_IDLE; break; } + size_t len = sizeof(struct mission_item_s); + + if (dm_write(DM_KEY_WAYPOINTS, wp.seq, DM_PERSIST_IN_FLIGHT_RESET, &mission_item, len) != len) { + mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_ERROR); + wpm->current_state = MAVLINK_WPM_STATE_IDLE; + break; + } + if (wp.current) { mission.current_index = wp.seq; - warnx("current is: %d", wp.seq); - } else { - warnx("not current"); } wpm->current_wp_id = wp.seq + 1; @@ -1009,14 +1016,10 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi // } // TODO: update count? - /* Initialize mission publication if necessary */ - if (mission_pub < 0) { - mission_pub = orb_advertise(ORB_ID(mission), &mission); - - } else { - orb_publish(ORB_ID(mission), mission_pub, &mission); - } + mission.count = wpm->current_count; + + publish_mission(); wpm->size = wpm->current_count; @@ -1113,20 +1116,15 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi /* prepare mission topic */ mission.count = 0; - memset(mission.items, 0, sizeof(struct mission_item_s)*NUM_MISSIONS_SUPPORTED); - - /* Initialize mission publication if necessary */ - if (mission_pub < 0) { - mission_pub = orb_advertise(ORB_ID(mission), &mission); + if (dm_clear(DM_KEY_WAYPOINTS) == OK) { + mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_ACCEPTED); } else { - orb_publish(ORB_ID(mission), mission_pub, &mission); + mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_ERROR); } + publish_mission(); - warnx("Mission cleared"); - - mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_ACCEPTED); } else if (wpca.target_system == mavlink_system.sysid /*&& wpca.target_component == mavlink_wpm_comp_id */ && wpm->current_state != MAVLINK_WPM_STATE_IDLE) { // if (verbose) // printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_CLEAR_LIST from %u because i'm doing something else already (state=%i).\n", msg->sysid, wpm->current_state); diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 1c81245e0c..c6aac6af11 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -63,6 +63,7 @@ #include #include #include +#include #include #include #include @@ -141,7 +142,6 @@ private: int _vstatus_sub; /**< vehicle status subscription */ int _params_sub; /**< notification of parameter updates */ int _mission_sub; /**< notification of mission updates */ - int _onboard_mission_sub; /**< notification of onboard mission updates */ int _capabilities_sub; /**< notification of vehicle capabilities updates */ orb_advert_t _triplet_pub; /**< publish position setpoint triplet */ @@ -157,12 +157,8 @@ private: perf_counter_t _loop_perf; /**< loop performance counter */ unsigned _max_mission_item_count; /**< maximum number of mission items supported */ - unsigned _max_onboard_mission_item_count;/**< maximum number of onboard mission items supported */ unsigned _mission_item_count; /** number of mission items copied */ - unsigned _onboard_mission_item_count; /** number of onboard mission items copied */ - struct mission_item_s *_mission_item; /**< storage for mission */ - struct mission_item_s *_onboard_mission_item; /**< storage for onboard mission */ struct fence_s _fence; /**< storage for fence vertices */ bool _fence_valid; /**< flag if fence is valid */ @@ -174,11 +170,9 @@ private: bool _waypoint_yaw_reached; uint64_t _time_first_inside_orbit; bool _mission_item_reached; - bool _onboard_mission_item_reached; navigation_mode_t _mode; unsigned _current_mission_index; - unsigned _current_onboard_mission_index; struct { float min_altitude; @@ -204,11 +198,6 @@ private: */ void mission_update(); - /** - * Retrieve onboard mission. - */ - void onboard_mission_update(); - /** * Shim for calling task_main from task_create. */ @@ -281,7 +270,6 @@ Navigator::Navigator() : _vstatus_sub(-1), _params_sub(-1), _mission_sub(-1), - _onboard_mission_sub(-1), _capabilities_sub(-1), /* publications */ @@ -293,19 +281,15 @@ Navigator::Navigator() : _loop_perf(perf_alloc(PC_ELAPSED, "navigator")), /* states */ _max_mission_item_count(10), - _max_onboard_mission_item_count(10), _mission_item_count(0), - _onboard_mission_item_count(0), _fence_valid(false), _inside_fence(true), _waypoint_position_reached(false), _waypoint_yaw_reached(false), _time_first_inside_orbit(0), _mission_item_reached(false), - _onboard_mission_item_reached(false), _mode(NAVIGATION_MODE_NONE), - _current_mission_index(0), - _current_onboard_mission_index(0) + _current_mission_index(0) { _global_pos.valid = false; memset(&_fence, 0, sizeof(_fence)); @@ -313,9 +297,6 @@ Navigator::Navigator() : _parameter_handles.loiter_radius = param_find("NAV_LOITER_RAD"); _parameter_handles.onboard_mission_enabled = param_find("NAV_ONB_MIS_EN"); - _mission_item = (mission_item_s*)malloc(sizeof(mission_item_s) * _max_mission_item_count); - _onboard_mission_item = (mission_item_s*)malloc(sizeof(mission_item_s) * _max_onboard_mission_item_count); - _mission_item_triplet.previous_valid = false; _mission_item_triplet.current_valid = false; _mission_item_triplet.next_valid = false; @@ -370,131 +351,50 @@ Navigator::mission_update() { struct mission_s mission; if (orb_copy(ORB_ID(mission), _mission_sub, &mission) == OK) { - // XXX this is not optimal yet, but a first prototype / - // test implementation - if (mission.count <= _max_mission_item_count) { - - /* Check if first part of mission (up to _current_mission_index - 1) changed: - * if the first part changed: start again at first waypoint - * if the first part remained unchanged: continue with the (possibly changed second part) - */ - if (mission.current_index == -1 && _current_mission_index < _mission_item_count && _current_mission_index < mission.count) { //check if not finished and if the new mission is not a shorter mission - for (unsigned i = 0; i < _current_mission_index; i++) { - if (!cmp_mission_item_equivalent(_mission_item[i], mission.items[i])) { - /* set flag to restart mission next we're in auto */ - _current_mission_index = 0; - mavlink_log_info(_mavlink_fd, "[navigator] Reset to WP %d", _current_mission_index); - //warnx("First part of mission differs i=%d", i); - break; - } -// else { -// warnx("Mission item is equivalent i=%d", i); -// } - } - } else if (mission.current_index >= 0 && mission.current_index < mission.count) { - /* set flag to restart mission next we're in auto */ - _current_mission_index = mission.current_index; - mavlink_log_info(_mavlink_fd, "[navigator] Reset to WP %d", _current_mission_index); - } else { - _current_mission_index = 0; - mavlink_log_info(_mavlink_fd, "[navigator] Reset to WP %d", _current_mission_index); - } +// /* Check if first part of mission (up to _current_mission_index - 1) changed: +// * if the first part changed: start again at first waypoint +// * if the first part remained unchanged: continue with the (possibly changed second part) +// */ +// if (mission.current_index == -1 && _current_mission_index < _mission_item_count && _current_mission_index < mission.count) { //check if not finished and if the new mission is not a shorter mission +// for (unsigned i = 0; i < _current_mission_index; i++) { +// if (!cmp_mission_item_equivalent(_mission_item[i], mission.items[i])) { +// /* set flag to restart mission next we're in auto */ +// _current_mission_index = 0; +// mavlink_log_info(_mavlink_fd, "[navigator] Reset to WP %d", _current_mission_index); +// //warnx("First part of mission differs i=%d", i); +// break; +// } +// // else { +// // warnx("Mission item is equivalent i=%d", i); +// // } +// } +// } else if (mission.current_index >= 0 && mission.current_index < mission.count) { +// /* set flag to restart mission next we're in auto */ +// _current_mission_index = mission.current_index; +// mavlink_log_info(_mavlink_fd, "[navigator] Reset to WP %d", _current_mission_index); +// } else { +// _current_mission_index = 0; +// mavlink_log_info(_mavlink_fd, "[navigator] Reset to WP %d", _current_mission_index); +// } - /* - * Perform an atomic copy & state update - */ - irqstate_t flags = irqsave(); + _mission_item_count = mission.count; + _current_mission_index = mission.current_index; - memcpy(_mission_item, mission.items, mission.count * sizeof(struct mission_item_s)); - _mission_item_count = mission.count; - - irqrestore(flags); - - - - } else { - warnx("ERROR: too many waypoints, not supported"); - mavlink_log_critical(_mavlink_fd, "[navigator] too many waypoints, not supported"); - _mission_item_count = 0; - } - - if (_mode == NAVIGATION_MODE_WAYPOINT) { - start_waypoint(); - } - - /* TODO add checks if and how the mission has changed */ } else { _mission_item_count = 0; _current_mission_index = 0; } -} - -void -Navigator::onboard_mission_update() -{ - struct mission_s onboard_mission; - if (orb_copy(ORB_ID(onboard_mission), _onboard_mission_sub, &onboard_mission) == OK) { - // XXX this is not optimal yet, but a first prototype / - // test implementation - - if (onboard_mission.count <= _max_onboard_mission_item_count) { - - /* Check if first part of mission (up to _current_onboard_mission_index - 1) changed: - * if the first part changed: start again at first waypoint - * if the first part remained unchanged: continue with the (possibly changed second part) - */ - if (onboard_mission.current_index == -1 && _current_onboard_mission_index < _onboard_mission_item_count && _current_onboard_mission_index < onboard_mission.count) { //check if not finished and if the new mission is not a shorter mission - for (unsigned i = 0; i < _current_onboard_mission_index; i++) { - if (!cmp_mission_item_equivalent(_onboard_mission_item[i], onboard_mission.items[i])) { - /* set flag to restart mission next we're in auto */ - _current_onboard_mission_index = 0; - mavlink_log_info(_mavlink_fd, "[navigator] Reset to onboard WP %d", _current_onboard_mission_index); - //warnx("First part of onboard mission differs i=%d", i); - break; - } -// else { -// warnx("Onboard mission item is equivalent i=%d", i); -// } - } - } else if (onboard_mission.current_index >= 0 && onboard_mission.current_index < onboard_mission.count) { - /* set flag to restart mission next we're in auto */ - _current_onboard_mission_index = onboard_mission.current_index; - mavlink_log_info(_mavlink_fd, "[navigator] Reset to onboard WP %d", _current_onboard_mission_index); - } else { - _current_onboard_mission_index = 0; - mavlink_log_info(_mavlink_fd, "[navigator] Reset to onboard WP %d", _current_onboard_mission_index); - } - - /* - * Perform an atomic copy & state update - */ - irqstate_t flags = irqsave(); - - memcpy(_onboard_mission_item, onboard_mission.items, onboard_mission.count * sizeof(struct mission_item_s)); - _onboard_mission_item_count = onboard_mission.count; - - irqrestore(flags); - - - - } else { - warnx("ERROR: too many waypoints, not supported"); - mavlink_log_critical(_mavlink_fd, "[navigator] too many waypoints, not supported"); - _onboard_mission_item_count = 0; - } - - if (_mode == NAVIGATION_MODE_WAYPOINT) { - start_waypoint(); - } - - /* TODO add checks if and how the mission has changed */ - } else { - _onboard_mission_item_count = 0; - _current_onboard_mission_index = 0; + if (_mission_item_count == 0 && _mode == NAVIGATION_MODE_WAYPOINT) { + set_mode(NAVIGATION_MODE_LOITER); + } + else if (_mode == NAVIGATION_MODE_WAYPOINT) { + start_waypoint(); } } + + void Navigator::task_main_trampoline(int argc, char *argv[]) { @@ -514,7 +414,6 @@ Navigator::task_main() */ _global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); _mission_sub = orb_subscribe(ORB_ID(mission)); - _onboard_mission_sub = orb_subscribe(ORB_ID(onboard_mission)); _capabilities_sub = orb_subscribe(ORB_ID(navigation_capabilities)); _vstatus_sub = orb_subscribe(ORB_ID(vehicle_status)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); @@ -526,7 +425,6 @@ Navigator::task_main() } mission_update(); - onboard_mission_update(); /* rate limit vehicle status updates to 5Hz */ orb_set_interval(_vstatus_sub, 200); @@ -541,7 +439,7 @@ Navigator::task_main() orb_copy(ORB_ID(navigation_capabilities), _capabilities_sub, &_nav_caps); /* wakeup source(s) */ - struct pollfd fds[7]; + struct pollfd fds[6]; /* Setup of loop */ fds[0].fd = _params_sub; @@ -554,10 +452,8 @@ Navigator::task_main() fds[3].events = POLLIN; fds[4].fd = _mission_sub; fds[4].events = POLLIN; - fds[5].fd = _onboard_mission_sub; + fds[5].fd = _vstatus_sub; fds[5].events = POLLIN; - fds[6].fd = _vstatus_sub; - fds[6].events = POLLIN; while (!_task_should_exit) { @@ -579,7 +475,7 @@ Navigator::task_main() perf_begin(_loop_perf); /* only update vehicle status if it changed */ - if (fds[6].revents & POLLIN) { + if (fds[5].revents & POLLIN) { /* read from param to clear updated flag */ orb_copy(ORB_ID(vehicle_status), _vstatus_sub, &_vstatus); @@ -660,10 +556,6 @@ Navigator::task_main() mission_update(); } - if (fds[5].revents & POLLIN) { - onboard_mission_update(); - } - if (fds[2].revents & POLLIN) { orb_copy(ORB_ID(home_position), _home_pos_sub, &_home_pos); } @@ -974,22 +866,28 @@ Navigator::set_mode(navigation_mode_t new_nav_mode) int Navigator::set_waypoint_mission_item(unsigned mission_item_index, struct mission_item_s *new_mission_item) { - if (mission_item_index < _mission_item_count) { - memcpy(new_mission_item, &_mission_item[mission_item_index], sizeof(mission_item_s)); - - if (new_mission_item->nav_cmd == NAV_CMD_RETURN_TO_LAUNCH) { - /* if it is a RTL waypoint, append the home position */ - new_mission_item->lat = (double)_home_pos.lat / 1e7; - new_mission_item->lon = (double)_home_pos.lon / 1e7; - new_mission_item->altitude = (float)_home_pos.alt / 1e3f + _parameters.min_altitude; - new_mission_item->loiter_radius = _parameters.loiter_radius; // TODO: get rid of magic number - new_mission_item->radius = 50.0f; // TODO: get rid of magic number - } - // warnx("added mission item: %d", mission_item_index); - return OK; + if (mission_item_index >= _mission_item_count) { + return ERROR; } - // warnx("could not add mission item: %d", mission_item_index); - return ERROR; + + struct mission_item_s mission_item; + + if (dm_read(DM_KEY_WAYPOINTS, mission_item_index, &mission_item, sizeof(struct mission_item_s)) != sizeof(struct mission_item_s)) { + return ERROR; + } + + memcpy(new_mission_item, &mission_item, sizeof(struct mission_item_s)); + + if (new_mission_item->nav_cmd == NAV_CMD_RETURN_TO_LAUNCH) { + /* if it is a RTL waypoint, append the home position */ + new_mission_item->lat = (double)_home_pos.lat / 1e7; + new_mission_item->lon = (double)_home_pos.lon / 1e7; + new_mission_item->altitude = (float)_home_pos.alt / 1e3f + _parameters.min_altitude; + new_mission_item->loiter_radius = _parameters.loiter_radius; // TODO: get rid of magic number + new_mission_item->radius = 50.0f; // TODO: get rid of magic number + } + + return OK; } void @@ -1036,8 +934,6 @@ Navigator::advance_current_mission_item() return ERROR; } - reset_mission_item_reached(); - /* copy current mission to previous item */ memcpy(&_mission_item_triplet.previous, &_mission_item_triplet.current, sizeof(mission_item_s)); _mission_item_triplet.previous_valid = _mission_item_triplet.current_valid; @@ -1182,14 +1078,11 @@ Navigator::start_waypoint() { reset_mission_item_reached(); - /* this means we should start fresh */ - if (_current_mission_index == 0) { - - _mission_item_triplet.previous_valid = false; - - } else { + if (_current_mission_index > 0) { set_waypoint_mission_item(_current_mission_index - 1, &_mission_item_triplet.previous); _mission_item_triplet.previous_valid = true; + } else { + _mission_item_triplet.previous_valid = false; } set_waypoint_mission_item(_current_mission_index, &_mission_item_triplet.current); diff --git a/src/modules/uORB/topics/mission.h b/src/modules/uORB/topics/mission.h index 2427a1d579..30f06c3597 100644 --- a/src/modules/uORB/topics/mission.h +++ b/src/modules/uORB/topics/mission.h @@ -46,6 +46,8 @@ #include #include "../uORB.h" +#define NUM_MISSIONS_SUPPORTED 256 + /* compatible to mavlink MAV_CMD */ enum NAV_CMD { NAV_CMD_WAYPOINT=16, @@ -59,6 +61,11 @@ enum NAV_CMD { NAV_CMD_PATHPLANNING=81 }; +enum ORIGIN { + ORIGIN_MAVLINK = 0, + ORIGIN_ONBOARD +}; + /** * @addtogroup topics * @{ @@ -84,11 +91,11 @@ struct mission_item_s float time_inside; /**< time that the MAV should stay inside the radius before advancing in seconds */ bool autocontinue; /**< true if next waypoint should follow after this one */ int index; /**< index matching the mavlink waypoint */ + enum ORIGIN origin; /**< where the waypoint has been generated */ }; struct mission_s { - struct mission_item_s *items; unsigned count; int current_index; /**< default -1, start at the one changed latest */ }; @@ -99,6 +106,5 @@ struct mission_s /* register this as object request broker structure */ ORB_DECLARE(mission); -ORB_DECLARE(onboard_mission); #endif From e034f5135ebabeb751ea775f2d79440cf74c8047 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 3 Dec 2013 16:25:46 +0100 Subject: [PATCH 180/447] Dataman: Some minor fixes --- src/modules/dataman/dataman.c | 16 +++++++--------- src/modules/dataman/dataman.h | 5 ++--- src/systemcmds/tests/test_dataman.c | 23 +++++++++++++---------- 3 files changed, 22 insertions(+), 22 deletions(-) diff --git a/src/modules/dataman/dataman.c b/src/modules/dataman/dataman.c index 05b77da20d..dd3573d9a3 100644 --- a/src/modules/dataman/dataman.c +++ b/src/modules/dataman/dataman.c @@ -33,10 +33,8 @@ * ****************************************************************************/ /** - * @file navigator_main.c - * Implementation of the main navigation state machine. - * - * Handles missions, geo fencing and failsafe navigation behavior. + * @file dataman.c + * DATAMANAGER driver. */ #include @@ -113,7 +111,7 @@ static unsigned g_func_counts[dm_number_of_funcs]; static const unsigned g_per_item_max_index[DM_KEY_NUM_KEYS] = { DM_KEY_SAFE_POINTS_MAX, DM_KEY_FENCE_POINTS_MAX, - DM_KEY_WAY_POINTS_MAX, + DM_KEY_WAYPOINTS_MAX, }; /* Table of offset for index 0 of each item type */ @@ -138,7 +136,7 @@ static work_q_t g_work_q; sem_t g_work_queued_sema; sem_t g_init_sema; -static bool g_task_should_exit; /**< if true, sensor task should exit */ +static bool g_task_should_exit; /**< if true, dataman task should exit */ #define DM_SECTOR_HDR_SIZE 4 static const unsigned k_sector_size = DM_MAX_DATA_SIZE + DM_SECTOR_HDR_SIZE; @@ -266,11 +264,11 @@ _write(dm_item_t item, unsigned char index, dm_persitence_t persistence, const v /* Get the offset for this item */ offset = calculate_offset(item, index); - if (offset < 0) + if (offset < 0) return -1; /* Make sure caller has not given us more data than we can handle */ - if (count > DM_MAX_DATA_SIZE) + if (count > DM_MAX_DATA_SIZE) return -1; /* Write out the data, prefixed with length and persistence level */ @@ -456,7 +454,7 @@ dm_write(dm_item_t item, unsigned char index, dm_persitence_t persistence, const return -1; /* Will return with queues locked */ - if ((work = create_work_item()) == NULL) + if ((work = create_work_item()) == NULL) return -1; /* queues unlocked on failure */ work->func = dm_write_func; diff --git a/src/modules/dataman/dataman.h b/src/modules/dataman/dataman.h index 41ddfaf61b..9e1f789ad3 100644 --- a/src/modules/dataman/dataman.h +++ b/src/modules/dataman/dataman.h @@ -41,7 +41,6 @@ #include #include -#include #ifdef __cplusplus extern "C" { @@ -51,7 +50,7 @@ extern "C" { typedef enum { DM_KEY_SAFE_POINTS = 0, /* Safe points coordinates, safe point 0 is home point */ DM_KEY_FENCE_POINTS, /* Fence vertex coordinates */ - DM_KEY_WAY_POINTS, /* Mission way point coordinates */ + DM_KEY_WAYPOINTS, /* Mission way point coordinates */ DM_KEY_NUM_KEYS /* Total number of item types defined */ } dm_item_t; @@ -59,7 +58,7 @@ extern "C" { enum { DM_KEY_SAFE_POINTS_MAX = 8, DM_KEY_FENCE_POINTS_MAX = GEOFENCE_MAX_VERTICES, - DM_KEY_WAY_POINTS_MAX = MAVLINK_WPM_MAX_WP_COUNT, + DM_KEY_WAYPOINTS_MAX = NUM_MISSIONS_SUPPORTED }; /* Data persistence levels */ diff --git a/src/systemcmds/tests/test_dataman.c b/src/systemcmds/tests/test_dataman.c index e33c5acebe..7de87b476f 100644 --- a/src/systemcmds/tests/test_dataman.c +++ b/src/systemcmds/tests/test_dataman.c @@ -83,12 +83,15 @@ task_main(int argc, char *argv[]) srand(hrt_absolute_time() ^ my_id); unsigned hit = 0, miss = 0; wstart = hrt_absolute_time(); - for (unsigned i = 0; i < 256; i++) { + for (unsigned i = 0; i < NUM_MISSIONS_SUPPORTED; i++) { memset(buffer, my_id, sizeof(buffer)); buffer[1] = i; unsigned hash = i ^ my_id; unsigned len = (hash & 63) + 2; - if (dm_write(DM_KEY_WAY_POINTS, hash, DM_PERSIST_IN_FLIGHT_RESET, buffer, len) != len) { + + int ret = dm_write(DM_KEY_WAYPOINTS, hash, DM_PERSIST_IN_FLIGHT_RESET, buffer, len); + warnx("ret: %d", ret); + if (ret != len) { warnx("%d write failed, index %d, length %d", my_id, hash, len); goto fail; } @@ -97,10 +100,10 @@ task_main(int argc, char *argv[]) rstart = hrt_absolute_time(); wend = rstart; - for (unsigned i = 0; i < 256; i++) { + for (unsigned i = 0; i < NUM_MISSIONS_SUPPORTED; i++) { unsigned hash = i ^ my_id; unsigned len2, len = (hash & 63) + 2; - if ((len2 = dm_read(DM_KEY_WAY_POINTS, hash, buffer, sizeof(buffer))) < 2) { + if ((len2 = dm_read(DM_KEY_WAYPOINTS, hash, buffer, sizeof(buffer))) < 2) { warnx("%d read failed length test, index %d", my_id, hash); goto fail; } @@ -120,7 +123,7 @@ task_main(int argc, char *argv[]) } rend = hrt_absolute_time(); warnx("Test %d pass, hit %d, miss %d, io time read %llums. write %llums.", - my_id, hit, miss, (rend - rstart) / 256000, (wend - wstart) / 256000); + my_id, hit, miss, (rend - rstart) / NUM_MISSIONS_SUPPORTED / 1000, (wend - wstart) / NUM_MISSIONS_SUPPORTED / 1000); sem_post(sems + my_id); return 0; fail: @@ -159,18 +162,18 @@ int test_dataman(int argc, char *argv[]) } free(sems); dm_restart(DM_INIT_REASON_IN_FLIGHT); - for (i = 0; i < 256; i++) { - if (dm_read(DM_KEY_WAY_POINTS, i, buffer, sizeof(buffer)) != 0) + for (i = 0; i < NUM_MISSIONS_SUPPORTED; i++) { + if (dm_read(DM_KEY_WAYPOINTS, i, buffer, sizeof(buffer)) != 0) break; } - if (i >= 256) { + if (i >= NUM_MISSIONS_SUPPORTED) { warnx("Restart in-flight failed"); return -1; } dm_restart(DM_INIT_REASON_POWER_ON); - for (i = 0; i < 256; i++) { - if (dm_read(DM_KEY_WAY_POINTS, i, buffer, sizeof(buffer)) != 0) { + for (i = 0; i < NUM_MISSIONS_SUPPORTED; i++) { + if (dm_read(DM_KEY_WAYPOINTS, i, buffer, sizeof(buffer)) != 0) { warnx("Restart power-on failed"); return -1; } From 4d846b480c9118090fe60a887fb1eb0824b38f56 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 5 Dec 2013 16:24:11 +0100 Subject: [PATCH 181/447] fix typo in makefile (resulted from previous merge) --- src/systemcmds/tests/module.mk | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/systemcmds/tests/module.mk b/src/systemcmds/tests/module.mk index cccd1b8ec8..c1f8074eac 100644 --- a/src/systemcmds/tests/module.mk +++ b/src/systemcmds/tests/module.mk @@ -25,9 +25,9 @@ SRCS = test_adc.c \ test_uart_send.c \ test_mixer.cpp \ test_dataman.c \ - tests_file.c \ + test_file.c \ tests_main.c \ - tests_param.c \ + test_param.c \ test_ppm_loopback.c INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink From 278e05e425f6aca75e2d6b43a17945b095176997 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 8 Dec 2013 16:52:41 +0100 Subject: [PATCH 182/447] added simple flight termination state machine which enbales parachute on request --- src/modules/commander/commander.cpp | 19 ++++++- .../commander/state_machine_helper.cpp | 51 +++++++++++++++++++ src/modules/commander/state_machine_helper.h | 4 ++ .../fw_att_control/fw_att_control_main.cpp | 44 ++++++++++++---- .../uORB/topics/vehicle_control_mode.h | 1 + src/modules/uORB/topics/vehicle_status.h | 8 +++ 6 files changed, 117 insertions(+), 10 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index dfd4d2f733..40562a4e1f 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -509,6 +509,21 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe } break; + /* Flight termination */ + case VEHICLE_CMD_DO_SET_SERVO: { //xxx: needs its own mavlink command + + if (armed->armed && cmd->param3 > 0.5) { //xxx: for safety only for now, param3 is unused by VEHICLE_CMD_DO_SET_SERVO + transition_result_t flighttermination_res = flighttermination_state_transition(status, FLIGHTTERMINATION_STATE_ON, control_mode); + result = VEHICLE_CMD_RESULT_ACCEPTED; + + } else { + /* reject parachute depoyment not armed */ + result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + } + + } + break; + default: break; } @@ -1199,6 +1214,7 @@ int commander_thread_main(int argc, char *argv[]) bool arming_state_changed = check_arming_state_changed(); bool main_state_changed = check_main_state_changed(); bool navigation_state_changed = check_navigation_state_changed(); + bool flighttermination_state_changed = check_flighttermination_state_changed(); hrt_abstime t1 = hrt_absolute_time(); @@ -1725,7 +1741,8 @@ void *commander_low_prio_loop(void *arg) /* ignore commands the high-prio loop handles */ if (cmd.command == VEHICLE_CMD_DO_SET_MODE || cmd.command == VEHICLE_CMD_COMPONENT_ARM_DISARM || - cmd.command == VEHICLE_CMD_NAV_TAKEOFF) + cmd.command == VEHICLE_CMD_NAV_TAKEOFF || + cmd.command == VEHICLE_CMD_DO_SET_SERVO) continue; /* only handle low-priority commands here */ diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index a50af7dafe..6c21dfab0c 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -65,6 +65,7 @@ static const int ERROR = -1; static bool arming_state_changed = true; static bool main_state_changed = true; static bool navigation_state_changed = true; +static bool flighttermination_state_changed = true; transition_result_t arming_state_transition(struct vehicle_status_s *status, const struct safety_s *safety, @@ -451,6 +452,18 @@ check_navigation_state_changed() } } +bool +check_flighttermination_state_changed() +{ + if (flighttermination_state_changed) { + flighttermination_state_changed = false; + return true; + + } else { + return false; + } +} + void set_navigation_state_changed() { @@ -523,6 +536,44 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s } +/** +* Transition from one flightermination state to another +*/ +transition_result_t flighttermination_state_transition(struct vehicle_status_s *status, flighttermination_state_t new_flighttermination_state, struct vehicle_control_mode_s *control_mode) +{ + transition_result_t ret = TRANSITION_DENIED; + + /* only check transition if the new state is actually different from the current one */ + if (new_flighttermination_state == status->navigation_state) { + ret = TRANSITION_NOT_CHANGED; + + } else { + + switch (new_flighttermination_state) { + case FLIGHTTERMINATION_STATE_ON: + ret = TRANSITION_CHANGED; + status->flighttermination_state = FLIGHTTERMINATION_STATE_ON; + warnx("state machine helper: change to FLIGHTTERMINATION_STATE_ON"); + break; + case FLIGHTTERMINATION_STATE_OFF: + ret = TRANSITION_CHANGED; + status->flighttermination_state = FLIGHTTERMINATION_STATE_OFF; + break; + + default: + break; + } + + if (ret == TRANSITION_CHANGED) { + flighttermination_state_changed = true; + control_mode->flag_control_flighttermination_enabled = status->flighttermination_state == FLIGHTTERMINATION_STATE_ON; + } + } + + return ret; +} + + // /* // * Wrapper functions (to be used in the commander), all functions assume lock on current_status diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h index 0bfdf36a8f..e1ec9d4adf 100644 --- a/src/modules/commander/state_machine_helper.h +++ b/src/modules/commander/state_machine_helper.h @@ -70,8 +70,12 @@ bool check_main_state_changed(); transition_result_t navigation_state_transition(struct vehicle_status_s *status, navigation_state_t new_navigation_state, struct vehicle_control_mode_s *control_mode); +transition_result_t flighttermination_state_transition(struct vehicle_status_s *status, flighttermination_state_t new_flighttermination_state, struct vehicle_control_mode_s *control_mode); + bool check_navigation_state_changed(); +bool check_flighttermination_state_changed(); + void set_navigation_state_changed(); int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_state, int control_mode_pub, struct vehicle_control_mode_s *current_control_mode, const int mavlink_fd); diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index ff3f133065..bb74534f0f 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -121,6 +121,7 @@ private: orb_advert_t _rate_sp_pub; /**< rate setpoint publication */ orb_advert_t _attitude_sp_pub; /**< attitude setpoint point */ orb_advert_t _actuators_0_pub; /**< actuator control group 0 setpoint */ + orb_advert_t _actuators_1_pub; /**< actuator control group 1 setpoint (Airframe) */ struct vehicle_attitude_s _att; /**< vehicle attitude */ struct accel_report _accel; /**< body frame accelerations */ @@ -128,7 +129,8 @@ private: struct manual_control_setpoint_s _manual; /**< r/c channel data */ struct airspeed_s _airspeed; /**< airspeed */ struct vehicle_control_mode_s _vcontrol_mode; /**< vehicle control mode */ - struct actuator_controls_s _actuators; /**< actuator control inputs */ + struct actuator_controls_s _actuators; /**< actuator control inputs */ + struct actuator_controls_s _actuators_airframe; /**< actuator control inputs */ struct vehicle_global_position_s _global_pos; /**< global position */ perf_counter_t _loop_perf; /**< loop performance counter */ @@ -294,14 +296,15 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : _airspeed_valid(false) { /* safely initialize structs */ - vehicle_attitude_s _att = {0}; - accel_report _accel = {0}; - vehicle_attitude_setpoint_s _att_sp = {0}; - manual_control_setpoint_s _manual = {0}; - airspeed_s _airspeed = {0}; - vehicle_control_mode_s _vcontrol_mode = {0}; - actuator_controls_s _actuators = {0}; - vehicle_global_position_s _global_pos = {0}; + _att = {0}; + _accel = {0}; + _att_sp = {0}; + _manual = {0}; + _airspeed = {0}; + _vcontrol_mode = {0}; + _actuators = {0}; + _actuators_airframe = {0}; + _global_pos = {0}; _parameter_handles.tconst = param_find("FW_ATT_TC"); @@ -625,6 +628,15 @@ FixedwingAttitudeControl::task_main() lock_integrator = true; } + /* Simple handling of failsafe: deploy parachute if failsafe is on */ + if (_vcontrol_mode.flag_control_flighttermination_enabled) { + _actuators_airframe.control[1] = 1.0f; +// warnx("_actuators_airframe.control[1] = 1.0f;"); + } else { + _actuators_airframe.control[1] = -1.0f; +// warnx("_actuators_airframe.control[1] = -1.0f;"); + } + /* decide if in stabilized or full manual control */ if (_vcontrol_mode.flag_control_attitude_enabled) { @@ -794,6 +806,7 @@ FixedwingAttitudeControl::task_main() /* lazily publish the setpoint only once available */ _actuators.timestamp = hrt_absolute_time(); + _actuators_airframe.timestamp = hrt_absolute_time(); if (_actuators_0_pub > 0) { /* publish the attitude setpoint */ @@ -804,6 +817,19 @@ FixedwingAttitudeControl::task_main() _actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators); } + if (_actuators_1_pub > 0) { + /* publish the attitude setpoint */ + orb_publish(ORB_ID(actuator_controls_1), _actuators_1_pub, &_actuators_airframe); + warnx("%.2f %.2f %.2f %.2f %.2f %.2f %.2f %.2f", + (double)_actuators_airframe.control[0], (double)_actuators_airframe.control[1], (double)_actuators_airframe.control[2], + (double)_actuators_airframe.control[3], (double)_actuators_airframe.control[4], (double)_actuators_airframe.control[5], + (double)_actuators_airframe.control[6], (double)_actuators_airframe.control[7]); + + } else { + /* advertise and publish */ + _actuators_1_pub = orb_advertise(ORB_ID(actuator_controls_1), &_actuators_airframe); + } + } perf_end(_loop_perf); diff --git a/src/modules/uORB/topics/vehicle_control_mode.h b/src/modules/uORB/topics/vehicle_control_mode.h index 38fb74c9b9..e26fb97c8a 100644 --- a/src/modules/uORB/topics/vehicle_control_mode.h +++ b/src/modules/uORB/topics/vehicle_control_mode.h @@ -78,6 +78,7 @@ struct vehicle_control_mode_s bool flag_control_position_enabled; /**< true if position is controlled */ bool flag_control_altitude_enabled; /**< true if altitude is controlled */ bool flag_control_climb_rate_enabled; /**< true if climb rate is controlled */ + bool flag_control_flighttermination_enabled; /**< true if flighttermination is enabled */ bool flag_control_auto_enabled; // TEMP uint8_t auto_state; // TEMP navigation state for AUTO modes diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index 1c184d3a7a..1c3763924c 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -95,6 +95,12 @@ typedef enum { HIL_STATE_ON } hil_state_t; + +typedef enum { + FLIGHTTERMINATION_STATE_OFF = 0, + FLIGHTTERMINATION_STATE_ON +} flighttermination_state_t; + typedef enum { MODE_SWITCH_MANUAL = 0, MODE_SWITCH_ASSISTED, @@ -229,6 +235,8 @@ struct vehicle_status_s uint16_t errors_count2; uint16_t errors_count3; uint16_t errors_count4; + + flighttermination_state_t flighttermination_state; }; /** From 8acea79918e20eabb73f6c6bacc5ebd7f30ae577 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 8 Dec 2013 20:14:32 +0100 Subject: [PATCH 183/447] fix small copy paste error in px4io driver --- src/drivers/px4io/px4io.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 702ec1c3ae..f313a98f2b 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -950,9 +950,9 @@ PX4IO::io_set_control_groups() bool attitude_ok = io_set_control_state(0); /* send auxiliary control groups */ - (void)io_set_control_state(0); (void)io_set_control_state(1); (void)io_set_control_state(2); + (void)io_set_control_state(3); return attitude_ok; } From 86aa2f85cb923b7a45d3a0139ae0cf108d0cb002 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 8 Dec 2013 21:34:31 +0100 Subject: [PATCH 184/447] px4iofirmware: in manual mode: ignore control indices which are not controlled by the rmeote control --- src/modules/px4iofirmware/mixer.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index 9bb93ce9f1..87844ca70a 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -248,7 +248,7 @@ mixer_callback(uintptr_t handle, return -1; case MIX_OVERRIDE: - if (r_page_rc_input[PX4IO_P_RC_VALID] & (1 << control_index)) { + if ((r_page_rc_input[PX4IO_P_RC_VALID] & (1 << control_index)) && CONTROL_PAGE_INDEX(control_group, control_index) < PX4IO_RC_INPUT_CHANNELS) { control = REG_TO_FLOAT(r_page_rc_input[PX4IO_P_RC_BASE + control_index]); break; } From edd1f4dbb8e2afd44503edc4e5c8c0ea5a3bdb18 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 9 Dec 2013 09:08:41 +0100 Subject: [PATCH 185/447] set parachute deployed to 0 and handle correct scaling in mixer --- src/modules/fw_att_control/fw_att_control_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index bb74534f0f..4dc000a6a0 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -633,7 +633,7 @@ FixedwingAttitudeControl::task_main() _actuators_airframe.control[1] = 1.0f; // warnx("_actuators_airframe.control[1] = 1.0f;"); } else { - _actuators_airframe.control[1] = -1.0f; + _actuators_airframe.control[1] = 0.0f; // warnx("_actuators_airframe.control[1] = -1.0f;"); } From 2761d47be2792a3e28e92bfd60407a0aaa243106 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 9 Dec 2013 09:53:51 +0100 Subject: [PATCH 186/447] disable printf --- src/modules/fw_att_control/fw_att_control_main.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index 4dc000a6a0..1651cb3993 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -820,10 +820,10 @@ FixedwingAttitudeControl::task_main() if (_actuators_1_pub > 0) { /* publish the attitude setpoint */ orb_publish(ORB_ID(actuator_controls_1), _actuators_1_pub, &_actuators_airframe); - warnx("%.2f %.2f %.2f %.2f %.2f %.2f %.2f %.2f", - (double)_actuators_airframe.control[0], (double)_actuators_airframe.control[1], (double)_actuators_airframe.control[2], - (double)_actuators_airframe.control[3], (double)_actuators_airframe.control[4], (double)_actuators_airframe.control[5], - (double)_actuators_airframe.control[6], (double)_actuators_airframe.control[7]); +// warnx("%.2f %.2f %.2f %.2f %.2f %.2f %.2f %.2f", +// (double)_actuators_airframe.control[0], (double)_actuators_airframe.control[1], (double)_actuators_airframe.control[2], +// (double)_actuators_airframe.control[3], (double)_actuators_airframe.control[4], (double)_actuators_airframe.control[5], +// (double)_actuators_airframe.control[6], (double)_actuators_airframe.control[7]); } else { /* advertise and publish */ From 6f316b352dfd4fffd7513388487c5f9fff0a9c8f Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 13 Dec 2013 21:12:03 +0400 Subject: [PATCH 187/447] multirotor_pos_control rewritten to use rotation matrix instead of euler angles --- .../multirotor_pos_control.c | 180 ++++++++++++++---- 1 file changed, 141 insertions(+), 39 deletions(-) diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c index 834df9655c..a0541d41a3 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c @@ -171,6 +171,45 @@ static float norm(float x, float y) return sqrtf(x * x + y * y); } +static void cross3(float a[3], float b[3], float res[3]) { + res[0] = a[1] * b[2] - a[2] * b[1]; + res[1] = a[2] * b[0] - a[0] * b[2]; + res[2] = a[0] * b[1] - a[1] * b[0]; +} + +static float rt_atan2f_snf(float u0, float u1) +{ + float y; + int32_t b_u0; + int32_t b_u1; + if (isnanf(u0) || isnanf(u1)) { + y = NAN; + } else if (isinff(u0) && isinff(u1)) { + if (u0 > 0.0f) { + b_u0 = 1; + } else { + b_u0 = -1; + } + if (u1 > 0.0f) { + b_u1 = 1; + } else { + b_u1 = -1; + } + y = atan2f((float)b_u0, (float)b_u1); + } else if (u1 == 0.0f) { + if (u0 > 0.0f) { + y = M_PI_F / 2.0f; + } else if (u0 < 0.0f) { + y = -(M_PI_F / 2.0f); + } else { + y = 0.0F; + } + } else { + y = atan2f(u0, u1); + } + return y; +} + static int multirotor_pos_control_thread_main(int argc, char *argv[]) { /* welcome user */ @@ -236,6 +275,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) PID_t xy_vel_pids[2]; PID_t z_pos_pid; thrust_pid_t z_vel_pid; + float thrust_int[3] = { 0.0f, 0.0f, 0.0f }; thread_running = true; @@ -475,7 +515,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) } /* update yaw setpoint only if value is valid */ - if (isfinite(global_pos_sp.yaw) && fabsf(global_pos_sp.yaw) < M_TWOPI) { + if (isfinite(global_pos_sp.yaw) && fabsf(global_pos_sp.yaw) < M_TWOPI_F) { att_sp.yaw_body = global_pos_sp.yaw; } @@ -575,12 +615,14 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) global_vel_sp.vx = pid_calculate(&xy_pos_pids[0], local_pos_sp.x, local_pos.x, local_pos.vx - sp_move_rate[0], dt) + sp_move_rate[0]; global_vel_sp.vy = pid_calculate(&xy_pos_pids[1], local_pos_sp.y, local_pos.y, local_pos.vy - sp_move_rate[1], dt) + sp_move_rate[1]; - /* limit horizontal speed */ - float xy_vel_sp_norm = norm(global_vel_sp.vx, global_vel_sp.vy) / params.xy_vel_max; + if (!control_mode.flag_control_manual_enabled) { + /* limit horizontal speed only in AUTO mode */ + float xy_vel_sp_norm = norm(global_vel_sp.vx, global_vel_sp.vy) / params.xy_vel_max; - if (xy_vel_sp_norm > 1.0f) { - global_vel_sp.vx /= xy_vel_sp_norm; - global_vel_sp.vy /= xy_vel_sp_norm; + if (xy_vel_sp_norm > 1.0f) { + global_vel_sp.vx /= xy_vel_sp_norm; + global_vel_sp.vy /= xy_vel_sp_norm; + } } } else { @@ -594,9 +636,8 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) // TODO subscribe to velocity setpoint if altitude/position control disabled if (control_mode.flag_control_climb_rate_enabled || control_mode.flag_control_velocity_enabled) { - /* run velocity controllers, calculate thrust vector with attitude-thrust compensation */ + /* calculate desired thrust vector in NED frame */ float thrust_sp[3] = { 0.0f, 0.0f, 0.0f }; - if (control_mode.flag_control_climb_rate_enabled) { if (reset_int_z) { reset_int_z = false; @@ -613,51 +654,112 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) } } - thrust_pid_set_integral(&z_vel_pid, -i); + thrust_int[2] = -i; mavlink_log_info(mavlink_fd, "[mpc] reset hovering thrust: %.2f", (double)i); } - - thrust_sp[2] = thrust_pid_calculate(&z_vel_pid, global_vel_sp.vz, local_pos.vz, dt, att.R[2][2]); - att_sp.thrust = -thrust_sp[2]; - + thrust_int[2] += (global_vel_sp.vz - local_pos.vz) * params.z_vel_i * dt; + thrust_sp[2] = (global_vel_sp.vz - local_pos.vz) * params.z_vel_p + thrust_int[2]; + if (-thrust_sp[2] < params.thr_min) + thrust_sp[2] = -params.thr_min; } else { - /* reset thrust integral when altitude control enabled */ reset_int_z = true; } - if (control_mode.flag_control_velocity_enabled) { - /* calculate thrust set point in NED frame */ if (reset_int_xy) { reset_int_xy = false; - pid_reset_integral(&xy_vel_pids[0]); - pid_reset_integral(&xy_vel_pids[1]); + thrust_int[0] = 0.0f; + thrust_int[1] = 0.0f; mavlink_log_info(mavlink_fd, "[mpc] reset pos integral"); } - - thrust_sp[0] = pid_calculate(&xy_vel_pids[0], global_vel_sp.vx, local_pos.vx, 0.0f, dt); - thrust_sp[1] = pid_calculate(&xy_vel_pids[1], global_vel_sp.vy, local_pos.vy, 0.0f, dt); - - /* thrust_vector now contains desired acceleration (but not in m/s^2) in NED frame */ - /* limit horizontal part of thrust */ - float thrust_xy_dir = atan2f(thrust_sp[1], thrust_sp[0]); - /* assuming that vertical component of thrust is g, - * horizontal component = g * tan(alpha) */ - float tilt = atanf(norm(thrust_sp[0], thrust_sp[1])); - - if (tilt > params.tilt_max) { - tilt = params.tilt_max; - } - - /* convert direction to body frame */ - thrust_xy_dir -= att.yaw; - /* calculate roll and pitch */ - att_sp.roll_body = sinf(thrust_xy_dir) * tilt; - att_sp.pitch_body = -cosf(thrust_xy_dir) * tilt / cosf(att_sp.roll_body); - + thrust_int[0] += (global_vel_sp.vx - local_pos.vx) * params.xy_vel_i * dt; + thrust_int[1] += (global_vel_sp.vy - local_pos.vy) * params.xy_vel_i * dt; + thrust_sp[0] = (global_vel_sp.vx - local_pos.vx) * params.xy_vel_p + thrust_int[0]; + thrust_sp[1] = (global_vel_sp.vy - local_pos.vy) * params.xy_vel_p + thrust_int[1]; } else { reset_int_xy = true; } + float thrust_abs = sqrtf(thrust_sp[0] * thrust_sp[0] + thrust_sp[1] * thrust_sp[1] + thrust_sp[2] * thrust_sp[2]); + if (thrust_abs > params.thr_max) { + if (thrust_sp[2] < 0.0f) { + if (-thrust_sp[2] > params.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; + } 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_abs = sqrtf(thrust_sp[0] * thrust_sp[0] + thrust_sp[1] * thrust_sp[1]); + float k = thrust_xy_max / thrust_xy_abs; + thrust_sp[0] *= k; + thrust_sp[1] *= k; + } + + } else { + /* Z component is negative, going down, simply limit thrust vector */ + float k = params.thr_max / thrust_abs; + thrust_sp[0] *= k; + thrust_sp[1] *= k; + thrust_sp[2] *= k; + } + thrust_abs = params.thr_max; + } + + /* calculate attitude and thrust from thrust vector */ + if (control_mode.flag_control_velocity_enabled) { + /* vector of desired yaw direction in XY plane, rotated by PI/2 */ + float y_C[3]; + y_C[0] = -sinf(att_sp.yaw_body); + y_C[1] = cosf(att_sp.yaw_body); + y_C[2] = 0; + + /* desired body_z axis = -normalize(thrust_vector) */ + float body_x[3]; + float body_y[3]; + float body_z[3]; + if (thrust_abs > 0.0f) { + body_z[0] = -thrust_sp[0] / thrust_abs; + body_z[1] = -thrust_sp[1] / thrust_abs; + body_z[2] = -thrust_sp[2] / thrust_abs; + } else { + body_z[0] = 0.0f; + body_z[1] = 0.0f; + body_z[2] = -1.0f; + } + /* desired body_x axis = cross(x_C, body_z) */ + cross3(y_C, body_z, body_x); + + /* desired body_y axis = cross(body_z, body_x) */ + cross3(body_z, body_x, body_y); + + /* fill rotation matrix */ + for (int i = 0; i < 3; i++) { + att_sp.R_body[i][0] = body_x[i]; + att_sp.R_body[i][1] = body_y[i]; + att_sp.R_body[i][2] = body_z[i]; + } + att_sp.R_valid = true; + + /* calculate roll, pitch from rotation matrix, yaw already used to construct rot matrix */ + att_sp.roll_body = rt_atan2f_snf(att_sp.R_body[2][1], att_sp.R_body[2][2]); + att_sp.pitch_body = -asinf(att_sp.R_body[2][0]); + //att_sp.yaw_body = rt_atan2f_snf(att_sp.R_body[1][0], att_sp.R_body[0][0]); + + } else { + /* thrust compensation for altitude only control mode */ + float att_comp; + if (att.R[2][2] > 0.8f) + att_comp = 1.0f / att.R[2][2]; + else if (att.R[2][2] > 0.0f) + att_comp = ((1.0f / 0.8f - 1.0f) / 0.8f) * att.R[2][2] + 1.0f; + else + att_comp = 1.0f; + thrust_abs *= att_comp; + } + + att_sp.thrust = thrust_abs; + att_sp.timestamp = hrt_absolute_time(); /* publish new attitude setpoint */ From 2fb493e639bb78d862529062dedb79b97c96a769 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 11 Dec 2013 16:38:11 +0100 Subject: [PATCH 188/447] px4io frimware: improve handling of manual mode when fmu is still healthy, use data from fmu for channels which are not controlled by rc --- src/modules/px4iofirmware/mixer.cpp | 27 ++++++++++++++++++++++++--- 1 file changed, 24 insertions(+), 3 deletions(-) diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index 87844ca70a..fdf01e09cb 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -77,7 +77,8 @@ enum mixer_source { MIX_NONE, MIX_FMU, MIX_OVERRIDE, - MIX_FAILSAFE + MIX_FAILSAFE, + MIX_OVERRIDE_FMU_OK }; static mixer_source source; @@ -135,10 +136,19 @@ mixer_tick(void) if ( (r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) && (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) && (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) && - !(r_setup_arming & PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED)) { + !(r_setup_arming & PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED) && + !(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK)) { /* if allowed, mix from RC inputs directly */ source = MIX_OVERRIDE; + } else if ( (r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) && + (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) && + (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) && + !(r_setup_arming & PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED) && + (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK)) { + + /* if allowed, mix from RC inputs directly up to available rc channels */ + source = MIX_OVERRIDE_FMU_OK; } } @@ -241,7 +251,7 @@ mixer_callback(uintptr_t handle, switch (source) { case MIX_FMU: - if (control_index < PX4IO_CONTROL_CHANNELS) { + if (control_index < PX4IO_CONTROL_CHANNELS && control_group < PX4IO_CONTROL_GROUPS ) { control = REG_TO_FLOAT(r_page_controls[CONTROL_PAGE_INDEX(control_group, control_index)]); break; } @@ -254,6 +264,17 @@ mixer_callback(uintptr_t handle, } return -1; + case MIX_OVERRIDE_FMU_OK: + /* FMU is ok but we are in override mode, use direct rc control for the available rc channels. The remaining channels are still controlled by the fmu */ + if ((r_page_rc_input[PX4IO_P_RC_VALID] & (1 << control_index)) && CONTROL_PAGE_INDEX(control_group, control_index) < PX4IO_RC_INPUT_CHANNELS) { + control = REG_TO_FLOAT(r_page_rc_input[PX4IO_P_RC_BASE + control_index]); + break; + } else if (control_index < PX4IO_CONTROL_CHANNELS && control_group < PX4IO_CONTROL_GROUPS) { + control = REG_TO_FLOAT(r_page_controls[CONTROL_PAGE_INDEX(control_group, control_index)]); + break; + } + return -1; + case MIX_FAILSAFE: case MIX_NONE: control = 0.0f; From 4ab7ac67a5a7ab66e5a6d452630691e3fbefc478 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 11 Dec 2013 16:57:19 +0100 Subject: [PATCH 189/447] px4iofirmware: improve check for rc controlled channels in manual mode --- src/modules/px4iofirmware/mixer.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index fdf01e09cb..9fc86db5e7 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -258,7 +258,7 @@ mixer_callback(uintptr_t handle, return -1; case MIX_OVERRIDE: - if ((r_page_rc_input[PX4IO_P_RC_VALID] & (1 << control_index)) && CONTROL_PAGE_INDEX(control_group, control_index) < PX4IO_RC_INPUT_CHANNELS) { + if (r_page_rc_input[PX4IO_P_RC_VALID] & (1 << CONTROL_PAGE_INDEX(control_group, control_index))) { control = REG_TO_FLOAT(r_page_rc_input[PX4IO_P_RC_BASE + control_index]); break; } @@ -266,7 +266,7 @@ mixer_callback(uintptr_t handle, case MIX_OVERRIDE_FMU_OK: /* FMU is ok but we are in override mode, use direct rc control for the available rc channels. The remaining channels are still controlled by the fmu */ - if ((r_page_rc_input[PX4IO_P_RC_VALID] & (1 << control_index)) && CONTROL_PAGE_INDEX(control_group, control_index) < PX4IO_RC_INPUT_CHANNELS) { + if (r_page_rc_input[PX4IO_P_RC_VALID] & (1 << CONTROL_PAGE_INDEX(control_group, control_index))) { control = REG_TO_FLOAT(r_page_rc_input[PX4IO_P_RC_BASE + control_index]); break; } else if (control_index < PX4IO_CONTROL_CHANNELS && control_group < PX4IO_CONTROL_GROUPS) { From 367d5d0cf28d6c857fdcd6c4ad20809f9e52e310 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 14 Dec 2013 11:02:16 +0100 Subject: [PATCH 190/447] fix wrong usage of navigation state in flighttermination state machine --- src/modules/commander/state_machine_helper.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 6c21dfab0c..ca3ec94b88 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -544,7 +544,7 @@ transition_result_t flighttermination_state_transition(struct vehicle_status_s * transition_result_t ret = TRANSITION_DENIED; /* only check transition if the new state is actually different from the current one */ - if (new_flighttermination_state == status->navigation_state) { + if (new_flighttermination_state == status->flighttermination_state) { ret = TRANSITION_NOT_CHANGED; } else { From 23d0c6f8dd1a0b9aa64dafe26cfd56e43637c5ee Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 14 Dec 2013 11:03:02 +0100 Subject: [PATCH 191/447] temporary workaround to trigger failsafe with remote control --- src/modules/commander/commander.cpp | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 40562a4e1f..9ed9051fa0 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1189,6 +1189,16 @@ int commander_thread_main(int argc, char *argv[]) } } + /* Flight termination in manual mode if assisted switch is on easy position //xxx hack! */ + if (armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.assisted_switch > STICK_ON_OFF_LIMIT) { + transition_result_t flighttermination_res = flighttermination_state_transition(&status, FLIGHTTERMINATION_STATE_ON, &control_mode); + if (flighttermination_res == TRANSITION_CHANGED) { + tune_positive(); + } + } else { + flighttermination_state_transition(&status, FLIGHTTERMINATION_STATE_OFF, &control_mode); + } + /* handle commands last, as the system needs to be updated to handle them */ orb_check(cmd_sub, &updated); From faa3826de6c47c5516df7b4c01b70c96f9e9d9d4 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 15 Dec 2013 11:47:26 +0400 Subject: [PATCH 192/447] multirotor_pos_control: fixes and improvements --- .../multirotor_pos_control.c | 198 ++++++++++++------ 1 file changed, 139 insertions(+), 59 deletions(-) diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c index a0541d41a3..cf2d5f931f 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c @@ -171,42 +171,67 @@ static float norm(float x, float y) return sqrtf(x * x + y * y); } -static void cross3(float a[3], float b[3], float res[3]) { +static void cross3(float a[3], float b[3], float res[3]) +{ res[0] = a[1] * b[2] - a[2] * b[1]; res[1] = a[2] * b[0] - a[0] * b[2]; res[2] = a[0] * b[1] - a[1] * b[0]; } +static float normalize3(float x[3]) +{ + float n = sqrtf(x[0] * x[0] + x[1] * x[1] + x[2] * x[2]); + + if (n > 0.0f) { + x[0] /= n; + x[1] /= n; + x[2] /= n; + } + + return n; +} + static float rt_atan2f_snf(float u0, float u1) { float y; int32_t b_u0; int32_t b_u1; + if (isnanf(u0) || isnanf(u1)) { y = NAN; + } else if (isinff(u0) && isinff(u1)) { if (u0 > 0.0f) { b_u0 = 1; + } else { b_u0 = -1; } + if (u1 > 0.0f) { b_u1 = 1; + } else { b_u1 = -1; } + y = atan2f((float)b_u0, (float)b_u1); + } else if (u1 == 0.0f) { if (u0 > 0.0f) { y = M_PI_F / 2.0f; + } else if (u0 < 0.0f) { y = -(M_PI_F / 2.0f); + } else { y = 0.0F; } + } else { y = atan2f(u0, u1); } + return y; } @@ -272,9 +297,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) uint64_t local_ref_timestamp = 0; PID_t xy_pos_pids[2]; - PID_t xy_vel_pids[2]; PID_t z_pos_pid; - thrust_pid_t z_vel_pid; float thrust_int[3] = { 0.0f, 0.0f, 0.0f }; thread_running = true; @@ -286,11 +309,9 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) for (int i = 0; i < 2; i++) { pid_init(&(xy_pos_pids[i]), PID_MODE_DERIVATIV_SET, 0.02f); - pid_init(&(xy_vel_pids[i]), PID_MODE_DERIVATIV_CALC_NO_SP, 0.02f); } pid_init(&z_pos_pid, PID_MODE_DERIVATIV_SET, 0.02f); - thrust_pid_init(&z_vel_pid, 0.02f); bool param_updated = true; @@ -307,23 +328,11 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) /* update params */ parameters_update(¶ms_h, ¶ms); - /* integral_limit * ki = tilt_max / 2 */ - float i_limit; - - if (params.xy_vel_i > 0.0f) { - i_limit = params.tilt_max / params.xy_vel_i / 2.0f; - - } else { - i_limit = 0.0f; // not used - } - for (int i = 0; i < 2; i++) { pid_set_parameters(&(xy_pos_pids[i]), params.xy_p, 0.0f, params.xy_d, 0.0f, 0.0f); - pid_set_parameters(&(xy_vel_pids[i]), params.xy_vel_p, params.xy_vel_i, params.xy_vel_d, i_limit, params.tilt_max); } pid_set_parameters(&z_pos_pid, params.z_p, 0.0f, params.z_d, 1.0f, params.z_vel_max); - thrust_pid_set_parameters(&z_vel_pid, params.z_vel_p, params.z_vel_i, params.z_vel_d, -params.thr_max, -params.thr_min); } bool updated; @@ -367,7 +376,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) t_prev = t; - if (control_mode.flag_control_altitude_enabled || control_mode.flag_control_velocity_enabled || control_mode.flag_control_position_enabled) { + if (control_mode.flag_control_altitude_enabled || control_mode.flag_control_position_enabled || control_mode.flag_control_climb_rate_enabled || control_mode.flag_control_velocity_enabled) { orb_copy(ORB_ID(manual_control_setpoint), manual_sub, &manual); orb_copy(ORB_ID(vehicle_attitude), att_sub, &att); orb_copy(ORB_ID(vehicle_attitude_setpoint), att_sp_sub, &att_sp); @@ -420,8 +429,6 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) reset_man_sp_xy = false; local_pos_sp.x = local_pos.x; local_pos_sp.y = local_pos.y; - pid_reset_integral(&xy_vel_pids[0]); - pid_reset_integral(&xy_vel_pids[1]); mavlink_log_info(mavlink_fd, "[mpc] reset pos sp: %.2f, %.2f", (double)local_pos_sp.x, (double)local_pos_sp.y); } @@ -638,48 +645,67 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) if (control_mode.flag_control_climb_rate_enabled || control_mode.flag_control_velocity_enabled) { /* calculate desired thrust vector in NED frame */ float thrust_sp[3] = { 0.0f, 0.0f, 0.0f }; - if (control_mode.flag_control_climb_rate_enabled) { - if (reset_int_z) { - reset_int_z = false; - float i = params.thr_min; - if (reset_int_z_manual) { - i = manual.throttle; + if (reset_int_z) { + reset_int_z = false; + float i = params.thr_min; - if (i < params.thr_min) { - i = params.thr_min; + if (reset_int_z_manual) { + i = manual.throttle; - } else if (i > params.thr_max) { - i = params.thr_max; - } + if (i < params.thr_min) { + i = params.thr_min; + + } else if (i > params.thr_max) { + i = params.thr_max; } - - thrust_int[2] = -i; - mavlink_log_info(mavlink_fd, "[mpc] reset hovering thrust: %.2f", (double)i); } - thrust_int[2] += (global_vel_sp.vz - local_pos.vz) * params.z_vel_i * dt; - thrust_sp[2] = (global_vel_sp.vz - local_pos.vz) * params.z_vel_p + thrust_int[2]; - if (-thrust_sp[2] < params.thr_min) - thrust_sp[2] = -params.thr_min; - } else { - reset_int_z = true; + + thrust_int[2] = -i; + mavlink_log_info(mavlink_fd, "[mpc] reset hovering thrust: %.2f", (double)i); } + + thrust_sp[2] = (global_vel_sp.vz - local_pos.vz) * params.z_vel_p + thrust_int[2]; + if (control_mode.flag_control_velocity_enabled) { if (reset_int_xy) { reset_int_xy = false; thrust_int[0] = 0.0f; thrust_int[1] = 0.0f; - mavlink_log_info(mavlink_fd, "[mpc] reset pos integral"); + mavlink_log_info(mavlink_fd, "[mpc] reset xy vel integral"); } - thrust_int[0] += (global_vel_sp.vx - local_pos.vx) * params.xy_vel_i * dt; - thrust_int[1] += (global_vel_sp.vy - local_pos.vy) * params.xy_vel_i * dt; + thrust_sp[0] = (global_vel_sp.vx - local_pos.vx) * params.xy_vel_p + thrust_int[0]; thrust_sp[1] = (global_vel_sp.vy - local_pos.vy) * params.xy_vel_p + thrust_int[1]; + } else { reset_int_xy = true; } + /* limit thrust vector and check for saturation */ + bool saturation_xy = false; + bool saturation_z = false; + + /* limit min lift */ + if (-thrust_sp[2] < params.thr_min) { + thrust_sp[2] = -params.thr_min; + saturation_z = true; + } + + /* limit max tilt */ + float tilt = atan2f(norm(thrust_sp[0], thrust_sp[1]), -thrust_sp[2]); + + if (tilt > params.tilt_max && params.thr_min > 0.0f) { + /* crop horizontal component */ + float k = tanf(params.tilt_max) / tanf(tilt); + thrust_sp[0] *= k; + thrust_sp[1] *= k; + saturation_xy = true; + } + + /* limit max thrust */ float thrust_abs = sqrtf(thrust_sp[0] * thrust_sp[0] + thrust_sp[1] * thrust_sp[1] + thrust_sp[2] * thrust_sp[2]); + if (thrust_abs > params.thr_max) { if (thrust_sp[2] < 0.0f) { if (-thrust_sp[2] > params.thr_max) { @@ -687,13 +713,17 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) thrust_sp[0] = 0.0f; thrust_sp[1] = 0.0f; thrust_sp[2] = -params.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_abs = sqrtf(thrust_sp[0] * thrust_sp[0] + thrust_sp[1] * thrust_sp[1]); + float thrust_xy_abs = norm(thrust_sp[0], thrust_sp[1]); float k = thrust_xy_max / thrust_xy_abs; thrust_sp[0] *= k; thrust_sp[1] *= k; + saturation_xy = true; } } else { @@ -702,59 +732,106 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) thrust_sp[0] *= k; thrust_sp[1] *= k; thrust_sp[2] *= k; + saturation_xy = true; + saturation_z = true; } + thrust_abs = params.thr_max; } + /* update integrals */ + if (control_mode.flag_control_velocity_enabled && !saturation_xy) { + thrust_int[0] += (global_vel_sp.vx - local_pos.vx) * params.xy_vel_i * dt; + thrust_int[1] += (global_vel_sp.vy - local_pos.vy) * params.xy_vel_i * dt; + } + + if (control_mode.flag_control_climb_rate_enabled && !saturation_z) { + thrust_int[2] += (global_vel_sp.vz - local_pos.vz) * params.z_vel_i * dt; + } + /* calculate attitude and thrust from thrust vector */ if (control_mode.flag_control_velocity_enabled) { + /* desired body_z axis = -normalize(thrust_vector) */ + float body_x[3]; + float body_y[3]; + float body_z[3]; + + if (thrust_abs > 0.0f) { + body_z[0] = -thrust_sp[0] / thrust_abs; + body_z[1] = -thrust_sp[1] / thrust_abs; + body_z[2] = -thrust_sp[2] / thrust_abs; + + } else { + body_z[0] = 0.0f; + body_z[1] = 0.0f; + body_z[2] = -1.0f; + } + /* vector of desired yaw direction in XY plane, rotated by PI/2 */ float y_C[3]; y_C[0] = -sinf(att_sp.yaw_body); y_C[1] = cosf(att_sp.yaw_body); y_C[2] = 0; - /* desired body_z axis = -normalize(thrust_vector) */ - float body_x[3]; - float body_y[3]; - float body_z[3]; - if (thrust_abs > 0.0f) { - body_z[0] = -thrust_sp[0] / thrust_abs; - body_z[1] = -thrust_sp[1] / thrust_abs; - body_z[2] = -thrust_sp[2] / thrust_abs; - } else { - body_z[0] = 0.0f; - body_z[1] = 0.0f; - body_z[2] = -1.0f; - } - /* desired body_x axis = cross(x_C, body_z) */ + /* desired body_x axis = cross(y_C, body_z) */ cross3(y_C, body_z, body_x); + float body_x_norm = normalize3(body_x); + static const float body_x_norm_max = 0.5f; /* desired body_y axis = cross(body_z, body_x) */ cross3(body_z, body_x, body_y); + if (body_x_norm < body_x_norm_max) { + /* roll is close to +/- PI/2, don't try to hold yaw exactly */ + float x_C[3]; + x_C[0] = cos(att_sp.yaw_body); + x_C[1] = sinf(att_sp.yaw_body); + x_C[2] = 0; + + float body_y_1[3]; + /* desired body_y axis for approximate yaw = cross(body_z, x_C) */ + cross3(body_z, x_C, body_y_1); + + float w = body_x_norm / body_x_norm_max; + float w1 = 1.0f - w; + + /* mix two body_y axes */ + body_y[0] = body_y[0] * w + body_y_1[0] * w1; + body_y[1] = body_y[1] * w + body_y_1[1] * w1; + body_y[2] = body_y[2] * w + body_y_1[2] * w1; + + normalize3(body_y); + + /* desired body_x axis = cross(body_y, body_z) */ + cross3(body_y, body_z, body_x); + } + /* fill rotation matrix */ for (int i = 0; i < 3; i++) { att_sp.R_body[i][0] = body_x[i]; att_sp.R_body[i][1] = body_y[i]; att_sp.R_body[i][2] = body_z[i]; } + att_sp.R_valid = true; - /* calculate roll, pitch from rotation matrix, yaw already used to construct rot matrix */ + /* calculate roll, pitch from rotation matrix */ att_sp.roll_body = rt_atan2f_snf(att_sp.R_body[2][1], att_sp.R_body[2][2]); att_sp.pitch_body = -asinf(att_sp.R_body[2][0]); + /* yaw already used to construct rot matrix, but actual rotation matrix can have different yaw near singularity */ //att_sp.yaw_body = rt_atan2f_snf(att_sp.R_body[1][0], att_sp.R_body[0][0]); } else { /* thrust compensation for altitude only control mode */ float att_comp; + if (att.R[2][2] > 0.8f) att_comp = 1.0f / att.R[2][2]; else if (att.R[2][2] > 0.0f) att_comp = ((1.0f / 0.8f - 1.0f) / 0.8f) * att.R[2][2] + 1.0f; else att_comp = 1.0f; + thrust_abs *= att_comp; } @@ -764,6 +841,9 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) /* publish new attitude setpoint */ orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); + + } else { + reset_int_z = true; } } else { From f5c24c6e71bc918ac1b92df88f4ba8f3cdecd57a Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 15 Dec 2013 12:34:56 +0400 Subject: [PATCH 193/447] pid library fix --- src/modules/systemlib/pid/pid.c | 25 +++++++++++++++---------- 1 file changed, 15 insertions(+), 10 deletions(-) diff --git a/src/modules/systemlib/pid/pid.c b/src/modules/systemlib/pid/pid.c index 0bec724951..6a4e9392a2 100644 --- a/src/modules/systemlib/pid/pid.c +++ b/src/modules/systemlib/pid/pid.c @@ -143,17 +143,22 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo /* calculate PD output */ float output = (error * pid->kp) + (d * pid->kd); - /* check for saturation */ - if (isfinite(i)) { - if ((pid->output_limit < SIGMA || (fabsf(output + (i * pid->ki)) <= pid->output_limit)) && - fabsf(i) <= pid->integral_limit) { - /* not saturated, use new integral value */ - pid->integral = i; - } - } + if (pid->ki > SIGMA) { + // Calculate the error integral and check for saturation + i = pid->integral + (error * dt); - /* add I component to output */ - output += pid->integral * pid-> ki; + /* check for saturation */ + if (isfinite(i)) { + if ((pid->output_limit < SIGMA || (fabsf(output + (i * pid->ki)) <= pid->output_limit)) && + fabsf(i) <= pid->integral_limit) { + /* not saturated, use new integral value */ + pid->integral = i; + } + } + + /* add I component to output */ + output += pid->integral * pid->ki; + } /* limit output */ if (isfinite(output)) { From badf146e192d2341df1a76a31798a4ce6900e538 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 15 Dec 2013 16:24:45 +0400 Subject: [PATCH 194/447] mc_att_control_vector: independent thrust vector and attitude control --- .../mc_att_control_vector_main.cpp | 45 +++++++++++++++++-- 1 file changed, 42 insertions(+), 3 deletions(-) diff --git a/src/modules/mc_att_control_vector/mc_att_control_vector_main.cpp b/src/modules/mc_att_control_vector/mc_att_control_vector_main.cpp index 834189a546..93806165d2 100644 --- a/src/modules/mc_att_control_vector/mc_att_control_vector_main.cpp +++ b/src/modules/mc_att_control_vector/mc_att_control_vector_main.cpp @@ -539,9 +539,48 @@ MulticopterAttitudeControl::task_main() /* current body angular rates */ math::Vector3 rates(_att.rollspeed, _att.pitchspeed, _att.yawspeed); - /* orientation error between R and R_des */ - math::Matrix e_R_m = (R_des.transpose() * R - R.transpose() * R_des).transpose() * 0.5f; - math::Vector3 e_R(-e_R_m(1, 2), e_R_m(0, 2), -e_R_m(0, 1)); + /* try to move thrust vector shortest way, because yaw response is slower than roll/pitch */ + /* angle error between current and desired thrust vectors (Z axes) */ + math::Vector3 R_z(R(0, 2), R(1, 2), R(2, 2)); + math::Vector3 R_z_des(R_des(0, 2), R_des(1, 2), R_des(2, 2)); + math::Vector3 e_R = R_z.cross(R_z_des); + + /* convert angle error from NED to bady frame */ + e_R = R.transpose() * e_R; + + /* e_R has zero Z component, set it according to yaw */ + /* calculate rotation matrix after roll/pitch only rotation */ + math::Matrix R_rp(3, 3); + float e_R_angle = e_R.norm(); + if (e_R_angle > 0.0f) { + /* get axis-angle representation */ + math::Vector3 e_R_axis = e_R / e_R_angle; + /* cross product matrix for e_R_axis */ + math::Matrix e_R_cp(3, 3); + e_R_cp.setAll(0.0f); + e_R_cp(0, 1) = -e_R_axis(2); + e_R_cp(0, 2) = e_R_axis(1); + e_R_cp(1, 0) = e_R_axis(2); + e_R_cp(1, 2) = -e_R_axis(0); + e_R_cp(2, 0) = -e_R_axis(1); + e_R_cp(2, 1) = e_R_axis(0); + /* rotation matrix for roll/pitch only rotation */ + math::Matrix I(3, 3); + I.setAll(0.0f); + I(0, 0) = 1.0f; + I(1, 1) = 1.0f; + I(2, 2) = 1.0f; + R_rp = R * (I + e_R_cp * sinf(e_R_angle) + e_R_cp * e_R_cp * (1.0f - cosf(e_R_angle))); + } else { + /* zero roll/pitch rotation */ + R_rp = R; + } + + /* R_rp and R_des has the same Z axis, calculate yaw error */ + math::Vector3 R_des_z(R_des(0, 2), R_des(1, 2), R_des(2, 2)); + math::Vector3 R_des_x(R_des(0, 0), R_des(1, 0), R_des(2, 0)); + math::Vector3 R_rp_x(R_rp(0, 0), R_rp(1, 0), R_rp(2, 0)); + e_R(2) = R_rp_x.cross(R_des_x) * R_des_z; /* angular rates setpoint*/ math::Vector3 rates_sp = _K * e_R; From 86d5f0808d0146efc2442f651de224956a2818cc Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 15 Dec 2013 20:42:47 +0400 Subject: [PATCH 195/447] mc_att_control_vector: fixes --- .../mc_att_control_vector_main.cpp | 23 ++++++++++++------- 1 file changed, 15 insertions(+), 8 deletions(-) diff --git a/src/modules/mc_att_control_vector/mc_att_control_vector_main.cpp b/src/modules/mc_att_control_vector/mc_att_control_vector_main.cpp index 93806165d2..bc75885c2d 100644 --- a/src/modules/mc_att_control_vector/mc_att_control_vector_main.cpp +++ b/src/modules/mc_att_control_vector/mc_att_control_vector_main.cpp @@ -540,21 +540,24 @@ MulticopterAttitudeControl::task_main() math::Vector3 rates(_att.rollspeed, _att.pitchspeed, _att.yawspeed); /* try to move thrust vector shortest way, because yaw response is slower than roll/pitch */ - /* angle error between current and desired thrust vectors (Z axes) */ + /* sin(angle error) between current and desired thrust vectors (Z axes) */ math::Vector3 R_z(R(0, 2), R(1, 2), R(2, 2)); math::Vector3 R_z_des(R_des(0, 2), R_des(1, 2), R_des(2, 2)); - math::Vector3 e_R = R_z.cross(R_z_des); + math::Vector3 e_R = R.transpose() * R_z.cross(R_z_des); - /* convert angle error from NED to bady frame */ - e_R = R.transpose() * e_R; + /* calculate angle error */ + float e_R_sin = e_R.norm(); + float e_R_cos = R_z * R_z_des; + float angle = atan2f(e_R_sin, e_R_cos); /* e_R has zero Z component, set it according to yaw */ /* calculate rotation matrix after roll/pitch only rotation */ math::Matrix R_rp(3, 3); - float e_R_angle = e_R.norm(); - if (e_R_angle > 0.0f) { + + if (e_R_sin > 0.0f) { /* get axis-angle representation */ - math::Vector3 e_R_axis = e_R / e_R_angle; + math::Vector3 e_R_axis = e_R / e_R_sin; + /* cross product matrix for e_R_axis */ math::Matrix e_R_cp(3, 3); e_R_cp.setAll(0.0f); @@ -564,13 +567,14 @@ MulticopterAttitudeControl::task_main() e_R_cp(1, 2) = -e_R_axis(0); e_R_cp(2, 0) = -e_R_axis(1); e_R_cp(2, 1) = e_R_axis(0); + /* rotation matrix for roll/pitch only rotation */ math::Matrix I(3, 3); I.setAll(0.0f); I(0, 0) = 1.0f; I(1, 1) = 1.0f; I(2, 2) = 1.0f; - R_rp = R * (I + e_R_cp * sinf(e_R_angle) + e_R_cp * e_R_cp * (1.0f - cosf(e_R_angle))); + R_rp = R * (I + e_R_cp * e_R_sin + e_R_cp * e_R_cp * (1.0f - e_R_cos)); } else { /* zero roll/pitch rotation */ R_rp = R; @@ -582,6 +586,9 @@ MulticopterAttitudeControl::task_main() math::Vector3 R_rp_x(R_rp(0, 0), R_rp(1, 0), R_rp(2, 0)); e_R(2) = R_rp_x.cross(R_des_x) * R_des_z; + /* don't try to control yaw when thrust vector has opposite direction to desired */ + e_R(2) *= (e_R_cos > 0.0f ? 1.0f : e_R_sin); + /* angular rates setpoint*/ math::Vector3 rates_sp = _K * e_R; /* feed forward yaw setpoint rate */ From 69c4f6f5e45c054a22ed1a36b323d1b9bd7561d3 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 15 Dec 2013 20:43:21 +0400 Subject: [PATCH 196/447] mc_att_control_vector: code style fixed --- .../mc_att_control_vector/mc_att_control_vector_main.cpp | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/src/modules/mc_att_control_vector/mc_att_control_vector_main.cpp b/src/modules/mc_att_control_vector/mc_att_control_vector_main.cpp index bc75885c2d..45dd639da9 100644 --- a/src/modules/mc_att_control_vector/mc_att_control_vector_main.cpp +++ b/src/modules/mc_att_control_vector/mc_att_control_vector_main.cpp @@ -459,7 +459,7 @@ MulticopterAttitudeControl::task_main() if (_att_sp.thrust < 0.1f) { // TODO //if (_status.condition_landed) { - /* reset yaw setpoint if on ground */ + /* reset yaw setpoint if on ground */ // reset_yaw_sp = true; //} } else { @@ -486,6 +486,7 @@ MulticopterAttitudeControl::task_main() } _att_sp_valid = true; + } else { /* manual rate inputs (ACRO) */ // TODO @@ -512,17 +513,20 @@ MulticopterAttitudeControl::task_main() /* controller uses rotation matrix, not euler angles, convert if necessary */ math::EulerAngles euler_sp(_att_sp.roll_body, _att_sp.pitch_body, _att_sp.yaw_body); math::Dcm R_sp(euler_sp); + for (int i = 0; i < 3; i++) { for (int j = 0; j < 3; j++) { _att_sp.R_body[i][j] = R_sp(i, j); } } + _att_sp.R_valid = true; } if (publish_att_sp) { /* publish the attitude setpoint */ _att_sp.timestamp = hrt_absolute_time(); + if (_att_sp_pub > 0) { orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &_att_sp); @@ -575,6 +579,7 @@ MulticopterAttitudeControl::task_main() I(1, 1) = 1.0f; I(2, 2) = 1.0f; R_rp = R * (I + e_R_cp * e_R_sin + e_R_cp * e_R_cp * (1.0f - e_R_cos)); + } else { /* zero roll/pitch rotation */ R_rp = R; From 72aa171ef982deef1519cc21129024a96d9633db Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 15 Dec 2013 22:52:05 +0400 Subject: [PATCH 197/447] mc_att_control_vector: attitude rate D component implemented --- .../mc_att_control_vector_main.cpp | 36 +++++++++++++++---- 1 file changed, 29 insertions(+), 7 deletions(-) diff --git a/src/modules/mc_att_control_vector/mc_att_control_vector_main.cpp b/src/modules/mc_att_control_vector/mc_att_control_vector_main.cpp index 45dd639da9..230fa15e40 100644 --- a/src/modules/mc_att_control_vector/mc_att_control_vector_main.cpp +++ b/src/modules/mc_att_control_vector/mc_att_control_vector_main.cpp @@ -136,13 +136,18 @@ private: bool _att_sp_valid; /**< flag if the attitude setpoint is valid */ math::Matrix _K; /**< diagonal gain matrix for position error */ - math::Matrix _K_rate; /**< diagonal gain matrix for angular rate error */ + math::Matrix _K_rate_p; /**< diagonal gain matrix for angular rate error */ + math::Matrix _K_rate_d; /**< diagonal gain matrix for angular rate derivative */ + + math::Vector3 _rates_prev; /**< angular rates on previous step */ struct { param_t att_p; param_t att_rate_p; + param_t att_rate_d; param_t yaw_p; param_t yaw_rate_p; + param_t yaw_rate_d; } _parameter_handles; /**< handles for interesting parameters */ /** @@ -224,7 +229,10 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : /* gain matrices */ _K(3, 3), - _K_rate(3, 3) + _K_rate_p(3, 3), + _K_rate_d(3, 3), + + _rates_prev(0.0f, 0.0f, 0.0f) { memset(&_att, 0, sizeof(_att)); @@ -235,8 +243,10 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : _parameter_handles.att_p = param_find("MC_ATT_P"); _parameter_handles.att_rate_p = param_find("MC_ATTRATE_P"); + _parameter_handles.att_rate_d = param_find("MC_ATTRATE_D"); _parameter_handles.yaw_p = param_find("MC_YAWPOS_P"); _parameter_handles.yaw_rate_p = param_find("MC_YAWRATE_P"); + _parameter_handles.yaw_rate_d = param_find("MC_YAWRATE_D"); /* fetch initial parameter values */ parameters_update(); @@ -271,22 +281,32 @@ MulticopterAttitudeControl::parameters_update() { float att_p; float att_rate_p; + float att_rate_d; float yaw_p; float yaw_rate_p; + float yaw_rate_d; param_get(_parameter_handles.att_p, &att_p); param_get(_parameter_handles.att_rate_p, &att_rate_p); + param_get(_parameter_handles.att_rate_d, &att_rate_d); param_get(_parameter_handles.yaw_p, &yaw_p); param_get(_parameter_handles.yaw_rate_p, &yaw_rate_p); + param_get(_parameter_handles.yaw_rate_d, &yaw_rate_d); _K.setAll(0.0f); _K(0, 0) = att_p; _K(1, 1) = att_p; _K(2, 2) = yaw_p; - _K_rate.setAll(0.0f); - _K_rate(0, 0) = att_rate_p; - _K_rate(1, 1) = att_rate_p; - _K_rate(2, 2) = yaw_rate_p; + + _K_rate_p.setAll(0.0f); + _K_rate_p(0, 0) = att_rate_p; + _K_rate_p(1, 1) = att_rate_p; + _K_rate_p(2, 2) = yaw_rate_p; + + _K_rate_d.setAll(0.0f); + _K_rate_d(0, 0) = att_rate_d; + _K_rate_d(1, 1) = att_rate_d; + _K_rate_d(2, 2) = yaw_rate_d; return OK; } @@ -596,9 +616,11 @@ MulticopterAttitudeControl::task_main() /* angular rates setpoint*/ math::Vector3 rates_sp = _K * e_R; + /* feed forward yaw setpoint rate */ rates_sp(2) += yaw_sp_move_rate; - math::Vector3 control = _K_rate * (rates_sp - rates); + math::Vector3 control = _K_rate_p * (rates_sp - rates) + _K_rate_d * (_rates_prev - rates) / fmaxf(dt, 0.003f); + _rates_prev = rates; /* publish the attitude rates setpoint */ _rates_sp.roll = rates_sp(0); From 373888b16d4efda40e36bceac87755114d5590c7 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 16 Dec 2013 12:53:38 +0400 Subject: [PATCH 198/447] multirotor_pos_control: default parameters updated --- .../multirotor_pos_control_params.c | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control_params.c b/src/modules/multirotor_pos_control/multirotor_pos_control_params.c index b7041e4d57..cfb5dc0d47 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control_params.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control_params.c @@ -45,14 +45,14 @@ PARAM_DEFINE_FLOAT(MPC_THR_MIN, 0.2f); PARAM_DEFINE_FLOAT(MPC_THR_MAX, 0.8f); PARAM_DEFINE_FLOAT(MPC_Z_P, 1.0f); PARAM_DEFINE_FLOAT(MPC_Z_D, 0.0f); -PARAM_DEFINE_FLOAT(MPC_Z_VEL_P, 0.1f); -PARAM_DEFINE_FLOAT(MPC_Z_VEL_I, 0.0f); +PARAM_DEFINE_FLOAT(MPC_Z_VEL_P, 0.2f); +PARAM_DEFINE_FLOAT(MPC_Z_VEL_I, 0.1f); PARAM_DEFINE_FLOAT(MPC_Z_VEL_D, 0.0f); PARAM_DEFINE_FLOAT(MPC_Z_VEL_MAX, 3.0f); PARAM_DEFINE_FLOAT(MPC_XY_P, 0.5f); PARAM_DEFINE_FLOAT(MPC_XY_D, 0.0f); -PARAM_DEFINE_FLOAT(MPC_XY_VEL_P, 0.2f); -PARAM_DEFINE_FLOAT(MPC_XY_VEL_I, 0.0f); +PARAM_DEFINE_FLOAT(MPC_XY_VEL_P, 0.1f); +PARAM_DEFINE_FLOAT(MPC_XY_VEL_I, 0.05f); PARAM_DEFINE_FLOAT(MPC_XY_VEL_D, 0.0f); PARAM_DEFINE_FLOAT(MPC_XY_VEL_MAX, 5.0f); PARAM_DEFINE_FLOAT(MPC_TILT_MAX, 0.5f); From 084287132acf3409900ce0629cf5db13785ae538 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 16 Dec 2013 12:56:27 +0400 Subject: [PATCH 199/447] HIL rc scripts fixed --- ROMFS/px4fmu_common/init.d/1001_rc_quad.hil | 16 ++++------------ ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil | 16 ++++------------ 2 files changed, 8 insertions(+), 24 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/1001_rc_quad.hil b/ROMFS/px4fmu_common/init.d/1001_rc_quad.hil index 9b664d63ed..6dd7d460b8 100644 --- a/ROMFS/px4fmu_common/init.d/1001_rc_quad.hil +++ b/ROMFS/px4fmu_common/init.d/1001_rc_quad.hil @@ -60,16 +60,8 @@ hil mode_pwm # param set MAV_TYPE 2 -# -# Check if we got an IO -# -if px4io start -then - echo "IO started" -else - fmu mode_serial - echo "FMU started" -fi +fmu mode_serial +echo "FMU started" # # Start the sensors (depends on orb, px4io) @@ -79,7 +71,7 @@ sh /etc/init.d/rc.sensors # # Start the attitude estimator (depends on orb) # -att_pos_estimator_ekf start +attitude_estimator_ekf start # # Load mixer and start controllers (depends on px4io) @@ -94,7 +86,7 @@ position_estimator_inav start # # Start attitude control # -multirotor_att_control start +mc_att_control_vector start # # Start position control diff --git a/ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil b/ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil index 0cc07ad343..c295ede1e5 100644 --- a/ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil +++ b/ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil @@ -60,16 +60,8 @@ hil mode_pwm # param set MAV_TYPE 2 -# -# Check if we got an IO -# -if px4io start -then - echo "IO started" -else - fmu mode_serial - echo "FMU started" -fi +fmu mode_serial +echo "FMU started" # # Start the sensors (depends on orb, px4io) @@ -79,7 +71,7 @@ sh /etc/init.d/rc.sensors # # Start the attitude estimator (depends on orb) # -att_pos_estimator_ekf start +attitude_estimator_ekf start # # Load mixer and start controllers (depends on px4io) @@ -94,7 +86,7 @@ position_estimator_inav start # # Start attitude control # -multirotor_att_control start +mc_att_control_vector start # # Start position control From e8df08f13905c2a71d71469ba7d6cecc23c2eb70 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 4 Dec 2013 10:37:51 +0100 Subject: [PATCH 200/447] Dataman: Also reserve space for onboard missions --- src/modules/dataman/dataman.c | 1 + src/modules/dataman/dataman.h | 6 ++++-- 2 files changed, 5 insertions(+), 2 deletions(-) diff --git a/src/modules/dataman/dataman.c b/src/modules/dataman/dataman.c index dd3573d9a3..acd612d9e9 100644 --- a/src/modules/dataman/dataman.c +++ b/src/modules/dataman/dataman.c @@ -112,6 +112,7 @@ static const unsigned g_per_item_max_index[DM_KEY_NUM_KEYS] = { DM_KEY_SAFE_POINTS_MAX, DM_KEY_FENCE_POINTS_MAX, DM_KEY_WAYPOINTS_MAX, + DM_KEY_WAYPOINTS_ONBOARD_MAX }; /* Table of offset for index 0 of each item type */ diff --git a/src/modules/dataman/dataman.h b/src/modules/dataman/dataman.h index 9e1f789ad3..dab96eb9b1 100644 --- a/src/modules/dataman/dataman.h +++ b/src/modules/dataman/dataman.h @@ -50,7 +50,8 @@ extern "C" { typedef enum { DM_KEY_SAFE_POINTS = 0, /* Safe points coordinates, safe point 0 is home point */ DM_KEY_FENCE_POINTS, /* Fence vertex coordinates */ - DM_KEY_WAYPOINTS, /* Mission way point coordinates */ + DM_KEY_WAYPOINTS, /* Mission way point coordinates sent over mavlink */ + DM_KEY_WAYPOINTS_ONBOARD, /* Mission way point coordinates generated onboard */ DM_KEY_NUM_KEYS /* Total number of item types defined */ } dm_item_t; @@ -58,7 +59,8 @@ extern "C" { enum { DM_KEY_SAFE_POINTS_MAX = 8, DM_KEY_FENCE_POINTS_MAX = GEOFENCE_MAX_VERTICES, - DM_KEY_WAYPOINTS_MAX = NUM_MISSIONS_SUPPORTED + DM_KEY_WAYPOINTS_MAX = NUM_MISSIONS_SUPPORTED, + DM_KEY_WAYPOINTS_ONBOARD_MAX = NUM_MISSIONS_SUPPORTED }; /* Data persistence levels */ From bed40c962e94aaa9b1f398a201ef88096a35810a Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 4 Dec 2013 10:38:56 +0100 Subject: [PATCH 201/447] Navigator: handle onboard and mavlink missions --- src/modules/navigator/navigator_main.cpp | 174 +++++++++++++++-------- src/modules/uORB/topics/mission.h | 3 +- 2 files changed, 114 insertions(+), 63 deletions(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index c6aac6af11..e2e2949e25 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -142,6 +142,7 @@ private: int _vstatus_sub; /**< vehicle status subscription */ int _params_sub; /**< notification of parameter updates */ int _mission_sub; /**< notification of mission updates */ + int _onboard_mission_sub; /**< notification of onboard mission updates */ int _capabilities_sub; /**< notification of vehicle capabilities updates */ orb_advert_t _triplet_pub; /**< publish position setpoint triplet */ @@ -155,11 +156,9 @@ private: struct mission_result_s _mission_result; /**< mission result for commander/mavlink */ perf_counter_t _loop_perf; /**< loop performance counter */ - - unsigned _max_mission_item_count; /**< maximum number of mission items supported */ unsigned _mission_item_count; /** number of mission items copied */ - + unsigned _onboard_mission_item_count; /** number of onboard mission items copied */ struct fence_s _fence; /**< storage for fence vertices */ bool _fence_valid; /**< flag if fence is valid */ bool _inside_fence; /**< vehicle is inside fence */ @@ -173,6 +172,7 @@ private: navigation_mode_t _mode; unsigned _current_mission_index; + unsigned _current_onboard_mission_index; struct { float min_altitude; @@ -198,6 +198,11 @@ private: */ void mission_update(); + /** + * Retrieve onboard mission. + */ + void onboard_mission_update(); + /** * Shim for calling task_main from task_create. */ @@ -216,7 +221,11 @@ private: void set_mode(navigation_mode_t new_nav_mode); - int set_waypoint_mission_item(unsigned mission_item_index, struct mission_item_s *new_mission_item); + bool mission_possible(); + + bool onboard_mission_possible(); + + int set_waypoint_mission_item(dm_item_t dm_item, unsigned mission_item_index, struct mission_item_s *new_mission_item); void publish_mission_item_triplet(); @@ -270,6 +279,7 @@ Navigator::Navigator() : _vstatus_sub(-1), _params_sub(-1), _mission_sub(-1), + _onboard_mission_sub(-1), _capabilities_sub(-1), /* publications */ @@ -280,8 +290,8 @@ Navigator::Navigator() : /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "navigator")), /* states */ - _max_mission_item_count(10), _mission_item_count(0), + _onboard_mission_item_count(0), _fence_valid(false), _inside_fence(true), _waypoint_position_reached(false), @@ -289,7 +299,8 @@ Navigator::Navigator() : _time_first_inside_orbit(0), _mission_item_reached(false), _mode(NAVIGATION_MODE_NONE), - _current_mission_index(0) + _current_mission_index(0), + _current_onboard_mission_index(0) { _global_pos.valid = false; memset(&_fence, 0, sizeof(_fence)); @@ -352,32 +363,6 @@ Navigator::mission_update() struct mission_s mission; if (orb_copy(ORB_ID(mission), _mission_sub, &mission) == OK) { -// /* Check if first part of mission (up to _current_mission_index - 1) changed: -// * if the first part changed: start again at first waypoint -// * if the first part remained unchanged: continue with the (possibly changed second part) -// */ -// if (mission.current_index == -1 && _current_mission_index < _mission_item_count && _current_mission_index < mission.count) { //check if not finished and if the new mission is not a shorter mission -// for (unsigned i = 0; i < _current_mission_index; i++) { -// if (!cmp_mission_item_equivalent(_mission_item[i], mission.items[i])) { -// /* set flag to restart mission next we're in auto */ -// _current_mission_index = 0; -// mavlink_log_info(_mavlink_fd, "[navigator] Reset to WP %d", _current_mission_index); -// //warnx("First part of mission differs i=%d", i); -// break; -// } -// // else { -// // warnx("Mission item is equivalent i=%d", i); -// // } -// } -// } else if (mission.current_index >= 0 && mission.current_index < mission.count) { -// /* set flag to restart mission next we're in auto */ -// _current_mission_index = mission.current_index; -// mavlink_log_info(_mavlink_fd, "[navigator] Reset to WP %d", _current_mission_index); -// } else { -// _current_mission_index = 0; -// mavlink_log_info(_mavlink_fd, "[navigator] Reset to WP %d", _current_mission_index); -// } - _mission_item_count = mission.count; _current_mission_index = mission.current_index; @@ -385,7 +370,7 @@ Navigator::mission_update() _mission_item_count = 0; _current_mission_index = 0; } - if (_mission_item_count == 0 && _mode == NAVIGATION_MODE_WAYPOINT) { + if ((!mission_possible() || !onboard_mission_possible()) && _mode == NAVIGATION_MODE_WAYPOINT) { set_mode(NAVIGATION_MODE_LOITER); } else if (_mode == NAVIGATION_MODE_WAYPOINT) { @@ -393,7 +378,27 @@ Navigator::mission_update() } } +void +Navigator::onboard_mission_update() +{ + struct mission_s onboard_mission; + if (orb_copy(ORB_ID(onboard_mission), _onboard_mission_sub, &onboard_mission) == OK) { + _onboard_mission_item_count = onboard_mission.count; + _current_onboard_mission_index = onboard_mission.current_index; + + } else { + _onboard_mission_item_count = 0; + _current_onboard_mission_index = 0; + } + + if ((!mission_possible() || !onboard_mission_possible()) && _mode == NAVIGATION_MODE_WAYPOINT) { + set_mode(NAVIGATION_MODE_LOITER); + } + else if (_mode == NAVIGATION_MODE_WAYPOINT) { + start_waypoint(); + } +} void Navigator::task_main_trampoline(int argc, char *argv[]) @@ -414,6 +419,7 @@ Navigator::task_main() */ _global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); _mission_sub = orb_subscribe(ORB_ID(mission)); + _onboard_mission_sub = orb_subscribe(ORB_ID(onboard_mission)); _capabilities_sub = orb_subscribe(ORB_ID(navigation_capabilities)); _vstatus_sub = orb_subscribe(ORB_ID(vehicle_status)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); @@ -425,6 +431,7 @@ Navigator::task_main() } mission_update(); + onboard_mission_update(); /* rate limit vehicle status updates to 5Hz */ orb_set_interval(_vstatus_sub, 200); @@ -439,7 +446,7 @@ Navigator::task_main() orb_copy(ORB_ID(navigation_capabilities), _capabilities_sub, &_nav_caps); /* wakeup source(s) */ - struct pollfd fds[6]; + struct pollfd fds[7]; /* Setup of loop */ fds[0].fd = _params_sub; @@ -452,8 +459,10 @@ Navigator::task_main() fds[3].events = POLLIN; fds[4].fd = _mission_sub; fds[4].events = POLLIN; - fds[5].fd = _vstatus_sub; + fds[5].fd = _onboard_mission_sub; fds[5].events = POLLIN; + fds[6].fd = _vstatus_sub; + fds[6].events = POLLIN; while (!_task_should_exit) { @@ -475,7 +484,7 @@ Navigator::task_main() perf_begin(_loop_perf); /* only update vehicle status if it changed */ - if (fds[5].revents & POLLIN) { + if (fds[6].revents & POLLIN) { /* read from param to clear updated flag */ orb_copy(ORB_ID(vehicle_status), _vstatus_sub, &_vstatus); @@ -505,8 +514,8 @@ Navigator::task_main() case NAVIGATION_STATE_AUTO_MISSION: - if (_mission_item_count > 0 && !(_current_mission_index >= _mission_item_count)) { - /* Start mission if there is a mission available and the last waypoint has not been reached */ + if (mission_possible() || onboard_mission_possible()) { + /* Start mission or onboard mission if available */ set_mode(NAVIGATION_MODE_WAYPOINT); } else { /* else fallback to loiter */ @@ -556,6 +565,10 @@ Navigator::task_main() mission_update(); } + if (fds[5].revents & POLLIN) { + onboard_mission_update(); + } + if (fds[2].revents & POLLIN) { orb_copy(ORB_ID(home_position), _home_pos_sub, &_home_pos); } @@ -586,9 +599,15 @@ Navigator::task_main() if (_mission_item_reached) { - report_mission_reached(); + + + if (onboard_mission_possible()) { + mavlink_log_info(_mavlink_fd, "[navigator] reached onboard WP %d", _current_onboard_mission_index); + } else { + mavlink_log_info(_mavlink_fd, "[navigator] reached WP %d", _current_mission_index); + report_mission_reached(); + } - mavlink_log_info(_mavlink_fd, "[navigator] reached WP %d", _current_mission_index); if (advance_current_mission_item() != OK) { set_mode(NAVIGATION_MODE_LOITER_WAYPOINT); } @@ -863,16 +882,27 @@ Navigator::set_mode(navigation_mode_t new_nav_mode) } } -int -Navigator::set_waypoint_mission_item(unsigned mission_item_index, struct mission_item_s *new_mission_item) +bool +Navigator::mission_possible() +{ + return _mission_item_count > 0 && + !(_current_mission_index >= _mission_item_count); +} + +bool +Navigator::onboard_mission_possible() +{ + return _onboard_mission_item_count > 0 && + !(_current_onboard_mission_index >= _onboard_mission_item_count) && + _parameters.onboard_mission_enabled; +} + +int +Navigator::set_waypoint_mission_item(dm_item_t dm_item, unsigned mission_item_index, struct mission_item_s *new_mission_item) { - if (mission_item_index >= _mission_item_count) { - return ERROR; - } - struct mission_item_s mission_item; - - if (dm_read(DM_KEY_WAYPOINTS, mission_item_index, &mission_item, sizeof(struct mission_item_s)) != sizeof(struct mission_item_s)) { + + if (dm_read(dm_item, mission_item_index, &mission_item, sizeof(struct mission_item_s)) != sizeof(struct mission_item_s)) { return ERROR; } @@ -926,8 +956,7 @@ Navigator::advance_current_mission_item() // warnx("advancing from %d to %d", _current_mission_index, _current_mission_index+1); /* ultimately this index will be == _mission_item_count and this flags the mission as completed */ - _current_mission_index++; - + /* if there is no more mission available, don't advance and return */ if (!_mission_item_triplet.next_valid) { // warnx("no next available"); @@ -941,9 +970,20 @@ Navigator::advance_current_mission_item() /* copy the next to current */ memcpy(&_mission_item_triplet.current, &_mission_item_triplet.next, sizeof(mission_item_s)); _mission_item_triplet.current_valid = _mission_item_triplet.next_valid; + + int ret = ERROR; + + if (onboard_mission_possible()) { + _current_onboard_mission_index++; + ret = set_waypoint_mission_item(DM_KEY_WAYPOINTS_ONBOARD, _current_onboard_mission_index + 1, &_mission_item_triplet.next); + } else if (mission_possible()) { + _current_mission_index++; + ret = set_waypoint_mission_item(DM_KEY_WAYPOINTS, _current_mission_index + 1, &_mission_item_triplet.next); + } else { + warnx("Error: nothing to advance"); + } - - if(set_waypoint_mission_item(_current_mission_index + 1, &_mission_item_triplet.next) == OK) { + if(ret == OK) { _mission_item_triplet.next_valid = true; } else { @@ -1078,17 +1118,27 @@ Navigator::start_waypoint() { reset_mission_item_reached(); - if (_current_mission_index > 0) { - set_waypoint_mission_item(_current_mission_index - 1, &_mission_item_triplet.previous); - _mission_item_triplet.previous_valid = true; - } else { - _mission_item_triplet.previous_valid = false; - } + // if (_current_mission_index > 0) { + // set_waypoint_mission_item(_current_mission_index - 1, &_mission_item_triplet.previous); + // _mission_item_triplet.previous_valid = true; + // } else { + // _mission_item_triplet.previous_valid = false; + // } + _mission_item_triplet.previous_valid = false; - set_waypoint_mission_item(_current_mission_index, &_mission_item_triplet.current); - _mission_item_triplet.current_valid = true; + int ret = ERROR; - mavlink_log_info(_mavlink_fd, "[navigator] heading to WP %d", _current_mission_index); + if (onboard_mission_possible()) { + set_waypoint_mission_item(DM_KEY_WAYPOINTS_ONBOARD, _current_onboard_mission_index, &_mission_item_triplet.current); + mavlink_log_info(_mavlink_fd, "[navigator] heading to onboard WP %d", _current_onboard_mission_index); + ret = set_waypoint_mission_item(DM_KEY_WAYPOINTS_ONBOARD, _current_onboard_mission_index + 1, &_mission_item_triplet.next); + } else if (mission_possible()) { + set_waypoint_mission_item(DM_KEY_WAYPOINTS, _current_mission_index, &_mission_item_triplet.current); + mavlink_log_info(_mavlink_fd, "[navigator] heading to WP %d", _current_mission_index); + ret = set_waypoint_mission_item(DM_KEY_WAYPOINTS, _current_mission_index + 1, &_mission_item_triplet.next); + } + + _mission_item_triplet.current_valid = true; // if (_mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_UNLIMITED) { @@ -1100,7 +1150,7 @@ Navigator::start_waypoint() // _mission_item_triplet.next_valid = true; // } - if(set_waypoint_mission_item(_current_mission_index + 1, &_mission_item_triplet.next) == OK) { + if(ret == OK) { _mission_item_triplet.next_valid = true; } else { diff --git a/src/modules/uORB/topics/mission.h b/src/modules/uORB/topics/mission.h index 30f06c3597..3702420076 100644 --- a/src/modules/uORB/topics/mission.h +++ b/src/modules/uORB/topics/mission.h @@ -96,7 +96,7 @@ struct mission_item_s struct mission_s { - unsigned count; + unsigned count; /**< count of the missions stored in the datamanager */ int current_index; /**< default -1, start at the one changed latest */ }; @@ -106,5 +106,6 @@ struct mission_s /* register this as object request broker structure */ ORB_DECLARE(mission); +ORB_DECLARE(onboard_mission); #endif From 6a624abd7c8fe9cf32b5f5a91bceeb93f730a0ec Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sat, 14 Dec 2013 20:54:25 +0100 Subject: [PATCH 202/447] Datamanager: Rename mavlink/offboard key --- src/modules/dataman/dataman.c | 2 +- src/modules/dataman/dataman.h | 4 ++-- src/modules/mavlink/waypoints.c | 6 +++--- src/systemcmds/tests/test_dataman.c | 8 ++++---- 4 files changed, 10 insertions(+), 10 deletions(-) diff --git a/src/modules/dataman/dataman.c b/src/modules/dataman/dataman.c index acd612d9e9..874a47be7f 100644 --- a/src/modules/dataman/dataman.c +++ b/src/modules/dataman/dataman.c @@ -111,7 +111,7 @@ static unsigned g_func_counts[dm_number_of_funcs]; static const unsigned g_per_item_max_index[DM_KEY_NUM_KEYS] = { DM_KEY_SAFE_POINTS_MAX, DM_KEY_FENCE_POINTS_MAX, - DM_KEY_WAYPOINTS_MAX, + DM_KEY_WAYPOINTS_OFFBOARD_MAX, DM_KEY_WAYPOINTS_ONBOARD_MAX }; diff --git a/src/modules/dataman/dataman.h b/src/modules/dataman/dataman.h index dab96eb9b1..2a781405aa 100644 --- a/src/modules/dataman/dataman.h +++ b/src/modules/dataman/dataman.h @@ -50,7 +50,7 @@ extern "C" { typedef enum { DM_KEY_SAFE_POINTS = 0, /* Safe points coordinates, safe point 0 is home point */ DM_KEY_FENCE_POINTS, /* Fence vertex coordinates */ - DM_KEY_WAYPOINTS, /* Mission way point coordinates sent over mavlink */ + DM_KEY_WAYPOINTS_OFFBOARD, /* Mission way point coordinates sent over mavlink */ DM_KEY_WAYPOINTS_ONBOARD, /* Mission way point coordinates generated onboard */ DM_KEY_NUM_KEYS /* Total number of item types defined */ } dm_item_t; @@ -59,7 +59,7 @@ extern "C" { enum { DM_KEY_SAFE_POINTS_MAX = 8, DM_KEY_FENCE_POINTS_MAX = GEOFENCE_MAX_VERTICES, - DM_KEY_WAYPOINTS_MAX = NUM_MISSIONS_SUPPORTED, + DM_KEY_WAYPOINTS_OFFBOARD_MAX = NUM_MISSIONS_SUPPORTED, DM_KEY_WAYPOINTS_ONBOARD_MAX = NUM_MISSIONS_SUPPORTED }; diff --git a/src/modules/mavlink/waypoints.c b/src/modules/mavlink/waypoints.c index 7aad5038d2..52a580d5be 100644 --- a/src/modules/mavlink/waypoints.c +++ b/src/modules/mavlink/waypoints.c @@ -701,7 +701,7 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi struct mission_item_s mission_item; ssize_t len = sizeof(struct mission_item_s); - if (dm_read(DM_KEY_WAYPOINTS, wpr.seq, &mission_item, len) == len) { + if (dm_read(DM_KEY_WAYPOINTS_OFFBOARD, wpr.seq, &mission_item, len) == len) { if (mission.current_index == wpr.seq) { wp.current = true; @@ -975,7 +975,7 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi size_t len = sizeof(struct mission_item_s); - if (dm_write(DM_KEY_WAYPOINTS, wp.seq, DM_PERSIST_IN_FLIGHT_RESET, &mission_item, len) != len) { + if (dm_write(DM_KEY_WAYPOINTS_OFFBOARD, wp.seq, DM_PERSIST_IN_FLIGHT_RESET, &mission_item, len) != len) { mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_ERROR); wpm->current_state = MAVLINK_WPM_STATE_IDLE; break; @@ -1117,7 +1117,7 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi /* prepare mission topic */ mission.count = 0; - if (dm_clear(DM_KEY_WAYPOINTS) == OK) { + if (dm_clear(DM_KEY_WAYPOINTS_OFFBOARD) == OK) { mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_ACCEPTED); } else { mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_ERROR); diff --git a/src/systemcmds/tests/test_dataman.c b/src/systemcmds/tests/test_dataman.c index 7de87b476f..5b121e34ed 100644 --- a/src/systemcmds/tests/test_dataman.c +++ b/src/systemcmds/tests/test_dataman.c @@ -89,7 +89,7 @@ task_main(int argc, char *argv[]) unsigned hash = i ^ my_id; unsigned len = (hash & 63) + 2; - int ret = dm_write(DM_KEY_WAYPOINTS, hash, DM_PERSIST_IN_FLIGHT_RESET, buffer, len); + int ret = dm_write(DM_KEY_WAYPOINTS_OFFBOARD, hash, DM_PERSIST_IN_FLIGHT_RESET, buffer, len); warnx("ret: %d", ret); if (ret != len) { warnx("%d write failed, index %d, length %d", my_id, hash, len); @@ -103,7 +103,7 @@ task_main(int argc, char *argv[]) for (unsigned i = 0; i < NUM_MISSIONS_SUPPORTED; i++) { unsigned hash = i ^ my_id; unsigned len2, len = (hash & 63) + 2; - if ((len2 = dm_read(DM_KEY_WAYPOINTS, hash, buffer, sizeof(buffer))) < 2) { + if ((len2 = dm_read(DM_KEY_WAYPOINTS_OFFBOARD, hash, buffer, sizeof(buffer))) < 2) { warnx("%d read failed length test, index %d", my_id, hash); goto fail; } @@ -163,7 +163,7 @@ int test_dataman(int argc, char *argv[]) free(sems); dm_restart(DM_INIT_REASON_IN_FLIGHT); for (i = 0; i < NUM_MISSIONS_SUPPORTED; i++) { - if (dm_read(DM_KEY_WAYPOINTS, i, buffer, sizeof(buffer)) != 0) + if (dm_read(DM_KEY_WAYPOINTS_OFFBOARD, i, buffer, sizeof(buffer)) != 0) break; } if (i >= NUM_MISSIONS_SUPPORTED) { @@ -173,7 +173,7 @@ int test_dataman(int argc, char *argv[]) } dm_restart(DM_INIT_REASON_POWER_ON); for (i = 0; i < NUM_MISSIONS_SUPPORTED; i++) { - if (dm_read(DM_KEY_WAYPOINTS, i, buffer, sizeof(buffer)) != 0) { + if (dm_read(DM_KEY_WAYPOINTS_OFFBOARD, i, buffer, sizeof(buffer)) != 0) { warnx("Restart power-on failed"); return -1; } From 624ae85efa078ddd63209fe53c2c215986116ad1 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sat, 14 Dec 2013 20:55:03 +0100 Subject: [PATCH 203/447] Navigator: Use state table for main FSM --- src/modules/navigator/navigator_main.cpp | 1089 ++++++++++++---------- src/modules/systemlib/state_table.h | 75 ++ 2 files changed, 680 insertions(+), 484 deletions(-) create mode 100644 src/modules/systemlib/state_table.h diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index e2e2949e25..d258aa8a69 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -38,6 +38,7 @@ * Implementation of the main navigation state machine. * * Handles missions, geo fencing and failsafe navigation behavior. + * Published the mission item triplet for the position controller. */ #include @@ -66,21 +67,15 @@ #include #include #include -#include +#include #include #include +#include #include #include #include -typedef enum { - NAVIGATION_MODE_NONE, - NAVIGATION_MODE_LOITER, - NAVIGATION_MODE_WAYPOINT, - NAVIGATION_MODE_LOITER_WAYPOINT, - NAVIGATION_MODE_RTL, - NAVIGATION_MODE_LOITER_RTL -} navigation_mode_t; + /* oddly, ERROR is not defined for c++ */ #ifdef ERROR @@ -95,7 +90,7 @@ static const int ERROR = -1; */ extern "C" __EXPORT int navigator_main(int argc, char *argv[]); -class Navigator +class Navigator : public StateTable { public: /** @@ -141,7 +136,7 @@ private: int _home_pos_sub; /**< home position subscription */ int _vstatus_sub; /**< vehicle status subscription */ int _params_sub; /**< notification of parameter updates */ - int _mission_sub; /**< notification of mission updates */ + int _offboard_mission_sub; /**< notification of offboard mission updates */ int _onboard_mission_sub; /**< notification of onboard mission updates */ int _capabilities_sub; /**< notification of vehicle capabilities updates */ @@ -157,22 +152,26 @@ private: perf_counter_t _loop_perf; /**< loop performance counter */ - unsigned _mission_item_count; /** number of mission items copied */ - unsigned _onboard_mission_item_count; /** number of onboard mission items copied */ - struct fence_s _fence; /**< storage for fence vertices */ + struct fence_s _fence; /**< storage for fence vertices */ bool _fence_valid; /**< flag if fence is valid */ bool _inside_fence; /**< vehicle is inside fence */ struct navigation_capabilities_s _nav_caps; + unsigned _current_offboard_mission_index; + unsigned _current_onboard_mission_index; + unsigned _offboard_mission_item_count; /** number of offboard mission items copied */ + unsigned _onboard_mission_item_count; /** number of onboard mission items copied */ + + enum { + MISSION_TYPE_NONE, + MISSION_TYPE_ONBOARD, + MISSION_TYPE_OFFBOARD, + } _mission_type; + bool _waypoint_position_reached; bool _waypoint_yaw_reached; uint64_t _time_first_inside_orbit; - bool _mission_item_reached; - - navigation_mode_t _mode; - unsigned _current_mission_index; - unsigned _current_onboard_mission_index; struct { float min_altitude; @@ -187,22 +186,69 @@ private: } _parameter_handles; /**< handles for parameters */ + enum Event { + EVENT_NONE_REQUESTED, + EVENT_LOITER_REQUESTED, + EVENT_MISSION_REQUESTED, + EVENT_RTL_REQUESTED, + EVENT_MISSION_FINISHED, + EVENT_MISSION_CHANGED, + EVENT_HOME_POSITION_CHANGED, + MAX_EVENT + }; + + enum State { + STATE_INIT, + STATE_NONE, + STATE_LOITER, + STATE_MISSION, + STATE_MISSION_LOITER, + STATE_RTL, + STATE_RTL_LOITER, + MAX_STATE + }; + + /** + * State machine transition table + */ + static StateTable::Tran const myTable[MAX_STATE][MAX_EVENT]; /** * Update our local parameter cache. */ - int parameters_update(); + void parameters_update(); /** - * Retrieve mission. - */ - void mission_update(); + * Retrieve global position + */ + void global_position_update(); /** - * Retrieve onboard mission. - */ + * Retrieve home position + */ + void home_position_update(); + + /** + * Retreive navigation capabilities + */ + void navigation_capabilities_update(); + + /** + * Retrieve offboard mission. + */ + void offboard_mission_update(); + + /** + * Retrieve onboard mission. + */ void onboard_mission_update(); + /** + * Retrieve vehicle status + */ + void vehicle_status_update(); + + /** * Shim for calling task_main from task_create. */ @@ -219,31 +265,52 @@ private: bool fence_valid(const struct fence_s &fence); - void set_mode(navigation_mode_t new_nav_mode); + /** + * Functions that are triggered when a new state is entered. + */ + void start_none(); + void start_loiter(); + void start_mission(); + void start_mission_loiter(); + void start_rtl(); + void start_rtl_loiter(); - bool mission_possible(); + /** + * Guards offboard mission + */ + bool offboard_mission_available(unsigned relative_index); - bool onboard_mission_possible(); + /** + * Guards onboard mission + */ + bool onboard_mission_available(unsigned relative_index); - int set_waypoint_mission_item(dm_item_t dm_item, unsigned mission_item_index, struct mission_item_s *new_mission_item); + /** + * Check if current mission item has been reached. + */ + bool mission_item_reached(); + /** + * Move to next waypoint + */ + int advance_mission(); + + /** + * Helper function to set a mission item + */ + int set_mission_item(unsigned relative_index, struct mission_item_s *mission_item) ; + + /** + * Helper function to set a loiter item + */ + void set_loiter_item(mission_item_s *new_loiter_position); + + /** + * Publish a new mission item triplet for position controller + */ void publish_mission_item_triplet(); - void publish_mission_result(); - int advance_current_mission_item(); - - void reset_mission_item_reached(); - - void check_mission_item_reached(); - - void report_mission_reached(); - - void start_waypoint(); - - void start_loiter(mission_item_s *new_loiter_position); - - void start_rtl(); /** * Compare two mission items if they are equivalent @@ -266,11 +333,13 @@ static const int ERROR = -1; Navigator *g_navigator; } -Navigator::Navigator() : +Navigator::Navigator() : + +/* state machine transition table */ + StateTable(&myTable[0][0], MAX_STATE, MAX_EVENT), _task_should_exit(false), _navigator_task(-1), - _mavlink_fd(-1), /* subscriptions */ @@ -278,7 +347,7 @@ Navigator::Navigator() : _home_pos_sub(-1), _vstatus_sub(-1), _params_sub(-1), - _mission_sub(-1), + _offboard_mission_sub(-1), _onboard_mission_sub(-1), _capabilities_sub(-1), @@ -289,21 +358,20 @@ Navigator::Navigator() : /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "navigator")), + /* states */ - _mission_item_count(0), - _onboard_mission_item_count(0), _fence_valid(false), _inside_fence(true), + _current_offboard_mission_index(0), + _current_onboard_mission_index(0), + _offboard_mission_item_count(0), + _onboard_mission_item_count(0), _waypoint_position_reached(false), _waypoint_yaw_reached(false), - _time_first_inside_orbit(0), - _mission_item_reached(false), - _mode(NAVIGATION_MODE_NONE), - _current_mission_index(0), - _current_onboard_mission_index(0) + _time_first_inside_orbit(0) { - _global_pos.valid = false; memset(&_fence, 0, sizeof(_fence)); + _parameter_handles.min_altitude = param_find("NAV_MIN_ALT"); _parameter_handles.loiter_radius = param_find("NAV_LOITER_RAD"); _parameter_handles.onboard_mission_enabled = param_find("NAV_ONB_MIS_EN"); @@ -317,8 +385,8 @@ Navigator::Navigator() : memset(&_mission_result, 0, sizeof(struct mission_result_s)); - /* fetch initial values */ - parameters_update(); + /* Initialize state machine */ + myState = STATE_INIT; } Navigator::~Navigator() @@ -346,35 +414,53 @@ Navigator::~Navigator() navigator::g_navigator = nullptr; } -int +void Navigator::parameters_update() { + /* read from param to clear updated flag */ + struct parameter_update_s update; + orb_copy(ORB_ID(parameter_update), _params_sub, &update); param_get(_parameter_handles.min_altitude, &(_parameters.min_altitude)); param_get(_parameter_handles.loiter_radius, &(_parameters.loiter_radius)); param_get(_parameter_handles.onboard_mission_enabled, &(_parameters.onboard_mission_enabled)); - - return OK; } void -Navigator::mission_update() +Navigator::global_position_update() { - struct mission_s mission; - if (orb_copy(ORB_ID(mission), _mission_sub, &mission) == OK) { + /* load local copies */ + orb_copy(ORB_ID(vehicle_global_position), _global_pos_sub, &_global_pos); +} - _mission_item_count = mission.count; - _current_mission_index = mission.current_index; +void +Navigator::home_position_update() +{ + orb_copy(ORB_ID(home_position), _home_pos_sub, &_home_pos); +} + +void +Navigator::navigation_capabilities_update() +{ + orb_copy(ORB_ID(navigation_capabilities), _capabilities_sub, &_nav_caps); +} + + +void +Navigator::offboard_mission_update() +{ + struct mission_s offboard_mission; + if (orb_copy(ORB_ID(mission), _offboard_mission_sub, &offboard_mission) == OK) { + + _offboard_mission_item_count = offboard_mission.count; + + if (offboard_mission.current_index != -1) { + _current_offboard_mission_index = offboard_mission.current_index; + } } else { - _mission_item_count = 0; - _current_mission_index = 0; - } - if ((!mission_possible() || !onboard_mission_possible()) && _mode == NAVIGATION_MODE_WAYPOINT) { - set_mode(NAVIGATION_MODE_LOITER); - } - else if (_mode == NAVIGATION_MODE_WAYPOINT) { - start_waypoint(); + _offboard_mission_item_count = 0; + _current_offboard_mission_index = 0; } } @@ -385,18 +471,23 @@ Navigator::onboard_mission_update() if (orb_copy(ORB_ID(onboard_mission), _onboard_mission_sub, &onboard_mission) == OK) { _onboard_mission_item_count = onboard_mission.count; - _current_onboard_mission_index = onboard_mission.current_index; + + if (onboard_mission.current_index != -1) { + _current_onboard_mission_index = onboard_mission.current_index; + } } else { _onboard_mission_item_count = 0; _current_onboard_mission_index = 0; } +} - if ((!mission_possible() || !onboard_mission_possible()) && _mode == NAVIGATION_MODE_WAYPOINT) { - set_mode(NAVIGATION_MODE_LOITER); - } - else if (_mode == NAVIGATION_MODE_WAYPOINT) { - start_waypoint(); +void +Navigator::vehicle_status_update() +{ + /* try to load initial states */ + if (orb_copy(ORB_ID(vehicle_status), _vstatus_sub, &_vstatus) != OK) { + _vstatus.arming_state = ARMING_STATE_STANDBY; /* in case the commander is not be running */ } } @@ -414,37 +505,34 @@ Navigator::task_main() warnx("Initializing.."); fflush(stdout); + _fence_valid = load_fence(GEOFENCE_MAX_VERTICES); + /* * do subscriptions */ _global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); - _mission_sub = orb_subscribe(ORB_ID(mission)); + _offboard_mission_sub = orb_subscribe(ORB_ID(mission)); _onboard_mission_sub = orb_subscribe(ORB_ID(onboard_mission)); _capabilities_sub = orb_subscribe(ORB_ID(navigation_capabilities)); _vstatus_sub = orb_subscribe(ORB_ID(vehicle_status)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); _home_pos_sub = orb_subscribe(ORB_ID(home_position)); - // Load initial states - if (orb_copy(ORB_ID(vehicle_status), _vstatus_sub, &_vstatus) != OK) { - _vstatus.arming_state = ARMING_STATE_STANDBY; // for testing... commander may not be running - } - - mission_update(); + + /* copy all topics first time */ + parameters_update(); + global_position_update(); + home_position_update(); + navigation_capabilities_update(); + offboard_mission_update(); onboard_mission_update(); + vehicle_status_update(); /* rate limit vehicle status updates to 5Hz */ orb_set_interval(_vstatus_sub, 200); /* rate limit position updates to 50 Hz */ orb_set_interval(_global_pos_sub, 20); - parameters_update(); - - _fence_valid = load_fence(GEOFENCE_MAX_VERTICES); - - /* load the craft capabilities */ - orb_copy(ORB_ID(navigation_capabilities), _capabilities_sub, &_nav_caps); - /* wakeup source(s) */ struct pollfd fds[7]; @@ -457,7 +545,7 @@ Navigator::task_main() fds[2].events = POLLIN; fds[3].fd = _capabilities_sub; fds[3].events = POLLIN; - fds[4].fd = _mission_sub; + fds[4].fd = _offboard_mission_sub; fds[4].events = POLLIN; fds[5].fd = _onboard_mission_sub; fds[5].events = POLLIN; @@ -485,8 +573,8 @@ Navigator::task_main() /* only update vehicle status if it changed */ if (fds[6].revents & POLLIN) { - /* read from param to clear updated flag */ - orb_copy(ORB_ID(vehicle_status), _vstatus_sub, &_vstatus); + + vehicle_status_update(); /* Evaluate state machine from commander and set the navigator mode accordingly */ if (_vstatus.main_state == MAIN_STATE_AUTO) { @@ -497,135 +585,96 @@ Navigator::task_main() case NAVIGATION_STATE_ALTHOLD: case NAVIGATION_STATE_VECTOR: - set_mode(NAVIGATION_MODE_NONE); + /* don't publish a mission item triplet */ + dispatch(EVENT_NONE_REQUESTED); break; case NAVIGATION_STATE_AUTO_READY: case NAVIGATION_STATE_AUTO_TAKEOFF: + case NAVIGATION_STATE_AUTO_MISSION: - /* TODO probably not needed since takeoff WPs will just be passed on */ - //set_mode(NAVIGATION_MODE_TAKEOFF); + /* try mission if none is available, fallback to loiter instead */ + if (onboard_mission_available(0) || offboard_mission_available(0)) { + dispatch(EVENT_MISSION_REQUESTED); + } else { + dispatch(EVENT_LOITER_REQUESTED); + } break; case NAVIGATION_STATE_AUTO_LOITER: - set_mode(NAVIGATION_MODE_LOITER); + dispatch(EVENT_LOITER_REQUESTED); break; - case NAVIGATION_STATE_AUTO_MISSION: - - if (mission_possible() || onboard_mission_possible()) { - /* Start mission or onboard mission if available */ - set_mode(NAVIGATION_MODE_WAYPOINT); - } else { - /* else fallback to loiter */ - set_mode(NAVIGATION_MODE_LOITER); - } - break; case NAVIGATION_STATE_AUTO_RTL: - set_mode(NAVIGATION_MODE_RTL); + dispatch(EVENT_RTL_REQUESTED); break; case NAVIGATION_STATE_AUTO_LAND: /* TODO add this */ - //set_mode(NAVIGATION_MODE_LAND); + break; default: - warnx("Navigation state not supported"); + warnx("ERROR: Navigation state not supported"); break; } } else { - set_mode(NAVIGATION_MODE_NONE); + /* not in AUTO */ + dispatch(EVENT_NONE_REQUESTED); + } + + /* XXX Hack to get mavlink output going, try opening the fd with 5Hz */ + if (_mavlink_fd < 0) { + /* try to open the mavlink log device every once in a while */ + _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); } } + /* only update parameters if it changed */ if (fds[0].revents & POLLIN) { - /* read from param to clear updated flag */ - struct parameter_update_s update; - orb_copy(ORB_ID(parameter_update), _params_sub, &update); - /* update parameters from storage */ parameters_update(); - /* note that these new parameters won't be in effect until a mission triplet is published again */ } /* only update craft capabilities if they have changed */ if (fds[3].revents & POLLIN) { - orb_copy(ORB_ID(navigation_capabilities), _capabilities_sub, &_nav_caps); + navigation_capabilities_update(); } if (fds[4].revents & POLLIN) { - mission_update(); + offboard_mission_update(); + // XXX check if mission really changed + dispatch(EVENT_MISSION_CHANGED); } if (fds[5].revents & POLLIN) { onboard_mission_update(); + // XXX check if mission really changed + dispatch(EVENT_MISSION_CHANGED); } if (fds[2].revents & POLLIN) { - orb_copy(ORB_ID(home_position), _home_pos_sub, &_home_pos); + home_position_update(); + // XXX check if home position really changed + dispatch(EVENT_HOME_POSITION_CHANGED); } /* only run controller if position changed */ if (fds[1].revents & POLLIN) { - - /* XXX Hack to get mavlink output going */ - if (_mavlink_fd < 0) { - /* try to open the mavlink log device every once in a while */ - _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); - } - - /* load local copies */ - orb_copy(ORB_ID(vehicle_global_position), _global_pos_sub, &_global_pos); - - /* do stuff according to mode */ - switch (_mode) { - case NAVIGATION_MODE_NONE: - case NAVIGATION_MODE_LOITER: - case NAVIGATION_MODE_LOITER_WAYPOINT: - case NAVIGATION_MODE_LOITER_RTL: - break; - - case NAVIGATION_MODE_WAYPOINT: - - check_mission_item_reached(); - - if (_mission_item_reached) { - - - - if (onboard_mission_possible()) { - mavlink_log_info(_mavlink_fd, "[navigator] reached onboard WP %d", _current_onboard_mission_index); - } else { - mavlink_log_info(_mavlink_fd, "[navigator] reached WP %d", _current_mission_index); - report_mission_reached(); - } - - if (advance_current_mission_item() != OK) { - set_mode(NAVIGATION_MODE_LOITER_WAYPOINT); - } - } - break; - - case NAVIGATION_MODE_RTL: - - check_mission_item_reached(); - - if (_mission_item_reached) { - mavlink_log_info(_mavlink_fd, "[navigator] reached RTL position"); - set_mode(NAVIGATION_MODE_LOITER_RTL); - } - break; - default: - warnx("navigation mode not supported"); - break; + global_position_update(); + /* only check if waypoint has been reached in Mission or RTL mode */ + if (mission_item_reached()) { + /* try to advance mission */ + if (advance_mission() != OK) { + dispatch(EVENT_MISSION_FINISHED); + } } } perf_end(_loop_perf); @@ -668,15 +717,57 @@ Navigator::status() (double)_global_pos.alt, (double)_global_pos.relative_alt); warnx("Ground velocity in m/s, x %5.5f, y %5.5f, z %5.5f", (double)_global_pos.vx, (double)_global_pos.vy, (double)_global_pos.vz); - warnx("Compass heading in degrees %5.5f", (double)_global_pos.yaw * 57.2957795); + warnx("Compass heading in degrees %5.5f", (double)(_global_pos.yaw * M_RAD_TO_DEG_F)); } if (_fence_valid) { warnx("Geofence is valid"); warnx("Vertex longitude latitude"); for (unsigned i = 0; i < _fence.count; i++) warnx("%6u %9.5f %8.5f", i, (double)_fence.vertices[i].lon, (double)_fence.vertices[i].lat); - } else + } else { warnx("Geofence not set"); + } + + switch (myState) { + case STATE_INIT: + warnx("State: Init"); + break; + case STATE_NONE: + warnx("State: None"); + break; + case STATE_LOITER: + warnx("State: Loiter"); + break; + case STATE_MISSION: + warnx("State: Mission"); + switch (_mission_type) { + case MISSION_TYPE_ONBOARD: + warnx("Mission type: Onboard"); + break; + case MISSION_TYPE_OFFBOARD: + warnx("Mission type: Offboard"); + break; + case MISSION_TYPE_NONE: + default: + warnx("ERROR: Mission type unsupported"); + break; + } + warnx("Onboard mission: %d of %d", _current_onboard_mission_index, _onboard_mission_item_count); + warnx("Offboard mission: %d of %d", _current_offboard_mission_index, _offboard_mission_item_count); + break; + case STATE_MISSION_LOITER: + warnx("State: Loiter after Mission"); + break; + case STATE_RTL: + warnx("State: RTL"); + break; + case STATE_RTL_LOITER: + warnx("State: Loiter after RTL"); + break; + default: + warnx("State: Unknown"); + break; + } } void @@ -767,199 +858,177 @@ Navigator::fence_point(int argc, char *argv[]) errx(1, "can't store fence point"); } + + +StateTable::Tran const Navigator::myTable[MAX_STATE][MAX_EVENT] = { + { + /* STATE_INIT */ + /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), STATE_NONE}, + /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), STATE_LOITER}, + /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), STATE_MISSION}, + /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), STATE_RTL}, + /* EVENT_MISSION_FINISHED */ {NO_ACTION, STATE_INIT}, + /* EVENT_MISSION_CHANGED */ {NO_ACTION, STATE_INIT}, + /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, STATE_INIT}, + }, + { + /* STATE_NONE */ + /* EVENT_NONE_REQUESTED */ {NO_ACTION, STATE_NONE}, + /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), STATE_LOITER}, + /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), STATE_MISSION}, + /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), STATE_RTL}, + /* EVENT_MISSION_FINISHED */ {NO_ACTION, STATE_NONE}, + /* EVENT_MISSION_CHANGED */ {NO_ACTION, STATE_NONE}, + /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, STATE_NONE}, + }, + { + /* STATE_LOITER */ + /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), STATE_NONE}, + /* EVENT_LOITER_REQUESTED */ {NO_ACTION, STATE_LOITER}, + /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), STATE_MISSION}, + /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), STATE_RTL}, + /* EVENT_MISSION_FINISHED */ {NO_ACTION, STATE_LOITER}, + /* EVENT_MISSION_CHANGED */ {NO_ACTION, STATE_LOITER}, + /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, STATE_LOITER}, + }, + { + /* STATE_MISSION */ + /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), STATE_NONE}, + /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), STATE_LOITER}, + /* EVENT_MISSION_REQUESTED */ {NO_ACTION, STATE_MISSION}, + /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), STATE_RTL}, + /* EVENT_MISSION_FINISHED */ {ACTION(&Navigator::start_mission_loiter), STATE_MISSION_LOITER}, + /* EVENT_MISSION_CHANGED */ {ACTION(&Navigator::start_mission), STATE_MISSION}, + /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, STATE_MISSION}, + }, + { + /* STATE_MISSION_LOITER */ + /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), STATE_NONE}, + /* EVENT_LOITER_REQUESTED */ {NO_ACTION, STATE_MISSION_LOITER}, + /* EVENT_MISSION_REQUESTED */ {NO_ACTION, STATE_MISSION_LOITER}, + /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), STATE_RTL}, + /* EVENT_MISSION_FINISHED */ {NO_ACTION, STATE_MISSION_LOITER}, + /* EVENT_MISSION_CHANGED */ {ACTION(&Navigator::start_mission), STATE_MISSION}, + /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, STATE_MISSION_LOITER}, + }, + { + /* STATE_RTL */ + /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), STATE_NONE}, + /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), STATE_LOITER}, + /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), STATE_MISSION}, + /* EVENT_RTL_REQUESTED */ {NO_ACTION, STATE_RTL}, + /* EVENT_MISSION_FINISHED */ {ACTION(&Navigator::start_rtl_loiter), STATE_RTL_LOITER}, + /* EVENT_MISSION_CHANGED */ {NO_ACTION, STATE_RTL}, + /* EVENT_HOME_POSITION_CHANGED */ {ACTION(&Navigator::start_loiter), STATE_RTL}, + }, + { + /* STATE_RTL_LOITER */ + /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), STATE_NONE}, + /* EVENT_LOITER_REQUESTED */ {NO_ACTION, STATE_RTL_LOITER}, + /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), STATE_MISSION}, + /* EVENT_RTL_REQUESTED */ {NO_ACTION, STATE_RTL_LOITER}, + /* EVENT_MISSION_FINISHED */ {NO_ACTION, STATE_RTL_LOITER}, + /* EVENT_MISSION_CHANGED */ {NO_ACTION, STATE_RTL_LOITER}, + /* EVENT_HOME_POSITION_CHANGED */ {ACTION(&Navigator::start_loiter), STATE_LOITER}, + }, +}; + void -Navigator::set_mode(navigation_mode_t new_nav_mode) +Navigator::start_none() { - if (new_nav_mode == _mode) { - /* no change, return */ - return; + _mission_item_triplet.previous_valid = false; + _mission_item_triplet.current_valid = false; + _mission_item_triplet.next_valid = false; + + publish_mission_item_triplet(); +} + +void +Navigator::start_loiter() +{ + struct mission_item_s loiter_item; + + loiter_item.lat = (double)_global_pos.lat / 1e7; + loiter_item.lon = (double)_global_pos.lon / 1e7; + loiter_item.yaw = 0.0f; + + /* XXX get rid of ugly conversion for home position altitude */ + float global_min_alt = _parameters.min_altitude + (float)_home_pos.alt/1e3f; + + /* Use current altitude if above min altitude set by parameter */ + if (_global_pos.alt < global_min_alt) { + loiter_item.altitude = global_min_alt; + mavlink_log_info(_mavlink_fd, "[navigator] loiter %.1fm higher", (double)(global_min_alt - _global_pos.alt)); + } else { + loiter_item.altitude = _global_pos.alt; + mavlink_log_info(_mavlink_fd, "[navigator] loiter here"); } - switch (new_nav_mode) { - case NAVIGATION_MODE_NONE: + set_loiter_item(&loiter_item); - // warnx("Set mode NONE"); - _mode = new_nav_mode; + publish_mission_item_triplet(); +} + + +void +Navigator::start_mission() +{ + /* leave previous mission item as isas is */ + + if(set_mission_item(0, &_mission_item_triplet.current) == OK) { + _mission_item_triplet.current_valid = true; + } else { + _mission_item_triplet.current_valid = false; + warnx("ERROR: current WP can't be set"); + } + + if(set_mission_item(1, &_mission_item_triplet.next) == OK) { + _mission_item_triplet.next_valid = true; + } else { + _mission_item_triplet.next_valid = false; + } + + switch (_mission_type) { + case MISSION_TYPE_ONBOARD: + mavlink_log_info(_mavlink_fd, "[navigator] heading to onboard WP %d", _current_onboard_mission_index); break; - - case NAVIGATION_MODE_LOITER: - - /* decide depending on previous navigation mode */ - switch (_mode) { - case NAVIGATION_MODE_NONE: - - case NAVIGATION_MODE_WAYPOINT: - case NAVIGATION_MODE_RTL: { - - /* use current position and loiter around it */ - mission_item_s global_position_mission_item; - global_position_mission_item.lat = (double)_global_pos.lat / 1e7; - global_position_mission_item.lon = (double)_global_pos.lon / 1e7; - - /* XXX get rid of ugly conversion for home position altitude */ - float global_min_alt = _parameters.min_altitude + (float)_home_pos.alt/1e3f; - - /* Use current altitude if above min altitude set by parameter */ - if (_global_pos.alt < global_min_alt) { - global_position_mission_item.altitude = global_min_alt; - mavlink_log_info(_mavlink_fd, "[navigator] loiter %.1fm higher", (double)(global_min_alt - _global_pos.alt)); - } else { - global_position_mission_item.altitude = _global_pos.alt; - mavlink_log_info(_mavlink_fd, "[navigator] loiter here"); - } - start_loiter(&global_position_mission_item); - _mode = new_nav_mode; - - break; - } - case NAVIGATION_MODE_LOITER_WAYPOINT: - case NAVIGATION_MODE_LOITER_RTL: - /* already loitering, just continue */ - _mode = new_nav_mode; - // warnx("continue loiter"); - break; - - case NAVIGATION_MODE_LOITER: - default: - // warnx("previous navigation mode not supported"); - break; - } + case MISSION_TYPE_OFFBOARD: + mavlink_log_info(_mavlink_fd, "[navigator] heading to offboard WP %d", _current_offboard_mission_index); break; - - case NAVIGATION_MODE_WAYPOINT: - - /* Start mission if there is one available */ - start_waypoint(); - _mode = new_nav_mode; - mavlink_log_info(_mavlink_fd, "[navigator] start waypoint mission"); - break; - - case NAVIGATION_MODE_LOITER_WAYPOINT: - - start_loiter(&_mission_item_triplet.current); - _mode = new_nav_mode; - mavlink_log_info(_mavlink_fd, "[navigator] loiter at WP %d", _current_mission_index-1); - break; - - case NAVIGATION_MODE_RTL: - - /* decide depending on previous navigation mode */ - switch (_mode) { - case NAVIGATION_MODE_NONE: - case NAVIGATION_MODE_LOITER: - case NAVIGATION_MODE_WAYPOINT: - case NAVIGATION_MODE_LOITER_WAYPOINT: - - /* start RTL here */ - start_rtl(); - _mode = new_nav_mode; - mavlink_log_info(_mavlink_fd, "[navigator] start RTL"); - break; - - case NAVIGATION_MODE_LOITER_RTL: - /* already loitering after RTL, just continue */ - // warnx("stay in loiter after RTL"); - break; - - case NAVIGATION_MODE_RTL: - default: - warnx("previous navigation mode not supported"); - break; - } - break; - - case NAVIGATION_MODE_LOITER_RTL: - - /* TODO: get rid of this conversion */ - mission_item_s home_position_mission_item; - home_position_mission_item.lat = (double)_home_pos.lat / 1e7; - home_position_mission_item.lon = (double)_home_pos.lon / 1e7; - home_position_mission_item.altitude = _home_pos.alt / 1e3f + _parameters.min_altitude; - start_loiter(&home_position_mission_item); - mavlink_log_info(_mavlink_fd, "[navigator] loiter after RTL"); - _mode = new_nav_mode; + case MISSION_TYPE_NONE: + default: + warnx("ERROR: Mission type unsupported"); break; } + + publish_mission_item_triplet(); } -bool -Navigator::mission_possible() -{ - return _mission_item_count > 0 && - !(_current_mission_index >= _mission_item_count); -} -bool -Navigator::onboard_mission_possible() -{ - return _onboard_mission_item_count > 0 && - !(_current_onboard_mission_index >= _onboard_mission_item_count) && - _parameters.onboard_mission_enabled; -} int -Navigator::set_waypoint_mission_item(dm_item_t dm_item, unsigned mission_item_index, struct mission_item_s *new_mission_item) +Navigator::advance_mission() { - struct mission_item_s mission_item; - - if (dm_read(dm_item, mission_item_index, &mission_item, sizeof(struct mission_item_s)) != sizeof(struct mission_item_s)) { - return ERROR; + switch (_mission_type) { + case MISSION_TYPE_ONBOARD: + warnx("advance onboard before: %d", _current_onboard_mission_index); + _current_onboard_mission_index++; + warnx("advance onboard after: %d", _current_onboard_mission_index); + break; + case MISSION_TYPE_OFFBOARD: + warnx("advance offboard before: %d", _current_offboard_mission_index); + _current_offboard_mission_index++; + warnx("advance offboard after: %d", _current_offboard_mission_index); + break; + case MISSION_TYPE_NONE: + default: + warnx("ERROR: Mission type unsupported"); + return ERROR; } - memcpy(new_mission_item, &mission_item, sizeof(struct mission_item_s)); - - if (new_mission_item->nav_cmd == NAV_CMD_RETURN_TO_LAUNCH) { - /* if it is a RTL waypoint, append the home position */ - new_mission_item->lat = (double)_home_pos.lat / 1e7; - new_mission_item->lon = (double)_home_pos.lon / 1e7; - new_mission_item->altitude = (float)_home_pos.alt / 1e3f + _parameters.min_altitude; - new_mission_item->loiter_radius = _parameters.loiter_radius; // TODO: get rid of magic number - new_mission_item->radius = 50.0f; // TODO: get rid of magic number - } - - return OK; -} - -void -Navigator::publish_mission_item_triplet() -{ - /* lazily publish the mission triplet only once available */ - if (_triplet_pub > 0) { - /* publish the mission triplet */ - orb_publish(ORB_ID(mission_item_triplet), _triplet_pub, &_mission_item_triplet); - - } else { - /* advertise and publish */ - _triplet_pub = orb_advertise(ORB_ID(mission_item_triplet), &_mission_item_triplet); - } -} - -void -Navigator::publish_mission_result() -{ - /* lazily publish the mission result only once available */ - if (_mission_result_pub > 0) { - /* publish the mission result */ - orb_publish(ORB_ID(mission_result), _mission_result_pub, &_mission_result); - - } else { - /* advertise and publish */ - _mission_result_pub = orb_advertise(ORB_ID(mission_result), &_mission_result); - } -} - -int -Navigator::advance_current_mission_item() -{ - reset_mission_item_reached(); - - // warnx("advancing from %d to %d", _current_mission_index, _current_mission_index+1); - - /* ultimately this index will be == _mission_item_count and this flags the mission as completed */ - - /* if there is no more mission available, don't advance and return */ + /* if there is no more mission available, don't advance and switch to loiter at current WP */ if (!_mission_item_triplet.next_valid) { - // warnx("no next available"); + warnx("no next valid"); return ERROR; } @@ -967,62 +1036,182 @@ Navigator::advance_current_mission_item() memcpy(&_mission_item_triplet.previous, &_mission_item_triplet.current, sizeof(mission_item_s)); _mission_item_triplet.previous_valid = _mission_item_triplet.current_valid; - /* copy the next to current */ - memcpy(&_mission_item_triplet.current, &_mission_item_triplet.next, sizeof(mission_item_s)); - _mission_item_triplet.current_valid = _mission_item_triplet.next_valid; - - int ret = ERROR; - - if (onboard_mission_possible()) { - _current_onboard_mission_index++; - ret = set_waypoint_mission_item(DM_KEY_WAYPOINTS_ONBOARD, _current_onboard_mission_index + 1, &_mission_item_triplet.next); - } else if (mission_possible()) { - _current_mission_index++; - ret = set_waypoint_mission_item(DM_KEY_WAYPOINTS, _current_mission_index + 1, &_mission_item_triplet.next); + if(set_mission_item(0, &_mission_item_triplet.current) == OK) { + _mission_item_triplet.current_valid = true; + } else { - warnx("Error: nothing to advance"); + /* should never ever happen */ + _mission_item_triplet.current_valid = false; + warnx("no current available"); + return ERROR; } - if(ret == OK) { + if(set_mission_item(1, &_mission_item_triplet.next) == OK) { _mission_item_triplet.next_valid = true; - } - else { + + } else { _mission_item_triplet.next_valid = false; } publish_mission_item_triplet(); - return OK; } - void -Navigator::reset_mission_item_reached() +Navigator::start_mission_loiter() { - /* reset all states */ - _waypoint_position_reached = false; - _waypoint_yaw_reached = false; - _time_first_inside_orbit = 0; + /* make sure the current WP is valid */ + if (!_mission_item_triplet.current_valid) { + warnx("ERROR: cannot switch to offboard mission loiter"); + return; + } - _mission_item_reached = false; + set_loiter_item(&_mission_item_triplet.current); - _mission_result.mission_reached = false; - _mission_result.mission_index = 0; + switch (_mission_type) { + case MISSION_TYPE_ONBOARD: + mavlink_log_info(_mavlink_fd, "[navigator] loiter at onboard WP %d", _current_onboard_mission_index-1); + break; + case MISSION_TYPE_OFFBOARD: + mavlink_log_info(_mavlink_fd, "[navigator] loiter at offboard WP %d", _current_offboard_mission_index-1); + break; + case MISSION_TYPE_NONE: + default: + warnx("ERROR: Mission type unsupported"); + break; + } } void -Navigator::check_mission_item_reached() +Navigator::start_rtl() { - /* don't check if mission item is already reached */ - if (_mission_item_reached) { - return; + + /* discard all mission item and insert RTL item */ + _mission_item_triplet.previous_valid = false; + _mission_item_triplet.current_valid = true; + _mission_item_triplet.next_valid = false; + + _mission_item_triplet.current.lat = (double)_home_pos.lat / 1e7; + _mission_item_triplet.current.lon = (double)_home_pos.lon / 1e7; + _mission_item_triplet.current.altitude = (float)_home_pos.alt / 1e3f + _parameters.min_altitude; + _mission_item_triplet.current.yaw = 0.0f; + _mission_item_triplet.current.nav_cmd = NAV_CMD_RETURN_TO_LAUNCH; + _mission_item_triplet.current.loiter_direction = 1; + _mission_item_triplet.current.loiter_radius = _parameters.loiter_radius; // TODO: get rid of magic number + _mission_item_triplet.current.radius = 50.0f; // TODO: get rid of magic number + _mission_item_triplet.current.autocontinue = false; + _mission_item_triplet.current_valid = true; + + publish_mission_item_triplet(); + + mavlink_log_info(_mavlink_fd, "[navigator] return to launch"); +} + + +void +Navigator::start_rtl_loiter() +{ + mission_item_s home_position_mission_item; + home_position_mission_item.lat = (double)_home_pos.lat / 1e7; + home_position_mission_item.lon = (double)_home_pos.lon / 1e7; + home_position_mission_item.altitude = _home_pos.alt / 1e3f + _parameters.min_altitude; + + set_loiter_item(&home_position_mission_item); + + mavlink_log_info(_mavlink_fd, "[navigator] loiter after RTL"); +} + +bool +Navigator::offboard_mission_available(unsigned relative_index) +{ + return _offboard_mission_item_count > _current_offboard_mission_index + relative_index; +} + +bool +Navigator::onboard_mission_available(unsigned relative_index) +{ + return _onboard_mission_item_count > _current_onboard_mission_index + relative_index && _parameters.onboard_mission_enabled; +} + +int +Navigator::set_mission_item(unsigned relative_index, struct mission_item_s *mission_item) +{ + struct mission_item_s new_mission_item; + + /* try onboard mission first */ + if (onboard_mission_available(relative_index)) { + if (_mission_type != MISSION_TYPE_ONBOARD && relative_index == 1) { + relative_index--; + } + if (dm_read(DM_KEY_WAYPOINTS_ONBOARD, _current_onboard_mission_index + relative_index, &new_mission_item, sizeof(struct mission_item_s)) != sizeof(struct mission_item_s)) { + /* not supposed to happen unless the datamanager can't access the SD card, etc. */ + _mission_type = MISSION_TYPE_NONE; + return ERROR; + } + /* base the mission type on the current mission item, not on future ones */ + if (relative_index == 0) { + _mission_type = MISSION_TYPE_ONBOARD; + } + /* otherwise fallback to offboard */ + } else if (offboard_mission_available(relative_index)) { + + warnx("fallback try offboard: %d / %d", _current_offboard_mission_index + relative_index, _offboard_mission_item_count); + + if (_mission_type != MISSION_TYPE_OFFBOARD && relative_index == 1) { + relative_index--; + } + + if (dm_read(DM_KEY_WAYPOINTS_OFFBOARD, _current_offboard_mission_index + relative_index, &new_mission_item, sizeof(struct mission_item_s)) != sizeof(struct mission_item_s)) { + /* not supposed to happen unless the datamanager can't access the SD card, etc. */ + _mission_type = MISSION_TYPE_NONE; + warnx("failed"); + return ERROR; + } + /* base the mission type on the current mission item, not on future ones */ + if (relative_index == 0) { + _mission_type = MISSION_TYPE_OFFBOARD; + } + } else { + /* happens when no more mission items can be added as a next item */ + return ERROR; } - /* don't try to reach the landing mission, just stay in that mode */ - if (_mission_item_triplet.current.nav_cmd == NAV_CMD_LAND) { - return; + if (new_mission_item.nav_cmd == NAV_CMD_RETURN_TO_LAUNCH) { + /* if it is a RTL waypoint, append the home position */ + new_mission_item.lat = (double)_home_pos.lat / 1e7; + new_mission_item.lon = (double)_home_pos.lon / 1e7; + new_mission_item.altitude = (float)_home_pos.alt / 1e3f + _parameters.min_altitude; + new_mission_item.loiter_radius = _parameters.loiter_radius; // TODO: get rid of magic number + new_mission_item.radius = 50.0f; // TODO: get rid of magic number } + memcpy(mission_item, &new_mission_item, sizeof(struct mission_item_s)); + + return OK; +} + +bool +Navigator::mission_item_reached() +{ + /* only check if there is actually a mission item to check */ + if (!_mission_item_triplet.current_valid) { + return false; + } + + /* don't try to reach the landing mission, just stay in that mode, XXX maybe add another state for this */ + if (_mission_item_triplet.current.nav_cmd == NAV_CMD_LAND) { + return false; + } + + /* XXX TODO count turns */ + if ((_mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_TURN_COUNT || + _mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || + _mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_UNLIMITED) && + _mission_item_triplet.current.loiter_radius > 0.01f) { + + return false; + } + uint64_t now = hrt_absolute_time(); float orbit; @@ -1030,12 +1219,6 @@ Navigator::check_mission_item_reached() orbit = _mission_item_triplet.current.radius; - } else if ((_mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_TURN_COUNT || - _mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || - _mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_UNLIMITED) && - _mission_item_triplet.current.loiter_radius > 0.01f) { - - orbit = _mission_item_triplet.current.loiter_radius; } else { // XXX set default orbit via param @@ -1098,73 +1281,19 @@ Navigator::check_mission_item_reached() if ((now - _time_first_inside_orbit >= (uint64_t)_mission_item_triplet.current.time_inside * 1e6) || _mission_item_triplet.current.nav_cmd == NAV_CMD_TAKEOFF) { - _mission_item_reached = true; + _time_first_inside_orbit = 0; + _waypoint_yaw_reached = false; + _waypoint_position_reached = false; + return true; } } + return false; } void -Navigator::report_mission_reached() +Navigator::set_loiter_item(struct mission_item_s *new_loiter_position) { - _mission_result.mission_reached = true; - _mission_result.mission_index = _current_mission_index; - - publish_mission_result(); -} - -void -Navigator::start_waypoint() -{ - reset_mission_item_reached(); - - // if (_current_mission_index > 0) { - // set_waypoint_mission_item(_current_mission_index - 1, &_mission_item_triplet.previous); - // _mission_item_triplet.previous_valid = true; - // } else { - // _mission_item_triplet.previous_valid = false; - // } - _mission_item_triplet.previous_valid = false; - - int ret = ERROR; - - if (onboard_mission_possible()) { - set_waypoint_mission_item(DM_KEY_WAYPOINTS_ONBOARD, _current_onboard_mission_index, &_mission_item_triplet.current); - mavlink_log_info(_mavlink_fd, "[navigator] heading to onboard WP %d", _current_onboard_mission_index); - ret = set_waypoint_mission_item(DM_KEY_WAYPOINTS_ONBOARD, _current_onboard_mission_index + 1, &_mission_item_triplet.next); - } else if (mission_possible()) { - set_waypoint_mission_item(DM_KEY_WAYPOINTS, _current_mission_index, &_mission_item_triplet.current); - mavlink_log_info(_mavlink_fd, "[navigator] heading to WP %d", _current_mission_index); - ret = set_waypoint_mission_item(DM_KEY_WAYPOINTS, _current_mission_index + 1, &_mission_item_triplet.next); - } - - _mission_item_triplet.current_valid = true; - - // if (_mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_UNLIMITED) { - - // if the current mission is to loiter unlimited, don't bother about a next mission item - // _mission_item_triplet.next_valid = false; - // } else { - /* if we are not loitering yet, try to add the next mission item */ - // set_waypoint_mission_item(_current_mission_index + 1, &_mission_item_triplet.next); - // _mission_item_triplet.next_valid = true; - // } - - if(ret == OK) { - _mission_item_triplet.next_valid = true; - } - else { - _mission_item_triplet.next_valid = false; - } - - publish_mission_item_triplet(); -} - -void -Navigator::start_loiter(mission_item_s *new_loiter_position) -{ - //reset_mission_item_reached(); - _mission_item_triplet.previous_valid = false; _mission_item_triplet.current_valid = true; _mission_item_triplet.next_valid = false; @@ -1184,27 +1313,38 @@ Navigator::start_loiter(mission_item_s *new_loiter_position) } void -Navigator::start_rtl() +Navigator::publish_mission_item_triplet() { - reset_mission_item_reached(); + /* lazily publish the mission triplet only once available */ + if (_triplet_pub > 0) { + /* publish the mission triplet */ + orb_publish(ORB_ID(mission_item_triplet), _triplet_pub, &_mission_item_triplet); - /* discard all mission item and insert RTL item */ - _mission_item_triplet.previous_valid = false; - _mission_item_triplet.current_valid = true; - _mission_item_triplet.next_valid = false; + } else { + /* advertise and publish */ + _triplet_pub = orb_advertise(ORB_ID(mission_item_triplet), &_mission_item_triplet); + } +} - _mission_item_triplet.current.lat = (double)_home_pos.lat / 1e7; - _mission_item_triplet.current.lon = (double)_home_pos.lon / 1e7; - _mission_item_triplet.current.altitude = (float)_home_pos.alt / 1e3f + _parameters.min_altitude; - _mission_item_triplet.current.yaw = 0.0f; - _mission_item_triplet.current.nav_cmd = NAV_CMD_RETURN_TO_LAUNCH; - _mission_item_triplet.current.loiter_direction = 1; - _mission_item_triplet.current.loiter_radius = _parameters.loiter_radius; // TODO: get rid of magic number - _mission_item_triplet.current.radius = 50.0f; // TODO: get rid of magic number - _mission_item_triplet.current.autocontinue = false; - _mission_item_triplet.current_valid = true; - publish_mission_item_triplet(); +bool Navigator::cmp_mission_item_equivalent(const struct mission_item_s a, const struct mission_item_s b) { + if (fabsf(a.altitude_is_relative - b.altitude_is_relative) < FLT_EPSILON && + fabsf(a.lat - b.lat) < FLT_EPSILON && + fabsf(a.lon - b.lon) < FLT_EPSILON && + fabsf(a.altitude - b.altitude) < FLT_EPSILON && + fabsf(a.yaw - b.yaw) < FLT_EPSILON && + fabsf(a.loiter_radius - b.loiter_radius) < FLT_EPSILON && + fabsf(a.loiter_direction - b.loiter_direction) < FLT_EPSILON && + fabsf(a.nav_cmd - b.nav_cmd) < FLT_EPSILON && + fabsf(a.radius - b.radius) < FLT_EPSILON && + fabsf(a.time_inside - b.time_inside) < FLT_EPSILON && + fabsf(a.autocontinue - b.autocontinue) < FLT_EPSILON && + fabsf(a.index - b.index) < FLT_EPSILON) { + return true; + } else { + warnx("a.index %d, b.index %d", a.index, b.index); + return false; + } } @@ -1260,22 +1400,3 @@ int navigator_main(int argc, char *argv[]) return 0; } -bool Navigator::cmp_mission_item_equivalent(const struct mission_item_s a, const struct mission_item_s b) { - if (fabsf(a.altitude_is_relative - b.altitude_is_relative) < FLT_EPSILON && - fabsf(a.lat - b.lat) < FLT_EPSILON && - fabsf(a.lon - b.lon) < FLT_EPSILON && - fabsf(a.altitude - b.altitude) < FLT_EPSILON && - fabsf(a.yaw - b.yaw) < FLT_EPSILON && - fabsf(a.loiter_radius - b.loiter_radius) < FLT_EPSILON && - fabsf(a.loiter_direction - b.loiter_direction) < FLT_EPSILON && - fabsf(a.nav_cmd - b.nav_cmd) < FLT_EPSILON && - fabsf(a.radius - b.radius) < FLT_EPSILON && - fabsf(a.time_inside - b.time_inside) < FLT_EPSILON && - fabsf(a.autocontinue - b.autocontinue) < FLT_EPSILON && - fabsf(a.index - b.index) < FLT_EPSILON) { - return true; - } else { - warnx("a.index %d, b.index %d", a.index, b.index); - return false; - } -} diff --git a/src/modules/systemlib/state_table.h b/src/modules/systemlib/state_table.h new file mode 100644 index 0000000000..f2709d29ff --- /dev/null +++ b/src/modules/systemlib/state_table.h @@ -0,0 +1,75 @@ +/**************************************************************************** + * + * Copyright (C) 2012 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 state_table.h + * + * Finite-State-Machine helper class for state table + */ + +#ifndef __SYSTEMLIB_STATE_TABLE_H +#define __SYSTEMLIB_STATE_TABLE_H + +class StateTable +{ +public: + typedef void (StateTable::*Action)(); + struct Tran { + Action action; + unsigned nextState; + }; + + StateTable(Tran const *table, unsigned nStates, unsigned nSignals) + : myTable(table), myNsignals(nSignals), myNstates(nStates) {} + + #define NO_ACTION &StateTable::doNothing + #define ACTION(_target) static_cast(_target) + + virtual ~StateTable() {} + + void dispatch(unsigned const sig) { + register Tran const *t = myTable + myState*myNsignals + sig; + (this->*(t->action))(); + + myState = t->nextState; + } + void doNothing() {} +protected: + unsigned myState; +private: + Tran const *myTable; + unsigned myNsignals; + unsigned myNstates; +}; + +#endif \ No newline at end of file From b5fb5f9dbb2d26cea1ab50f005cedff52e992eca Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 16 Dec 2013 16:59:24 +0100 Subject: [PATCH 204/447] Navigator: Moved mission stuff in separate class --- src/modules/navigator/module.mk | 3 +- src/modules/navigator/navigator_main.cpp | 343 ++++++++------------ src/modules/navigator/navigator_mission.cpp | 234 +++++++++++++ src/modules/navigator/navigator_mission.h | 95 ++++++ 4 files changed, 458 insertions(+), 217 deletions(-) create mode 100644 src/modules/navigator/navigator_mission.cpp create mode 100644 src/modules/navigator/navigator_mission.h diff --git a/src/modules/navigator/module.mk b/src/modules/navigator/module.mk index 7f7443c43e..fc59c956a3 100644 --- a/src/modules/navigator/module.mk +++ b/src/modules/navigator/module.mk @@ -38,6 +38,7 @@ MODULE_COMMAND = navigator SRCS = navigator_main.cpp \ - navigator_params.c + navigator_params.c \ + navigator_mission.cpp INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index d258aa8a69..d93ecc7cd3 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -75,6 +75,7 @@ #include #include +#include "navigator_mission.h" /* oddly, ERROR is not defined for c++ */ @@ -99,7 +100,7 @@ public: Navigator(); /** - * Destructor, also kills the sensors task. + * Destructor, also kills the navigators task. */ ~Navigator(); @@ -158,16 +159,7 @@ private: struct navigation_capabilities_s _nav_caps; - unsigned _current_offboard_mission_index; - unsigned _current_onboard_mission_index; - unsigned _offboard_mission_item_count; /** number of offboard mission items copied */ - unsigned _onboard_mission_item_count; /** number of onboard mission items copied */ - - enum { - MISSION_TYPE_NONE, - MISSION_TYPE_ONBOARD, - MISSION_TYPE_OFFBOARD, - } _mission_type; + class Mission _mission; bool _waypoint_position_reached; bool _waypoint_yaw_reached; @@ -293,17 +285,12 @@ private: /** * Move to next waypoint */ - int advance_mission(); + void advance_mission(); /** - * Helper function to set a mission item + * Helper function to get a loiter item */ - int set_mission_item(unsigned relative_index, struct mission_item_s *mission_item) ; - - /** - * Helper function to set a loiter item - */ - void set_loiter_item(mission_item_s *new_loiter_position); + void get_loiter_item(mission_item_s *new_loiter_position); /** * Publish a new mission item triplet for position controller @@ -319,6 +306,8 @@ private: * @return true if equivalent, false otherwise */ bool cmp_mission_item_equivalent(const struct mission_item_s a, const struct mission_item_s b); + + void add_home_pos_to_rtl(struct mission_item_s *new_mission_item); }; namespace navigator @@ -362,10 +351,7 @@ Navigator::Navigator() : /* states */ _fence_valid(false), _inside_fence(true), - _current_offboard_mission_index(0), - _current_onboard_mission_index(0), - _offboard_mission_item_count(0), - _onboard_mission_item_count(0), + _mission(), _waypoint_position_reached(false), _waypoint_yaw_reached(false), _time_first_inside_orbit(0) @@ -424,6 +410,8 @@ Navigator::parameters_update() param_get(_parameter_handles.min_altitude, &(_parameters.min_altitude)); param_get(_parameter_handles.loiter_radius, &(_parameters.loiter_radius)); param_get(_parameter_handles.onboard_mission_enabled, &(_parameters.onboard_mission_enabled)); + + _mission.set_onboard_mission_allowed((bool)_parameter_handles.onboard_mission_enabled); } void @@ -452,15 +440,12 @@ Navigator::offboard_mission_update() struct mission_s offboard_mission; if (orb_copy(ORB_ID(mission), _offboard_mission_sub, &offboard_mission) == OK) { - _offboard_mission_item_count = offboard_mission.count; - - if (offboard_mission.current_index != -1) { - _current_offboard_mission_index = offboard_mission.current_index; - } + _mission.set_current_offboard_mission_index(offboard_mission.current_index); + _mission.set_offboard_mission_count(offboard_mission.count); } else { - _offboard_mission_item_count = 0; - _current_offboard_mission_index = 0; + _mission.set_current_offboard_mission_index(0); + _mission.set_offboard_mission_count(0); } } @@ -468,17 +453,14 @@ void Navigator::onboard_mission_update() { struct mission_s onboard_mission; - if (orb_copy(ORB_ID(onboard_mission), _onboard_mission_sub, &onboard_mission) == OK) { + if (orb_copy(ORB_ID(mission), _onboard_mission_sub, &onboard_mission) == OK) { - _onboard_mission_item_count = onboard_mission.count; - - if (onboard_mission.current_index != -1) { - _current_onboard_mission_index = onboard_mission.current_index; - } + _mission.set_current_onboard_mission_index(onboard_mission.current_index); + _mission.set_onboard_mission_count(onboard_mission.count); } else { - _onboard_mission_item_count = 0; - _current_onboard_mission_index = 0; + _mission.set_current_onboard_mission_index(0); + _mission.set_onboard_mission_count(0); } } @@ -594,7 +576,7 @@ Navigator::task_main() case NAVIGATION_STATE_AUTO_MISSION: /* try mission if none is available, fallback to loiter instead */ - if (onboard_mission_available(0) || offboard_mission_available(0)) { + if (_mission.current_mission_available()) { dispatch(EVENT_MISSION_REQUESTED); } else { dispatch(EVENT_LOITER_REQUESTED); @@ -671,8 +653,22 @@ Navigator::task_main() global_position_update(); /* only check if waypoint has been reached in Mission or RTL mode */ if (mission_item_reached()) { - /* try to advance mission */ - if (advance_mission() != OK) { + + if (_vstatus.main_state == MAIN_STATE_AUTO && + (_vstatus.navigation_state == NAVIGATION_STATE_AUTO_READY || + _vstatus.navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF || + _vstatus.navigation_state == NAVIGATION_STATE_AUTO_MISSION)) { + + /* advance by one mission item */ + _mission.move_to_next(); + + /* if no more mission items available send this to state machine and start loiter at the last WP */ + if (_mission.current_mission_available()) { + advance_mission(); + } else { + dispatch(EVENT_MISSION_FINISHED); + } + } else { dispatch(EVENT_MISSION_FINISHED); } } @@ -740,20 +736,6 @@ Navigator::status() break; case STATE_MISSION: warnx("State: Mission"); - switch (_mission_type) { - case MISSION_TYPE_ONBOARD: - warnx("Mission type: Onboard"); - break; - case MISSION_TYPE_OFFBOARD: - warnx("Mission type: Offboard"); - break; - case MISSION_TYPE_NONE: - default: - warnx("ERROR: Mission type unsupported"); - break; - } - warnx("Onboard mission: %d of %d", _current_onboard_mission_index, _onboard_mission_item_count); - warnx("Offboard mission: %d of %d", _current_offboard_mission_index, _offboard_mission_item_count); break; case STATE_MISSION_LOITER: warnx("State: Loiter after Mission"); @@ -946,26 +928,28 @@ Navigator::start_none() void Navigator::start_loiter() { - struct mission_item_s loiter_item; + _mission_item_triplet.previous_valid = false; + _mission_item_triplet.current_valid = true; + _mission_item_triplet.next_valid = false; - loiter_item.lat = (double)_global_pos.lat / 1e7; - loiter_item.lon = (double)_global_pos.lon / 1e7; - loiter_item.yaw = 0.0f; + _mission_item_triplet.current.lat = (double)_global_pos.lat / 1e7; + _mission_item_triplet.current.lon = (double)_global_pos.lon / 1e7; + _mission_item_triplet.current.yaw = 0.0f; + + get_loiter_item(&_mission_item_triplet.current); /* XXX get rid of ugly conversion for home position altitude */ float global_min_alt = _parameters.min_altitude + (float)_home_pos.alt/1e3f; /* Use current altitude if above min altitude set by parameter */ if (_global_pos.alt < global_min_alt) { - loiter_item.altitude = global_min_alt; + _mission_item_triplet.current.altitude = global_min_alt; mavlink_log_info(_mavlink_fd, "[navigator] loiter %.1fm higher", (double)(global_min_alt - _global_pos.alt)); } else { - loiter_item.altitude = _global_pos.alt; + _mission_item_triplet.current.altitude = _global_pos.alt; mavlink_log_info(_mavlink_fd, "[navigator] loiter here"); } - set_loiter_item(&loiter_item); - publish_mission_item_triplet(); } @@ -975,86 +959,86 @@ Navigator::start_mission() { /* leave previous mission item as isas is */ - if(set_mission_item(0, &_mission_item_triplet.current) == OK) { + int ret; + bool onboard; + unsigned index; + + ret = _mission.get_current_mission_item(&_mission_item_triplet.current, &onboard, &index); + + if (ret == OK) { + + add_home_pos_to_rtl(&_mission_item_triplet.current); _mission_item_triplet.current_valid = true; + + if (onboard) { + mavlink_log_info(_mavlink_fd, "[navigator] heading to onboard WP %d", index); + } else { + mavlink_log_info(_mavlink_fd, "[navigator] heading to offboard WP %d", index); + } } else { + /* since a mission is not started without WPs available, this is not supposed to happen */ _mission_item_triplet.current_valid = false; warnx("ERROR: current WP can't be set"); } - if(set_mission_item(1, &_mission_item_triplet.next) == OK) { + ret = _mission.get_next_mission_item(&_mission_item_triplet.next); + + if (ret == OK) { + + add_home_pos_to_rtl(&_mission_item_triplet.next); _mission_item_triplet.next_valid = true; } else { + /* this will fail for the last WP */ _mission_item_triplet.next_valid = false; } - switch (_mission_type) { - case MISSION_TYPE_ONBOARD: - mavlink_log_info(_mavlink_fd, "[navigator] heading to onboard WP %d", _current_onboard_mission_index); - break; - case MISSION_TYPE_OFFBOARD: - mavlink_log_info(_mavlink_fd, "[navigator] heading to offboard WP %d", _current_offboard_mission_index); - break; - case MISSION_TYPE_NONE: - default: - warnx("ERROR: Mission type unsupported"); - break; - } - publish_mission_item_triplet(); } -int +void Navigator::advance_mission() { - switch (_mission_type) { - case MISSION_TYPE_ONBOARD: - warnx("advance onboard before: %d", _current_onboard_mission_index); - _current_onboard_mission_index++; - warnx("advance onboard after: %d", _current_onboard_mission_index); - break; - case MISSION_TYPE_OFFBOARD: - warnx("advance offboard before: %d", _current_offboard_mission_index); - _current_offboard_mission_index++; - warnx("advance offboard after: %d", _current_offboard_mission_index); - break; - case MISSION_TYPE_NONE: - default: - warnx("ERROR: Mission type unsupported"); - return ERROR; - } - - /* if there is no more mission available, don't advance and switch to loiter at current WP */ - if (!_mission_item_triplet.next_valid) { - warnx("no next valid"); - return ERROR; - } - /* copy current mission to previous item */ memcpy(&_mission_item_triplet.previous, &_mission_item_triplet.current, sizeof(mission_item_s)); _mission_item_triplet.previous_valid = _mission_item_triplet.current_valid; - if(set_mission_item(0, &_mission_item_triplet.current) == OK) { + int ret; + bool onboard; + unsigned index; + + ret = _mission.get_current_mission_item(&_mission_item_triplet.current, &onboard, &index); + + if (ret == OK) { + + add_home_pos_to_rtl(&_mission_item_triplet.current); _mission_item_triplet.current_valid = true; - + + if (onboard) { + mavlink_log_info(_mavlink_fd, "[navigator] heading to onboard WP %d", index); + } else { + mavlink_log_info(_mavlink_fd, "[navigator] heading to offboard WP %d", index); + } } else { - /* should never ever happen */ + /* since a mission is not advanced without WPs available, this is not supposed to happen */ _mission_item_triplet.current_valid = false; - warnx("no current available"); - return ERROR; + warnx("ERROR: current WP can't be set"); } - - if(set_mission_item(1, &_mission_item_triplet.next) == OK) { + + ret = _mission.get_next_mission_item(&_mission_item_triplet.next); + + if (ret == OK) { + + add_home_pos_to_rtl(&_mission_item_triplet.next); + _mission_item_triplet.next_valid = true; - } else { + /* this will fail for the last WP */ _mission_item_triplet.next_valid = false; } publish_mission_item_triplet(); - return OK; } void @@ -1063,23 +1047,13 @@ Navigator::start_mission_loiter() /* make sure the current WP is valid */ if (!_mission_item_triplet.current_valid) { warnx("ERROR: cannot switch to offboard mission loiter"); - return; } - set_loiter_item(&_mission_item_triplet.current); + get_loiter_item(&_mission_item_triplet.current); - switch (_mission_type) { - case MISSION_TYPE_ONBOARD: - mavlink_log_info(_mavlink_fd, "[navigator] loiter at onboard WP %d", _current_onboard_mission_index-1); - break; - case MISSION_TYPE_OFFBOARD: - mavlink_log_info(_mavlink_fd, "[navigator] loiter at offboard WP %d", _current_offboard_mission_index-1); - break; - case MISSION_TYPE_NONE: - default: - warnx("ERROR: Mission type unsupported"); - break; - } + publish_mission_item_triplet(); + + mavlink_log_info(_mavlink_fd, "[navigator] loiter at last WP"); } void @@ -1111,85 +1085,21 @@ Navigator::start_rtl() void Navigator::start_rtl_loiter() { - mission_item_s home_position_mission_item; - home_position_mission_item.lat = (double)_home_pos.lat / 1e7; - home_position_mission_item.lon = (double)_home_pos.lon / 1e7; - home_position_mission_item.altitude = _home_pos.alt / 1e3f + _parameters.min_altitude; + _mission_item_triplet.previous_valid = false; + _mission_item_triplet.current_valid = true; + _mission_item_triplet.next_valid = false; + + _mission_item_triplet.current.lat = (double)_home_pos.lat / 1e7; + _mission_item_triplet.current.lon = (double)_home_pos.lon / 1e7; + _mission_item_triplet.current.altitude = _home_pos.alt / 1e3f + _parameters.min_altitude; - set_loiter_item(&home_position_mission_item); + get_loiter_item(&_mission_item_triplet.current); + + publish_mission_item_triplet(); mavlink_log_info(_mavlink_fd, "[navigator] loiter after RTL"); } -bool -Navigator::offboard_mission_available(unsigned relative_index) -{ - return _offboard_mission_item_count > _current_offboard_mission_index + relative_index; -} - -bool -Navigator::onboard_mission_available(unsigned relative_index) -{ - return _onboard_mission_item_count > _current_onboard_mission_index + relative_index && _parameters.onboard_mission_enabled; -} - -int -Navigator::set_mission_item(unsigned relative_index, struct mission_item_s *mission_item) -{ - struct mission_item_s new_mission_item; - - /* try onboard mission first */ - if (onboard_mission_available(relative_index)) { - if (_mission_type != MISSION_TYPE_ONBOARD && relative_index == 1) { - relative_index--; - } - if (dm_read(DM_KEY_WAYPOINTS_ONBOARD, _current_onboard_mission_index + relative_index, &new_mission_item, sizeof(struct mission_item_s)) != sizeof(struct mission_item_s)) { - /* not supposed to happen unless the datamanager can't access the SD card, etc. */ - _mission_type = MISSION_TYPE_NONE; - return ERROR; - } - /* base the mission type on the current mission item, not on future ones */ - if (relative_index == 0) { - _mission_type = MISSION_TYPE_ONBOARD; - } - /* otherwise fallback to offboard */ - } else if (offboard_mission_available(relative_index)) { - - warnx("fallback try offboard: %d / %d", _current_offboard_mission_index + relative_index, _offboard_mission_item_count); - - if (_mission_type != MISSION_TYPE_OFFBOARD && relative_index == 1) { - relative_index--; - } - - if (dm_read(DM_KEY_WAYPOINTS_OFFBOARD, _current_offboard_mission_index + relative_index, &new_mission_item, sizeof(struct mission_item_s)) != sizeof(struct mission_item_s)) { - /* not supposed to happen unless the datamanager can't access the SD card, etc. */ - _mission_type = MISSION_TYPE_NONE; - warnx("failed"); - return ERROR; - } - /* base the mission type on the current mission item, not on future ones */ - if (relative_index == 0) { - _mission_type = MISSION_TYPE_OFFBOARD; - } - } else { - /* happens when no more mission items can be added as a next item */ - return ERROR; - } - - if (new_mission_item.nav_cmd == NAV_CMD_RETURN_TO_LAUNCH) { - /* if it is a RTL waypoint, append the home position */ - new_mission_item.lat = (double)_home_pos.lat / 1e7; - new_mission_item.lon = (double)_home_pos.lon / 1e7; - new_mission_item.altitude = (float)_home_pos.alt / 1e3f + _parameters.min_altitude; - new_mission_item.loiter_radius = _parameters.loiter_radius; // TODO: get rid of magic number - new_mission_item.radius = 50.0f; // TODO: get rid of magic number - } - - memcpy(mission_item, &new_mission_item, sizeof(struct mission_item_s)); - - return OK; -} - bool Navigator::mission_item_reached() { @@ -1292,24 +1202,13 @@ Navigator::mission_item_reached() } void -Navigator::set_loiter_item(struct mission_item_s *new_loiter_position) +Navigator::get_loiter_item(struct mission_item_s *new_loiter_position) { - _mission_item_triplet.previous_valid = false; - _mission_item_triplet.current_valid = true; - _mission_item_triplet.next_valid = false; - - _mission_item_triplet.current.nav_cmd = NAV_CMD_LOITER_UNLIMITED; - _mission_item_triplet.current.loiter_direction = 1; - _mission_item_triplet.current.loiter_radius = _parameters.loiter_radius; // TODO: get rid of magic number - _mission_item_triplet.current.radius = 50.0f; // TODO: get rid of magic number - _mission_item_triplet.current.autocontinue = false; - - _mission_item_triplet.current.lat = new_loiter_position->lat; - _mission_item_triplet.current.lon = new_loiter_position->lon; - _mission_item_triplet.current.altitude = new_loiter_position->altitude; - _mission_item_triplet.current.yaw = new_loiter_position->yaw; - - publish_mission_item_triplet(); + new_loiter_position->nav_cmd = NAV_CMD_LOITER_UNLIMITED; + new_loiter_position->loiter_direction = 1; + new_loiter_position->loiter_radius = _parameters.loiter_radius; // TODO: get rid of magic number + new_loiter_position->radius = 50.0f; // TODO: get rid of magic number + new_loiter_position->autocontinue = false; } void @@ -1400,3 +1299,15 @@ int navigator_main(int argc, char *argv[]) return 0; } +void +Navigator::add_home_pos_to_rtl(struct mission_item_s *new_mission_item) +{ + if (new_mission_item->nav_cmd == NAV_CMD_RETURN_TO_LAUNCH) { + /* if it is a RTL waypoint, append the home position */ + new_mission_item->lat = (double)_home_pos.lat / 1e7; + new_mission_item->lon = (double)_home_pos.lon / 1e7; + new_mission_item->altitude = (float)_home_pos.alt / 1e3f + _parameters.min_altitude; + new_mission_item->loiter_radius = _parameters.loiter_radius; // TODO: get rid of magic number + new_mission_item->radius = 50.0f; // TODO: get rid of magic number + } +} \ No newline at end of file diff --git a/src/modules/navigator/navigator_mission.cpp b/src/modules/navigator/navigator_mission.cpp new file mode 100644 index 0000000000..993f8f1331 --- /dev/null +++ b/src/modules/navigator/navigator_mission.cpp @@ -0,0 +1,234 @@ +/**************************************************************************** + * + * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * Author: @author Julian Oes + * + * 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 navigator_mission.cpp + * Helper class to access missions + */ + +// #include +// #include +// #include +// #include + +#include +#include +#include "navigator_mission.h" + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; + + +Mission::Mission() : + + _current_offboard_mission_index(0), + _current_onboard_mission_index(0), + _offboard_mission_item_count(0), + _onboard_mission_item_count(0), + _onboard_mission_allowed(false), + _current_mission_type(MISSION_TYPE_NONE) +{} + +Mission::~Mission() +{ + +} + +void +Mission::set_current_offboard_mission_index(int new_index) +{ + if (new_index != -1) { + _current_offboard_mission_index = (unsigned)new_index; + } +} + +void +Mission::set_current_onboard_mission_index(int new_index) +{ + if (new_index != -1) { + _current_onboard_mission_index = (unsigned)new_index; + } +} + +void +Mission::set_offboard_mission_count(unsigned new_count) +{ + _offboard_mission_item_count = new_count; +} + +void +Mission::set_onboard_mission_count(unsigned new_count) +{ + _onboard_mission_item_count = new_count; +} + +void +Mission::set_onboard_mission_allowed(bool allowed) +{ + _onboard_mission_allowed = allowed; +} + +bool +Mission::current_mission_available() +{ + return (current_onboard_mission_available() || current_offboard_mission_available()); + +} + +bool +Mission::next_mission_available() +{ + return (next_onboard_mission_available() || next_offboard_mission_available()); +} + +int +Mission::get_current_mission_item(struct mission_item_s *new_mission_item, bool *onboard, unsigned *index) +{ + /* try onboard mission first */ + if (current_onboard_mission_available()) { + + const ssize_t len = sizeof(struct mission_item_s); + if (dm_read(DM_KEY_WAYPOINTS_ONBOARD, _current_onboard_mission_index, new_mission_item, len) != len) { + /* not supposed to happen unless the datamanager can't access the SD card, etc. */ + return ERROR; + } + _current_mission_type = MISSION_TYPE_ONBOARD; + *onboard = true; + *index = _current_onboard_mission_index; + + /* otherwise fallback to offboard */ + } else if (current_offboard_mission_available()) { + + const ssize_t len = sizeof(struct mission_item_s); + if (dm_read(DM_KEY_WAYPOINTS_OFFBOARD, _current_offboard_mission_index, new_mission_item, len) != len) { + /* not supposed to happen unless the datamanager can't access the SD card, etc. */ + _current_mission_type = MISSION_TYPE_NONE; + return ERROR; + } + _current_mission_type = MISSION_TYPE_OFFBOARD; + *onboard = false; + *index = _current_offboard_mission_index; + + } else { + /* happens when no more mission items can be added as a next item */ + _current_mission_type = MISSION_TYPE_NONE; + return ERROR; + } + + return OK; +} + +int +Mission::get_next_mission_item(struct mission_item_s *new_mission_item) +{ + /* try onboard mission first */ + if (next_onboard_mission_available()) { + + const ssize_t len = sizeof(struct mission_item_s); + if (dm_read(DM_KEY_WAYPOINTS_ONBOARD, _current_onboard_mission_index + 1, new_mission_item, len) != len) { + /* not supposed to happen unless the datamanager can't access the SD card, etc. */ + return ERROR; + } + + /* otherwise fallback to offboard */ + } else if (next_offboard_mission_available()) { + + const ssize_t len = sizeof(struct mission_item_s); + if (dm_read(DM_KEY_WAYPOINTS_OFFBOARD, _current_offboard_mission_index + 1, new_mission_item, len) != len) { + /* not supposed to happen unless the datamanager can't access the SD card, etc. */ + return ERROR; + } + + } else { + /* happens when no more mission items can be added as a next item */ + return ERROR; + } + + return OK; +} + + +bool +Mission::current_onboard_mission_available() +{ + return _onboard_mission_item_count > _current_onboard_mission_index && _onboard_mission_allowed; +} + +bool +Mission::current_offboard_mission_available() +{ + return _offboard_mission_item_count > _current_offboard_mission_index; +} + +bool +Mission::next_onboard_mission_available() +{ + unsigned next = 0; + + if (_current_mission_type != MISSION_TYPE_ONBOARD) { + next = 1; + } + + return _onboard_mission_item_count > (_current_onboard_mission_index + next) && _onboard_mission_allowed; +} + +bool +Mission::next_offboard_mission_available() +{ + unsigned next = 0; + + if (_current_mission_type != MISSION_TYPE_OFFBOARD) { + next = 1; + } + + return _offboard_mission_item_count > (_current_offboard_mission_index + next); +} + +void +Mission::move_to_next() +{ + switch (_current_mission_type) { + case MISSION_TYPE_ONBOARD: + _current_onboard_mission_index++; + break; + case MISSION_TYPE_OFFBOARD: + _current_offboard_mission_index++; + break; + case MISSION_TYPE_NONE: + default: + break; + } +} \ No newline at end of file diff --git a/src/modules/navigator/navigator_mission.h b/src/modules/navigator/navigator_mission.h new file mode 100644 index 0000000000..e8e4763825 --- /dev/null +++ b/src/modules/navigator/navigator_mission.h @@ -0,0 +1,95 @@ +/**************************************************************************** + * + * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * Author: @author Julian Oes + * + * 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 navigator_mission.h + * Helper class to access missions + */ + +#ifndef NAVIGATOR_MISSION_H +#define NAVIGATOR_MISSION_H + +#include + + +class __EXPORT Mission +{ +public: + /** + * Constructor + */ + Mission(); + + /** + * Destructor, also kills the sensors task. + */ + ~Mission(); + + void set_current_offboard_mission_index(int new_index); + void set_current_onboard_mission_index(int new_index); + void set_offboard_mission_count(unsigned new_count); + void set_onboard_mission_count(unsigned new_count); + + void set_onboard_mission_allowed(bool allowed); + + bool current_mission_available(); + bool next_mission_available(); + + int get_current_mission_item(struct mission_item_s *mission_item, bool *onboard, unsigned *index); + int get_next_mission_item(struct mission_item_s *mission_item); + + void move_to_next(); + + void add_home_pos(struct mission_item_s *new_mission_item); + +private: + bool current_onboard_mission_available(); + bool current_offboard_mission_available(); + bool next_onboard_mission_available(); + bool next_offboard_mission_available(); + + unsigned _current_offboard_mission_index; + unsigned _current_onboard_mission_index; + unsigned _offboard_mission_item_count; /** number of offboard mission items available */ + unsigned _onboard_mission_item_count; /** number of onboard mission items available */ + + bool _onboard_mission_allowed; + + enum { + MISSION_TYPE_NONE, + MISSION_TYPE_ONBOARD, + MISSION_TYPE_OFFBOARD, + } _current_mission_type; +}; + +#endif \ No newline at end of file From a83e3cd22276109301678c204e83050483200d6b Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 18 Dec 2013 19:33:47 +0400 Subject: [PATCH 205/447] New mathlib, WIP --- makefiles/config_px4fmu-v1_test.mk | 50 ++ src/lib/conversion/rotation.cpp | 15 +- src/lib/mathlib/math/Dcm.cpp | 174 ------- src/lib/mathlib/math/Dcm.hpp | 108 ----- src/lib/mathlib/math/EulerAngles.cpp | 126 ----- src/lib/mathlib/math/EulerAngles.hpp | 74 --- src/lib/mathlib/math/Matrix.cpp | 193 -------- src/lib/mathlib/math/Matrix.hpp | 253 +++++++++- .../math/{generic/Matrix.cpp => Matrix3.cpp} | 14 +- src/lib/mathlib/math/Matrix3.hpp | 348 ++++++++++++++ src/lib/mathlib/math/Quaternion.cpp | 174 ------- src/lib/mathlib/math/Quaternion.hpp | 83 +--- src/lib/mathlib/math/Vector.cpp | 100 ---- src/lib/mathlib/math/Vector.hpp | 214 ++++++++- src/lib/mathlib/math/Vector2.hpp | 269 +++++++++++ src/lib/mathlib/math/Vector2f.hpp | 79 ---- src/lib/mathlib/math/Vector3.cpp | 99 ---- src/lib/mathlib/math/Vector3.hpp | 265 +++++++++-- src/lib/mathlib/math/arm/Matrix.cpp | 40 -- src/lib/mathlib/math/arm/Matrix.hpp | 292 ------------ src/lib/mathlib/math/arm/Vector.cpp | 40 -- src/lib/mathlib/math/arm/Vector.hpp | 236 ---------- src/lib/mathlib/math/generic/Matrix.hpp | 437 ------------------ src/lib/mathlib/math/generic/Vector.cpp | 40 -- src/lib/mathlib/math/generic/Vector.hpp | 245 ---------- src/lib/mathlib/mathlib.h | 11 +- src/lib/mathlib/module.mk | 18 +- src/systemcmds/tests/module.mk | 1 + .../tests/test_mathlib.cpp} | 115 ++--- src/systemcmds/tests/tests.h | 1 + src/systemcmds/tests/tests_main.c | 1 + 31 files changed, 1438 insertions(+), 2677 deletions(-) create mode 100644 makefiles/config_px4fmu-v1_test.mk delete mode 100644 src/lib/mathlib/math/Dcm.cpp delete mode 100644 src/lib/mathlib/math/Dcm.hpp delete mode 100644 src/lib/mathlib/math/EulerAngles.cpp delete mode 100644 src/lib/mathlib/math/EulerAngles.hpp delete mode 100644 src/lib/mathlib/math/Matrix.cpp rename src/lib/mathlib/math/{generic/Matrix.cpp => Matrix3.cpp} (87%) create mode 100644 src/lib/mathlib/math/Matrix3.hpp delete mode 100644 src/lib/mathlib/math/Quaternion.cpp delete mode 100644 src/lib/mathlib/math/Vector.cpp create mode 100644 src/lib/mathlib/math/Vector2.hpp delete mode 100644 src/lib/mathlib/math/Vector2f.hpp delete mode 100644 src/lib/mathlib/math/Vector3.cpp delete mode 100644 src/lib/mathlib/math/arm/Matrix.cpp delete mode 100644 src/lib/mathlib/math/arm/Matrix.hpp delete mode 100644 src/lib/mathlib/math/arm/Vector.cpp delete mode 100644 src/lib/mathlib/math/arm/Vector.hpp delete mode 100644 src/lib/mathlib/math/generic/Matrix.hpp delete mode 100644 src/lib/mathlib/math/generic/Vector.cpp delete mode 100644 src/lib/mathlib/math/generic/Vector.hpp rename src/{lib/mathlib/math/Vector2f.cpp => systemcmds/tests/test_mathlib.cpp} (52%) diff --git a/makefiles/config_px4fmu-v1_test.mk b/makefiles/config_px4fmu-v1_test.mk new file mode 100644 index 0000000000..42a35c5dd0 --- /dev/null +++ b/makefiles/config_px4fmu-v1_test.mk @@ -0,0 +1,50 @@ +# +# Makefile for the px4fmu_default configuration +# + +# +# Use the configuration's ROMFS. +# +ROMFS_ROOT = $(PX4_BASE)/ROMFS/px4fmu_test + +# +# Board support modules +# +MODULES += drivers/device +MODULES += drivers/stm32 +MODULES += drivers/led +MODULES += drivers/boards/px4fmu-v1 +MODULES += systemcmds/perf +MODULES += systemcmds/reboot +MODULES += systemcmds/tests +MODULES += systemcmds/nshterm + +# +# Libraries +# +LIBRARIES += lib/mathlib/CMSIS +MODULES += lib/mathlib + +# +# Library modules +# +MODULES += modules/systemlib +MODULES += modules/uORB +MODULES += modules/systemlib/mixer + +# +# Transitional support - add commands from the NuttX export archive. +# +# In general, these should move to modules over time. +# +# Each entry here is ... but we use a helper macro +# to make the table a bit more readable. +# +define _B + $(strip $1).$(or $(strip $2),SCHED_PRIORITY_DEFAULT).$(or $(strip $3),CONFIG_PTHREAD_STACK_DEFAULT).$(strip $4) +endef + +# command priority stack entrypoint +BUILTIN_COMMANDS := \ + $(call _B, sercon, , 2048, sercon_main ) \ + $(call _B, serdis, , 2048, serdis_main ) diff --git a/src/lib/conversion/rotation.cpp b/src/lib/conversion/rotation.cpp index b078562c2d..bac594ab9d 100644 --- a/src/lib/conversion/rotation.cpp +++ b/src/lib/conversion/rotation.cpp @@ -41,22 +41,11 @@ #include "rotation.h" __EXPORT void -get_rot_matrix(enum Rotation rot, math::Matrix *rot_matrix) +get_rot_matrix(enum Rotation rot, math::Matrix3f *rot_matrix) { - /* first set to zero */ - rot_matrix->Matrix::zero(3, 3); - float roll = M_DEG_TO_RAD_F * (float)rot_lookup[rot].roll; float pitch = M_DEG_TO_RAD_F * (float)rot_lookup[rot].pitch; float yaw = M_DEG_TO_RAD_F * (float)rot_lookup[rot].yaw; - math::EulerAngles euler(roll, pitch, yaw); - - math::Dcm R(euler); - - for (int i = 0; i < 3; i++) { - for (int j = 0; j < 3; j++) { - (*rot_matrix)(i, j) = R(i, j); - } - } + rot_matrix->from_euler(roll, pitch, yaw); } diff --git a/src/lib/mathlib/math/Dcm.cpp b/src/lib/mathlib/math/Dcm.cpp deleted file mode 100644 index f509f7081b..0000000000 --- a/src/lib/mathlib/math/Dcm.cpp +++ /dev/null @@ -1,174 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 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 Dcm.cpp - * - * math direction cosine matrix - */ - -#include - -#include "Dcm.hpp" -#include "Quaternion.hpp" -#include "EulerAngles.hpp" -#include "Vector3.hpp" - -namespace math -{ - -Dcm::Dcm() : - Matrix(Matrix::identity(3)) -{ -} - -Dcm::Dcm(float c00, float c01, float c02, - float c10, float c11, float c12, - float c20, float c21, float c22) : - Matrix(3, 3) -{ - Dcm &dcm = *this; - dcm(0, 0) = c00; - dcm(0, 1) = c01; - dcm(0, 2) = c02; - dcm(1, 0) = c10; - dcm(1, 1) = c11; - dcm(1, 2) = c12; - dcm(2, 0) = c20; - dcm(2, 1) = c21; - dcm(2, 2) = c22; -} - -Dcm::Dcm(const float data[3][3]) : - Matrix(3, 3) -{ - Dcm &dcm = *this; - /* set rotation matrix */ - for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++) - dcm(i, j) = data[i][j]; -} - -Dcm::Dcm(const float *data) : - Matrix(3, 3, data) -{ -} - -Dcm::Dcm(const Quaternion &q) : - Matrix(3, 3) -{ - Dcm &dcm = *this; - double a = q.getA(); - double b = q.getB(); - double c = q.getC(); - double d = q.getD(); - double aSq = a * a; - double bSq = b * b; - double cSq = c * c; - double dSq = d * d; - dcm(0, 0) = aSq + bSq - cSq - dSq; - dcm(0, 1) = 2.0 * (b * c - a * d); - dcm(0, 2) = 2.0 * (a * c + b * d); - dcm(1, 0) = 2.0 * (b * c + a * d); - dcm(1, 1) = aSq - bSq + cSq - dSq; - dcm(1, 2) = 2.0 * (c * d - a * b); - dcm(2, 0) = 2.0 * (b * d - a * c); - dcm(2, 1) = 2.0 * (a * b + c * d); - dcm(2, 2) = aSq - bSq - cSq + dSq; -} - -Dcm::Dcm(const EulerAngles &euler) : - Matrix(3, 3) -{ - Dcm &dcm = *this; - double cosPhi = cos(euler.getPhi()); - double sinPhi = sin(euler.getPhi()); - double cosThe = cos(euler.getTheta()); - double sinThe = sin(euler.getTheta()); - double cosPsi = cos(euler.getPsi()); - double sinPsi = sin(euler.getPsi()); - - dcm(0, 0) = cosThe * cosPsi; - dcm(0, 1) = -cosPhi * sinPsi + sinPhi * sinThe * cosPsi; - dcm(0, 2) = sinPhi * sinPsi + cosPhi * sinThe * cosPsi; - - dcm(1, 0) = cosThe * sinPsi; - dcm(1, 1) = cosPhi * cosPsi + sinPhi * sinThe * sinPsi; - dcm(1, 2) = -sinPhi * cosPsi + cosPhi * sinThe * sinPsi; - - dcm(2, 0) = -sinThe; - dcm(2, 1) = sinPhi * cosThe; - dcm(2, 2) = cosPhi * cosThe; -} - -Dcm::Dcm(const Dcm &right) : - Matrix(right) -{ -} - -Dcm::~Dcm() -{ -} - -int __EXPORT dcmTest() -{ - printf("Test DCM\t\t: "); - // default ctor - ASSERT(matrixEqual(Dcm(), - Matrix::identity(3))); - // quaternion ctor - ASSERT(matrixEqual( - Dcm(Quaternion(0.983347f, 0.034271f, 0.106021f, 0.143572f)), - Dcm(0.9362934f, -0.2750958f, 0.2183507f, - 0.2896295f, 0.9564251f, -0.0369570f, - -0.1986693f, 0.0978434f, 0.9751703f))); - // euler angle ctor - ASSERT(matrixEqual( - Dcm(EulerAngles(0.1f, 0.2f, 0.3f)), - Dcm(0.9362934f, -0.2750958f, 0.2183507f, - 0.2896295f, 0.9564251f, -0.0369570f, - -0.1986693f, 0.0978434f, 0.9751703f))); - // rotations - Vector3 vB(1, 2, 3); - ASSERT(vectorEqual(Vector3(-2.0f, 1.0f, 3.0f), - Dcm(EulerAngles(0.0f, 0.0f, M_PI_2_F))*vB)); - ASSERT(vectorEqual(Vector3(3.0f, 2.0f, -1.0f), - Dcm(EulerAngles(0.0f, M_PI_2_F, 0.0f))*vB)); - ASSERT(vectorEqual(Vector3(1.0f, -3.0f, 2.0f), - Dcm(EulerAngles(M_PI_2_F, 0.0f, 0.0f))*vB)); - ASSERT(vectorEqual(Vector3(3.0f, 2.0f, -1.0f), - Dcm(EulerAngles( - M_PI_2_F, M_PI_2_F, M_PI_2_F))*vB)); - printf("PASS\n"); - return 0; -} -} // namespace math diff --git a/src/lib/mathlib/math/Dcm.hpp b/src/lib/mathlib/math/Dcm.hpp deleted file mode 100644 index df8970d3aa..0000000000 --- a/src/lib/mathlib/math/Dcm.hpp +++ /dev/null @@ -1,108 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 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 Dcm.hpp - * - * math direction cosine matrix - */ - -//#pragma once - -#include "Vector.hpp" -#include "Matrix.hpp" - -namespace math -{ - -class Quaternion; -class EulerAngles; - -/** - * This is a Tait Bryan, Body 3-2-1 sequence. - * (yaw)-(pitch)-(roll) - * The Dcm transforms a vector in the body frame - * to the navigation frame, typically represented - * as C_nb. C_bn can be obtained through use - * of the transpose() method. - */ -class __EXPORT Dcm : public Matrix -{ -public: - /** - * default ctor - */ - Dcm(); - - /** - * scalar ctor - */ - Dcm(float c00, float c01, float c02, - float c10, float c11, float c12, - float c20, float c21, float c22); - - /** - * data ctor - */ - Dcm(const float *data); - - /** - * array ctor - */ - Dcm(const float data[3][3]); - - /** - * quaternion ctor - */ - Dcm(const Quaternion &q); - - /** - * euler angles ctor - */ - Dcm(const EulerAngles &euler); - - /** - * copy ctor (deep) - */ - Dcm(const Dcm &right); - - /** - * dtor - */ - virtual ~Dcm(); -}; - -int __EXPORT dcmTest(); - -} // math - diff --git a/src/lib/mathlib/math/EulerAngles.cpp b/src/lib/mathlib/math/EulerAngles.cpp deleted file mode 100644 index e733d23bb0..0000000000 --- a/src/lib/mathlib/math/EulerAngles.cpp +++ /dev/null @@ -1,126 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 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 Vector.cpp - * - * math vector - */ - -#include "test/test.hpp" - -#include "EulerAngles.hpp" -#include "Quaternion.hpp" -#include "Dcm.hpp" -#include "Vector3.hpp" - -namespace math -{ - -EulerAngles::EulerAngles() : - Vector(3) -{ - setPhi(0.0f); - setTheta(0.0f); - setPsi(0.0f); -} - -EulerAngles::EulerAngles(float phi, float theta, float psi) : - Vector(3) -{ - setPhi(phi); - setTheta(theta); - setPsi(psi); -} - -EulerAngles::EulerAngles(const Quaternion &q) : - Vector(3) -{ - (*this) = EulerAngles(Dcm(q)); -} - -EulerAngles::EulerAngles(const Dcm &dcm) : - Vector(3) -{ - setTheta(asinf(-dcm(2, 0))); - - if (fabsf(getTheta() - M_PI_2_F) < 1.0e-3f) { - setPhi(0.0f); - setPsi(atan2f(dcm(1, 2) - dcm(0, 1), - dcm(0, 2) + dcm(1, 1)) + getPhi()); - - } else if (fabsf(getTheta() + M_PI_2_F) < 1.0e-3f) { - setPhi(0.0f); - setPsi(atan2f(dcm(1, 2) - dcm(0, 1), - dcm(0, 2) + dcm(1, 1)) - getPhi()); - - } else { - setPhi(atan2f(dcm(2, 1), dcm(2, 2))); - setPsi(atan2f(dcm(1, 0), dcm(0, 0))); - } -} - -EulerAngles::~EulerAngles() -{ -} - -int __EXPORT eulerAnglesTest() -{ - printf("Test EulerAngles\t: "); - EulerAngles euler(0.1f, 0.2f, 0.3f); - - // test ctor - ASSERT(vectorEqual(Vector3(0.1f, 0.2f, 0.3f), euler)); - ASSERT(equal(euler.getPhi(), 0.1f)); - ASSERT(equal(euler.getTheta(), 0.2f)); - ASSERT(equal(euler.getPsi(), 0.3f)); - - // test dcm ctor - euler = Dcm(EulerAngles(0.1f, 0.2f, 0.3f)); - ASSERT(vectorEqual(Vector3(0.1f, 0.2f, 0.3f), euler)); - - // test quat ctor - euler = Quaternion(EulerAngles(0.1f, 0.2f, 0.3f)); - ASSERT(vectorEqual(Vector3(0.1f, 0.2f, 0.3f), euler)); - - // test assignment - euler.setPhi(0.4f); - euler.setTheta(0.5f); - euler.setPsi(0.6f); - ASSERT(vectorEqual(Vector3(0.4f, 0.5f, 0.6f), euler)); - - printf("PASS\n"); - return 0; -} - -} // namespace math diff --git a/src/lib/mathlib/math/EulerAngles.hpp b/src/lib/mathlib/math/EulerAngles.hpp deleted file mode 100644 index 399eecfa7d..0000000000 --- a/src/lib/mathlib/math/EulerAngles.hpp +++ /dev/null @@ -1,74 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 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 Vector.h - * - * math vector - */ - -#pragma once - -#include "Vector.hpp" - -namespace math -{ - -class Quaternion; -class Dcm; - -class __EXPORT EulerAngles : public Vector -{ -public: - EulerAngles(); - EulerAngles(float phi, float theta, float psi); - EulerAngles(const Quaternion &q); - EulerAngles(const Dcm &dcm); - virtual ~EulerAngles(); - - // alias - void setPhi(float phi) { (*this)(0) = phi; } - void setTheta(float theta) { (*this)(1) = theta; } - void setPsi(float psi) { (*this)(2) = psi; } - - // const accessors - const float &getPhi() const { return (*this)(0); } - const float &getTheta() const { return (*this)(1); } - const float &getPsi() const { return (*this)(2); } - -}; - -int __EXPORT eulerAnglesTest(); - -} // math - diff --git a/src/lib/mathlib/math/Matrix.cpp b/src/lib/mathlib/math/Matrix.cpp deleted file mode 100644 index ebd1aeda3d..0000000000 --- a/src/lib/mathlib/math/Matrix.cpp +++ /dev/null @@ -1,193 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 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 Matrix.cpp - * - * matrix code - */ - -#include "test/test.hpp" -#include - -#include "Matrix.hpp" - -namespace math -{ - -static const float data_testA[] = { - 1, 2, 3, - 4, 5, 6 -}; -static Matrix testA(2, 3, data_testA); - -static const float data_testB[] = { - 0, 1, 3, - 7, -1, 2 -}; -static Matrix testB(2, 3, data_testB); - -static const float data_testC[] = { - 0, 1, - 2, 1, - 3, 2 -}; -static Matrix testC(3, 2, data_testC); - -static const float data_testD[] = { - 0, 1, 2, - 2, 1, 4, - 5, 2, 0 -}; -static Matrix testD(3, 3, data_testD); - -static const float data_testE[] = { - 1, -1, 2, - 0, 2, 3, - 2, -1, 1 -}; -static Matrix testE(3, 3, data_testE); - -static const float data_testF[] = { - 3.777e006f, 2.915e007f, 0.000e000f, - 2.938e007f, 2.267e008f, 0.000e000f, - 0.000e000f, 0.000e000f, 6.033e008f -}; -static Matrix testF(3, 3, data_testF); - -int __EXPORT matrixTest() -{ - matrixAddTest(); - matrixSubTest(); - matrixMultTest(); - matrixInvTest(); - matrixDivTest(); - return 0; -} - -int matrixAddTest() -{ - printf("Test Matrix Add\t\t: "); - Matrix r = testA + testB; - float data_test[] = { - 1.0f, 3.0f, 6.0f, - 11.0f, 4.0f, 8.0f - }; - ASSERT(matrixEqual(Matrix(2, 3, data_test), r)); - printf("PASS\n"); - return 0; -} - -int matrixSubTest() -{ - printf("Test Matrix Sub\t\t: "); - Matrix r = testA - testB; - float data_test[] = { - 1.0f, 1.0f, 0.0f, - -3.0f, 6.0f, 4.0f - }; - ASSERT(matrixEqual(Matrix(2, 3, data_test), r)); - printf("PASS\n"); - return 0; -} - -int matrixMultTest() -{ - printf("Test Matrix Mult\t: "); - Matrix r = testC * testB; - float data_test[] = { - 7.0f, -1.0f, 2.0f, - 7.0f, 1.0f, 8.0f, - 14.0f, 1.0f, 13.0f - }; - ASSERT(matrixEqual(Matrix(3, 3, data_test), r)); - printf("PASS\n"); - return 0; -} - -int matrixInvTest() -{ - printf("Test Matrix Inv\t\t: "); - Matrix origF = testF; - Matrix r = testF.inverse(); - float data_test[] = { - -0.0012518f, 0.0001610f, 0.0000000f, - 0.0001622f, -0.0000209f, 0.0000000f, - 0.0000000f, 0.0000000f, 1.6580e-9f - }; - ASSERT(matrixEqual(Matrix(3, 3, data_test), r)); - // make sure F in unchanged - ASSERT(matrixEqual(origF, testF)); - printf("PASS\n"); - return 0; -} - -int matrixDivTest() -{ - printf("Test Matrix Div\t\t: "); - Matrix r = testD / testE; - float data_test[] = { - 0.2222222f, 0.5555556f, -0.1111111f, - 0.0f, 1.0f, 1.0, - -4.1111111f, 1.2222222f, 4.5555556f - }; - ASSERT(matrixEqual(Matrix(3, 3, data_test), r)); - printf("PASS\n"); - return 0; -} - -bool matrixEqual(const Matrix &a, const Matrix &b, float eps) -{ - if (a.getRows() != b.getRows()) { - printf("row number not equal a: %d, b:%d\n", a.getRows(), b.getRows()); - return false; - - } else if (a.getCols() != b.getCols()) { - printf("column number not equal a: %d, b:%d\n", a.getCols(), b.getCols()); - return false; - } - - bool ret = true; - - for (size_t i = 0; i < a.getRows(); i++) - for (size_t j = 0; j < a.getCols(); j++) { - if (!equal(a(i, j), b(i, j), eps)) { - printf("element mismatch (%d, %d)\n", i, j); - ret = false; - } - } - - return ret; -} - -} // namespace math diff --git a/src/lib/mathlib/math/Matrix.hpp b/src/lib/mathlib/math/Matrix.hpp index f19db15eca..f05360a193 100644 --- a/src/lib/mathlib/math/Matrix.hpp +++ b/src/lib/mathlib/math/Matrix.hpp @@ -1,6 +1,8 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Author: Will Perone + * Anton Babushkin * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -32,30 +34,241 @@ ****************************************************************************/ /** - * @file Matrix.h + * @file Matrix3.hpp * - * matrix code + * 3x3 Matrix */ -#pragma once +#ifndef MATRIX_HPP +#define MATRIX_HPP -#include - -#if defined(CONFIG_ARCH_CORTEXM4) && defined(CONFIG_ARCH_FPU) -#include "arm/Matrix.hpp" -#else -#include "generic/Matrix.hpp" -#endif +#include "../CMSIS/Include/arm_math.h" namespace math { + +template class Matrix; -int matrixTest(); -int matrixAddTest(); -int matrixSubTest(); -int matrixMultTest(); -int matrixInvTest(); -int matrixDivTest(); -int matrixArmTest(); -bool matrixEqual(const Matrix &a, const Matrix &b, float eps = 1.0e-5f); -} // namespace math + +// MxN matrix with float elements +template +class MatrixBase { +public: + /** + * matrix data[row][col] + */ + float data[M][N]; + + /** + * struct for using arm_math functions + */ + arm_matrix_instance_f32 arm_mat; + + /** + * trivial ctor + * note that this ctor will not initialize elements + */ + MatrixBase() { + //arm_mat_init_f32(&arm_mat, M, N, data); + arm_mat = {M, N, &data[0][0]}; + } + + /** + * access by index + */ + inline float &operator ()(unsigned int row, unsigned int col) { + return data[row][col]; + } + + /** + * access by index + */ + inline const float &operator ()(unsigned int row, unsigned int col) const { + return data[row][col]; + } + + unsigned int getRows() { + return M; + } + + unsigned int getCols() { + return N; + } + + /** + * test for equality + */ + bool operator ==(const MatrixBase &m) { + for (unsigned int i = 0; i < M; i++) + for (unsigned int j = 0; j < N; j++) + if (data[i][j] != m(i, j)) + return false; + return true; + } + + /** + * test for inequality + */ + bool operator !=(const MatrixBase &m) { + for (unsigned int i = 0; i < M; i++) + for (unsigned int j = 0; j < N; j++) + if (data[i][j] != m(i, j)) + return true; + return false; + } + + /** + * set to value + */ + const MatrixBase &operator =(const MatrixBase &m) { + memcpy(data, m.data, sizeof(data)); + return *this; + } + + /** + * negation + */ + MatrixBase operator -(void) const { + MatrixBase res; + for (unsigned int i = 0; i < N; i++) + for (unsigned int j = 0; j < M; j++) + res[i][j] = -data[i][j]; + return res; + } + + /** + * addition + */ + MatrixBase operator +(const MatrixBase &m) const { + MatrixBase res; + for (unsigned int i = 0; i < N; i++) + for (unsigned int j = 0; j < M; j++) + res[i][j] = data[i][j] + m(i, j); + return res; + } + + MatrixBase &operator +=(const MatrixBase &m) { + return *this = *this + m; + } + + /** + * subtraction + */ + MatrixBase operator -(const MatrixBase &m) const { + MatrixBase res; + for (unsigned int i = 0; i < M; i++) + for (unsigned int j = 0; j < N; j++) + res[i][j] = data[i][j] - m(i, j); + return res; + } + + MatrixBase &operator -=(const MatrixBase &m) { + return *this = *this - m; + } + + /** + * uniform scaling + */ + MatrixBase operator *(const float num) const { + MatrixBase res; + for (unsigned int i = 0; i < M; i++) + for (unsigned int j = 0; j < N; j++) + res[i][j] = data[i][j] * num; + return res; + } + + MatrixBase &operator *=(const float num) { + return *this = *this * num; + } + + MatrixBase operator /(const float num) const { + MatrixBase res; + for (unsigned int i = 0; i < M; i++) + for (unsigned int j = 0; j < N; j++) + res[i][j] = data[i][j] / num; + return res; + } + + MatrixBase &operator /=(const float num) { + return *this = *this / num; + } + + /** + * multiplication by another matrix + */ + template + Matrix operator *(const Matrix &m) const { + Matrix res; + arm_mat_mult_f32(&arm_mat, &m.arm_mat, &res.arm_mat); + return res; + } + + /** + * setup the identity matrix + */ + void identity(void) { + memset(data, 0, sizeof(data)); + for (unsigned int i = 0; i < M; i++) + data[i][i] = 1; + } + + void dump(void) { + for (unsigned int i = 0; i < M; i++) { + for (unsigned int j = 0; j < N; j++) + printf("%.3f\t", data[i][j]); + printf("\n"); + } + } +}; + +template +class Matrix : public MatrixBase { +public: + /** + * set to value + */ + const Matrix &operator =(const Matrix &m) { + memcpy(this->data, m.data, sizeof(this->data)); + return *this; + } + + /** + * multiplication by a vector + */ + /* + Vector operator *(const Vector &v) const { + Vector res; + arm_mat_mult_f32(&this->arm_mat, &v.arm_col, &res.arm_col); + return res; + } + */ +}; + +template <> +class Matrix<3, 3> : public MatrixBase<3, 3> { +public: + /** + * set to value + */ + const Matrix<3, 3> &operator =(const Matrix<3, 3> &m) { + memcpy(this->data, m.data, sizeof(this->data)); + return *this; + } + + /** + * multiplication by a vector + */ + /* + Vector<3> operator *(const Vector<3> &v) const { + Vector<3> res; + res(0) = data[0][0] * v(0) + data[0][1] * v(1) + data[0][2] * v(2); + res(1) = data[1][0] * v(0) + data[1][1] * v(1) + data[1][2] * v(2); + res(2) = data[2][0] * v(0) + data[2][1] * v(1) + data[2][2] * v(2); + return res; + } + */ +}; + +} + +#endif // MATRIX_HPP diff --git a/src/lib/mathlib/math/generic/Matrix.cpp b/src/lib/mathlib/math/Matrix3.cpp similarity index 87% rename from src/lib/mathlib/math/generic/Matrix.cpp rename to src/lib/mathlib/math/Matrix3.cpp index 21661622ab..3f1259dc40 100644 --- a/src/lib/mathlib/math/generic/Matrix.cpp +++ b/src/lib/mathlib/math/Matrix3.cpp @@ -1,6 +1,8 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Author: Will Perone + * Anton Babushkin * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -32,9 +34,13 @@ ****************************************************************************/ /** - * @file Matrix.cpp + * @file Matrix3.cpp * - * matrix code + * 3x3 Matrix */ -#include "Matrix.hpp" +#include "Matrix3.hpp" + +namespace math +{ +} diff --git a/src/lib/mathlib/math/Matrix3.hpp b/src/lib/mathlib/math/Matrix3.hpp new file mode 100644 index 0000000000..3236cd3d10 --- /dev/null +++ b/src/lib/mathlib/math/Matrix3.hpp @@ -0,0 +1,348 @@ +/**************************************************************************** + * + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Author: Will Perone + * Anton Babushkin + * + * 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 Matrix3.hpp + * + * 3x3 Matrix + */ + +#ifndef MATRIX3_HPP +#define MATRIX3_HPP + +#include "Vector3.hpp" +#include "../CMSIS/Include/arm_math.h" + +namespace math +{ + +// 3x3 matrix with elements of type T +template +class Matrix3 { +public: + /** + * matrix data[row][col] + */ + T data[3][3]; + + /** + * struct for using arm_math functions + */ + arm_matrix_instance_f32 arm_mat; + + /** + * trivial ctor + * note that this ctor will not initialize elements + */ + Matrix3() { + arm_mat = {3, 3, &data[0][0]}; + } + + /** + * setting ctor + */ + Matrix3(const T d[3][3]) { + memcpy(data, d, sizeof(data)); + arm_mat = {3, 3, &data[0][0]}; + } + + /** + * setting ctor + */ + Matrix3(const T ax, const T ay, const T az, const T bx, const T by, const T bz, const T cx, const T cy, const T cz) { + data[0][0] = ax; + data[0][1] = ay; + data[0][2] = az; + data[1][0] = bx; + data[1][1] = by; + data[1][2] = bz; + data[2][0] = cx; + data[2][1] = cy; + data[2][2] = cz; + arm_mat = {3, 3, &data[0][0]}; + } + + /** + * casting from a vector3f to a matrix is the tilde operator + */ + Matrix3(const Vector3 &v) { + arm_mat = {3, 3, &data[0][0]}; + data[0][0] = 0; + data[0][1] = -v.z; + data[0][2] = v.y; + data[1][0] = v.z; + data[1][1] = 0; + data[1][2] = -v.x; + data[2][0] = -v.y; + data[2][1] = v.x; + data[2][2] = 0; + } + + /** + * access by index + */ + inline T &operator ()(unsigned int row, unsigned int col) { + return data[row][col]; + } + + /** + * access to elements by index + */ + inline const T &operator ()(unsigned int row, unsigned int col) const { + return data[row][col]; + } + + /** + * test for equality + */ + bool operator ==(const Matrix3 &m) { + for (int i = 0; i < 3; i++) + for (int j = 0; j < 3; j++) + if (data[i][j] != m(i, j)) + return false; + return true; + } + + /** + * test for inequality + */ + bool operator !=(const Matrix3 &m) { + for (int i = 0; i < 3; i++) + for (int j = 0; j < 3; j++) + if (data[i][j] != m(i, j)) + return true; + return false; + } + + /** + * negation + */ + Matrix3 operator -(void) const { + Matrix3 res; + for (int i = 0; i < 3; i++) + for (int j = 0; j < 3; j++) + res[i][j] = -data[i][j]; + return res; + } + + /** + * addition + */ + Matrix3 operator +(const Matrix3 &m) const { + Matrix3 res; + for (int i = 0; i < 3; i++) + for (int j = 0; j < 3; j++) + res[i][j] = data[i][j] + m(i, j); + return res; + } + + Matrix3 &operator +=(const Matrix3 &m) { + return *this = *this + m; + } + + /** + * subtraction + */ + Matrix3 operator -(const Matrix3 &m) const { + Matrix3 res; + for (int i = 0; i < 3; i++) + for (int j = 0; j < 3; j++) + res[i][j] = data[i][j] - m(i, j); + return res; + } + + Matrix3 &operator -=(const Matrix3 &m) { + return *this = *this - m; + } + + /** + * uniform scaling + */ + Matrix3 operator *(const T num) const { + Matrix3 res; + for (int i = 0; i < 3; i++) + for (int j = 0; j < 3; j++) + res[i][j] = data[i][j] * num; + return res; + } + + Matrix3 &operator *=(const T num) { + return *this = *this * num; + } + + Matrix3 operator /(const T num) const { + Matrix3 res; + for (int i = 0; i < 3; i++) + for (int j = 0; j < 3; j++) + res[i][j] = data[i][j] / num; + return res; + } + + Matrix3 &operator /=(const T num) { + return *this = *this / num; + } + + /** + * multiplication by a vector + */ + Vector3 operator *(const Vector3 &v) const { + return Vector3( + data[0][0] * v.x + data[0][1] * v.y + data[0][2] * v.z, + data[1][0] * v.x + data[1][1] * v.y + data[1][2] * v.z, + data[2][0] * v.x + data[2][1] * v.y + data[2][2] * v.z); + } + + /** + * multiplication of transpose by a vector + */ + Vector3 mul_transpose(const Vector3 &v) const { + return Vector3( + data[0][0] * v.x + data[1][0] * v.y + data[2][0] * v.z, + data[0][1] * v.x + data[1][1] * v.y + data[2][1] * v.z, + data[0][2] * v.x + data[1][2] * v.y + data[2][2] * v.z); + } + + /** + * multiplication by another matrix + */ + Matrix3 operator *(const Matrix3 &m) const { +#if defined(CONFIG_ARCH_CORTEXM4) && defined(CONFIG_ARCH_FPU) + Matrix3 res; + arm_mat_mult_f32(&arm_mat, &m.arm_mat, &res.arm_mat); + return res; +#else + return Matrix3(data[0][0] * m(0, 0) + data[0][1] * m(1, 0) + data[0][2] * m(2, 0), + data[0][0] * m(0, 1) + data[0][1] * m(1, 1) + data[0][2] * m(2, 1), + data[0][0] * m(0, 2) + data[0][1] * m(1, 2) + data[0][2] * m(2, 2), + data[1][0] * m(0, 0) + data[1][1] * m(1, 0) + data[1][2] * m(2, 0), + data[1][0] * m(0, 1) + data[1][1] * m(1, 1) + data[1][2] * m(2, 1), + data[1][0] * m(0, 2) + data[1][1] * m(1, 2) + data[1][2] * m(2, 2), + data[2][0] * m(0, 0) + data[2][1] * m(1, 0) + data[2][2] * m(2, 0), + data[2][0] * m(0, 1) + data[2][1] * m(1, 1) + data[2][2] * m(2, 1), + data[2][0] * m(0, 2) + data[2][1] * m(1, 2) + data[2][2] * m(2, 2)); +#endif + } + + Matrix3 &operator *=(const Matrix3 &m) { + return *this = *this * m; + } + + /** + * transpose the matrix + */ + Matrix3 transposed(void) const { +#if defined(CONFIG_ARCH_CORTEXM4) && defined(CONFIG_ARCH_FPU) && T == float + Matrix3 res; + arm_mat_trans_f32(&arm_mat, &res.arm_mat); + return res; +#else + return Matrix3(data[0][0], data[1][0], data[2][0], + data[0][1], data[1][1], data[2][1], + data[0][2], data[1][2], data[2][2]); +#endif + } + + /** + * inverse the matrix + */ + Matrix3 inversed(void) const { + Matrix3 res; + arm_mat_inverse_f32(&arm_mat, &res.arm_mat); + return res; + } + + /** + * zero the matrix + */ + void zero(void) { + memset(data, 0, sizeof(data)); + } + + /** + * setup the identity matrix + */ + void identity(void) { + memset(data, 0, sizeof(data)); + data[0][0] = 1; + data[1][1] = 1; + data[2][2] = 1; + } + + /** + * check if any elements are NAN + */ + bool is_nan(void) { + for (int i = 0; i < 3; i++) + for (int j = 0; j < 3; j++) + if (isnan(data[i][j])) + return true; + return false; + } + + /** + * create a rotation matrix from given euler angles + * based on http://gentlenav.googlecode.com/files/EulerAngles.pdf + */ + void from_euler(T roll, T pitch, T yaw) { + T cp = (T)cosf(pitch); + T sp = (T)sinf(pitch); + T sr = (T)sinf(roll); + T cr = (T)cosf(roll); + T sy = (T)sinf(yaw); + T cy = (T)cosf(yaw); + + data[0][0] = cp * cy; + data[0][1] = (sr * sp * cy) - (cr * sy); + data[0][2] = (cr * sp * cy) + (sr * sy); + data[1][0] = cp * sy; + data[1][1] = (sr * sp * sy) + (cr * cy); + data[1][2] = (cr * sp * sy) - (sr * cy); + data[2][0] = -sp; + data[2][1] = sr * cp; + data[2][2] = cr * cp; + } + + // create eulers from a rotation matrix + //void to_euler(float *roll, float *pitch, float *yaw); + + // apply an additional rotation from a body frame gyro vector + // to a rotation matrix. + //void rotate(const Vector3 &g); +}; + +typedef Matrix3 Matrix3f; +} + +#endif // MATRIX3_HPP diff --git a/src/lib/mathlib/math/Quaternion.cpp b/src/lib/mathlib/math/Quaternion.cpp deleted file mode 100644 index 02fec4ca66..0000000000 --- a/src/lib/mathlib/math/Quaternion.cpp +++ /dev/null @@ -1,174 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 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 Quaternion.cpp - * - * math vector - */ - -#include "test/test.hpp" - - -#include "Quaternion.hpp" -#include "Dcm.hpp" -#include "EulerAngles.hpp" - -namespace math -{ - -Quaternion::Quaternion() : - Vector(4) -{ - setA(1.0f); - setB(0.0f); - setC(0.0f); - setD(0.0f); -} - -Quaternion::Quaternion(float a, float b, - float c, float d) : - Vector(4) -{ - setA(a); - setB(b); - setC(c); - setD(d); -} - -Quaternion::Quaternion(const float *data) : - Vector(4, data) -{ -} - -Quaternion::Quaternion(const Vector &v) : - Vector(v) -{ -} - -Quaternion::Quaternion(const Dcm &dcm) : - Vector(4) -{ - // avoiding singularities by not using - // division equations - setA(0.5 * sqrt(1.0 + - double(dcm(0, 0) + dcm(1, 1) + dcm(2, 2)))); - setB(0.5 * sqrt(1.0 + - double(dcm(0, 0) - dcm(1, 1) - dcm(2, 2)))); - setC(0.5 * sqrt(1.0 + - double(-dcm(0, 0) + dcm(1, 1) - dcm(2, 2)))); - setD(0.5 * sqrt(1.0 + - double(-dcm(0, 0) - dcm(1, 1) + dcm(2, 2)))); -} - -Quaternion::Quaternion(const EulerAngles &euler) : - Vector(4) -{ - double cosPhi_2 = cos(double(euler.getPhi()) / 2.0); - double sinPhi_2 = sin(double(euler.getPhi()) / 2.0); - double cosTheta_2 = cos(double(euler.getTheta()) / 2.0); - double sinTheta_2 = sin(double(euler.getTheta()) / 2.0); - double cosPsi_2 = cos(double(euler.getPsi()) / 2.0); - double sinPsi_2 = sin(double(euler.getPsi()) / 2.0); - setA(cosPhi_2 * cosTheta_2 * cosPsi_2 + - sinPhi_2 * sinTheta_2 * sinPsi_2); - setB(sinPhi_2 * cosTheta_2 * cosPsi_2 - - cosPhi_2 * sinTheta_2 * sinPsi_2); - setC(cosPhi_2 * sinTheta_2 * cosPsi_2 + - sinPhi_2 * cosTheta_2 * sinPsi_2); - setD(cosPhi_2 * cosTheta_2 * sinPsi_2 - - sinPhi_2 * sinTheta_2 * cosPsi_2); -} - -Quaternion::Quaternion(const Quaternion &right) : - Vector(right) -{ -} - -Quaternion::~Quaternion() -{ -} - -Vector Quaternion::derivative(const Vector &w) -{ -#ifdef QUATERNION_ASSERT - ASSERT(w.getRows() == 3); -#endif - float dataQ[] = { - getA(), -getB(), -getC(), -getD(), - getB(), getA(), -getD(), getC(), - getC(), getD(), getA(), -getB(), - getD(), -getC(), getB(), getA() - }; - Vector v(4); - v(0) = 0.0f; - v(1) = w(0); - v(2) = w(1); - v(3) = w(2); - Matrix Q(4, 4, dataQ); - return Q * v * 0.5f; -} - -int __EXPORT quaternionTest() -{ - printf("Test Quaternion\t\t: "); - // test default ctor - Quaternion q; - ASSERT(equal(q.getA(), 1.0f)); - ASSERT(equal(q.getB(), 0.0f)); - ASSERT(equal(q.getC(), 0.0f)); - ASSERT(equal(q.getD(), 0.0f)); - // test float ctor - q = Quaternion(0.1825742f, 0.3651484f, 0.5477226f, 0.7302967f); - ASSERT(equal(q.getA(), 0.1825742f)); - ASSERT(equal(q.getB(), 0.3651484f)); - ASSERT(equal(q.getC(), 0.5477226f)); - ASSERT(equal(q.getD(), 0.7302967f)); - // test euler ctor - q = Quaternion(EulerAngles(0.1f, 0.2f, 0.3f)); - ASSERT(vectorEqual(q, Quaternion(0.983347f, 0.034271f, 0.106021f, 0.143572f))); - // test dcm ctor - q = Quaternion(Dcm()); - ASSERT(vectorEqual(q, Quaternion(1.0f, 0.0f, 0.0f, 0.0f))); - // TODO test derivative - // test accessors - q.setA(0.1f); - q.setB(0.2f); - q.setC(0.3f); - q.setD(0.4f); - ASSERT(vectorEqual(q, Quaternion(0.1f, 0.2f, 0.3f, 0.4f))); - printf("PASS\n"); - return 0; -} - -} // namespace math diff --git a/src/lib/mathlib/math/Quaternion.hpp b/src/lib/mathlib/math/Quaternion.hpp index 048a55d332..64b980ad81 100644 --- a/src/lib/mathlib/math/Quaternion.hpp +++ b/src/lib/mathlib/math/Quaternion.hpp @@ -1,6 +1,8 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Author: Will Perone + * Anton Babushkin * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -34,82 +36,35 @@ /** * @file Quaternion.hpp * - * math quaternion lib + * Quaternion */ -#pragma once +#ifndef QUATERNION_HPP +#define QUATERNION_HPP -#include "Vector.hpp" -#include "Matrix.hpp" +#include +#include "../CMSIS/Include/arm_math.h" namespace math { -class Dcm; -class EulerAngles; - -class __EXPORT Quaternion : public Vector -{ +class Quaternion { public: + float real; + Vector3 imag; /** - * default ctor + * trivial ctor */ - Quaternion(); + Quaternion() { + } /** - * ctor from floats + * setting ctor */ - Quaternion(float a, float b, float c, float d); - - /** - * ctor from data - */ - Quaternion(const float *data); - - /** - * ctor from Vector - */ - Quaternion(const Vector &v); - - /** - * ctor from EulerAngles - */ - Quaternion(const EulerAngles &euler); - - /** - * ctor from Dcm - */ - Quaternion(const Dcm &dcm); - - /** - * deep copy ctor - */ - Quaternion(const Quaternion &right); - - /** - * dtor - */ - virtual ~Quaternion(); - - /** - * derivative - */ - Vector derivative(const Vector &w); - - /** - * accessors - */ - void setA(float a) { (*this)(0) = a; } - void setB(float b) { (*this)(1) = b; } - void setC(float c) { (*this)(2) = c; } - void setD(float d) { (*this)(3) = d; } - const float &getA() const { return (*this)(0); } - const float &getB() const { return (*this)(1); } - const float &getC() const { return (*this)(2); } - const float &getD() const { return (*this)(3); } + Quaternion(const float a, const float b, const float c, const float d): real(a), imag(b, c, d) { + } }; +} -int __EXPORT quaternionTest(); -} // math - +#endif // QUATERNION_HPP diff --git a/src/lib/mathlib/math/Vector.cpp b/src/lib/mathlib/math/Vector.cpp deleted file mode 100644 index 35158a396c..0000000000 --- a/src/lib/mathlib/math/Vector.cpp +++ /dev/null @@ -1,100 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 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 Vector.cpp - * - * math vector - */ - -#include "test/test.hpp" - -#include "Vector.hpp" - -namespace math -{ - -static const float data_testA[] = {1, 3}; -static const float data_testB[] = {4, 1}; - -static Vector testA(2, data_testA); -static Vector testB(2, data_testB); - -int __EXPORT vectorTest() -{ - vectorAddTest(); - vectorSubTest(); - return 0; -} - -int vectorAddTest() -{ - printf("Test Vector Add\t\t: "); - Vector r = testA + testB; - float data_test[] = {5.0f, 4.0f}; - ASSERT(vectorEqual(Vector(2, data_test), r)); - printf("PASS\n"); - return 0; -} - -int vectorSubTest() -{ - printf("Test Vector Sub\t\t: "); - Vector r(2); - r = testA - testB; - float data_test[] = { -3.0f, 2.0f}; - ASSERT(vectorEqual(Vector(2, data_test), r)); - printf("PASS\n"); - return 0; -} - -bool vectorEqual(const Vector &a, const Vector &b, float eps) -{ - if (a.getRows() != b.getRows()) { - printf("row number not equal a: %d, b:%d\n", a.getRows(), b.getRows()); - return false; - } - - bool ret = true; - - for (size_t i = 0; i < a.getRows(); i++) { - if (!equal(a(i), b(i), eps)) { - printf("element mismatch (%d)\n", i); - ret = false; - } - } - - return ret; -} - -} // namespace math diff --git a/src/lib/mathlib/math/Vector.hpp b/src/lib/mathlib/math/Vector.hpp index 73de793d51..adb2293e15 100644 --- a/src/lib/mathlib/math/Vector.hpp +++ b/src/lib/mathlib/math/Vector.hpp @@ -1,6 +1,8 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Author: Will Perone + * Anton Babushkin * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -32,26 +34,204 @@ ****************************************************************************/ /** - * @file Vector.h + * @file Vector.hpp * - * math vector + * Generic Vector */ -#pragma once +#ifndef VECTOR_HPP +#define VECTOR_HPP -#include - -#if defined(CONFIG_ARCH_CORTEXM4) && defined(CONFIG_ARCH_FPU) -#include "arm/Vector.hpp" -#else -#include "generic/Vector.hpp" -#endif +#include +#include "../CMSIS/Include/arm_math.h" namespace math { -class Vector; -int __EXPORT vectorTest(); -int __EXPORT vectorAddTest(); -int __EXPORT vectorSubTest(); -bool vectorEqual(const Vector &a, const Vector &b, float eps = 1.0e-5f); -} // math + +template +class Vector { +public: + float data[N]; + arm_matrix_instance_f32 arm_col; + + /** + * trivial ctor + */ + Vector() { + arm_col = {N, 1, &data[0]}; + } + + /** + * setting ctor + */ + Vector(const float *d) { + memcpy(data, d, sizeof(data)); + arm_col = {N, 1, &data[0]}; + } + + /** + * access to elements by index + */ + inline float &operator ()(unsigned int i) { + return data[i]; + } + + /** + * access to elements by index + */ + inline const float &operator ()(unsigned int i) const { + return data[i]; + } + + /** + * test for equality + */ + bool operator ==(const Vector &v) { + for (unsigned int i = 0; i < N; i++) + if (data[i] != v(i)) + return false; + return true; + } + + /** + * test for inequality + */ + bool operator !=(const Vector &v) { + for (unsigned int i = 0; i < N; i++) + if (data[i] != v(i)) + return true; + return false; + } + + /** + * set to value + */ + const Vector &operator =(const Vector &v) { + memcpy(data, v.data, sizeof(data)); + return *this; + } + + /** + * negation + */ + const Vector operator -(void) const { + Vector res; + for (unsigned int i = 0; i < N; i++) + res[i] = -data[i]; + return res; + } + + /** + * addition + */ + const Vector operator +(const Vector &v) const { + Vector res; + for (unsigned int i = 0; i < N; i++) + res[i] = data[i] + v(i); + return res; + } + + /** + * subtraction + */ + const Vector operator -(const Vector &v) const { + Vector res; + for (unsigned int i = 0; i < N; i++) + res[i] = data[i] - v(i); + return res; + } + + /** + * uniform scaling + */ + const Vector operator *(const float num) const { + Vector temp(*this); + return temp *= num; + } + + /** + * uniform scaling + */ + const Vector operator /(const float num) const { + Vector temp(*this); + return temp /= num; + } + + /** + * addition + */ + const Vector &operator +=(const Vector &v) { + for (unsigned int i = 0; i < N; i++) + data[i] += v(i); + return *this; + } + + /** + * subtraction + */ + const Vector &operator -=(const Vector &v) { + for (unsigned int i = 0; i < N; i++) + data[i] -= v(i); + return *this; + } + + /** + * uniform scaling + */ + const Vector &operator *=(const float num) { + for (unsigned int i = 0; i < N; i++) + data[i] *= num; + return *this; + } + + /** + * uniform scaling + */ + const Vector &operator /=(const float num) { + for (unsigned int i = 0; i < N; i++) + data[i] /= num; + return *this; + } + + /** + * dot product + */ + float operator *(const Vector &v) const { + float res; + for (unsigned int i = 0; i < N; i++) + res += data[i] * v(i); + return res; + } + + /** + * gets the length of this vector squared + */ + float length_squared() const { + return (*this * *this); + } + + /** + * gets the length of this vector + */ + float length() const { + return sqrtf(*this * *this); + } + + /** + * normalizes this vector + */ + void normalize() { + *this /= length(); + } + + /** + * returns the normalized version of this vector + */ + Vector normalized() const { + return *this / length(); + } +}; + +} + +#endif // VECTOR_HPP diff --git a/src/lib/mathlib/math/Vector2.hpp b/src/lib/mathlib/math/Vector2.hpp new file mode 100644 index 0000000000..501740614a --- /dev/null +++ b/src/lib/mathlib/math/Vector2.hpp @@ -0,0 +1,269 @@ +/**************************************************************************** + * + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Author: Will Perone + * Anton Babushkin + * + * 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 Vector2.hpp + * + * 2D Vector + */ + +#ifndef VECTOR2_HPP +#define VECTOR2_HPP + +#include + +namespace math +{ + +template +class Vector2 { +public: + T x, y; + + /** + * trivial ctor + */ + Vector2() { + } + + /** + * setting ctor + */ + Vector2(const T x0, const T y0): x(x0), y(y0) { + } + + /** + * setter + */ + void set(const T x0, const T y0) { + x = x0; + y = y0; + } + + /** + * access to elements by index + */ + T operator ()(unsigned int i) { + return *(&x + i); + } + + /** + * access to elements by index + */ + const T operator ()(unsigned int i) const { + return *(&x + i); + } + + /** + * test for equality + */ + bool operator ==(const Vector2 &v) { + return (x == v.x && y == v.y); + } + + /** + * test for inequality + */ + bool operator !=(const Vector2 &v) { + return (x != v.x || y != v.y); + } + + /** + * set to value + */ + const Vector2 &operator =(const Vector2 &v) { + x = v.x; + y = v.y; + return *this; + } + + /** + * negation + */ + const Vector2 operator -(void) const { + return Vector2(-x, -y); + } + + /** + * addition + */ + const Vector2 operator +(const Vector2 &v) const { + return Vector2(x + v.x, y + v.y); + } + + /** + * subtraction + */ + const Vector2 operator -(const Vector2 &v) const { + return Vector2(x - v.x, y - v.y); + } + + /** + * uniform scaling + */ + const Vector2 operator *(const T num) const { + Vector2 temp(*this); + return temp *= num; + } + + /** + * uniform scaling + */ + const Vector2 operator /(const T num) const { + Vector2 temp(*this); + return temp /= num; + } + + /** + * addition + */ + const Vector2 &operator +=(const Vector2 &v) { + x += v.x; + y += v.y; + return *this; + } + + /** + * subtraction + */ + const Vector2 &operator -=(const Vector2 &v) { + x -= v.x; + y -= v.y; + return *this; + } + + /** + * uniform scaling + */ + const Vector2 &operator *=(const T num) { + x *= num; + y *= num; + return *this; + } + + /** + * uniform scaling + */ + const Vector2 &operator /=(const T num) { + x /= num; + y /= num; + return *this; + } + + /** + * dot product + */ + T operator *(const Vector2 &v) const { + return x * v.x + y * v.y; + } + + /** + * cross product + */ + const float operator %(const Vector2 &v) const { + return x * v.y - y * v.x; + } + + /** + * gets the length of this vector squared + */ + float length_squared() const { + return (*this * *this); + } + + /** + * gets the length of this vector + */ + float length() const { + return (T)sqrt(*this * *this); + } + + /** + * normalizes this vector + */ + void normalize() { + *this /= length(); + } + + /** + * returns the normalized version of this vector + */ + Vector2 normalized() const { + return *this / length(); + } + + /** + * reflects this vector about n + */ + void reflect(const Vector2 &n) + { + Vector2 orig(*this); + project(n); + *this = *this * 2 - orig; + } + + /** + * projects this vector onto v + */ + void project(const Vector2 &v) { + *this = v * (*this * v) / (v * v); + } + + /** + * returns this vector projected onto v + */ + Vector2 projected(const Vector2 &v) { + return v * (*this * v) / (v * v); + } + + /** + * computes the angle between 2 arbitrary vectors + */ + static inline float angle(const Vector2 &v1, const Vector2 &v2) { + return acosf((v1 * v2) / (v1.length() * v2.length())); + } + + /** + * computes the angle between 2 arbitrary normalized vectors + */ + static inline float angle_normalized(const Vector2 &v1, const Vector2 &v2) { + return acosf(v1 * v2); + } +}; + +typedef Vector2 Vector2f; +} + +#endif // VECTOR2_HPP diff --git a/src/lib/mathlib/math/Vector2f.hpp b/src/lib/mathlib/math/Vector2f.hpp deleted file mode 100644 index ecd62e81cb..0000000000 --- a/src/lib/mathlib/math/Vector2f.hpp +++ /dev/null @@ -1,79 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2013 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 Vector2f.hpp - * - * math 3 vector - */ - -#pragma once - -#include "Vector.hpp" - -namespace math -{ - -class __EXPORT Vector2f : - public Vector -{ -public: - Vector2f(); - Vector2f(const Vector &right); - Vector2f(float x, float y); - Vector2f(const float *data); - virtual ~Vector2f(); - float cross(const Vector2f &b) const; - float operator %(const Vector2f &v) const; - float operator *(const Vector2f &v) const; - inline Vector2f operator*(const float &right) const { - return Vector::operator*(right); - } - - /** - * accessors - */ - void setX(float x) { (*this)(0) = x; } - void setY(float y) { (*this)(1) = y; } - const float &getX() const { return (*this)(0); } - const float &getY() const { return (*this)(1); } -}; - -class __EXPORT Vector2 : - public Vector2f -{ -}; - -int __EXPORT vector2fTest(); -} // math - diff --git a/src/lib/mathlib/math/Vector3.cpp b/src/lib/mathlib/math/Vector3.cpp deleted file mode 100644 index dcb85600ef..0000000000 --- a/src/lib/mathlib/math/Vector3.cpp +++ /dev/null @@ -1,99 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 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 Vector3.cpp - * - * math vector - */ - -#include "test/test.hpp" - -#include "Vector3.hpp" - -namespace math -{ - -Vector3::Vector3() : - Vector(3) -{ -} - -Vector3::Vector3(const Vector &right) : - Vector(right) -{ -#ifdef VECTOR_ASSERT - ASSERT(right.getRows() == 3); -#endif -} - -Vector3::Vector3(float x, float y, float z) : - Vector(3) -{ - setX(x); - setY(y); - setZ(z); -} - -Vector3::Vector3(const float *data) : - Vector(3, data) -{ -} - -Vector3::~Vector3() -{ -} - -Vector3 Vector3::cross(const Vector3 &b) const -{ - const Vector3 &a = *this; - Vector3 result; - result(0) = a(1) * b(2) - a(2) * b(1); - result(1) = a(2) * b(0) - a(0) * b(2); - result(2) = a(0) * b(1) - a(1) * b(0); - return result; -} - -int __EXPORT vector3Test() -{ - printf("Test Vector3\t\t: "); - // test float ctor - Vector3 v(1, 2, 3); - ASSERT(equal(v(0), 1)); - ASSERT(equal(v(1), 2)); - ASSERT(equal(v(2), 3)); - printf("PASS\n"); - return 0; -} - -} // namespace math diff --git a/src/lib/mathlib/math/Vector3.hpp b/src/lib/mathlib/math/Vector3.hpp index 568d9669ab..a0c3b92903 100644 --- a/src/lib/mathlib/math/Vector3.hpp +++ b/src/lib/mathlib/math/Vector3.hpp @@ -1,6 +1,8 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Author: Will Perone + * Anton Babushkin * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -34,43 +36,252 @@ /** * @file Vector3.hpp * - * math 3 vector + * 3D Vector */ -#pragma once +#ifndef VECTOR3_HPP +#define VECTOR3_HPP -#include "Vector.hpp" +#include +#include "../CMSIS/Include/arm_math.h" namespace math { -class __EXPORT Vector3 : - public Vector -{ +template +class Vector3 { public: - Vector3(); - Vector3(const Vector &right); - Vector3(float x, float y, float z); - Vector3(const float *data); - virtual ~Vector3(); - Vector3 cross(const Vector3 &b) const; + T x, y, z; + arm_matrix_instance_f32 arm_col; /** - * accessors + * trivial ctor */ - void setX(float x) { (*this)(0) = x; } - void setY(float y) { (*this)(1) = y; } - void setZ(float z) { (*this)(2) = z; } - const float &getX() const { return (*this)(0); } - const float &getY() const { return (*this)(1); } - const float &getZ() const { return (*this)(2); } -}; - -class __EXPORT Vector3f : - public Vector3 -{ + Vector3() { + arm_col = {3, 1, &x}; + } + + /** + * setting ctor + */ + Vector3(const T x0, const T y0, const T z0): x(x0), y(y0), z(z0) { + arm_col = {3, 1, &x}; + } + + /** + * setting ctor + */ + Vector3(const T data[3]): x(data[0]), y(data[1]), z(data[2]) { + arm_col = {3, 1, &x}; + } + + /** + * setter + */ + void set(const T x0, const T y0, const T z0) { + x = x0; + y = y0; + z = z0; + } + + /** + * access to elements by index + */ + T operator ()(unsigned int i) { + return *(&x + i); + } + + /** + * access to elements by index + */ + const T operator ()(unsigned int i) const { + return *(&x + i); + } + + /** + * test for equality + */ + bool operator ==(const Vector3 &v) { + return (x == v.x && y == v.y && z == v.z); + } + + /** + * test for inequality + */ + bool operator !=(const Vector3 &v) { + return (x != v.x || y != v.y || z != v.z); + } + + /** + * set to value + */ + const Vector3 &operator =(const Vector3 &v) { + x = v.x; + y = v.y; + z = v.z; + return *this; + } + + /** + * negation + */ + const Vector3 operator -(void) const { + return Vector3(-x, -y, -z); + } + + /** + * addition + */ + const Vector3 operator +(const Vector3 &v) const { + return Vector3(x + v.x, y + v.y, z + v.z); + } + + /** + * subtraction + */ + const Vector3 operator -(const Vector3 &v) const { + return Vector3(x - v.x, y - v.y, z - v.z); + } + + /** + * uniform scaling + */ + const Vector3 operator *(const T num) const { + Vector3 temp(*this); + return temp *= num; + } + + /** + * uniform scaling + */ + const Vector3 operator /(const T num) const { + Vector3 temp(*this); + return temp /= num; + } + + /** + * addition + */ + const Vector3 &operator +=(const Vector3 &v) { + x += v.x; + y += v.y; + z += v.z; + return *this; + } + + /** + * subtraction + */ + const Vector3 &operator -=(const Vector3 &v) { + x -= v.x; + y -= v.y; + z -= v.z; + return *this; + } + + /** + * uniform scaling + */ + const Vector3 &operator *=(const T num) { + x *= num; + y *= num; + z *= num; + return *this; + } + + /** + * uniform scaling + */ + const Vector3 &operator /=(const T num) { + x /= num; + y /= num; + z /= num; + return *this; + } + + /** + * dot product + */ + T operator *(const Vector3 &v) const { + return x * v.x + y * v.y + z * v.z; + } + + /** + * cross product + */ + const Vector3 operator %(const Vector3 &v) const { + Vector3 temp(y * v.z - z * v.y, z * v.x - x * v.z, x * v.y - y * v.x); + return temp; + } + + /** + * gets the length of this vector squared + */ + float length_squared() const { + return (*this * *this); + } + + /** + * gets the length of this vector + */ + float length() const { + return (T)sqrt(*this * *this); + } + + /** + * normalizes this vector + */ + void normalize() { + *this /= length(); + } + + /** + * returns the normalized version of this vector + */ + Vector3 normalized() const { + return *this / length(); + } + + /** + * reflects this vector about n + */ + void reflect(const Vector3 &n) + { + Vector3 orig(*this); + project(n); + *this = *this * 2 - orig; + } + + /** + * projects this vector onto v + */ + void project(const Vector3 &v) { + *this = v * (*this * v) / (v * v); + } + + /** + * returns this vector projected onto v + */ + Vector3 projected(const Vector3 &v) { + return v * (*this * v) / (v * v); + } + + /** + * computes the angle between 2 arbitrary vectors + */ + static inline float angle(const Vector3 &v1, const Vector3 &v2) { + return acosf((v1 * v2) / (v1.length() * v2.length())); + } + + /** + * computes the angle between 2 arbitrary normalized vectors + */ + static inline float angle_normalized(const Vector3 &v1, const Vector3 &v2) { + return acosf(v1 * v2); + } }; -int __EXPORT vector3Test(); -} // math +typedef Vector3 Vector3f; +} +#endif // VECTOR3_HPP diff --git a/src/lib/mathlib/math/arm/Matrix.cpp b/src/lib/mathlib/math/arm/Matrix.cpp deleted file mode 100644 index 21661622ab..0000000000 --- a/src/lib/mathlib/math/arm/Matrix.cpp +++ /dev/null @@ -1,40 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 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 Matrix.cpp - * - * matrix code - */ - -#include "Matrix.hpp" diff --git a/src/lib/mathlib/math/arm/Matrix.hpp b/src/lib/mathlib/math/arm/Matrix.hpp deleted file mode 100644 index 1945bb02db..0000000000 --- a/src/lib/mathlib/math/arm/Matrix.hpp +++ /dev/null @@ -1,292 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 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 Matrix.h - * - * matrix code - */ - -#pragma once - - -#include -#include -#include -#include -#include -#include - -#include "../Vector.hpp" -#include "../Matrix.hpp" - -// arm specific -#include "../../CMSIS/Include/arm_math.h" - -namespace math -{ - -class __EXPORT Matrix -{ -public: - // constructor - Matrix(size_t rows, size_t cols) : - _matrix() { - arm_mat_init_f32(&_matrix, - rows, cols, - (float *)calloc(rows * cols, sizeof(float))); - } - Matrix(size_t rows, size_t cols, const float *data) : - _matrix() { - arm_mat_init_f32(&_matrix, - rows, cols, - (float *)malloc(rows * cols * sizeof(float))); - memcpy(getData(), data, getSize()); - } - // deconstructor - virtual ~Matrix() { - delete [] _matrix.pData; - } - // copy constructor (deep) - Matrix(const Matrix &right) : - _matrix() { - arm_mat_init_f32(&_matrix, - right.getRows(), right.getCols(), - (float *)malloc(right.getRows()* - right.getCols()*sizeof(float))); - memcpy(getData(), right.getData(), - getSize()); - } - // assignment - inline Matrix &operator=(const Matrix &right) { -#ifdef MATRIX_ASSERT - ASSERT(getRows() == right.getRows()); - ASSERT(getCols() == right.getCols()); -#endif - - if (this != &right) { - memcpy(getData(), right.getData(), - right.getSize()); - } - - return *this; - } - // element accessors - inline float &operator()(size_t i, size_t j) { -#ifdef MATRIX_ASSERT - ASSERT(i < getRows()); - ASSERT(j < getCols()); -#endif - return getData()[i * getCols() + j]; - } - inline const float &operator()(size_t i, size_t j) const { -#ifdef MATRIX_ASSERT - ASSERT(i < getRows()); - ASSERT(j < getCols()); -#endif - return getData()[i * getCols() + j]; - } - // output - inline void print() const { - for (size_t i = 0; i < getRows(); i++) { - for (size_t j = 0; j < getCols(); j++) { - float sig; - int exponent; - float num = (*this)(i, j); - float2SigExp(num, sig, exponent); - printf("%6.3fe%03d ", (double)sig, exponent); - } - - printf("\n"); - } - } - // boolean ops - inline bool operator==(const Matrix &right) const { - for (size_t i = 0; i < getRows(); i++) { - for (size_t j = 0; j < getCols(); j++) { - if (fabsf((*this)(i, j) - right(i, j)) > 1e-30f) - return false; - } - } - - return true; - } - // scalar ops - inline Matrix operator+(float right) const { - Matrix result(getRows(), getCols()); - arm_offset_f32((float *)getData(), right, - (float *)result.getData(), getRows()*getCols()); - return result; - } - inline Matrix operator-(float right) const { - Matrix result(getRows(), getCols()); - arm_offset_f32((float *)getData(), -right, - (float *)result.getData(), getRows()*getCols()); - return result; - } - inline Matrix operator*(float right) const { - Matrix result(getRows(), getCols()); - arm_mat_scale_f32(&_matrix, right, - &(result._matrix)); - return result; - } - inline Matrix operator/(float right) const { - Matrix result(getRows(), getCols()); - arm_mat_scale_f32(&_matrix, 1.0f / right, - &(result._matrix)); - return result; - } - // vector ops - inline Vector operator*(const Vector &right) const { -#ifdef MATRIX_ASSERT - ASSERT(getCols() == right.getRows()); -#endif - Matrix resultMat = (*this) * - Matrix(right.getRows(), 1, right.getData()); - return Vector(getRows(), resultMat.getData()); - } - // matrix ops - inline Matrix operator+(const Matrix &right) const { -#ifdef MATRIX_ASSERT - ASSERT(getRows() == right.getRows()); - ASSERT(getCols() == right.getCols()); -#endif - Matrix result(getRows(), getCols()); - arm_mat_add_f32(&_matrix, &(right._matrix), - &(result._matrix)); - return result; - } - inline Matrix operator-(const Matrix &right) const { -#ifdef MATRIX_ASSERT - ASSERT(getRows() == right.getRows()); - ASSERT(getCols() == right.getCols()); -#endif - Matrix result(getRows(), getCols()); - arm_mat_sub_f32(&_matrix, &(right._matrix), - &(result._matrix)); - return result; - } - inline Matrix operator*(const Matrix &right) const { -#ifdef MATRIX_ASSERT - ASSERT(getCols() == right.getRows()); -#endif - Matrix result(getRows(), right.getCols()); - arm_mat_mult_f32(&_matrix, &(right._matrix), - &(result._matrix)); - return result; - } - inline Matrix operator/(const Matrix &right) const { -#ifdef MATRIX_ASSERT - ASSERT(right.getRows() == right.getCols()); - ASSERT(getCols() == right.getCols()); -#endif - return (*this) * right.inverse(); - } - // other functions - inline Matrix transpose() const { - Matrix result(getCols(), getRows()); - arm_mat_trans_f32(&_matrix, &(result._matrix)); - return result; - } - inline void swapRows(size_t a, size_t b) { - if (a == b) return; - - for (size_t j = 0; j < getCols(); j++) { - float tmp = (*this)(a, j); - (*this)(a, j) = (*this)(b, j); - (*this)(b, j) = tmp; - } - } - inline void swapCols(size_t a, size_t b) { - if (a == b) return; - - for (size_t i = 0; i < getRows(); i++) { - float tmp = (*this)(i, a); - (*this)(i, a) = (*this)(i, b); - (*this)(i, b) = tmp; - } - } - /** - * inverse based on LU factorization with partial pivotting - */ - Matrix inverse() const { -#ifdef MATRIX_ASSERT - ASSERT(getRows() == getCols()); -#endif - Matrix result(getRows(), getCols()); - Matrix work = (*this); - arm_mat_inverse_f32(&(work._matrix), - &(result._matrix)); - return result; - } - inline void setAll(const float &val) { - for (size_t i = 0; i < getRows(); i++) { - for (size_t j = 0; j < getCols(); j++) { - (*this)(i, j) = val; - } - } - } - inline void set(const float *data) { - memcpy(getData(), data, getSize()); - } - inline size_t getRows() const { return _matrix.numRows; } - inline size_t getCols() const { return _matrix.numCols; } - inline static Matrix identity(size_t size) { - Matrix result(size, size); - - for (size_t i = 0; i < size; i++) { - result(i, i) = 1.0f; - } - - return result; - } - inline static Matrix zero(size_t size) { - Matrix result(size, size); - result.setAll(0.0f); - return result; - } - inline static Matrix zero(size_t m, size_t n) { - Matrix result(m, n); - result.setAll(0.0f); - return result; - } -protected: - inline size_t getSize() const { return sizeof(float) * getRows() * getCols(); } - inline float *getData() { return _matrix.pData; } - inline const float *getData() const { return _matrix.pData; } - inline void setData(float *data) { _matrix.pData = data; } -private: - arm_matrix_instance_f32 _matrix; -}; - -} // namespace math diff --git a/src/lib/mathlib/math/arm/Vector.cpp b/src/lib/mathlib/math/arm/Vector.cpp deleted file mode 100644 index 7ea6496bb1..0000000000 --- a/src/lib/mathlib/math/arm/Vector.cpp +++ /dev/null @@ -1,40 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 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 Vector.cpp - * - * math vector - */ - -#include "Vector.hpp" diff --git a/src/lib/mathlib/math/arm/Vector.hpp b/src/lib/mathlib/math/arm/Vector.hpp deleted file mode 100644 index 52220fc152..0000000000 --- a/src/lib/mathlib/math/arm/Vector.hpp +++ /dev/null @@ -1,236 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 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 Vector.h - * - * math vector - */ - -#pragma once - -#include -#include -#include -#include -#include -#include - -#include "../Vector.hpp" -#include "../test/test.hpp" - -// arm specific -#include "../../CMSIS/Include/arm_math.h" - -namespace math -{ - -class __EXPORT Vector -{ -public: - // constructor - Vector(size_t rows) : - _rows(rows), - _data((float *)calloc(rows, sizeof(float))) { - } - Vector(size_t rows, const float *data) : - _rows(rows), - _data((float *)malloc(getSize())) { - memcpy(getData(), data, getSize()); - } - // deconstructor - virtual ~Vector() { - delete [] getData(); - } - // copy constructor (deep) - Vector(const Vector &right) : - _rows(right.getRows()), - _data((float *)malloc(getSize())) { - memcpy(getData(), right.getData(), - right.getSize()); - } - // assignment - inline Vector &operator=(const Vector &right) { -#ifdef VECTOR_ASSERT - ASSERT(getRows() == right.getRows()); -#endif - - if (this != &right) { - memcpy(getData(), right.getData(), - right.getSize()); - } - - return *this; - } - // element accessors - inline float &operator()(size_t i) { -#ifdef VECTOR_ASSERT - ASSERT(i < getRows()); -#endif - return getData()[i]; - } - inline const float &operator()(size_t i) const { -#ifdef VECTOR_ASSERT - ASSERT(i < getRows()); -#endif - return getData()[i]; - } - // output - inline void print() const { - for (size_t i = 0; i < getRows(); i++) { - float sig; - int exponent; - float num = (*this)(i); - float2SigExp(num, sig, exponent); - printf("%6.3fe%03d ", (double)sig, exponent); - } - - printf("\n"); - } - // boolean ops - inline bool operator==(const Vector &right) const { - for (size_t i = 0; i < getRows(); i++) { - if (fabsf(((*this)(i) - right(i))) > 1e-30f) - return false; - } - - return true; - } - // scalar ops - inline Vector operator+(float right) const { - Vector result(getRows()); - arm_offset_f32((float *)getData(), - right, result.getData(), - getRows()); - return result; - } - inline Vector operator-(float right) const { - Vector result(getRows()); - arm_offset_f32((float *)getData(), - -right, result.getData(), - getRows()); - return result; - } - inline Vector operator*(float right) const { - Vector result(getRows()); - arm_scale_f32((float *)getData(), - right, result.getData(), - getRows()); - return result; - } - inline Vector operator/(float right) const { - Vector result(getRows()); - arm_scale_f32((float *)getData(), - 1.0f / right, result.getData(), - getRows()); - return result; - } - // vector ops - inline Vector operator+(const Vector &right) const { -#ifdef VECTOR_ASSERT - ASSERT(getRows() == right.getRows()); -#endif - Vector result(getRows()); - arm_add_f32((float *)getData(), - (float *)right.getData(), - result.getData(), - getRows()); - return result; - } - inline Vector operator-(const Vector &right) const { -#ifdef VECTOR_ASSERT - ASSERT(getRows() == right.getRows()); -#endif - Vector result(getRows()); - arm_sub_f32((float *)getData(), - (float *)right.getData(), - result.getData(), - getRows()); - return result; - } - inline Vector operator-(void) const { - Vector result(getRows()); - arm_negate_f32((float *)getData(), - result.getData(), - getRows()); - return result; - } - // other functions - inline float dot(const Vector &right) const { - float result = 0; - arm_dot_prod_f32((float *)getData(), - (float *)right.getData(), - getRows(), - &result); - return result; - } - inline float norm() const { - return sqrtf(dot(*this)); - } - inline float length() const { - return norm(); - } - inline Vector unit() const { - return (*this) / norm(); - } - inline Vector normalized() const { - return unit(); - } - inline void normalize() { - (*this) = (*this) / norm(); - } - inline static Vector zero(size_t rows) { - Vector result(rows); - // calloc returns zeroed memory - return result; - } - inline void setAll(const float &val) { - for (size_t i = 0; i < getRows(); i++) { - (*this)(i) = val; - } - } - inline void set(const float *data) { - memcpy(getData(), data, getSize()); - } - inline size_t getRows() const { return _rows; } - inline const float *getData() const { return _data; } -protected: - inline size_t getSize() const { return sizeof(float) * getRows(); } - inline float *getData() { return _data; } - inline void setData(float *data) { _data = data; } -private: - size_t _rows; - float *_data; -}; - -} // math diff --git a/src/lib/mathlib/math/generic/Matrix.hpp b/src/lib/mathlib/math/generic/Matrix.hpp deleted file mode 100644 index 5601a3447b..0000000000 --- a/src/lib/mathlib/math/generic/Matrix.hpp +++ /dev/null @@ -1,437 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 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 Matrix.h - * - * matrix code - */ - -#pragma once - - -#include -#include -#include -#include -#include -#include - -#include "../Vector.hpp" -#include "../Matrix.hpp" - -namespace math -{ - -class __EXPORT Matrix -{ -public: - // constructor - Matrix(size_t rows, size_t cols) : - _rows(rows), - _cols(cols), - _data((float *)calloc(rows *cols, sizeof(float))) { - } - Matrix(size_t rows, size_t cols, const float *data) : - _rows(rows), - _cols(cols), - _data((float *)malloc(getSize())) { - memcpy(getData(), data, getSize()); - } - // deconstructor - virtual ~Matrix() { - delete [] getData(); - } - // copy constructor (deep) - Matrix(const Matrix &right) : - _rows(right.getRows()), - _cols(right.getCols()), - _data((float *)malloc(getSize())) { - memcpy(getData(), right.getData(), - right.getSize()); - } - // assignment - inline Matrix &operator=(const Matrix &right) { -#ifdef MATRIX_ASSERT - ASSERT(getRows() == right.getRows()); - ASSERT(getCols() == right.getCols()); -#endif - - if (this != &right) { - memcpy(getData(), right.getData(), - right.getSize()); - } - - return *this; - } - // element accessors - inline float &operator()(size_t i, size_t j) { -#ifdef MATRIX_ASSERT - ASSERT(i < getRows()); - ASSERT(j < getCols()); -#endif - return getData()[i * getCols() + j]; - } - inline const float &operator()(size_t i, size_t j) const { -#ifdef MATRIX_ASSERT - ASSERT(i < getRows()); - ASSERT(j < getCols()); -#endif - return getData()[i * getCols() + j]; - } - // output - inline void print() const { - for (size_t i = 0; i < getRows(); i++) { - for (size_t j = 0; j < getCols(); j++) { - float sig; - int exp; - float num = (*this)(i, j); - float2SigExp(num, sig, exp); - printf("%6.3fe%03.3d,", (double)sig, exp); - } - - printf("\n"); - } - } - // boolean ops - inline bool operator==(const Matrix &right) const { - for (size_t i = 0; i < getRows(); i++) { - for (size_t j = 0; j < getCols(); j++) { - if (fabsf((*this)(i, j) - right(i, j)) > 1e-30f) - return false; - } - } - - return true; - } - // scalar ops - inline Matrix operator+(const float &right) const { - Matrix result(getRows(), getCols()); - - for (size_t i = 0; i < getRows(); i++) { - for (size_t j = 0; j < getCols(); j++) { - result(i, j) = (*this)(i, j) + right; - } - } - - return result; - } - inline Matrix operator-(const float &right) const { - Matrix result(getRows(), getCols()); - - for (size_t i = 0; i < getRows(); i++) { - for (size_t j = 0; j < getCols(); j++) { - result(i, j) = (*this)(i, j) - right; - } - } - - return result; - } - inline Matrix operator*(const float &right) const { - Matrix result(getRows(), getCols()); - - for (size_t i = 0; i < getRows(); i++) { - for (size_t j = 0; j < getCols(); j++) { - result(i, j) = (*this)(i, j) * right; - } - } - - return result; - } - inline Matrix operator/(const float &right) const { - Matrix result(getRows(), getCols()); - - for (size_t i = 0; i < getRows(); i++) { - for (size_t j = 0; j < getCols(); j++) { - result(i, j) = (*this)(i, j) / right; - } - } - - return result; - } - // vector ops - inline Vector operator*(const Vector &right) const { -#ifdef MATRIX_ASSERT - ASSERT(getCols() == right.getRows()); -#endif - Vector result(getRows()); - - for (size_t i = 0; i < getRows(); i++) { - for (size_t j = 0; j < getCols(); j++) { - result(i) += (*this)(i, j) * right(j); - } - } - - return result; - } - // matrix ops - inline Matrix operator+(const Matrix &right) const { -#ifdef MATRIX_ASSERT - ASSERT(getRows() == right.getRows()); - ASSERT(getCols() == right.getCols()); -#endif - Matrix result(getRows(), getCols()); - - for (size_t i = 0; i < getRows(); i++) { - for (size_t j = 0; j < getCols(); j++) { - result(i, j) = (*this)(i, j) + right(i, j); - } - } - - return result; - } - inline Matrix operator-(const Matrix &right) const { -#ifdef MATRIX_ASSERT - ASSERT(getRows() == right.getRows()); - ASSERT(getCols() == right.getCols()); -#endif - Matrix result(getRows(), getCols()); - - for (size_t i = 0; i < getRows(); i++) { - for (size_t j = 0; j < getCols(); j++) { - result(i, j) = (*this)(i, j) - right(i, j); - } - } - - return result; - } - inline Matrix operator*(const Matrix &right) const { -#ifdef MATRIX_ASSERT - ASSERT(getCols() == right.getRows()); -#endif - Matrix result(getRows(), right.getCols()); - - for (size_t i = 0; i < getRows(); i++) { - for (size_t j = 0; j < right.getCols(); j++) { - for (size_t k = 0; k < right.getRows(); k++) { - result(i, j) += (*this)(i, k) * right(k, j); - } - } - } - - return result; - } - inline Matrix operator/(const Matrix &right) const { -#ifdef MATRIX_ASSERT - ASSERT(right.getRows() == right.getCols()); - ASSERT(getCols() == right.getCols()); -#endif - return (*this) * right.inverse(); - } - // other functions - inline Matrix transpose() const { - Matrix result(getCols(), getRows()); - - for (size_t i = 0; i < getRows(); i++) { - for (size_t j = 0; j < getCols(); j++) { - result(j, i) = (*this)(i, j); - } - } - - return result; - } - inline void swapRows(size_t a, size_t b) { - if (a == b) return; - - for (size_t j = 0; j < getCols(); j++) { - float tmp = (*this)(a, j); - (*this)(a, j) = (*this)(b, j); - (*this)(b, j) = tmp; - } - } - inline void swapCols(size_t a, size_t b) { - if (a == b) return; - - for (size_t i = 0; i < getRows(); i++) { - float tmp = (*this)(i, a); - (*this)(i, a) = (*this)(i, b); - (*this)(i, b) = tmp; - } - } - /** - * inverse based on LU factorization with partial pivotting - */ - Matrix inverse() const { -#ifdef MATRIX_ASSERT - ASSERT(getRows() == getCols()); -#endif - size_t N = getRows(); - Matrix L = identity(N); - const Matrix &A = (*this); - Matrix U = A; - Matrix P = identity(N); - - //printf("A:\n"); A.print(); - - // for all diagonal elements - for (size_t n = 0; n < N; n++) { - - // if diagonal is zero, swap with row below - if (fabsf(U(n, n)) < 1e-8f) { - //printf("trying pivot for row %d\n",n); - for (size_t i = 0; i < N; i++) { - if (i == n) continue; - - //printf("\ttrying row %d\n",i); - if (fabsf(U(i, n)) > 1e-8f) { - //printf("swapped %d\n",i); - U.swapRows(i, n); - P.swapRows(i, n); - } - } - } - -#ifdef MATRIX_ASSERT - //printf("A:\n"); A.print(); - //printf("U:\n"); U.print(); - //printf("P:\n"); P.print(); - //fflush(stdout); - ASSERT(fabsf(U(n, n)) > 1e-8f); -#endif - - // failsafe, return zero matrix - if (fabsf(U(n, n)) < 1e-8f) { - return Matrix::zero(n); - } - - // for all rows below diagonal - for (size_t i = (n + 1); i < N; i++) { - L(i, n) = U(i, n) / U(n, n); - - // add i-th row and n-th row - // multiplied by: -a(i,n)/a(n,n) - for (size_t k = n; k < N; k++) { - U(i, k) -= L(i, n) * U(n, k); - } - } - } - - //printf("L:\n"); L.print(); - //printf("U:\n"); U.print(); - - // solve LY=P*I for Y by forward subst - Matrix Y = P; - - // for all columns of Y - for (size_t c = 0; c < N; c++) { - // for all rows of L - for (size_t i = 0; i < N; i++) { - // for all columns of L - for (size_t j = 0; j < i; j++) { - // for all existing y - // subtract the component they - // contribute to the solution - Y(i, c) -= L(i, j) * Y(j, c); - } - - // divide by the factor - // on current - // term to be solved - // Y(i,c) /= L(i,i); - // but L(i,i) = 1.0 - } - } - - //printf("Y:\n"); Y.print(); - - // solve Ux=y for x by back subst - Matrix X = Y; - - // for all columns of X - for (size_t c = 0; c < N; c++) { - // for all rows of U - for (size_t k = 0; k < N; k++) { - // have to go in reverse order - size_t i = N - 1 - k; - - // for all columns of U - for (size_t j = i + 1; j < N; j++) { - // for all existing x - // subtract the component they - // contribute to the solution - X(i, c) -= U(i, j) * X(j, c); - } - - // divide by the factor - // on current - // term to be solved - X(i, c) /= U(i, i); - } - } - - //printf("X:\n"); X.print(); - return X; - } - inline void setAll(const float &val) { - for (size_t i = 0; i < getRows(); i++) { - for (size_t j = 0; j < getCols(); j++) { - (*this)(i, j) = val; - } - } - } - inline void set(const float *data) { - memcpy(getData(), data, getSize()); - } - inline size_t getRows() const { return _rows; } - inline size_t getCols() const { return _cols; } - inline static Matrix identity(size_t size) { - Matrix result(size, size); - - for (size_t i = 0; i < size; i++) { - result(i, i) = 1.0f; - } - - return result; - } - inline static Matrix zero(size_t size) { - Matrix result(size, size); - result.setAll(0.0f); - return result; - } - inline static Matrix zero(size_t m, size_t n) { - Matrix result(m, n); - result.setAll(0.0f); - return result; - } -protected: - inline size_t getSize() const { return sizeof(float) * getRows() * getCols(); } - inline float *getData() { return _data; } - inline const float *getData() const { return _data; } - inline void setData(float *data) { _data = data; } -private: - size_t _rows; - size_t _cols; - float *_data; -}; - -} // namespace math diff --git a/src/lib/mathlib/math/generic/Vector.cpp b/src/lib/mathlib/math/generic/Vector.cpp deleted file mode 100644 index 7ea6496bb1..0000000000 --- a/src/lib/mathlib/math/generic/Vector.cpp +++ /dev/null @@ -1,40 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 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 Vector.cpp - * - * math vector - */ - -#include "Vector.hpp" diff --git a/src/lib/mathlib/math/generic/Vector.hpp b/src/lib/mathlib/math/generic/Vector.hpp deleted file mode 100644 index 8cfdc676d7..0000000000 --- a/src/lib/mathlib/math/generic/Vector.hpp +++ /dev/null @@ -1,245 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 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 Vector.h - * - * math vector - */ - -#pragma once - -#include -#include -#include -#include -#include -#include - -#include "../Vector.hpp" - -namespace math -{ - -class __EXPORT Vector -{ -public: - // constructor - Vector(size_t rows) : - _rows(rows), - _data((float *)calloc(rows, sizeof(float))) { - } - Vector(size_t rows, const float *data) : - _rows(rows), - _data((float *)malloc(getSize())) { - memcpy(getData(), data, getSize()); - } - // deconstructor - virtual ~Vector() { - delete [] getData(); - } - // copy constructor (deep) - Vector(const Vector &right) : - _rows(right.getRows()), - _data((float *)malloc(getSize())) { - memcpy(getData(), right.getData(), - right.getSize()); - } - // assignment - inline Vector &operator=(const Vector &right) { -#ifdef VECTOR_ASSERT - ASSERT(getRows() == right.getRows()); -#endif - - if (this != &right) { - memcpy(getData(), right.getData(), - right.getSize()); - } - - return *this; - } - // element accessors - inline float &operator()(size_t i) { -#ifdef VECTOR_ASSERT - ASSERT(i < getRows()); -#endif - return getData()[i]; - } - inline const float &operator()(size_t i) const { -#ifdef VECTOR_ASSERT - ASSERT(i < getRows()); -#endif - return getData()[i]; - } - // output - inline void print() const { - for (size_t i = 0; i < getRows(); i++) { - float sig; - int exp; - float num = (*this)(i); - float2SigExp(num, sig, exp); - printf("%6.3fe%03.3d,", (double)sig, exp); - } - - printf("\n"); - } - // boolean ops - inline bool operator==(const Vector &right) const { - for (size_t i = 0; i < getRows(); i++) { - if (fabsf(((*this)(i) - right(i))) > 1e-30f) - return false; - } - - return true; - } - // scalar ops - inline Vector operator+(const float &right) const { - Vector result(getRows()); - - for (size_t i = 0; i < getRows(); i++) { - result(i) = (*this)(i) + right; - } - - return result; - } - inline Vector operator-(const float &right) const { - Vector result(getRows()); - - for (size_t i = 0; i < getRows(); i++) { - result(i) = (*this)(i) - right; - } - - return result; - } - inline Vector operator*(const float &right) const { - Vector result(getRows()); - - for (size_t i = 0; i < getRows(); i++) { - result(i) = (*this)(i) * right; - } - - return result; - } - inline Vector operator/(const float &right) const { - Vector result(getRows()); - - for (size_t i = 0; i < getRows(); i++) { - result(i) = (*this)(i) / right; - } - - return result; - } - // vector ops - inline Vector operator+(const Vector &right) const { -#ifdef VECTOR_ASSERT - ASSERT(getRows() == right.getRows()); -#endif - Vector result(getRows()); - - for (size_t i = 0; i < getRows(); i++) { - result(i) = (*this)(i) + right(i); - } - - return result; - } - inline Vector operator-(const Vector &right) const { -#ifdef VECTOR_ASSERT - ASSERT(getRows() == right.getRows()); -#endif - Vector result(getRows()); - - for (size_t i = 0; i < getRows(); i++) { - result(i) = (*this)(i) - right(i); - } - - return result; - } - inline Vector operator-(void) const { - Vector result(getRows()); - - for (size_t i = 0; i < getRows(); i++) { - result(i) = -((*this)(i)); - } - - return result; - } - // other functions - inline float dot(const Vector &right) const { - float result = 0; - - for (size_t i = 0; i < getRows(); i++) { - result += (*this)(i) * (*this)(i); - } - - return result; - } - inline float norm() const { - return sqrtf(dot(*this)); - } - inline float length() const { - return norm(); - } - inline Vector unit() const { - return (*this) / norm(); - } - inline Vector normalized() const { - return unit(); - } - inline void normalize() { - (*this) = (*this) / norm(); - } - inline static Vector zero(size_t rows) { - Vector result(rows); - // calloc returns zeroed memory - return result; - } - inline void setAll(const float &val) { - for (size_t i = 0; i < getRows(); i++) { - (*this)(i) = val; - } - } - inline void set(const float *data) { - memcpy(getData(), data, getSize()); - } - inline size_t getRows() const { return _rows; } -protected: - inline size_t getSize() const { return sizeof(float) * getRows(); } - inline float *getData() { return _data; } - inline const float *getData() const { return _data; } - inline void setData(float *data) { _data = data; } -private: - size_t _rows; - float *_data; -}; - -} // math diff --git a/src/lib/mathlib/mathlib.h b/src/lib/mathlib/mathlib.h index 40ffb22bc4..d8e95b46b1 100644 --- a/src/lib/mathlib/mathlib.h +++ b/src/lib/mathlib/mathlib.h @@ -41,13 +41,12 @@ #pragma once -#include "math/Dcm.hpp" -#include "math/EulerAngles.hpp" -#include "math/Matrix.hpp" -#include "math/Quaternion.hpp" #include "math/Vector.hpp" +#include "math/Vector2.hpp" #include "math/Vector3.hpp" -#include "math/Vector2f.hpp" +#include "math/Matrix.hpp" +#include "math/Matrix3.hpp" +#include "math/Quaternion.hpp" #include "math/Limits.hpp" #endif @@ -56,4 +55,4 @@ #include "CMSIS/Include/arm_math.h" -#endif \ No newline at end of file +#endif diff --git a/src/lib/mathlib/module.mk b/src/lib/mathlib/module.mk index 72bc7db8a1..50951c6174 100644 --- a/src/lib/mathlib/module.mk +++ b/src/lib/mathlib/module.mk @@ -35,13 +35,7 @@ # Math library # SRCS = math/test/test.cpp \ - math/Vector.cpp \ - math/Vector2f.cpp \ - math/Vector3.cpp \ - math/EulerAngles.cpp \ - math/Quaternion.cpp \ - math/Dcm.cpp \ - math/Matrix.cpp \ + math/Matrix3.cpp \ math/Limits.cpp # @@ -49,13 +43,3 @@ SRCS = math/test/test.cpp \ # current makefile name, since app.mk needs it. # APP_MAKEFILE := $(lastword $(MAKEFILE_LIST)) - -ifeq ($(CONFIG_ARCH_CORTEXM4)$(CONFIG_ARCH_FPU),yy) -INCLUDE_DIRS += math/arm -SRCS += math/arm/Vector.cpp \ - math/arm/Matrix.cpp -else -#INCLUDE_DIRS += math/generic -#SRCS += math/generic/Vector.cpp \ -# math/generic/Matrix.cpp -endif diff --git a/src/systemcmds/tests/module.mk b/src/systemcmds/tests/module.mk index 5d5fe50d33..20182c5d8c 100644 --- a/src/systemcmds/tests/module.mk +++ b/src/systemcmds/tests/module.mk @@ -24,6 +24,7 @@ SRCS = test_adc.c \ test_uart_loopback.c \ test_uart_send.c \ test_mixer.cpp \ + test_mathlib.cpp \ test_file.c \ tests_main.c \ test_param.c \ diff --git a/src/lib/mathlib/math/Vector2f.cpp b/src/systemcmds/tests/test_mathlib.cpp similarity index 52% rename from src/lib/mathlib/math/Vector2f.cpp rename to src/systemcmds/tests/test_mathlib.cpp index 68e741817d..13e6dad518 100644 --- a/src/lib/mathlib/math/Vector2f.cpp +++ b/src/systemcmds/tests/test_mathlib.cpp @@ -32,72 +32,83 @@ ****************************************************************************/ /** - * @file Vector2f.cpp + * @file test_mathlib.cpp * - * math vector + * Mathlib test */ -#include "test/test.hpp" +#include +#include +#include +#include +#include +#include +#include +#include -#include "Vector2f.hpp" +#include "tests.h" -namespace math -{ +using namespace math; -Vector2f::Vector2f() : - Vector(2) -{ +const char* formatResult(bool res) { + return res ? "OK" : "ERROR"; } -Vector2f::Vector2f(const Vector &right) : - Vector(right) +int test_mathlib(int argc, char *argv[]) { -#ifdef VECTOR_ASSERT - ASSERT(right.getRows() == 2); -#endif -} + warnx("testing mathlib"); -Vector2f::Vector2f(float x, float y) : - Vector(2) -{ - setX(x); - setY(y); -} + Matrix3f m; + m.identity(); + Matrix3f m1; + Matrix<3,3> mq; + mq.identity(); + Matrix<3,3> mq1; + m1(0, 0) = 5.0; + Vector3f v = Vector3f(1.0f, 2.0f, 3.0f); + Vector3f v1; -Vector2f::Vector2f(const float *data) : - Vector(2, data) -{ -} + unsigned int n = 60000; -Vector2f::~Vector2f() -{ -} + hrt_abstime t0, t1; -float Vector2f::cross(const Vector2f &b) const -{ - const Vector2f &a = *this; - return a(0)*b(1) - a(1)*b(0); -} + t0 = hrt_absolute_time(); + for (unsigned int j = 0; j < n; j++) { + v1 = m * v; + } + t1 = hrt_absolute_time(); + warnx("Matrix * Vector: %s %.6fus", formatResult(v1 == v), (double)(t1 - t0) / n); -float Vector2f::operator %(const Vector2f &v) const -{ - return cross(v); -} - -float Vector2f::operator *(const Vector2f &v) const -{ - return dot(v); -} + t0 = hrt_absolute_time(); + for (unsigned int j = 0; j < n; j++) { + mq1 = mq * mq; + } + t1 = hrt_absolute_time(); + warnx("Matrix * Matrix: %s %.6fus", formatResult(mq1 == mq), (double)(t1 - t0) / n); + mq1.dump(); -int __EXPORT vector2fTest() -{ - printf("Test Vector2f\t\t: "); - // test float ctor - Vector2f v(1, 2); - ASSERT(equal(v(0), 1)); - ASSERT(equal(v(1), 2)); - printf("PASS\n"); + t0 = hrt_absolute_time(); + for (unsigned int j = 0; j < n; j++) { + m1 = m.transposed(); + } + t1 = hrt_absolute_time(); + warnx("Matrix Transpose: %s %.6fus", formatResult(m1 == m), (double)(t1 - t0) / n); + + t0 = hrt_absolute_time(); + for (unsigned int j = 0; j < n; j++) { + m1 = m.inversed(); + } + t1 = hrt_absolute_time(); + warnx("Matrix Invert: %s %.6fus", formatResult(m1 == m), (double)(t1 - t0) / n); + + Matrix<4,4> mn; + mn(0, 0) = 2.0f; + mn(1, 0) = 3.0f; + for (int i = 0; i < mn.getRows(); i++) { + for (int j = 0; j < mn.getCols(); j++) { + printf("%.3f ", mn(i, j)); + } + printf("\n"); + } return 0; } - -} // namespace math diff --git a/src/systemcmds/tests/tests.h b/src/systemcmds/tests/tests.h index 5cbc5ad88f..e2105f8ee9 100644 --- a/src/systemcmds/tests/tests.h +++ b/src/systemcmds/tests/tests.h @@ -108,6 +108,7 @@ extern int test_param(int argc, char *argv[]); extern int test_bson(int argc, char *argv[]); extern int test_file(int argc, char *argv[]); extern int test_mixer(int argc, char *argv[]); +extern int test_mathlib(int argc, char *argv[]); __END_DECLS diff --git a/src/systemcmds/tests/tests_main.c b/src/systemcmds/tests/tests_main.c index cd998dd18d..fd026d70e9 100644 --- a/src/systemcmds/tests/tests_main.c +++ b/src/systemcmds/tests/tests_main.c @@ -105,6 +105,7 @@ const struct { {"bson", test_bson, 0}, {"file", test_file, 0}, {"mixer", test_mixer, OPT_NOJIGTEST | OPT_NOALLTEST}, + {"mathlib", test_mathlib, 0}, {"help", test_help, OPT_NOALLTEST | OPT_NOHELP | OPT_NOJIGTEST}, {NULL, NULL, 0} }; From e3a5a384d7b3678d1cbef63dc28fbe9a8f1de940 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 18 Dec 2013 23:01:02 +0400 Subject: [PATCH 206/447] mathlib: fixes and improvements, WIP --- src/lib/mathlib/math/Matrix.hpp | 38 +++++++---- src/lib/mathlib/math/Matrix3.hpp | 12 +++- src/lib/mathlib/math/Vector.hpp | 90 ++++++++++++++++++++------- src/systemcmds/tests/test_mathlib.cpp | 78 +++++++++++++++-------- 4 files changed, 156 insertions(+), 62 deletions(-) diff --git a/src/lib/mathlib/math/Matrix.hpp b/src/lib/mathlib/math/Matrix.hpp index f05360a193..1849f58be2 100644 --- a/src/lib/mathlib/math/Matrix.hpp +++ b/src/lib/mathlib/math/Matrix.hpp @@ -34,9 +34,9 @@ ****************************************************************************/ /** - * @file Matrix3.hpp + * @file Matrix.hpp * - * 3x3 Matrix + * Generic Matrix */ #ifndef MATRIX_HPP @@ -203,6 +203,24 @@ public: return res; } + /** + * transpose the matrix + */ + Matrix transposed(void) const { + Matrix res; + arm_mat_trans_f32(&this->arm_mat, &res.arm_mat); + return res; + } + + /** + * invert the matrix + */ + Matrix inversed(void) const { + Matrix res; + arm_mat_inverse_f32(&this->arm_mat, &res.arm_mat); + return res; + } + /** * setup the identity matrix */ @@ -224,6 +242,8 @@ public: template class Matrix : public MatrixBase { public: + using MatrixBase::operator *; + /** * set to value */ @@ -235,18 +255,18 @@ public: /** * multiplication by a vector */ - /* Vector operator *(const Vector &v) const { Vector res; arm_mat_mult_f32(&this->arm_mat, &v.arm_col, &res.arm_col); return res; } - */ }; template <> class Matrix<3, 3> : public MatrixBase<3, 3> { public: + using MatrixBase<3, 3>::operator *; + /** * set to value */ @@ -258,15 +278,11 @@ public: /** * multiplication by a vector */ - /* Vector<3> operator *(const Vector<3> &v) const { - Vector<3> res; - res(0) = data[0][0] * v(0) + data[0][1] * v(1) + data[0][2] * v(2); - res(1) = data[1][0] * v(0) + data[1][1] * v(1) + data[1][2] * v(2); - res(2) = data[2][0] * v(0) + data[2][1] * v(1) + data[2][2] * v(2); - return res; + return Vector<3>(data[0][0] * v.data[0] + data[0][1] * v.data[1] + data[0][2] * v.data[2], + data[1][0] * v.data[0] + data[1][1] * v.data[1] + data[1][2] * v.data[2], + data[2][0] * v.data[0] + data[2][1] * v.data[1] + data[2][2] * v.data[2]); } - */ }; } diff --git a/src/lib/mathlib/math/Matrix3.hpp b/src/lib/mathlib/math/Matrix3.hpp index 3236cd3d10..c7eb67ba72 100644 --- a/src/lib/mathlib/math/Matrix3.hpp +++ b/src/lib/mathlib/math/Matrix3.hpp @@ -74,14 +74,15 @@ public: * setting ctor */ Matrix3(const T d[3][3]) { - memcpy(data, d, sizeof(data)); arm_mat = {3, 3, &data[0][0]}; + memcpy(data, d, sizeof(data)); } /** * setting ctor */ Matrix3(const T ax, const T ay, const T az, const T bx, const T by, const T bz, const T cx, const T cy, const T cz) { + arm_mat = {3, 3, &data[0][0]}; data[0][0] = ax; data[0][1] = ay; data[0][2] = az; @@ -91,7 +92,6 @@ public: data[2][0] = cx; data[2][1] = cy; data[2][2] = cz; - arm_mat = {3, 3, &data[0][0]}; } /** @@ -124,6 +124,14 @@ public: return data[row][col]; } + /** + * set to value + */ + const Matrix3 &operator =(const Matrix3 &m) { + memcpy(data, m.data, sizeof(data)); + return *this; + } + /** * test for equality */ diff --git a/src/lib/mathlib/math/Vector.hpp b/src/lib/mathlib/math/Vector.hpp index adb2293e15..cd00058b52 100644 --- a/src/lib/mathlib/math/Vector.hpp +++ b/src/lib/mathlib/math/Vector.hpp @@ -49,7 +49,7 @@ namespace math { template -class Vector { +class VectorBase { public: float data[N]; arm_matrix_instance_f32 arm_col; @@ -57,15 +57,22 @@ public: /** * trivial ctor */ - Vector() { + VectorBase() { arm_col = {N, 1, &data[0]}; } /** * setting ctor */ - Vector(const float *d) { - memcpy(data, d, sizeof(data)); +// VectorBase(const float *d) { + // memcpy(data, d, sizeof(data)); + //arm_col = {N, 1, &data[0]}; + //} + + /** + * setting ctor + */ + VectorBase(const float d[]) : data(d) { arm_col = {N, 1, &data[0]}; } @@ -86,7 +93,7 @@ public: /** * test for equality */ - bool operator ==(const Vector &v) { + bool operator ==(const VectorBase &v) { for (unsigned int i = 0; i < N; i++) if (data[i] != v(i)) return false; @@ -96,7 +103,7 @@ public: /** * test for inequality */ - bool operator !=(const Vector &v) { + bool operator !=(const VectorBase &v) { for (unsigned int i = 0; i < N; i++) if (data[i] != v(i)) return true; @@ -106,7 +113,7 @@ public: /** * set to value */ - const Vector &operator =(const Vector &v) { + const VectorBase &operator =(const VectorBase &v) { memcpy(data, v.data, sizeof(data)); return *this; } @@ -114,8 +121,8 @@ public: /** * negation */ - const Vector operator -(void) const { - Vector res; + const VectorBase operator -(void) const { + VectorBase res; for (unsigned int i = 0; i < N; i++) res[i] = -data[i]; return res; @@ -124,8 +131,8 @@ public: /** * addition */ - const Vector operator +(const Vector &v) const { - Vector res; + const VectorBase operator +(const VectorBase &v) const { + VectorBase res; for (unsigned int i = 0; i < N; i++) res[i] = data[i] + v(i); return res; @@ -134,8 +141,8 @@ public: /** * subtraction */ - const Vector operator -(const Vector &v) const { - Vector res; + const VectorBase operator -(const VectorBase &v) const { + VectorBase res; for (unsigned int i = 0; i < N; i++) res[i] = data[i] - v(i); return res; @@ -144,23 +151,23 @@ public: /** * uniform scaling */ - const Vector operator *(const float num) const { - Vector temp(*this); + const VectorBase operator *(const float num) const { + VectorBase temp(*this); return temp *= num; } /** * uniform scaling */ - const Vector operator /(const float num) const { - Vector temp(*this); + const VectorBase operator /(const float num) const { + VectorBase temp(*this); return temp /= num; } /** * addition */ - const Vector &operator +=(const Vector &v) { + const VectorBase &operator +=(const VectorBase &v) { for (unsigned int i = 0; i < N; i++) data[i] += v(i); return *this; @@ -169,7 +176,7 @@ public: /** * subtraction */ - const Vector &operator -=(const Vector &v) { + const VectorBase &operator -=(const VectorBase &v) { for (unsigned int i = 0; i < N; i++) data[i] -= v(i); return *this; @@ -178,7 +185,7 @@ public: /** * uniform scaling */ - const Vector &operator *=(const float num) { + const VectorBase &operator *=(const float num) { for (unsigned int i = 0; i < N; i++) data[i] *= num; return *this; @@ -187,7 +194,7 @@ public: /** * uniform scaling */ - const Vector &operator /=(const float num) { + const VectorBase &operator /=(const float num) { for (unsigned int i = 0; i < N; i++) data[i] /= num; return *this; @@ -196,7 +203,7 @@ public: /** * dot product */ - float operator *(const Vector &v) const { + float operator *(const VectorBase &v) const { float res; for (unsigned int i = 0; i < N; i++) res += data[i] * v(i); @@ -227,11 +234,48 @@ public: /** * returns the normalized version of this vector */ - Vector normalized() const { + VectorBase normalized() const { return *this / length(); } }; +template +class Vector : public VectorBase { +public: + /** + * set to value + */ + const Vector &operator =(const Vector &v) { + memcpy(this->data, v.data, sizeof(this->data)); + return *this; + } +}; + +template <> +class Vector<3> : public VectorBase<3> { +public: + Vector<3>() { + arm_col = {3, 1, &this->data[0]}; + } + + Vector<3>(const float x, const float y, const float z) { + data[0] = x; + data[1] = y; + data[2] = z; + arm_col = {3, 1, &this->data[0]}; + } + + /** + * set to value + */ + const Vector<3> &operator =(const Vector<3> &v) { + data[0] = v.data[0]; + data[1] = v.data[1]; + data[2] = v.data[2]; + return *this; + } +}; + } #endif // VECTOR_HPP diff --git a/src/systemcmds/tests/test_mathlib.cpp b/src/systemcmds/tests/test_mathlib.cpp index 13e6dad518..305ebbefa9 100644 --- a/src/systemcmds/tests/test_mathlib.cpp +++ b/src/systemcmds/tests/test_mathlib.cpp @@ -58,15 +58,31 @@ int test_mathlib(int argc, char *argv[]) { warnx("testing mathlib"); - Matrix3f m; - m.identity(); - Matrix3f m1; - Matrix<3,3> mq; - mq.identity(); - Matrix<3,3> mq1; - m1(0, 0) = 5.0; - Vector3f v = Vector3f(1.0f, 2.0f, 3.0f); - Vector3f v1; + Matrix<3,3> m3; + m3.identity(); + Matrix<4,4> m4; + m4.identity(); + Vector<3> v3; + v3(0) = 1.0f; + v3(1) = 2.0f; + v3(2) = 3.0f; + Vector<4> v4; + v4(0) = 1.0f; + v4(1) = 2.0f; + v4(2) = 3.0f; + v4(3) = 4.0f; + Vector<3> vres3; + Matrix<3,3> mres3; + Matrix<4,4> mres4; + + Matrix3f m3old; + m3old.identity(); + Vector3f v3old; + v3old.x = 1.0f; + v3old.y = 2.0f; + v3old.z = 3.0f; + Vector3f vres3old; + Matrix3f mres3old; unsigned int n = 60000; @@ -74,41 +90,51 @@ int test_mathlib(int argc, char *argv[]) t0 = hrt_absolute_time(); for (unsigned int j = 0; j < n; j++) { - v1 = m * v; + vres3 = m3 * v3; } t1 = hrt_absolute_time(); - warnx("Matrix * Vector: %s %.6fus", formatResult(v1 == v), (double)(t1 - t0) / n); + warnx("Matrix3 * Vector3: %s %.6fus", formatResult(vres3 == v3), (double)(t1 - t0) / n); t0 = hrt_absolute_time(); for (unsigned int j = 0; j < n; j++) { - mq1 = mq * mq; + vres3old = m3old * v3old; } t1 = hrt_absolute_time(); - warnx("Matrix * Matrix: %s %.6fus", formatResult(mq1 == mq), (double)(t1 - t0) / n); - mq1.dump(); + warnx("Matrix3 * Vector3 OLD: %s %.6fus", formatResult(vres3old == v3old), (double)(t1 - t0) / n); t0 = hrt_absolute_time(); for (unsigned int j = 0; j < n; j++) { - m1 = m.transposed(); + mres3 = m3 * m3; } t1 = hrt_absolute_time(); - warnx("Matrix Transpose: %s %.6fus", formatResult(m1 == m), (double)(t1 - t0) / n); + warnx("Matrix3 * Matrix3: %s %.6fus", formatResult(mres3 == m3), (double)(t1 - t0) / n); t0 = hrt_absolute_time(); for (unsigned int j = 0; j < n; j++) { - m1 = m.inversed(); + mres3old = m3old * m3old; } t1 = hrt_absolute_time(); - warnx("Matrix Invert: %s %.6fus", formatResult(m1 == m), (double)(t1 - t0) / n); + warnx("Matrix3 * Matrix3 OLD: %s %.6fus", formatResult(mres3old == m3old), (double)(t1 - t0) / n); - Matrix<4,4> mn; - mn(0, 0) = 2.0f; - mn(1, 0) = 3.0f; - for (int i = 0; i < mn.getRows(); i++) { - for (int j = 0; j < mn.getCols(); j++) { - printf("%.3f ", mn(i, j)); - } - printf("\n"); + t0 = hrt_absolute_time(); + for (unsigned int j = 0; j < n; j++) { + mres4 = m4 * m4; } + t1 = hrt_absolute_time(); + warnx("Matrix4 * Matrix4: %s %.6fus", formatResult(mres4 == m4), (double)(t1 - t0) / n); + + t0 = hrt_absolute_time(); + for (unsigned int j = 0; j < n; j++) { + mres3 = m3.transposed(); + } + t1 = hrt_absolute_time(); + warnx("Matrix3 Transpose: %s %.6fus", formatResult(mres3 == m3), (double)(t1 - t0) / n); + + t0 = hrt_absolute_time(); + for (unsigned int j = 0; j < n; j++) { + mres3 = m3.inversed(); + } + t1 = hrt_absolute_time(); + warnx("Matrix3 Invert: %s %.6fus", formatResult(mres3 == m3), (double)(t1 - t0) / n); return 0; } From ba612c3ee8b88b9352e7cfa723997887dd736b76 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 19 Dec 2013 14:10:25 +0400 Subject: [PATCH 207/447] mathlib fixes --- src/lib/conversion/rotation.cpp | 2 +- src/lib/conversion/rotation.h | 2 +- src/lib/ecl/l1/ecl_l1_pos_controller.cpp | 43 +-- src/lib/ecl/l1/ecl_l1_pos_controller.h | 12 +- src/lib/external_lgpl/tecs/tecs.cpp | 6 +- src/lib/external_lgpl/tecs/tecs.h | 6 +- src/lib/mathlib/math/Matrix.hpp | 172 +++++++-- src/lib/mathlib/math/Matrix3.cpp | 46 --- src/lib/mathlib/math/Matrix3.hpp | 356 ------------------ src/lib/mathlib/math/Quaternion.hpp | 79 +++- src/lib/mathlib/math/Vector.hpp | 230 +++++++++-- src/lib/mathlib/math/Vector2.hpp | 269 ------------- src/lib/mathlib/math/Vector3.hpp | 287 -------------- src/lib/mathlib/mathlib.h | 3 - src/lib/mathlib/module.mk | 1 - .../att_pos_estimator_ekf/KalmanNav.cpp | 92 +++-- .../att_pos_estimator_ekf/KalmanNav.hpp | 20 +- .../commander/accelerometer_calibration.cpp | 14 +- src/modules/mavlink/mavlink_receiver.cpp | 11 +- src/modules/navigator/navigator_main.cpp | 16 +- src/modules/sensors/sensors.cpp | 14 +- src/systemcmds/tests/test_mathlib.cpp | 23 -- 22 files changed, 513 insertions(+), 1191 deletions(-) delete mode 100644 src/lib/mathlib/math/Matrix3.cpp delete mode 100644 src/lib/mathlib/math/Matrix3.hpp delete mode 100644 src/lib/mathlib/math/Vector2.hpp delete mode 100644 src/lib/mathlib/math/Vector3.hpp diff --git a/src/lib/conversion/rotation.cpp b/src/lib/conversion/rotation.cpp index bac594ab9d..614877b183 100644 --- a/src/lib/conversion/rotation.cpp +++ b/src/lib/conversion/rotation.cpp @@ -41,7 +41,7 @@ #include "rotation.h" __EXPORT void -get_rot_matrix(enum Rotation rot, math::Matrix3f *rot_matrix) +get_rot_matrix(enum Rotation rot, math::Matrix<3,3> *rot_matrix) { float roll = M_DEG_TO_RAD_F * (float)rot_lookup[rot].roll; float pitch = M_DEG_TO_RAD_F * (float)rot_lookup[rot].pitch; diff --git a/src/lib/conversion/rotation.h b/src/lib/conversion/rotation.h index 85c63c0fcb..0c56494c5e 100644 --- a/src/lib/conversion/rotation.h +++ b/src/lib/conversion/rotation.h @@ -116,6 +116,6 @@ const rot_lookup_t rot_lookup[] = { * Get the rotation matrix */ __EXPORT void -get_rot_matrix(enum Rotation rot, math::Matrix *rot_matrix); +get_rot_matrix(enum Rotation rot, math::Matrix<3,3> *rot_matrix); #endif /* ROTATION_H_ */ diff --git a/src/lib/ecl/l1/ecl_l1_pos_controller.cpp b/src/lib/ecl/l1/ecl_l1_pos_controller.cpp index 27d76f959c..fb7ed486df 100644 --- a/src/lib/ecl/l1/ecl_l1_pos_controller.cpp +++ b/src/lib/ecl/l1/ecl_l1_pos_controller.cpp @@ -83,8 +83,8 @@ float ECL_L1_Pos_Controller::crosstrack_error(void) return _crosstrack_error; } -void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, const math::Vector2f &vector_B, const math::Vector2f &vector_curr_position, - const math::Vector2f &ground_speed_vector) +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] */ @@ -94,7 +94,7 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, c float ltrack_vel; /* get the direction between the last (visited) and next waypoint */ - _target_bearing = get_bearing_to_next_waypoint(vector_curr_position.getX(), vector_curr_position.getY(), vector_B.getX(), vector_B.getY()); + _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); @@ -103,7 +103,7 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, c _L1_distance = _L1_ratio * ground_speed; /* calculate vector from A to B */ - math::Vector2f vector_AB = get_local_planar_vector(vector_A, vector_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, @@ -116,7 +116,7 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, c vector_AB.normalize(); /* calculate the vector from waypoint A to the aircraft */ - math::Vector2f vector_A_to_airplane = get_local_planar_vector(vector_A, vector_curr_position); + 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; @@ -130,7 +130,7 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, c float alongTrackDist = vector_A_to_airplane * vector_AB; /* estimate airplane position WRT to B */ - math::Vector2f vector_B_to_P_unit = get_local_planar_vector(vector_B, vector_curr_position).normalized(); + 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) */ @@ -143,14 +143,14 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, c /* calculate eta to fly to waypoint A */ /* unit vector from waypoint A to current position */ - math::Vector2f vector_A_to_airplane_unit = vector_A_to_airplane.normalized(); + 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.getY() , -vector_A_to_airplane_unit.getX()); + _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 @@ -174,7 +174,7 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, c 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.getY() , -vector_B_to_P_unit.getX()); + _nav_bearing = atan2f(-vector_B_to_P_unit(1) , -vector_B_to_P_unit(0)); } else { @@ -194,7 +194,7 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, c float eta1 = asinf(sine_eta1); eta = eta1 + eta2; /* bearing from current position to L1 point */ - _nav_bearing = atan2f(vector_AB.getY(), vector_AB.getX()) + eta1; + _nav_bearing = atan2f(vector_AB(1), vector_AB(0)) + eta1; } @@ -209,8 +209,8 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, c _bearing_error = eta; } -void ECL_L1_Pos_Controller::navigate_loiter(const math::Vector2f &vector_A, const math::Vector2f &vector_curr_position, float radius, int8_t loiter_direction, - const math::Vector2f &ground_speed_vector) +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] */ @@ -220,7 +220,7 @@ void ECL_L1_Pos_Controller::navigate_loiter(const math::Vector2f &vector_A, cons float K_velocity = 2.0f * _L1_damping * omega; /* update bearing to next waypoint */ - _target_bearing = get_bearing_to_next_waypoint(vector_curr_position.getX(), vector_curr_position.getY(), vector_A.getX(), vector_A.getY()); + _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); @@ -229,10 +229,10 @@ void ECL_L1_Pos_Controller::navigate_loiter(const math::Vector2f &vector_A, cons _L1_distance = _L1_ratio * ground_speed; /* calculate the vector from waypoint A to current position */ - math::Vector2f vector_A_to_airplane = get_local_planar_vector(vector_A, vector_curr_position); + math::Vector<2> vector_A_to_airplane = get_local_planar_vector(vector_A, vector_curr_position); /* store the normalized vector from waypoint A to current position */ - math::Vector2f vector_A_to_airplane_unit = (vector_A_to_airplane).normalized(); + math::Vector<2> vector_A_to_airplane_unit = (vector_A_to_airplane).normalized(); /* calculate eta angle towards the loiter center */ @@ -287,19 +287,19 @@ void ECL_L1_Pos_Controller::navigate_loiter(const math::Vector2f &vector_A, cons /* 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.getY() , -vector_A_to_airplane_unit.getX()); + _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.getY() , -vector_A_to_airplane_unit.getX()); + _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::Vector2f &ground_speed_vector) +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] */ @@ -352,14 +352,11 @@ void ECL_L1_Pos_Controller::navigate_level_flight(float current_heading) } -math::Vector2f ECL_L1_Pos_Controller::get_local_planar_vector(const math::Vector2f &origin, const math::Vector2f &target) const +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::Vector2f out; - - out.setX(math::radians((target.getX() - origin.getX()))); - out.setY(math::radians((target.getY() - origin.getY())*cosf(math::radians(origin.getX())))); + 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 index 7a3c42a925..5c0804a392 100644 --- a/src/lib/ecl/l1/ecl_l1_pos_controller.h +++ b/src/lib/ecl/l1/ecl_l1_pos_controller.h @@ -160,8 +160,8 @@ public: * * @return sets _lateral_accel setpoint */ - void navigate_waypoints(const math::Vector2f &vector_A, const math::Vector2f &vector_B, const math::Vector2f &vector_curr_position, - const math::Vector2f &ground_speed); + 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); /** @@ -172,8 +172,8 @@ public: * * @return sets _lateral_accel setpoint */ - void navigate_loiter(const math::Vector2f &vector_A, const math::Vector2f &vector_curr_position, float radius, int8_t loiter_direction, - const math::Vector2f &ground_speed_vector); + 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); /** @@ -185,7 +185,7 @@ public: * * @return sets _lateral_accel setpoint */ - void navigate_heading(float navigation_heading, float current_heading, const math::Vector2f &ground_speed); + void navigate_heading(float navigation_heading, float current_heading, const math::Vector<2> &ground_speed); /** @@ -260,7 +260,7 @@ private: * @param wp The point to convert to into the local coordinates, in WGS84 coordinates * @return The vector in meters pointing from the reference position to the coordinates */ - math::Vector2f get_local_planar_vector(const math::Vector2f &origin, const math::Vector2f &target) const; + math::Vector<2> get_local_planar_vector(const math::Vector<2> &origin, const math::Vector<2> &target) const; }; diff --git a/src/lib/external_lgpl/tecs/tecs.cpp b/src/lib/external_lgpl/tecs/tecs.cpp index a733ef1d2a..d7f350e5c6 100644 --- a/src/lib/external_lgpl/tecs/tecs.cpp +++ b/src/lib/external_lgpl/tecs/tecs.cpp @@ -29,7 +29,7 @@ using namespace math; * */ -void TECS::update_50hz(float baro_altitude, float airspeed, const math::Dcm &rotMat, const math::Vector3 &accel_body, const math::Vector3 &accel_earth) +void TECS::update_50hz(float baro_altitude, float airspeed, const math::Matrix<3,3> &rotMat, const math::Vector<3> &accel_body, const math::Vector<3> &accel_earth) { // Implement third order complementary filter for height and height rate // estimted height rate = _integ2_state @@ -257,7 +257,7 @@ void TECS::_update_energies(void) _SKEdot = _integ5_state * _vel_dot; } -void TECS::_update_throttle(float throttle_cruise, const math::Dcm &rotMat) +void TECS::_update_throttle(float throttle_cruise, const math::Matrix<3,3> &rotMat) { // Calculate total energy values _STE_error = _SPE_dem - _SPE_est + _SKE_dem - _SKE_est; @@ -468,7 +468,7 @@ void TECS::_update_STE_rate_lim(void) _STEdot_min = - _minSinkRate * CONSTANTS_ONE_G; } -void TECS::update_pitch_throttle(const math::Dcm &rotMat, float pitch, float baro_altitude, float hgt_dem, float EAS_dem, float indicated_airspeed, float EAS2TAS, bool climbOutDem, float ptchMinCO, +void TECS::update_pitch_throttle(const math::Matrix<3,3> &rotMat, float pitch, float baro_altitude, float hgt_dem, float EAS_dem, float indicated_airspeed, float EAS2TAS, bool climbOutDem, float ptchMinCO, float throttle_min, float throttle_max, float throttle_cruise, float pitch_limit_min, float pitch_limit_max) { diff --git a/src/lib/external_lgpl/tecs/tecs.h b/src/lib/external_lgpl/tecs/tecs.h index f8f832ed76..68832a0390 100644 --- a/src/lib/external_lgpl/tecs/tecs.h +++ b/src/lib/external_lgpl/tecs/tecs.h @@ -71,10 +71,10 @@ public: // Update of the estimated height and height rate internal state // Update of the inertial speed rate internal state // Should be called at 50Hz or greater - void update_50hz(float baro_altitude, float airspeed, const math::Dcm &rotMat, const math::Vector3 &accel_body, const math::Vector3 &accel_earth); + void update_50hz(float baro_altitude, float airspeed, const math::Matrix<3,3> &rotMat, const math::Vector<3> &accel_body, const math::Vector<3> &accel_earth); // Update the control loop calculations - void update_pitch_throttle(const math::Dcm &rotMat, float pitch, float baro_altitude, float hgt_dem, float EAS_dem, float indicated_airspeed, float EAS2TAS, bool climbOutDem, float ptchMinCO, + void update_pitch_throttle(const math::Matrix<3,3> &rotMat, float pitch, float baro_altitude, float hgt_dem, float EAS_dem, float indicated_airspeed, float EAS2TAS, bool climbOutDem, float ptchMinCO, float throttle_min, float throttle_max, float throttle_cruise, float pitch_limit_min, float pitch_limit_max); // demanded throttle in percentage @@ -338,7 +338,7 @@ private: void _update_energies(void); // Update Demanded Throttle - void _update_throttle(float throttle_cruise, const math::Dcm &rotMat); + void _update_throttle(float throttle_cruise, const math::Matrix<3,3> &rotMat); // Detect Bad Descent void _detect_bad_descent(void); diff --git a/src/lib/mathlib/math/Matrix.hpp b/src/lib/mathlib/math/Matrix.hpp index 1849f58be2..67214133ac 100644 --- a/src/lib/mathlib/math/Matrix.hpp +++ b/src/lib/mathlib/math/Matrix.hpp @@ -42,16 +42,19 @@ #ifndef MATRIX_HPP #define MATRIX_HPP +#include #include "../CMSIS/Include/arm_math.h" namespace math { -template +template class Matrix; +class Quaternion; + // MxN matrix with float elements -template +template class MatrixBase { public: /** @@ -69,10 +72,14 @@ public: * note that this ctor will not initialize elements */ MatrixBase() { - //arm_mat_init_f32(&arm_mat, M, N, data); arm_mat = {M, N, &data[0][0]}; } + MatrixBase(const float *d) { + arm_mat = {M, N, &data[0][0]}; + memcpy(data, d, sizeof(data)); + } + /** * access by index */ @@ -98,10 +105,10 @@ public: /** * test for equality */ - bool operator ==(const MatrixBase &m) { + bool operator ==(const Matrix &m) { for (unsigned int i = 0; i < M; i++) for (unsigned int j = 0; j < N; j++) - if (data[i][j] != m(i, j)) + if (data[i][j] != m.data[i][j]) return false; return true; } @@ -109,10 +116,10 @@ public: /** * test for inequality */ - bool operator !=(const MatrixBase &m) { + bool operator !=(const Matrix &m) { for (unsigned int i = 0; i < M; i++) for (unsigned int j = 0; j < N; j++) - if (data[i][j] != m(i, j)) + if (data[i][j] != m.data[i][j]) return true; return false; } @@ -120,85 +127,97 @@ public: /** * set to value */ - const MatrixBase &operator =(const MatrixBase &m) { + const Matrix &operator =(const Matrix &m) { memcpy(data, m.data, sizeof(data)); - return *this; + return *reinterpret_cast*>(this); } /** * negation */ - MatrixBase operator -(void) const { - MatrixBase res; + Matrix operator -(void) const { + Matrix res; for (unsigned int i = 0; i < N; i++) for (unsigned int j = 0; j < M; j++) - res[i][j] = -data[i][j]; + res.data[i][j] = -data[i][j]; return res; } /** * addition */ - MatrixBase operator +(const MatrixBase &m) const { - MatrixBase res; + Matrix operator +(const Matrix &m) const { + Matrix res; for (unsigned int i = 0; i < N; i++) for (unsigned int j = 0; j < M; j++) - res[i][j] = data[i][j] + m(i, j); + res.data[i][j] = data[i][j] + m.data[i][j]; return res; } - MatrixBase &operator +=(const MatrixBase &m) { - return *this = *this + m; + Matrix &operator +=(const Matrix &m) { + for (unsigned int i = 0; i < N; i++) + for (unsigned int j = 0; j < M; j++) + data[i][j] += m.data[i][j]; + return *reinterpret_cast*>(this); } /** * subtraction */ - MatrixBase operator -(const MatrixBase &m) const { - MatrixBase res; + Matrix operator -(const Matrix &m) const { + Matrix res; for (unsigned int i = 0; i < M; i++) for (unsigned int j = 0; j < N; j++) - res[i][j] = data[i][j] - m(i, j); + res.data[i][j] = data[i][j] - m.data[i][j]; return res; } - MatrixBase &operator -=(const MatrixBase &m) { - return *this = *this - m; + Matrix &operator -=(const Matrix &m) { + for (unsigned int i = 0; i < N; i++) + for (unsigned int j = 0; j < M; j++) + data[i][j] -= m.data[i][j]; + return *reinterpret_cast*>(this); } /** * uniform scaling */ - MatrixBase operator *(const float num) const { - MatrixBase res; + Matrix operator *(const float num) const { + Matrix res; for (unsigned int i = 0; i < M; i++) for (unsigned int j = 0; j < N; j++) - res[i][j] = data[i][j] * num; + res.data[i][j] = data[i][j] * num; return res; } - MatrixBase &operator *=(const float num) { - return *this = *this * num; + Matrix &operator *=(const float num) { + for (unsigned int i = 0; i < M; i++) + for (unsigned int j = 0; j < N; j++) + data[i][j] *= num; + return *reinterpret_cast*>(this); } - MatrixBase operator /(const float num) const { - MatrixBase res; + Matrix operator /(const float num) const { + Matrix res; for (unsigned int i = 0; i < M; i++) for (unsigned int j = 0; j < N; j++) res[i][j] = data[i][j] / num; return res; } - MatrixBase &operator /=(const float num) { - return *this = *this / num; + Matrix &operator /=(const float num) { + for (unsigned int i = 0; i < M; i++) + for (unsigned int j = 0; j < N; j++) + data[i][j] /= num; + return *this; } /** * multiplication by another matrix */ template - Matrix operator *(const Matrix &m) const { - Matrix res; + Matrix operator *(const Matrix &m) const { + Matrix res; arm_mat_mult_f32(&arm_mat, &m.arm_mat, &res.arm_mat); return res; } @@ -230,7 +249,7 @@ public: data[i][i] = 1; } - void dump(void) { + void print(void) { for (unsigned int i = 0; i < M; i++) { for (unsigned int j = 0; j < N; j++) printf("%.3f\t", data[i][j]); @@ -244,6 +263,12 @@ class Matrix : public MatrixBase { public: using MatrixBase::operator *; + Matrix() : MatrixBase() { + } + + Matrix(const float *d) : MatrixBase(d) { + } + /** * set to value */ @@ -255,18 +280,28 @@ public: /** * multiplication by a vector */ - Vector operator *(const Vector &v) const { + Vector operator *(const Vector &v) const { Vector res; arm_mat_mult_f32(&this->arm_mat, &v.arm_col, &res.arm_col); return res; } }; +} +#include "Quaternion.hpp" + +namespace math { template <> class Matrix<3, 3> : public MatrixBase<3, 3> { public: using MatrixBase<3, 3>::operator *; + Matrix() : MatrixBase<3, 3>() { + } + + Matrix(const float *d) : MatrixBase<3, 3>(d) { + } + /** * set to value */ @@ -279,9 +314,70 @@ public: * multiplication by a vector */ Vector<3> operator *(const Vector<3> &v) const { - return Vector<3>(data[0][0] * v.data[0] + data[0][1] * v.data[1] + data[0][2] * v.data[2], - data[1][0] * v.data[0] + data[1][1] * v.data[1] + data[1][2] * v.data[2], - data[2][0] * v.data[0] + data[2][1] * v.data[1] + data[2][2] * v.data[2]); + Vector<3> res(data[0][0] * v.data[0] + data[0][1] * v.data[1] + data[0][2] * v.data[2], + data[1][0] * v.data[0] + data[1][1] * v.data[1] + data[1][2] * v.data[2], + data[2][0] * v.data[0] + data[2][1] * v.data[1] + data[2][2] * v.data[2]); + return res; + } + + /** + * create a rotation matrix from given quaternion + */ + void from_quaternion(const Quaternion &q) { + float aSq = q.data[0] * q.data[0]; + float bSq = q.data[1] * q.data[1]; + float cSq = q.data[2] * q.data[2]; + float dSq = q.data[3] * q.data[3]; + data[0][0] = aSq + bSq - cSq - dSq; + data[0][1] = 2.0f * (q.data[1] * q.data[2] - q.data[0] * q.data[3]); + data[0][2] = 2.0f * (q.data[0] * q.data[2] + q.data[1] * q.data[3]); + data[1][0] = 2.0f * (q.data[1] * q.data[2] + q.data[0] * q.data[3]); + data[1][1] = aSq - bSq + cSq - dSq; + data[1][2] = 2.0f * (q.data[2] * q.data[3] - q.data[0] * q.data[1]); + data[2][0] = 2.0f * (q.data[1] * q.data[3] - q.data[0] * q.data[2]); + data[2][1] = 2.0f * (q.data[0] * q.data[1] + q.data[2] * q.data[3]); + data[2][2] = aSq - bSq - cSq + dSq; + } + + /** + * create a rotation matrix from given euler angles + * based on http://gentlenav.googlecode.com/files/EulerAngles.pdf + */ + void from_euler(float roll, float pitch, float yaw) { + float cp = cosf(pitch); + float sp = sinf(pitch); + float sr = sinf(roll); + float cr = cosf(roll); + float sy = sinf(yaw); + float cy = cosf(yaw); + + data[0][0] = cp * cy; + data[0][1] = (sr * sp * cy) - (cr * sy); + data[0][2] = (cr * sp * cy) + (sr * sy); + data[1][0] = cp * sy; + data[1][1] = (sr * sp * sy) + (cr * cy); + data[1][2] = (cr * sp * sy) - (sr * cy); + data[2][0] = -sp; + data[2][1] = sr * cp; + data[2][2] = cr * cp; + } + + Vector<3> to_euler(void) const { + Vector<3> euler; + euler.data[1] = asinf(-data[2][0]); + + if (fabsf(euler.data[1] - M_PI_2_F) < 1.0e-3f) { + euler.data[0] = 0.0f; + euler.data[2] = atan2f(data[1][2] - data[0][1], data[0][2] + data[1][1]) + euler.data[0]; + + } else if (fabsf(euler.data[1] + M_PI_2_F) < 1.0e-3f) { + euler.data[0] = 0.0f; + euler.data[2] = atan2f(data[1][2] - data[0][1], data[0][2] + data[1][1]) - euler.data[0]; + + } else { + euler.data[0] = atan2f(data[2][1], data[2][2]); + euler.data[2] = atan2f(data[1][0], data[0][0]); + } } }; diff --git a/src/lib/mathlib/math/Matrix3.cpp b/src/lib/mathlib/math/Matrix3.cpp deleted file mode 100644 index 3f1259dc40..0000000000 --- a/src/lib/mathlib/math/Matrix3.cpp +++ /dev/null @@ -1,46 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. - * Author: Will Perone - * Anton Babushkin - * - * 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 Matrix3.cpp - * - * 3x3 Matrix - */ - -#include "Matrix3.hpp" - -namespace math -{ -} diff --git a/src/lib/mathlib/math/Matrix3.hpp b/src/lib/mathlib/math/Matrix3.hpp deleted file mode 100644 index c7eb67ba72..0000000000 --- a/src/lib/mathlib/math/Matrix3.hpp +++ /dev/null @@ -1,356 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. - * Author: Will Perone - * Anton Babushkin - * - * 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 Matrix3.hpp - * - * 3x3 Matrix - */ - -#ifndef MATRIX3_HPP -#define MATRIX3_HPP - -#include "Vector3.hpp" -#include "../CMSIS/Include/arm_math.h" - -namespace math -{ - -// 3x3 matrix with elements of type T -template -class Matrix3 { -public: - /** - * matrix data[row][col] - */ - T data[3][3]; - - /** - * struct for using arm_math functions - */ - arm_matrix_instance_f32 arm_mat; - - /** - * trivial ctor - * note that this ctor will not initialize elements - */ - Matrix3() { - arm_mat = {3, 3, &data[0][0]}; - } - - /** - * setting ctor - */ - Matrix3(const T d[3][3]) { - arm_mat = {3, 3, &data[0][0]}; - memcpy(data, d, sizeof(data)); - } - - /** - * setting ctor - */ - Matrix3(const T ax, const T ay, const T az, const T bx, const T by, const T bz, const T cx, const T cy, const T cz) { - arm_mat = {3, 3, &data[0][0]}; - data[0][0] = ax; - data[0][1] = ay; - data[0][2] = az; - data[1][0] = bx; - data[1][1] = by; - data[1][2] = bz; - data[2][0] = cx; - data[2][1] = cy; - data[2][2] = cz; - } - - /** - * casting from a vector3f to a matrix is the tilde operator - */ - Matrix3(const Vector3 &v) { - arm_mat = {3, 3, &data[0][0]}; - data[0][0] = 0; - data[0][1] = -v.z; - data[0][2] = v.y; - data[1][0] = v.z; - data[1][1] = 0; - data[1][2] = -v.x; - data[2][0] = -v.y; - data[2][1] = v.x; - data[2][2] = 0; - } - - /** - * access by index - */ - inline T &operator ()(unsigned int row, unsigned int col) { - return data[row][col]; - } - - /** - * access to elements by index - */ - inline const T &operator ()(unsigned int row, unsigned int col) const { - return data[row][col]; - } - - /** - * set to value - */ - const Matrix3 &operator =(const Matrix3 &m) { - memcpy(data, m.data, sizeof(data)); - return *this; - } - - /** - * test for equality - */ - bool operator ==(const Matrix3 &m) { - for (int i = 0; i < 3; i++) - for (int j = 0; j < 3; j++) - if (data[i][j] != m(i, j)) - return false; - return true; - } - - /** - * test for inequality - */ - bool operator !=(const Matrix3 &m) { - for (int i = 0; i < 3; i++) - for (int j = 0; j < 3; j++) - if (data[i][j] != m(i, j)) - return true; - return false; - } - - /** - * negation - */ - Matrix3 operator -(void) const { - Matrix3 res; - for (int i = 0; i < 3; i++) - for (int j = 0; j < 3; j++) - res[i][j] = -data[i][j]; - return res; - } - - /** - * addition - */ - Matrix3 operator +(const Matrix3 &m) const { - Matrix3 res; - for (int i = 0; i < 3; i++) - for (int j = 0; j < 3; j++) - res[i][j] = data[i][j] + m(i, j); - return res; - } - - Matrix3 &operator +=(const Matrix3 &m) { - return *this = *this + m; - } - - /** - * subtraction - */ - Matrix3 operator -(const Matrix3 &m) const { - Matrix3 res; - for (int i = 0; i < 3; i++) - for (int j = 0; j < 3; j++) - res[i][j] = data[i][j] - m(i, j); - return res; - } - - Matrix3 &operator -=(const Matrix3 &m) { - return *this = *this - m; - } - - /** - * uniform scaling - */ - Matrix3 operator *(const T num) const { - Matrix3 res; - for (int i = 0; i < 3; i++) - for (int j = 0; j < 3; j++) - res[i][j] = data[i][j] * num; - return res; - } - - Matrix3 &operator *=(const T num) { - return *this = *this * num; - } - - Matrix3 operator /(const T num) const { - Matrix3 res; - for (int i = 0; i < 3; i++) - for (int j = 0; j < 3; j++) - res[i][j] = data[i][j] / num; - return res; - } - - Matrix3 &operator /=(const T num) { - return *this = *this / num; - } - - /** - * multiplication by a vector - */ - Vector3 operator *(const Vector3 &v) const { - return Vector3( - data[0][0] * v.x + data[0][1] * v.y + data[0][2] * v.z, - data[1][0] * v.x + data[1][1] * v.y + data[1][2] * v.z, - data[2][0] * v.x + data[2][1] * v.y + data[2][2] * v.z); - } - - /** - * multiplication of transpose by a vector - */ - Vector3 mul_transpose(const Vector3 &v) const { - return Vector3( - data[0][0] * v.x + data[1][0] * v.y + data[2][0] * v.z, - data[0][1] * v.x + data[1][1] * v.y + data[2][1] * v.z, - data[0][2] * v.x + data[1][2] * v.y + data[2][2] * v.z); - } - - /** - * multiplication by another matrix - */ - Matrix3 operator *(const Matrix3 &m) const { -#if defined(CONFIG_ARCH_CORTEXM4) && defined(CONFIG_ARCH_FPU) - Matrix3 res; - arm_mat_mult_f32(&arm_mat, &m.arm_mat, &res.arm_mat); - return res; -#else - return Matrix3(data[0][0] * m(0, 0) + data[0][1] * m(1, 0) + data[0][2] * m(2, 0), - data[0][0] * m(0, 1) + data[0][1] * m(1, 1) + data[0][2] * m(2, 1), - data[0][0] * m(0, 2) + data[0][1] * m(1, 2) + data[0][2] * m(2, 2), - data[1][0] * m(0, 0) + data[1][1] * m(1, 0) + data[1][2] * m(2, 0), - data[1][0] * m(0, 1) + data[1][1] * m(1, 1) + data[1][2] * m(2, 1), - data[1][0] * m(0, 2) + data[1][1] * m(1, 2) + data[1][2] * m(2, 2), - data[2][0] * m(0, 0) + data[2][1] * m(1, 0) + data[2][2] * m(2, 0), - data[2][0] * m(0, 1) + data[2][1] * m(1, 1) + data[2][2] * m(2, 1), - data[2][0] * m(0, 2) + data[2][1] * m(1, 2) + data[2][2] * m(2, 2)); -#endif - } - - Matrix3 &operator *=(const Matrix3 &m) { - return *this = *this * m; - } - - /** - * transpose the matrix - */ - Matrix3 transposed(void) const { -#if defined(CONFIG_ARCH_CORTEXM4) && defined(CONFIG_ARCH_FPU) && T == float - Matrix3 res; - arm_mat_trans_f32(&arm_mat, &res.arm_mat); - return res; -#else - return Matrix3(data[0][0], data[1][0], data[2][0], - data[0][1], data[1][1], data[2][1], - data[0][2], data[1][2], data[2][2]); -#endif - } - - /** - * inverse the matrix - */ - Matrix3 inversed(void) const { - Matrix3 res; - arm_mat_inverse_f32(&arm_mat, &res.arm_mat); - return res; - } - - /** - * zero the matrix - */ - void zero(void) { - memset(data, 0, sizeof(data)); - } - - /** - * setup the identity matrix - */ - void identity(void) { - memset(data, 0, sizeof(data)); - data[0][0] = 1; - data[1][1] = 1; - data[2][2] = 1; - } - - /** - * check if any elements are NAN - */ - bool is_nan(void) { - for (int i = 0; i < 3; i++) - for (int j = 0; j < 3; j++) - if (isnan(data[i][j])) - return true; - return false; - } - - /** - * create a rotation matrix from given euler angles - * based on http://gentlenav.googlecode.com/files/EulerAngles.pdf - */ - void from_euler(T roll, T pitch, T yaw) { - T cp = (T)cosf(pitch); - T sp = (T)sinf(pitch); - T sr = (T)sinf(roll); - T cr = (T)cosf(roll); - T sy = (T)sinf(yaw); - T cy = (T)cosf(yaw); - - data[0][0] = cp * cy; - data[0][1] = (sr * sp * cy) - (cr * sy); - data[0][2] = (cr * sp * cy) + (sr * sy); - data[1][0] = cp * sy; - data[1][1] = (sr * sp * sy) + (cr * cy); - data[1][2] = (cr * sp * sy) - (sr * cy); - data[2][0] = -sp; - data[2][1] = sr * cp; - data[2][2] = cr * cp; - } - - // create eulers from a rotation matrix - //void to_euler(float *roll, float *pitch, float *yaw); - - // apply an additional rotation from a body frame gyro vector - // to a rotation matrix. - //void rotate(const Vector3 &g); -}; - -typedef Matrix3 Matrix3f; -} - -#endif // MATRIX3_HPP diff --git a/src/lib/mathlib/math/Quaternion.hpp b/src/lib/mathlib/math/Quaternion.hpp index 64b980ad81..1b0cb3c411 100644 --- a/src/lib/mathlib/math/Quaternion.hpp +++ b/src/lib/mathlib/math/Quaternion.hpp @@ -44,15 +44,17 @@ #include #include "../CMSIS/Include/arm_math.h" +#include "Vector.hpp" +#include "Matrix.hpp" namespace math { -class Quaternion { -public: - float real; - Vector3 imag; +template +class Matrix; +class Quaternion : public Vector<4> { +public: /** * trivial ctor */ @@ -62,7 +64,74 @@ public: /** * setting ctor */ - Quaternion(const float a, const float b, const float c, const float d): real(a), imag(b, c, d) { + Quaternion(const float a0, const float b0, const float c0, const float d0): Vector(a0, b0, c0, d0) { + } + + Quaternion(const Vector<4> &v) : Vector(v) { + } + + Quaternion(const float *v) : Vector(v) { + } + + /** + * access to elements by index + */ + /* + inline float &operator ()(unsigned int i) { + return *(&a + i); + } + */ + + /** + * access to elements by index + */ + /* + inline const float &operator ()(unsigned int i) const { + return *(&a + i); + } + */ + + /** + * addition + */ + /* + const Quaternion operator +(const Quaternion &q) const { + return Quaternion(a + q.a, b + q.b, c + q.c, d + q.d); + } + */ + + /** + * subtraction + */ + /* + const Quaternion operator -(const Quaternion &q) const { + return Quaternion(a - q.a, b - q.b, c - q.c, d - q.d); + } + */ + + Quaternion derivative(const Vector<3> &w) { + float dataQ[] = { + data[0], -data[1], -data[2], -data[3], + data[1], data[0], -data[3], data[2], + data[2], data[3], data[0], -data[1], + data[3], -data[2], data[1], data[0] + }; + Matrix<4,4> Q(dataQ); + Vector<4> v(0.0f, w.data[0], w.data[1], w.data[2]); + return Q * v * 0.5f; + } + + void from_euler(float roll, float pitch, float yaw) { + double cosPhi_2 = cos(double(roll) / 2.0); + double sinPhi_2 = sin(double(roll) / 2.0); + double cosTheta_2 = cos(double(pitch) / 2.0); + double sinTheta_2 = sin(double(pitch) / 2.0); + double cosPsi_2 = cos(double(yaw) / 2.0); + double sinPsi_2 = sin(double(yaw) / 2.0); + data[0] = cosPhi_2 * cosTheta_2 * cosPsi_2 + sinPhi_2 * sinTheta_2 * sinPsi_2; + data[1] = sinPhi_2 * cosTheta_2 * cosPsi_2 - cosPhi_2 * sinTheta_2 * sinPsi_2; + data[2] = cosPhi_2 * sinTheta_2 * cosPsi_2 + sinPhi_2 * cosTheta_2 * sinPsi_2; + data[3] = cosPhi_2 * cosTheta_2 * sinPsi_2 - sinPhi_2 * sinTheta_2 * cosPsi_2; } }; } diff --git a/src/lib/mathlib/math/Vector.hpp b/src/lib/mathlib/math/Vector.hpp index cd00058b52..2df87c74bf 100644 --- a/src/lib/mathlib/math/Vector.hpp +++ b/src/lib/mathlib/math/Vector.hpp @@ -42,38 +42,50 @@ #ifndef VECTOR_HPP #define VECTOR_HPP +#include #include #include "../CMSIS/Include/arm_math.h" namespace math { +template +class Vector; + template class VectorBase { public: + /** + * vector data + */ float data[N]; + + /** + * struct for using arm_math functions, represents column vector + */ arm_matrix_instance_f32 arm_col; /** * trivial ctor */ - VectorBase() { + VectorBase() { arm_col = {N, 1, &data[0]}; } + /** + * copy ctor + */ + VectorBase(const VectorBase &v) { + arm_col = {N, 1, &data[0]}; + memcpy(data, v.data, sizeof(data)); + } + /** * setting ctor */ -// VectorBase(const float *d) { - // memcpy(data, d, sizeof(data)); - //arm_col = {N, 1, &data[0]}; - //} - - /** - * setting ctor - */ - VectorBase(const float d[]) : data(d) { + VectorBase(const float *d) { arm_col = {N, 1, &data[0]}; + memcpy(data, d, sizeof(data)); } /** @@ -90,10 +102,18 @@ public: return data[i]; } + unsigned int getRows() { + return N; + } + + unsigned int getCols() { + return 1; + } + /** * test for equality */ - bool operator ==(const VectorBase &v) { + bool operator ==(const Vector &v) { for (unsigned int i = 0; i < N; i++) if (data[i] != v(i)) return false; @@ -103,7 +123,7 @@ public: /** * test for inequality */ - bool operator !=(const VectorBase &v) { + bool operator !=(const Vector &v) { for (unsigned int i = 0; i < N; i++) if (data[i] != v(i)) return true; @@ -113,98 +133,98 @@ public: /** * set to value */ - const VectorBase &operator =(const VectorBase &v) { + const Vector &operator =(const Vector &v) { memcpy(data, v.data, sizeof(data)); - return *this; + return *reinterpret_cast*>(this); } /** * negation */ - const VectorBase operator -(void) const { - VectorBase res; + const Vector operator -(void) const { + Vector res; for (unsigned int i = 0; i < N; i++) - res[i] = -data[i]; + res.data[i] = -data[i]; return res; } /** * addition */ - const VectorBase operator +(const VectorBase &v) const { - VectorBase res; + const Vector operator +(const Vector &v) const { + Vector res; for (unsigned int i = 0; i < N; i++) - res[i] = data[i] + v(i); + res.data[i] = data[i] + v(i); return res; } /** * subtraction */ - const VectorBase operator -(const VectorBase &v) const { - VectorBase res; + const Vector operator -(const Vector &v) const { + Vector res; for (unsigned int i = 0; i < N; i++) - res[i] = data[i] - v(i); + res.data[i] = data[i] - v(i); return res; } /** * uniform scaling */ - const VectorBase operator *(const float num) const { - VectorBase temp(*this); + const Vector operator *(const float num) const { + Vector temp(*this); return temp *= num; } /** * uniform scaling */ - const VectorBase operator /(const float num) const { - VectorBase temp(*this); + const Vector operator /(const float num) const { + Vector temp(*reinterpret_cast*>(this)); return temp /= num; } /** * addition */ - const VectorBase &operator +=(const VectorBase &v) { + const Vector &operator +=(const Vector &v) { for (unsigned int i = 0; i < N; i++) data[i] += v(i); - return *this; + return *reinterpret_cast*>(this); } /** * subtraction */ - const VectorBase &operator -=(const VectorBase &v) { + const Vector &operator -=(const Vector &v) { for (unsigned int i = 0; i < N; i++) data[i] -= v(i); - return *this; + return *reinterpret_cast*>(this); } /** * uniform scaling */ - const VectorBase &operator *=(const float num) { + const Vector &operator *=(const float num) { for (unsigned int i = 0; i < N; i++) data[i] *= num; - return *this; + return *reinterpret_cast*>(this); } /** * uniform scaling */ - const VectorBase &operator /=(const float num) { + const Vector &operator /=(const float num) { for (unsigned int i = 0; i < N; i++) data[i] /= num; - return *this; + return *reinterpret_cast*>(this); } /** * dot product */ - float operator *(const VectorBase &v) const { - float res; + float operator *(const Vector &v) const { + float res = 0.0f; for (unsigned int i = 0; i < N; i++) res += data[i] * v(i); return res; @@ -221,7 +241,7 @@ public: * gets the length of this vector */ float length() const { - return sqrtf(*this * *this); + return sqrtf(*this * *reinterpret_cast*>(this)); } /** @@ -234,35 +254,113 @@ public: /** * returns the normalized version of this vector */ - VectorBase normalized() const { + Vector normalized() const { return *this / length(); } + + void print(void) { + printf("[ "); + for (unsigned int i = 0; i < N; i++) + printf("%.3f\t", data[i]); + printf("]\n"); + } }; template class Vector : public VectorBase { public: + using VectorBase::operator *; + + Vector() : VectorBase() { + } + + Vector(const float d[]) : VectorBase(d) { + } + + Vector(const Vector &v) : VectorBase(v) { + } + + Vector(const VectorBase &v) : VectorBase(v) { + } + /** * set to value */ const Vector &operator =(const Vector &v) { + this->arm_col = {N, 1, &this->data[0]}; memcpy(this->data, v.data, sizeof(this->data)); return *this; } }; +template <> +class Vector<2> : public VectorBase<2> { +public: + Vector() : VectorBase<2>() { + } + + Vector(const float x, const float y) : VectorBase() { + data[0] = x; + data[1] = y; + } + + Vector(const Vector<2> &v) : VectorBase() { + data[0] = v.data[0]; + data[1] = v.data[1]; + } + + Vector(const VectorBase<2> &v) : VectorBase() { + data[0] = v.data[0]; + data[1] = v.data[1]; + } + + /** + * set to value + */ + const Vector<2> &operator =(const Vector<2> &v) { + data[0] = v.data[0]; + data[1] = v.data[1]; + return *this; + } + + float cross(const Vector<2> &b) const { + return data[0]*b.data[1] - data[1]*b.data[0]; + } + + float operator %(const Vector<2> &v) const { + return cross(v); + } + +}; + template <> class Vector<3> : public VectorBase<3> { public: - Vector<3>() { + Vector() { arm_col = {3, 1, &this->data[0]}; } - Vector<3>(const float x, const float y, const float z) { + Vector(const float x, const float y, const float z) { + arm_col = {3, 1, &this->data[0]}; data[0] = x; data[1] = y; data[2] = z; + } + + Vector(const Vector<3> &v) : VectorBase<3>() { + data[0] = v.data[0]; + data[1] = v.data[1]; + data[2] = v.data[2]; + } + + /** + * setting ctor + */ + Vector(const float d[]) { arm_col = {3, 1, &this->data[0]}; + data[0] = d[0]; + data[1] = d[1]; + data[2] = d[2]; } /** @@ -276,6 +374,56 @@ public: } }; +template <> +class Vector<4> : public VectorBase<4> { +public: + Vector() : VectorBase() {} + + Vector(const float x, const float y, const float z, const float t) : VectorBase() { + data[0] = x; + data[1] = y; + data[2] = z; + data[3] = t; + } + + Vector(const Vector<4> &v) : VectorBase() { + data[0] = v.data[0]; + data[1] = v.data[1]; + data[2] = v.data[2]; + data[3] = v.data[3]; + } + + Vector(const VectorBase<4> &v) : VectorBase() { + data[0] = v.data[0]; + data[1] = v.data[1]; + data[2] = v.data[2]; + data[3] = v.data[3]; + } + + /** + * setting ctor + */ + /* + Vector(const float d[]) { + arm_col = {3, 1, &this->data[0]}; + data[0] = d[0]; + data[1] = d[1]; + data[2] = d[2]; + } +*/ + /** + * set to value + */ + /* + const Vector<3> &operator =(const Vector<3> &v) { + data[0] = v.data[0]; + data[1] = v.data[1]; + data[2] = v.data[2]; + return *this; + } + */ +}; + } #endif // VECTOR_HPP diff --git a/src/lib/mathlib/math/Vector2.hpp b/src/lib/mathlib/math/Vector2.hpp deleted file mode 100644 index 501740614a..0000000000 --- a/src/lib/mathlib/math/Vector2.hpp +++ /dev/null @@ -1,269 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. - * Author: Will Perone - * Anton Babushkin - * - * 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 Vector2.hpp - * - * 2D Vector - */ - -#ifndef VECTOR2_HPP -#define VECTOR2_HPP - -#include - -namespace math -{ - -template -class Vector2 { -public: - T x, y; - - /** - * trivial ctor - */ - Vector2() { - } - - /** - * setting ctor - */ - Vector2(const T x0, const T y0): x(x0), y(y0) { - } - - /** - * setter - */ - void set(const T x0, const T y0) { - x = x0; - y = y0; - } - - /** - * access to elements by index - */ - T operator ()(unsigned int i) { - return *(&x + i); - } - - /** - * access to elements by index - */ - const T operator ()(unsigned int i) const { - return *(&x + i); - } - - /** - * test for equality - */ - bool operator ==(const Vector2 &v) { - return (x == v.x && y == v.y); - } - - /** - * test for inequality - */ - bool operator !=(const Vector2 &v) { - return (x != v.x || y != v.y); - } - - /** - * set to value - */ - const Vector2 &operator =(const Vector2 &v) { - x = v.x; - y = v.y; - return *this; - } - - /** - * negation - */ - const Vector2 operator -(void) const { - return Vector2(-x, -y); - } - - /** - * addition - */ - const Vector2 operator +(const Vector2 &v) const { - return Vector2(x + v.x, y + v.y); - } - - /** - * subtraction - */ - const Vector2 operator -(const Vector2 &v) const { - return Vector2(x - v.x, y - v.y); - } - - /** - * uniform scaling - */ - const Vector2 operator *(const T num) const { - Vector2 temp(*this); - return temp *= num; - } - - /** - * uniform scaling - */ - const Vector2 operator /(const T num) const { - Vector2 temp(*this); - return temp /= num; - } - - /** - * addition - */ - const Vector2 &operator +=(const Vector2 &v) { - x += v.x; - y += v.y; - return *this; - } - - /** - * subtraction - */ - const Vector2 &operator -=(const Vector2 &v) { - x -= v.x; - y -= v.y; - return *this; - } - - /** - * uniform scaling - */ - const Vector2 &operator *=(const T num) { - x *= num; - y *= num; - return *this; - } - - /** - * uniform scaling - */ - const Vector2 &operator /=(const T num) { - x /= num; - y /= num; - return *this; - } - - /** - * dot product - */ - T operator *(const Vector2 &v) const { - return x * v.x + y * v.y; - } - - /** - * cross product - */ - const float operator %(const Vector2 &v) const { - return x * v.y - y * v.x; - } - - /** - * gets the length of this vector squared - */ - float length_squared() const { - return (*this * *this); - } - - /** - * gets the length of this vector - */ - float length() const { - return (T)sqrt(*this * *this); - } - - /** - * normalizes this vector - */ - void normalize() { - *this /= length(); - } - - /** - * returns the normalized version of this vector - */ - Vector2 normalized() const { - return *this / length(); - } - - /** - * reflects this vector about n - */ - void reflect(const Vector2 &n) - { - Vector2 orig(*this); - project(n); - *this = *this * 2 - orig; - } - - /** - * projects this vector onto v - */ - void project(const Vector2 &v) { - *this = v * (*this * v) / (v * v); - } - - /** - * returns this vector projected onto v - */ - Vector2 projected(const Vector2 &v) { - return v * (*this * v) / (v * v); - } - - /** - * computes the angle between 2 arbitrary vectors - */ - static inline float angle(const Vector2 &v1, const Vector2 &v2) { - return acosf((v1 * v2) / (v1.length() * v2.length())); - } - - /** - * computes the angle between 2 arbitrary normalized vectors - */ - static inline float angle_normalized(const Vector2 &v1, const Vector2 &v2) { - return acosf(v1 * v2); - } -}; - -typedef Vector2 Vector2f; -} - -#endif // VECTOR2_HPP diff --git a/src/lib/mathlib/math/Vector3.hpp b/src/lib/mathlib/math/Vector3.hpp deleted file mode 100644 index a0c3b92903..0000000000 --- a/src/lib/mathlib/math/Vector3.hpp +++ /dev/null @@ -1,287 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. - * Author: Will Perone - * Anton Babushkin - * - * 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 Vector3.hpp - * - * 3D Vector - */ - -#ifndef VECTOR3_HPP -#define VECTOR3_HPP - -#include -#include "../CMSIS/Include/arm_math.h" - -namespace math -{ - -template -class Vector3 { -public: - T x, y, z; - arm_matrix_instance_f32 arm_col; - - /** - * trivial ctor - */ - Vector3() { - arm_col = {3, 1, &x}; - } - - /** - * setting ctor - */ - Vector3(const T x0, const T y0, const T z0): x(x0), y(y0), z(z0) { - arm_col = {3, 1, &x}; - } - - /** - * setting ctor - */ - Vector3(const T data[3]): x(data[0]), y(data[1]), z(data[2]) { - arm_col = {3, 1, &x}; - } - - /** - * setter - */ - void set(const T x0, const T y0, const T z0) { - x = x0; - y = y0; - z = z0; - } - - /** - * access to elements by index - */ - T operator ()(unsigned int i) { - return *(&x + i); - } - - /** - * access to elements by index - */ - const T operator ()(unsigned int i) const { - return *(&x + i); - } - - /** - * test for equality - */ - bool operator ==(const Vector3 &v) { - return (x == v.x && y == v.y && z == v.z); - } - - /** - * test for inequality - */ - bool operator !=(const Vector3 &v) { - return (x != v.x || y != v.y || z != v.z); - } - - /** - * set to value - */ - const Vector3 &operator =(const Vector3 &v) { - x = v.x; - y = v.y; - z = v.z; - return *this; - } - - /** - * negation - */ - const Vector3 operator -(void) const { - return Vector3(-x, -y, -z); - } - - /** - * addition - */ - const Vector3 operator +(const Vector3 &v) const { - return Vector3(x + v.x, y + v.y, z + v.z); - } - - /** - * subtraction - */ - const Vector3 operator -(const Vector3 &v) const { - return Vector3(x - v.x, y - v.y, z - v.z); - } - - /** - * uniform scaling - */ - const Vector3 operator *(const T num) const { - Vector3 temp(*this); - return temp *= num; - } - - /** - * uniform scaling - */ - const Vector3 operator /(const T num) const { - Vector3 temp(*this); - return temp /= num; - } - - /** - * addition - */ - const Vector3 &operator +=(const Vector3 &v) { - x += v.x; - y += v.y; - z += v.z; - return *this; - } - - /** - * subtraction - */ - const Vector3 &operator -=(const Vector3 &v) { - x -= v.x; - y -= v.y; - z -= v.z; - return *this; - } - - /** - * uniform scaling - */ - const Vector3 &operator *=(const T num) { - x *= num; - y *= num; - z *= num; - return *this; - } - - /** - * uniform scaling - */ - const Vector3 &operator /=(const T num) { - x /= num; - y /= num; - z /= num; - return *this; - } - - /** - * dot product - */ - T operator *(const Vector3 &v) const { - return x * v.x + y * v.y + z * v.z; - } - - /** - * cross product - */ - const Vector3 operator %(const Vector3 &v) const { - Vector3 temp(y * v.z - z * v.y, z * v.x - x * v.z, x * v.y - y * v.x); - return temp; - } - - /** - * gets the length of this vector squared - */ - float length_squared() const { - return (*this * *this); - } - - /** - * gets the length of this vector - */ - float length() const { - return (T)sqrt(*this * *this); - } - - /** - * normalizes this vector - */ - void normalize() { - *this /= length(); - } - - /** - * returns the normalized version of this vector - */ - Vector3 normalized() const { - return *this / length(); - } - - /** - * reflects this vector about n - */ - void reflect(const Vector3 &n) - { - Vector3 orig(*this); - project(n); - *this = *this * 2 - orig; - } - - /** - * projects this vector onto v - */ - void project(const Vector3 &v) { - *this = v * (*this * v) / (v * v); - } - - /** - * returns this vector projected onto v - */ - Vector3 projected(const Vector3 &v) { - return v * (*this * v) / (v * v); - } - - /** - * computes the angle between 2 arbitrary vectors - */ - static inline float angle(const Vector3 &v1, const Vector3 &v2) { - return acosf((v1 * v2) / (v1.length() * v2.length())); - } - - /** - * computes the angle between 2 arbitrary normalized vectors - */ - static inline float angle_normalized(const Vector3 &v1, const Vector3 &v2) { - return acosf(v1 * v2); - } -}; - -typedef Vector3 Vector3f; -} - -#endif // VECTOR3_HPP diff --git a/src/lib/mathlib/mathlib.h b/src/lib/mathlib/mathlib.h index d8e95b46b1..9e03855c52 100644 --- a/src/lib/mathlib/mathlib.h +++ b/src/lib/mathlib/mathlib.h @@ -42,10 +42,7 @@ #pragma once #include "math/Vector.hpp" -#include "math/Vector2.hpp" -#include "math/Vector3.hpp" #include "math/Matrix.hpp" -#include "math/Matrix3.hpp" #include "math/Quaternion.hpp" #include "math/Limits.hpp" diff --git a/src/lib/mathlib/module.mk b/src/lib/mathlib/module.mk index 50951c6174..191e2da739 100644 --- a/src/lib/mathlib/module.mk +++ b/src/lib/mathlib/module.mk @@ -35,7 +35,6 @@ # Math library # SRCS = math/test/test.cpp \ - math/Matrix3.cpp \ math/Limits.cpp # diff --git a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp b/src/modules/att_pos_estimator_ekf/KalmanNav.cpp index ecca04dd7f..6533eb7cf0 100644 --- a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp +++ b/src/modules/att_pos_estimator_ekf/KalmanNav.cpp @@ -54,17 +54,17 @@ static const int8_t ret_error = -1; // error occurred KalmanNav::KalmanNav(SuperBlock *parent, const char *name) : SuperBlock(parent, name), // ekf matrices - F(9, 9), - G(9, 6), - P(9, 9), - P0(9, 9), - V(6, 6), + F(), + G(), + P(), + P0(), + V(), // attitude measurement ekf matrices - HAtt(4, 9), - RAtt(4, 4), + HAtt(), + RAtt(), // position measurement ekf matrices - HPos(6, 9), - RPos(6, 6), + HPos(), + RPos(), // attitude representations C_nb(), q(), @@ -113,7 +113,8 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) : using namespace math; // initial state covariance matrix - P0 = Matrix::identity(9) * 0.01f; + P0.identity(); + P0 *= 0.01f; P = P0; // initial state @@ -138,7 +139,7 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) : _sensors.magnetometer_ga[2]); // initialize dcm - C_nb = Dcm(q); + C_nb.from_quaternion(q); // HPos is constant HPos(0, 3) = 1.0f; @@ -404,28 +405,28 @@ int KalmanNav::predictState(float dt) // attitude prediction if (_attitudeInitialized) { - Vector3 w(_sensors.gyro_rad_s); + Vector<3> w(_sensors.gyro_rad_s); // attitude q = q + q.derivative(w) * dt; // renormalize quaternion if needed - if (fabsf(q.norm() - 1.0f) > 1e-4f) { - q = q.unit(); + if (fabsf(q.length() - 1.0f) > 1e-4f) { + q.normalize(); } // C_nb update - C_nb = Dcm(q); + C_nb.from_quaternion(q); // euler update - EulerAngles euler(C_nb); - phi = euler.getPhi(); - theta = euler.getTheta(); - psi = euler.getPsi(); + Vector<3> euler = C_nb.to_euler(); + phi = euler.data[0]; + theta = euler.data[1]; + psi = euler.data[2]; // specific acceleration in nav frame - Vector3 accelB(_sensors.accelerometer_m_s2); - Vector3 accelN = C_nb * accelB; + Vector<3> accelB(_sensors.accelerometer_m_s2); + Vector<3> accelN = C_nb * accelB; fN = accelN(0); fE = accelN(1); fD = accelN(2); @@ -549,10 +550,10 @@ int KalmanNav::predictStateCovariance(float dt) G(5, 4) = C_nb(2, 1); G(5, 5) = C_nb(2, 2); - // continuous predictioon equations - // for discrte time EKF + // continuous prediction equations + // for discrete time EKF // http://en.wikipedia.org/wiki/Extended_Kalman_filter - P = P + (F * P + P * F.transpose() + G * V * G.transpose()) * dt; + P = P + (F * P + P * F.transposed() + G * V * G.transposed()) * dt; return ret_ok; } @@ -577,13 +578,14 @@ int KalmanNav::correctAtt() // compensate roll and pitch, but not yaw // XXX take the vectors out of the C_nb matrix to avoid singularities - math::Dcm C_rp(math::EulerAngles(phi, theta, 0.0f));//C_nb.transpose(); + math::Matrix<3,3> C_rp; + C_rp.from_euler(phi, theta, 0.0f);//C_nb.transposed(); // mag measurement - Vector3 magBody(_sensors.magnetometer_ga); + Vector<3> magBody(_sensors.magnetometer_ga); // transform to earth frame - Vector3 magNav = C_rp * magBody; + Vector<3> magNav = C_rp * magBody; // calculate error between estimate and measurement // apply declination correction for true heading as well. @@ -592,12 +594,12 @@ int KalmanNav::correctAtt() if (yMag < -M_PI_F) yMag += 2*M_PI_F; // accel measurement - Vector3 zAccel(_sensors.accelerometer_m_s2); - float accelMag = zAccel.norm(); - zAccel = zAccel.unit(); + Vector<3> zAccel(_sensors.accelerometer_m_s2); + float accelMag = zAccel.length(); + zAccel.normalize(); // ignore accel correction when accel mag not close to g - Matrix RAttAdjust = RAtt; + Matrix<4,4> RAttAdjust = RAtt; bool ignoreAccel = fabsf(accelMag - _g.get()) > 1.1f; @@ -611,14 +613,10 @@ int KalmanNav::correctAtt() } // accel predicted measurement - Vector3 zAccelHat = (C_nb.transpose() * Vector3(0, 0, -_g.get())).unit(); + Vector<3> zAccelHat = (C_nb.transposed() * Vector<3>(0, 0, -_g.get())).normalized(); // calculate residual - Vector y(4); - y(0) = yMag; - y(1) = zAccel(0) - zAccelHat(0); - y(2) = zAccel(1) - zAccelHat(1); - y(3) = zAccel(2) - zAccelHat(2); + Vector<4> y(yMag, zAccel(0) - zAccelHat(0), zAccel(1) - zAccelHat(1), zAccel(2) - zAccelHat(2)); // HMag HAtt(0, 2) = 1; @@ -632,9 +630,9 @@ int KalmanNav::correctAtt() // compute correction // http://en.wikipedia.org/wiki/Extended_Kalman_filter - Matrix S = HAtt * P * HAtt.transpose() + RAttAdjust; // residual covariance - Matrix K = P * HAtt.transpose() * S.inverse(); - Vector xCorrect = K * y; + Matrix<4, 4> S = HAtt * P * HAtt.transposed() + RAttAdjust; // residual covariance + Matrix<9, 4> K = P * HAtt.transposed() * S.inversed(); + Vector<9> xCorrect = K * y; // check correciton is sane for (size_t i = 0; i < xCorrect.getRows(); i++) { @@ -669,7 +667,7 @@ int KalmanNav::correctAtt() P = P - K * HAtt * P; // fault detection - float beta = y.dot(S.inverse() * y); + float beta = y * (S.inversed() * y); if (beta > _faultAtt.get()) { warnx("fault in attitude: beta = %8.4f", (double)beta); @@ -678,7 +676,7 @@ int KalmanNav::correctAtt() // update quaternions from euler // angle correction - q = Quaternion(EulerAngles(phi, theta, psi)); + q.from_euler(phi, theta, psi); return ret_ok; } @@ -688,7 +686,7 @@ int KalmanNav::correctPos() using namespace math; // residual - Vector y(6); + Vector<6> y; y(0) = _gps.vel_n_m_s - vN; y(1) = _gps.vel_e_m_s - vE; y(2) = double(_gps.lat) - double(lat) * 1.0e7 * M_RAD_TO_DEG; @@ -698,9 +696,9 @@ int KalmanNav::correctPos() // compute correction // http://en.wikipedia.org/wiki/Extended_Kalman_filter - Matrix S = HPos * P * HPos.transpose() + RPos; // residual covariance - Matrix K = P * HPos.transpose() * S.inverse(); - Vector xCorrect = K * y; + Matrix<6,6> S = HPos * P * HPos.transposed() + RPos; // residual covariance + Matrix<9,6> K = P * HPos.transposed() * S.inversed(); + Vector<9> xCorrect = K * y; // check correction is sane for (size_t i = 0; i < xCorrect.getRows(); i++) { @@ -735,7 +733,7 @@ int KalmanNav::correctPos() P = P - K * HPos * P; // fault detetcion - float beta = y.dot(S.inverse() * y); + float beta = y * (S.inversed() * y); static int counter = 0; if (beta > _faultPos.get() && (counter % 10 == 0)) { diff --git a/src/modules/att_pos_estimator_ekf/KalmanNav.hpp b/src/modules/att_pos_estimator_ekf/KalmanNav.hpp index a69bde1a65..46ee4b6c8e 100644 --- a/src/modules/att_pos_estimator_ekf/KalmanNav.hpp +++ b/src/modules/att_pos_estimator_ekf/KalmanNav.hpp @@ -125,17 +125,17 @@ public: virtual void updateParams(); protected: // kalman filter - math::Matrix F; /**< Jacobian(f,x), where dx/dt = f(x,u) */ - math::Matrix G; /**< noise shaping matrix for gyro/accel */ - math::Matrix P; /**< state covariance matrix */ - math::Matrix P0; /**< initial state covariance matrix */ - math::Matrix V; /**< gyro/ accel noise matrix */ - math::Matrix HAtt; /**< attitude measurement matrix */ - math::Matrix RAtt; /**< attitude measurement noise matrix */ - math::Matrix HPos; /**< position measurement jacobian matrix */ - math::Matrix RPos; /**< position measurement noise matrix */ + math::Matrix<9,9> F; /**< Jacobian(f,x), where dx/dt = f(x,u) */ + math::Matrix<9,6> G; /**< noise shaping matrix for gyro/accel */ + math::Matrix<9,9> P; /**< state covariance matrix */ + math::Matrix<9,9> P0; /**< initial state covariance matrix */ + math::Matrix<6,6> V; /**< gyro/ accel noise matrix */ + math::Matrix<4,9> HAtt; /**< attitude measurement matrix */ + math::Matrix<4,4> RAtt; /**< attitude measurement noise matrix */ + math::Matrix<6,9> HPos; /**< position measurement jacobian matrix */ + math::Matrix<6,6> RPos; /**< position measurement noise matrix */ // attitude - math::Dcm C_nb; /**< direction cosine matrix from body to nav frame */ + math::Matrix<3,3> C_nb; /**< direction cosine matrix from body to nav frame */ math::Quaternion q; /**< quaternion from body to nav frame */ // subscriptions control::UOrbSubscription _sensors; /**< sensors sub. */ diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index 5eeca5a1a3..36b75dd58a 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -194,15 +194,13 @@ int do_accel_calibration(int mavlink_fd) int32_t board_rotation_int; param_get(board_rotation_h, &(board_rotation_int)); enum Rotation board_rotation_id = (enum Rotation)board_rotation_int; - math::Matrix board_rotation(3, 3); + math::Matrix<3,3> board_rotation; get_rot_matrix(board_rotation_id, &board_rotation); - math::Matrix board_rotation_t = board_rotation.transpose(); - math::Vector3 accel_offs_vec; - accel_offs_vec.set(&accel_offs[0]); - math::Vector3 accel_offs_rotated = board_rotation_t * accel_offs_vec; - math::Matrix accel_T_mat(3, 3); - accel_T_mat.set(&accel_T[0][0]); - math::Matrix accel_T_rotated = board_rotation_t * accel_T_mat * board_rotation; + math::Matrix<3,3> board_rotation_t = board_rotation.transposed(); + math::Vector<3> accel_offs_vec(&accel_offs[0]); + math::Vector<3> accel_offs_rotated = board_rotation_t * accel_offs_vec; + math::Matrix<3,3> accel_T_mat(&accel_T[0][0]); + math::Matrix<3,3> accel_T_rotated = board_rotation_t * accel_T_mat * board_rotation; accel_scale.x_offset = accel_offs_rotated(0); accel_scale.x_scale = accel_T_rotated(0, 0); diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 7b6fad6589..653d4b6b38 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -683,8 +683,9 @@ handle_message(mavlink_message_t *msg) /* Calculate Rotation Matrix */ math::Quaternion q(hil_state.attitude_quaternion); - math::Dcm C_nb(q); - math::EulerAngles euler(C_nb); + math::Matrix<3,3> C_nb; + C_nb.from_quaternion(q); + math::Vector<3> euler = C_nb.to_euler(); /* set rotation matrix */ for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++) @@ -699,9 +700,9 @@ handle_message(mavlink_message_t *msg) hil_attitude.q[3] = q(3); hil_attitude.q_valid = true; - hil_attitude.roll = euler.getPhi(); - hil_attitude.pitch = euler.getTheta(); - hil_attitude.yaw = euler.getPsi(); + hil_attitude.roll = euler(0); + hil_attitude.pitch = euler(1); + hil_attitude.yaw = euler(2); hil_attitude.rollspeed = hil_state.rollspeed; hil_attitude.pitchspeed = hil_state.pitchspeed; hil_attitude.yawspeed = hil_state.yawspeed; diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index f6c44444aa..04307d96b5 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -422,11 +422,11 @@ Navigator::task_main() mission_poll(); - math::Vector2f ground_speed(_global_pos.vx, _global_pos.vy); + math::Vector<2> ground_speed(_global_pos.vx, _global_pos.vy); // Current waypoint - math::Vector2f next_wp(_global_triplet.current.lat / 1e7f, _global_triplet.current.lon / 1e7f); + math::Vector<2> next_wp(_global_triplet.current.lat / 1e7f, _global_triplet.current.lon / 1e7f); // Global position - math::Vector2f current_position(_global_pos.lat / 1e7f, _global_pos.lon / 1e7f); + math::Vector<2> current_position(_global_pos.lat / 1e7f, _global_pos.lon / 1e7f); /* AUTONOMOUS FLIGHT */ @@ -436,19 +436,19 @@ Navigator::task_main() if (_mission_valid) { // Next waypoint - math::Vector2f prev_wp; + math::Vector<2> prev_wp; if (_global_triplet.previous_valid) { - prev_wp.setX(_global_triplet.previous.lat / 1e7f); - prev_wp.setY(_global_triplet.previous.lon / 1e7f); + prev_wp(0) = _global_triplet.previous.lat / 1e7f; + prev_wp(1) = _global_triplet.previous.lon / 1e7f; } else { /* * No valid next waypoint, go for heading hold. * This is automatically handled by the L1 library. */ - prev_wp.setX(_global_triplet.current.lat / 1e7f); - prev_wp.setY(_global_triplet.current.lon / 1e7f); + prev_wp(0) = _global_triplet.current.lat / 1e7f; + prev_wp(1) = _global_triplet.current.lon / 1e7f; } diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 6d38b98ec1..5baab4a5da 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -211,8 +211,8 @@ private: struct differential_pressure_s _diff_pres; struct airspeed_s _airspeed; - math::Matrix _board_rotation; /**< rotation matrix for the orientation that the board is mounted */ - math::Matrix _external_mag_rotation; /**< rotation matrix for the orientation that an external mag is mounted */ + math::Matrix<3,3> _board_rotation; /**< rotation matrix for the orientation that the board is mounted */ + math::Matrix<3,3> _external_mag_rotation; /**< rotation matrix for the orientation that an external mag is mounted */ bool _mag_is_external; /**< true if the active mag is on an external board */ struct { @@ -457,8 +457,8 @@ Sensors::Sensors() : /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "sensor task update")), - _board_rotation(3, 3), - _external_mag_rotation(3, 3), + _board_rotation(), + _external_mag_rotation(), _mag_is_external(false) { @@ -904,7 +904,7 @@ Sensors::accel_poll(struct sensor_combined_s &raw) orb_copy(ORB_ID(sensor_accel), _accel_sub, &accel_report); - math::Vector3 vect = {accel_report.x, accel_report.y, accel_report.z}; + math::Vector<3> vect = {accel_report.x, accel_report.y, accel_report.z}; vect = _board_rotation * vect; raw.accelerometer_m_s2[0] = vect(0); @@ -930,7 +930,7 @@ Sensors::gyro_poll(struct sensor_combined_s &raw) orb_copy(ORB_ID(sensor_gyro), _gyro_sub, &gyro_report); - math::Vector3 vect = {gyro_report.x, gyro_report.y, gyro_report.z}; + math::Vector<3> vect = {gyro_report.x, gyro_report.y, gyro_report.z}; vect = _board_rotation * vect; raw.gyro_rad_s[0] = vect(0); @@ -956,7 +956,7 @@ Sensors::mag_poll(struct sensor_combined_s &raw) orb_copy(ORB_ID(sensor_mag), _mag_sub, &mag_report); - math::Vector3 vect = {mag_report.x, mag_report.y, mag_report.z}; + math::Vector<3> vect(mag_report.x, mag_report.y, mag_report.z); if (_mag_is_external) vect = _external_mag_rotation * vect; diff --git a/src/systemcmds/tests/test_mathlib.cpp b/src/systemcmds/tests/test_mathlib.cpp index 305ebbefa9..d2e1a93e3b 100644 --- a/src/systemcmds/tests/test_mathlib.cpp +++ b/src/systemcmds/tests/test_mathlib.cpp @@ -75,15 +75,6 @@ int test_mathlib(int argc, char *argv[]) Matrix<3,3> mres3; Matrix<4,4> mres4; - Matrix3f m3old; - m3old.identity(); - Vector3f v3old; - v3old.x = 1.0f; - v3old.y = 2.0f; - v3old.z = 3.0f; - Vector3f vres3old; - Matrix3f mres3old; - unsigned int n = 60000; hrt_abstime t0, t1; @@ -95,13 +86,6 @@ int test_mathlib(int argc, char *argv[]) t1 = hrt_absolute_time(); warnx("Matrix3 * Vector3: %s %.6fus", formatResult(vres3 == v3), (double)(t1 - t0) / n); - t0 = hrt_absolute_time(); - for (unsigned int j = 0; j < n; j++) { - vres3old = m3old * v3old; - } - t1 = hrt_absolute_time(); - warnx("Matrix3 * Vector3 OLD: %s %.6fus", formatResult(vres3old == v3old), (double)(t1 - t0) / n); - t0 = hrt_absolute_time(); for (unsigned int j = 0; j < n; j++) { mres3 = m3 * m3; @@ -109,13 +93,6 @@ int test_mathlib(int argc, char *argv[]) t1 = hrt_absolute_time(); warnx("Matrix3 * Matrix3: %s %.6fus", formatResult(mres3 == m3), (double)(t1 - t0) / n); - t0 = hrt_absolute_time(); - for (unsigned int j = 0; j < n; j++) { - mres3old = m3old * m3old; - } - t1 = hrt_absolute_time(); - warnx("Matrix3 * Matrix3 OLD: %s %.6fus", formatResult(mres3old == m3old), (double)(t1 - t0) / n); - t0 = hrt_absolute_time(); for (unsigned int j = 0; j < n; j++) { mres4 = m4 * m4; From 2df2fd1d252fe1e53d86416070557dec8c996891 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 19 Dec 2013 16:58:25 +0400 Subject: [PATCH 208/447] mathlib minor fixes --- src/lib/mathlib/math/Matrix.hpp | 8 ++--- src/lib/mathlib/math/Quaternion.hpp | 36 ------------------- src/lib/mathlib/math/Vector.hpp | 14 ++++---- .../att_pos_estimator_ekf/KalmanNav.cpp | 15 -------- 4 files changed, 11 insertions(+), 62 deletions(-) diff --git a/src/lib/mathlib/math/Matrix.hpp b/src/lib/mathlib/math/Matrix.hpp index 67214133ac..47929ffcb9 100644 --- a/src/lib/mathlib/math/Matrix.hpp +++ b/src/lib/mathlib/math/Matrix.hpp @@ -129,7 +129,7 @@ public: */ const Matrix &operator =(const Matrix &m) { memcpy(data, m.data, sizeof(data)); - return *reinterpret_cast*>(this); + return *static_cast*>(this); } /** @@ -158,7 +158,7 @@ public: for (unsigned int i = 0; i < N; i++) for (unsigned int j = 0; j < M; j++) data[i][j] += m.data[i][j]; - return *reinterpret_cast*>(this); + return *static_cast*>(this); } /** @@ -176,7 +176,7 @@ public: for (unsigned int i = 0; i < N; i++) for (unsigned int j = 0; j < M; j++) data[i][j] -= m.data[i][j]; - return *reinterpret_cast*>(this); + return *static_cast*>(this); } /** @@ -194,7 +194,7 @@ public: for (unsigned int i = 0; i < M; i++) for (unsigned int j = 0; j < N; j++) data[i][j] *= num; - return *reinterpret_cast*>(this); + return *static_cast*>(this); } Matrix operator /(const float num) const { diff --git a/src/lib/mathlib/math/Quaternion.hpp b/src/lib/mathlib/math/Quaternion.hpp index 1b0cb3c411..3735fb3d33 100644 --- a/src/lib/mathlib/math/Quaternion.hpp +++ b/src/lib/mathlib/math/Quaternion.hpp @@ -73,42 +73,6 @@ public: Quaternion(const float *v) : Vector(v) { } - /** - * access to elements by index - */ - /* - inline float &operator ()(unsigned int i) { - return *(&a + i); - } - */ - - /** - * access to elements by index - */ - /* - inline const float &operator ()(unsigned int i) const { - return *(&a + i); - } - */ - - /** - * addition - */ - /* - const Quaternion operator +(const Quaternion &q) const { - return Quaternion(a + q.a, b + q.b, c + q.c, d + q.d); - } - */ - - /** - * subtraction - */ - /* - const Quaternion operator -(const Quaternion &q) const { - return Quaternion(a - q.a, b - q.b, c - q.c, d - q.d); - } - */ - Quaternion derivative(const Vector<3> &w) { float dataQ[] = { data[0], -data[1], -data[2], -data[3], diff --git a/src/lib/mathlib/math/Vector.hpp b/src/lib/mathlib/math/Vector.hpp index 2df87c74bf..8d754a3214 100644 --- a/src/lib/mathlib/math/Vector.hpp +++ b/src/lib/mathlib/math/Vector.hpp @@ -135,7 +135,7 @@ public: */ const Vector &operator =(const Vector &v) { memcpy(data, v.data, sizeof(data)); - return *reinterpret_cast*>(this); + return *static_cast*>(this); } /** @@ -180,7 +180,7 @@ public: * uniform scaling */ const Vector operator /(const float num) const { - Vector temp(*reinterpret_cast*>(this)); + Vector temp(*static_cast*>(this)); return temp /= num; } @@ -190,7 +190,7 @@ public: const Vector &operator +=(const Vector &v) { for (unsigned int i = 0; i < N; i++) data[i] += v(i); - return *reinterpret_cast*>(this); + return *static_cast*>(this); } /** @@ -199,7 +199,7 @@ public: const Vector &operator -=(const Vector &v) { for (unsigned int i = 0; i < N; i++) data[i] -= v(i); - return *reinterpret_cast*>(this); + return *static_cast*>(this); } /** @@ -208,7 +208,7 @@ public: const Vector &operator *=(const float num) { for (unsigned int i = 0; i < N; i++) data[i] *= num; - return *reinterpret_cast*>(this); + return *static_cast*>(this); } /** @@ -217,7 +217,7 @@ public: const Vector &operator /=(const float num) { for (unsigned int i = 0; i < N; i++) data[i] /= num; - return *reinterpret_cast*>(this); + return *static_cast*>(this); } /** @@ -241,7 +241,7 @@ public: * gets the length of this vector */ float length() const { - return sqrtf(*this * *reinterpret_cast*>(this)); + return sqrtf(*this * *static_cast*>(this)); } /** diff --git a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp b/src/modules/att_pos_estimator_ekf/KalmanNav.cpp index 6533eb7cf0..4ab41c6cee 100644 --- a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp +++ b/src/modules/att_pos_estimator_ekf/KalmanNav.cpp @@ -53,21 +53,6 @@ static const int8_t ret_error = -1; // error occurred KalmanNav::KalmanNav(SuperBlock *parent, const char *name) : SuperBlock(parent, name), - // ekf matrices - F(), - G(), - P(), - P0(), - V(), - // attitude measurement ekf matrices - HAtt(), - RAtt(), - // position measurement ekf matrices - HPos(), - RPos(), - // attitude representations - C_nb(), - q(), // subscriptions _sensors(&getSubscriptions(), ORB_ID(sensor_combined), 5), // limit to 200 Hz _gps(&getSubscriptions(), ORB_ID(vehicle_gps_position), 100), // limit to 10 Hz From a53af7e7b3ea34aa9023473a73489c836ac0e84a Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 19 Dec 2013 17:57:37 +0400 Subject: [PATCH 209/447] fw_pos_control_l1: use new mathlib --- .../fw_pos_control_l1_main.cpp | 36 +++++++++---------- 1 file changed, 18 insertions(+), 18 deletions(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index a9648b207d..5e32ac32f1 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -170,7 +170,7 @@ private: uint64_t _airspeed_last_valid; ///< last time airspeed was valid. Used to detect sensor failures float _groundspeed_undershoot; ///< ground speed error to min. speed in m/s bool _global_pos_valid; ///< global position is valid - math::Dcm _R_nb; ///< current attitude + math::Matrix<3, 3> _R_nb; ///< current attitude ECL_L1_Pos_Controller _l1_control; TECS _tecs; @@ -282,7 +282,7 @@ private: /** * Control position. */ - bool control_position(const math::Vector2f &global_pos, const math::Vector2f &ground_speed, + bool control_position(const math::Vector<2> &global_pos, const math::Vector<2> &ground_speed, const struct vehicle_global_position_set_triplet_s &global_triplet); float calculate_target_airspeed(float airspeed_demand); @@ -600,10 +600,10 @@ FixedwingPositionControl::calculate_gndspeed_undershoot() if (_global_pos_valid) { /* get ground speed vector */ - math::Vector2f ground_speed_vector(_global_pos.vx, _global_pos.vy); + math::Vector<2> ground_speed_vector(_global_pos.vx, _global_pos.vy); /* rotate with current attitude */ - math::Vector2f yaw_vector(_R_nb(0, 0), _R_nb(1, 0)); + math::Vector<2> yaw_vector(_R_nb(0, 0), _R_nb(1, 0)); yaw_vector.normalize(); float ground_speed_body = yaw_vector * ground_speed_vector; @@ -624,7 +624,7 @@ FixedwingPositionControl::calculate_gndspeed_undershoot() } bool -FixedwingPositionControl::control_position(const math::Vector2f ¤t_position, const math::Vector2f &ground_speed, +FixedwingPositionControl::control_position(const math::Vector<2> ¤t_position, const math::Vector<2> &ground_speed, const struct vehicle_global_position_set_triplet_s &global_triplet) { bool setpoint = true; @@ -637,8 +637,8 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio float baro_altitude = _global_pos.alt; /* filter speed and altitude for controller */ - math::Vector3 accel_body(_accel.x, _accel.y, _accel.z); - math::Vector3 accel_earth = _R_nb.transpose() * accel_body; + math::Vector<3> accel_body(_accel.x, _accel.y, _accel.z); + math::Vector<3> accel_earth = _R_nb.transposed() * accel_body; _tecs.update_50hz(baro_altitude, _airspeed.indicated_airspeed_m_s, _R_nb, accel_body, accel_earth); float altitude_error = _global_triplet.current.altitude - _global_pos.alt; @@ -662,29 +662,29 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio if (_setpoint_valid) { /* current waypoint (the one currently heading for) */ - math::Vector2f next_wp(global_triplet.current.lat / 1e7f, global_triplet.current.lon / 1e7f); + math::Vector<2> next_wp(global_triplet.current.lat / 1e7f, global_triplet.current.lon / 1e7f); /* previous waypoint */ - math::Vector2f prev_wp; + math::Vector<2> prev_wp; if (global_triplet.previous_valid) { - prev_wp.setX(global_triplet.previous.lat / 1e7f); - prev_wp.setY(global_triplet.previous.lon / 1e7f); + prev_wp(0) = global_triplet.previous.lat / 1e7f; + prev_wp(1) = global_triplet.previous.lon / 1e7f; } else { /* * No valid previous waypoint, go for the current wp. * This is automatically handled by the L1 library. */ - prev_wp.setX(global_triplet.current.lat / 1e7f); - prev_wp.setY(global_triplet.current.lon / 1e7f); + prev_wp(0) = global_triplet.current.lat / 1e7f; + prev_wp(1) = global_triplet.current.lon / 1e7f; } // XXX add RTL switch if (global_triplet.current.nav_cmd == NAV_CMD_RETURN_TO_LAUNCH && _launch_valid) { - math::Vector2f rtl_pos(_launch_lat, _launch_lon); + math::Vector<2> rtl_pos(_launch_lat, _launch_lon); _l1_control.navigate_waypoints(rtl_pos, rtl_pos, current_position, ground_speed); _att_sp.roll_body = _l1_control.nav_roll(); @@ -730,7 +730,7 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio /* switch to heading hold for the last meters, continue heading hold after */ - float wp_distance = get_distance_to_next_waypoint(prev_wp.getX(), prev_wp.getY(), current_position.getX(), current_position.getY()); + float wp_distance = get_distance_to_next_waypoint(prev_wp(0), prev_wp(1), current_position(0), current_position(1)); //warnx("wp dist: %d, alt err: %d, noret: %s", (int)wp_distance, (int)altitude_error, (land_noreturn) ? "YES" : "NO"); if (wp_distance < 15.0f || land_noreturn) { @@ -869,7 +869,7 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio altitude_error = _loiter_hold_alt - _global_pos.alt; - math::Vector2f loiter_hold_pos(_loiter_hold_lat, _loiter_hold_lon); + math::Vector<2> loiter_hold_pos(_loiter_hold_lat, _loiter_hold_lon); /* loiter around current position */ _l1_control.navigate_loiter(loiter_hold_pos, current_position, _parameters.loiter_hold_radius, @@ -1101,8 +1101,8 @@ FixedwingPositionControl::task_main() vehicle_airspeed_poll(); // vehicle_baro_poll(); - math::Vector2f ground_speed(_global_pos.vx, _global_pos.vy); - math::Vector2f current_position(_global_pos.lat / 1e7f, _global_pos.lon / 1e7f); + math::Vector<2> ground_speed(_global_pos.vx, _global_pos.vy); + math::Vector<2> current_position(_global_pos.lat / 1e7f, _global_pos.lon / 1e7f); /* * Attempt to control position, on success (= sensors present and not in manual mode), From 53192b5f4d11237c353faed72b326e67004fcef7 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 20 Dec 2013 22:20:07 +0400 Subject: [PATCH 210/447] multirotor_pos_control: seatbelt mode fix --- .../multirotor_pos_control.c | 15 +++++++++++---- 1 file changed, 11 insertions(+), 4 deletions(-) diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c index cf2d5f931f..7e36872f99 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c @@ -70,6 +70,7 @@ #include "multirotor_pos_control_params.h" #include "thrust_pid.h" +#define TILT_COS_MAX 0.7f static bool thread_should_exit = false; /**< Deamon exit flag */ static bool thread_running = false; /**< Deamon status flag */ @@ -687,8 +688,14 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) bool saturation_z = false; /* limit min lift */ - if (-thrust_sp[2] < params.thr_min) { - thrust_sp[2] = -params.thr_min; + float thr_min = params.thr_min; + if (!control_mode.flag_control_velocity_enabled && thr_min < 0.0f) { + /* don't allow downside thrust direction in manual attitude mode */ + thr_min = 0.0f; + } + + if (-thrust_sp[2] < thr_min) { + thrust_sp[2] = -thr_min; saturation_z = true; } @@ -825,10 +832,10 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) /* thrust compensation for altitude only control mode */ float att_comp; - if (att.R[2][2] > 0.8f) + if (att.R[2][2] > TILT_COS_MAX) att_comp = 1.0f / att.R[2][2]; else if (att.R[2][2] > 0.0f) - att_comp = ((1.0f / 0.8f - 1.0f) / 0.8f) * att.R[2][2] + 1.0f; + att_comp = ((1.0f / TILT_COS_MAX - 1.0f) / TILT_COS_MAX) * att.R[2][2] + 1.0f; else att_comp = 1.0f; From 05e9a30573f50dd271f10864f6e7ec37b6c94043 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 21 Dec 2013 16:21:16 +0400 Subject: [PATCH 211/447] multirotor_pos_control & mc_att_control_vector: singularities handling improved --- src/lib/mathlib/math/Vector3.hpp | 1 + .../mc_att_control_vector_main.cpp | 54 ++++++++++++------- .../multirotor_pos_control.c | 52 ++++++++---------- 3 files changed, 56 insertions(+), 51 deletions(-) diff --git a/src/lib/mathlib/math/Vector3.hpp b/src/lib/mathlib/math/Vector3.hpp index 2bf00f26be..1656e184ec 100644 --- a/src/lib/mathlib/math/Vector3.hpp +++ b/src/lib/mathlib/math/Vector3.hpp @@ -56,6 +56,7 @@ public: Vector3 cross(const Vector3 &b) const; Vector3 operator %(const Vector3 &v) const; float operator *(const Vector3 &v) const; + using Vector::operator *; /** * accessors diff --git a/src/modules/mc_att_control_vector/mc_att_control_vector_main.cpp b/src/modules/mc_att_control_vector/mc_att_control_vector_main.cpp index 230fa15e40..fb09078faf 100644 --- a/src/modules/mc_att_control_vector/mc_att_control_vector_main.cpp +++ b/src/modules/mc_att_control_vector/mc_att_control_vector_main.cpp @@ -564,33 +564,39 @@ MulticopterAttitudeControl::task_main() math::Vector3 rates(_att.rollspeed, _att.pitchspeed, _att.yawspeed); /* try to move thrust vector shortest way, because yaw response is slower than roll/pitch */ - /* sin(angle error) between current and desired thrust vectors (Z axes) */ math::Vector3 R_z(R(0, 2), R(1, 2), R(2, 2)); - math::Vector3 R_z_des(R_des(0, 2), R_des(1, 2), R_des(2, 2)); - math::Vector3 e_R = R.transpose() * R_z.cross(R_z_des); + math::Vector3 R_des_z(R_des(0, 2), R_des(1, 2), R_des(2, 2)); + + /* axis and sin(angle) of desired rotation */ + math::Vector3 e_R = R.transpose() * R_z.cross(R_des_z); /* calculate angle error */ - float e_R_sin = e_R.norm(); - float e_R_cos = R_z * R_z_des; - float angle = atan2f(e_R_sin, e_R_cos); + float e_R_z_sin = e_R.norm(); + float e_R_z_cos = R_z * R_des_z; + float e_R_z_angle = atan2f(e_R_z_sin, e_R_z_cos); + + /* calculate weight for yaw control */ + float cos_z = cosf(R_z(2)); + float yaw_w = cos_z * cos_z; - /* e_R has zero Z component, set it according to yaw */ /* calculate rotation matrix after roll/pitch only rotation */ math::Matrix R_rp(3, 3); - if (e_R_sin > 0.0f) { + if (e_R_z_sin > 0.0f) { /* get axis-angle representation */ - math::Vector3 e_R_axis = e_R / e_R_sin; + math::Vector3 e_R_z_axis = e_R / e_R_z_sin; + + e_R = e_R_z_axis * e_R_z_angle; /* cross product matrix for e_R_axis */ math::Matrix e_R_cp(3, 3); e_R_cp.setAll(0.0f); - e_R_cp(0, 1) = -e_R_axis(2); - e_R_cp(0, 2) = e_R_axis(1); - e_R_cp(1, 0) = e_R_axis(2); - e_R_cp(1, 2) = -e_R_axis(0); - e_R_cp(2, 0) = -e_R_axis(1); - e_R_cp(2, 1) = e_R_axis(0); + e_R_cp(0, 1) = -e_R_z_axis(2); + e_R_cp(0, 2) = e_R_z_axis(1); + e_R_cp(1, 0) = e_R_z_axis(2); + e_R_cp(1, 2) = -e_R_z_axis(0); + e_R_cp(2, 0) = -e_R_z_axis(1); + e_R_cp(2, 1) = e_R_z_axis(0); /* rotation matrix for roll/pitch only rotation */ math::Matrix I(3, 3); @@ -598,7 +604,7 @@ MulticopterAttitudeControl::task_main() I(0, 0) = 1.0f; I(1, 1) = 1.0f; I(2, 2) = 1.0f; - R_rp = R * (I + e_R_cp * e_R_sin + e_R_cp * e_R_cp * (1.0f - e_R_cos)); + R_rp = R * (I + e_R_cp * e_R_z_sin + e_R_cp * e_R_cp * (1.0f - e_R_z_cos)); } else { /* zero roll/pitch rotation */ @@ -606,13 +612,21 @@ MulticopterAttitudeControl::task_main() } /* R_rp and R_des has the same Z axis, calculate yaw error */ - math::Vector3 R_des_z(R_des(0, 2), R_des(1, 2), R_des(2, 2)); math::Vector3 R_des_x(R_des(0, 0), R_des(1, 0), R_des(2, 0)); math::Vector3 R_rp_x(R_rp(0, 0), R_rp(1, 0), R_rp(2, 0)); - e_R(2) = R_rp_x.cross(R_des_x) * R_des_z; + e_R(2) = atan2f(R_rp_x.cross(R_des_x) * R_des_z, R_rp_x * R_des_x) * yaw_w; - /* don't try to control yaw when thrust vector has opposite direction to desired */ - e_R(2) *= (e_R_cos > 0.0f ? 1.0f : e_R_sin); + if (e_R_z_cos < 0.0) { + /* for large thrust vector rotations use another rotation method: + * calculate angle and axis for R -> R_des rotation directly */ + math::Quaternion q(R.transpose() * R_des); + float angle_d = 2.0f * atan2f(sqrtf(q.getB() * q.getB() + q.getC() * q.getC() + q.getD() * q.getD()), q.getA()); + math::Vector3 e_R_d(q.getB(), q.getC(), q.getD()); + + /* use fusion of Z axis based rotation and direct rotation */ + float direct_w = e_R_z_cos * e_R_z_cos * yaw_w; + e_R = e_R * (1.0f - direct_w) + e_R_d * direct_w; + } /* angular rates setpoint*/ math::Vector3 rates_sp = _K * e_R; diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c index 7e36872f99..54f9e52b8c 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c @@ -71,6 +71,7 @@ #include "thrust_pid.h" #define TILT_COS_MAX 0.7f +#define SIGMA 0.000001f static bool thread_should_exit = false; /**< Deamon exit flag */ static bool thread_running = false; /**< Deamon status flag */ @@ -439,7 +440,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) if (pos_pitch_sp_ctl != 0.0f || pos_roll_sp_ctl != 0.0f) { /* calculate direction and increment of control in NED frame */ - float xy_sp_ctl_dir = att.yaw + atan2f(pos_roll_sp_ctl, pos_pitch_sp_ctl); + float xy_sp_ctl_dir = att_sp.yaw_body + atan2f(pos_roll_sp_ctl, pos_pitch_sp_ctl); float xy_sp_ctl_speed = norm(pos_pitch_sp_ctl, pos_roll_sp_ctl) * params.xy_vel_max; sp_move_rate[0] = cosf(xy_sp_ctl_dir) * xy_sp_ctl_speed; sp_move_rate[1] = sinf(xy_sp_ctl_dir) * xy_sp_ctl_speed; @@ -771,7 +772,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) } else { body_z[0] = 0.0f; body_z[1] = 0.0f; - body_z[2] = -1.0f; + body_z[2] = 1.0f; } /* vector of desired yaw direction in XY plane, rotated by PI/2 */ @@ -780,39 +781,28 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) y_C[1] = cosf(att_sp.yaw_body); y_C[2] = 0; - /* desired body_x axis = cross(y_C, body_z) */ - cross3(y_C, body_z, body_x); - float body_x_norm = normalize3(body_x); - static const float body_x_norm_max = 0.5f; + if (fabsf(body_z[2]) < SIGMA) { + /* desired thrust is in XY plane, set X downside to construct correct matrix, + * but yaw component will not be used actually */ + body_x[0] = -body_x[0]; + body_x[1] = -body_x[1]; + body_x[2] = -body_x[2]; + } else { + /* desired body_x axis = cross(y_C, body_z) */ + cross3(y_C, body_z, body_x); + + /* keep nose to front while inverted upside down */ + if (body_z[2] < 0.0f) { + body_x[0] = -body_x[0]; + body_x[1] = -body_x[1]; + body_x[2] = -body_x[2]; + } + normalize3(body_x); + } /* desired body_y axis = cross(body_z, body_x) */ cross3(body_z, body_x, body_y); - if (body_x_norm < body_x_norm_max) { - /* roll is close to +/- PI/2, don't try to hold yaw exactly */ - float x_C[3]; - x_C[0] = cos(att_sp.yaw_body); - x_C[1] = sinf(att_sp.yaw_body); - x_C[2] = 0; - - float body_y_1[3]; - /* desired body_y axis for approximate yaw = cross(body_z, x_C) */ - cross3(body_z, x_C, body_y_1); - - float w = body_x_norm / body_x_norm_max; - float w1 = 1.0f - w; - - /* mix two body_y axes */ - body_y[0] = body_y[0] * w + body_y_1[0] * w1; - body_y[1] = body_y[1] * w + body_y_1[1] * w1; - body_y[2] = body_y[2] * w + body_y_1[2] * w1; - - normalize3(body_y); - - /* desired body_x axis = cross(body_y, body_z) */ - cross3(body_y, body_z, body_x); - } - /* fill rotation matrix */ for (int i = 0; i < 3; i++) { att_sp.R_body[i][0] = body_x[i]; From 38e5d2b0fbff18f53bb86dada8def5302c36a3d7 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 21 Dec 2013 18:52:21 +0400 Subject: [PATCH 212/447] multirotor_pos_control: tunable velocity feed forward for SEATBELT/EASY modes --- src/modules/multirotor_pos_control/multirotor_pos_control.c | 6 +++--- .../multirotor_pos_control/multirotor_pos_control_params.c | 6 ++++++ .../multirotor_pos_control/multirotor_pos_control_params.h | 4 ++++ 3 files changed, 13 insertions(+), 3 deletions(-) diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c index 54f9e52b8c..fe9a292b04 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c @@ -612,7 +612,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) /* run position & altitude controllers, calculate velocity setpoint */ if (control_mode.flag_control_altitude_enabled) { - global_vel_sp.vz = pid_calculate(&z_pos_pid, local_pos_sp.z, local_pos.z, local_pos.vz - sp_move_rate[2], dt) + sp_move_rate[2]; + global_vel_sp.vz = pid_calculate(&z_pos_pid, local_pos_sp.z, local_pos.z, local_pos.vz - sp_move_rate[2], dt) + sp_move_rate[2] * params.z_ff; } else { reset_man_sp_z = true; @@ -621,8 +621,8 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) if (control_mode.flag_control_position_enabled) { /* calculate velocity set point in NED frame */ - global_vel_sp.vx = pid_calculate(&xy_pos_pids[0], local_pos_sp.x, local_pos.x, local_pos.vx - sp_move_rate[0], dt) + sp_move_rate[0]; - global_vel_sp.vy = pid_calculate(&xy_pos_pids[1], local_pos_sp.y, local_pos.y, local_pos.vy - sp_move_rate[1], dt) + sp_move_rate[1]; + global_vel_sp.vx = pid_calculate(&xy_pos_pids[0], local_pos_sp.x, local_pos.x, local_pos.vx - sp_move_rate[0], dt) + sp_move_rate[0] * params.xy_ff; + global_vel_sp.vy = pid_calculate(&xy_pos_pids[1], local_pos_sp.y, local_pos.y, local_pos.vy - sp_move_rate[1], dt) + sp_move_rate[1] * params.xy_ff; if (!control_mode.flag_control_manual_enabled) { /* limit horizontal speed only in AUTO mode */ diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control_params.c b/src/modules/multirotor_pos_control/multirotor_pos_control_params.c index cfb5dc0d47..1c798b0075 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control_params.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control_params.c @@ -49,12 +49,14 @@ PARAM_DEFINE_FLOAT(MPC_Z_VEL_P, 0.2f); PARAM_DEFINE_FLOAT(MPC_Z_VEL_I, 0.1f); PARAM_DEFINE_FLOAT(MPC_Z_VEL_D, 0.0f); PARAM_DEFINE_FLOAT(MPC_Z_VEL_MAX, 3.0f); +PARAM_DEFINE_FLOAT(MPC_Z_FF, 0.5f); PARAM_DEFINE_FLOAT(MPC_XY_P, 0.5f); PARAM_DEFINE_FLOAT(MPC_XY_D, 0.0f); PARAM_DEFINE_FLOAT(MPC_XY_VEL_P, 0.1f); PARAM_DEFINE_FLOAT(MPC_XY_VEL_I, 0.05f); PARAM_DEFINE_FLOAT(MPC_XY_VEL_D, 0.0f); PARAM_DEFINE_FLOAT(MPC_XY_VEL_MAX, 5.0f); +PARAM_DEFINE_FLOAT(MPC_XY_FF, 0.5f); PARAM_DEFINE_FLOAT(MPC_TILT_MAX, 0.5f); int parameters_init(struct multirotor_position_control_param_handles *h) @@ -69,12 +71,14 @@ int parameters_init(struct multirotor_position_control_param_handles *h) h->z_vel_i = param_find("MPC_Z_VEL_I"); h->z_vel_d = param_find("MPC_Z_VEL_D"); h->z_vel_max = param_find("MPC_Z_VEL_MAX"); + h->z_ff = param_find("MPC_Z_FF"); h->xy_p = param_find("MPC_XY_P"); h->xy_d = param_find("MPC_XY_D"); h->xy_vel_p = param_find("MPC_XY_VEL_P"); h->xy_vel_i = param_find("MPC_XY_VEL_I"); h->xy_vel_d = param_find("MPC_XY_VEL_D"); h->xy_vel_max = param_find("MPC_XY_VEL_MAX"); + h->xy_ff = param_find("MPC_XY_FF"); h->tilt_max = param_find("MPC_TILT_MAX"); h->rc_scale_pitch = param_find("RC_SCALE_PITCH"); @@ -96,12 +100,14 @@ int parameters_update(const struct multirotor_position_control_param_handles *h, param_get(h->z_vel_i, &(p->z_vel_i)); param_get(h->z_vel_d, &(p->z_vel_d)); param_get(h->z_vel_max, &(p->z_vel_max)); + param_get(h->z_ff, &(p->z_ff)); param_get(h->xy_p, &(p->xy_p)); param_get(h->xy_d, &(p->xy_d)); param_get(h->xy_vel_p, &(p->xy_vel_p)); param_get(h->xy_vel_i, &(p->xy_vel_i)); param_get(h->xy_vel_d, &(p->xy_vel_d)); param_get(h->xy_vel_max, &(p->xy_vel_max)); + param_get(h->xy_ff, &(p->xy_ff)); param_get(h->tilt_max, &(p->tilt_max)); param_get(h->rc_scale_pitch, &(p->rc_scale_pitch)); diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control_params.h b/src/modules/multirotor_pos_control/multirotor_pos_control_params.h index fc658dadb8..804716d11a 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control_params.h +++ b/src/modules/multirotor_pos_control/multirotor_pos_control_params.h @@ -51,12 +51,14 @@ struct multirotor_position_control_params { float z_vel_i; float z_vel_d; float z_vel_max; + float z_ff; float xy_p; float xy_d; float xy_vel_p; float xy_vel_i; float xy_vel_d; float xy_vel_max; + float xy_ff; float tilt_max; float rc_scale_pitch; @@ -75,12 +77,14 @@ struct multirotor_position_control_param_handles { param_t z_vel_i; param_t z_vel_d; param_t z_vel_max; + param_t z_ff; param_t xy_p; param_t xy_d; param_t xy_vel_p; param_t xy_vel_i; param_t xy_vel_d; param_t xy_vel_max; + param_t xy_ff; param_t tilt_max; param_t rc_scale_pitch; From ef5aa697c73e095f21143a64eb3cd112c85b3a08 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 21 Dec 2013 21:49:37 +0400 Subject: [PATCH 213/447] mc_att_control_vector: bugs fixed --- .../mc_att_control_vector/mc_att_control_vector_main.cpp | 8 +++----- 1 file changed, 3 insertions(+), 5 deletions(-) diff --git a/src/modules/mc_att_control_vector/mc_att_control_vector_main.cpp b/src/modules/mc_att_control_vector/mc_att_control_vector_main.cpp index fb09078faf..63aece1d95 100644 --- a/src/modules/mc_att_control_vector/mc_att_control_vector_main.cpp +++ b/src/modules/mc_att_control_vector/mc_att_control_vector_main.cpp @@ -373,9 +373,8 @@ MulticopterAttitudeControl::task_main_trampoline(int argc, char *argv[]) void MulticopterAttitudeControl::task_main() { - /* inform about start */ - warnx("Initializing.."); + warnx("started"); fflush(stdout); /* @@ -573,17 +572,16 @@ MulticopterAttitudeControl::task_main() /* calculate angle error */ float e_R_z_sin = e_R.norm(); float e_R_z_cos = R_z * R_des_z; - float e_R_z_angle = atan2f(e_R_z_sin, e_R_z_cos); /* calculate weight for yaw control */ - float cos_z = cosf(R_z(2)); - float yaw_w = cos_z * cos_z; + float yaw_w = R_des(2, 2) * R_des(2, 2); /* calculate rotation matrix after roll/pitch only rotation */ math::Matrix R_rp(3, 3); if (e_R_z_sin > 0.0f) { /* get axis-angle representation */ + float e_R_z_angle = atan2f(e_R_z_sin, e_R_z_cos); math::Vector3 e_R_z_axis = e_R / e_R_z_sin; e_R = e_R_z_axis * e_R_z_angle; From fae6c694239194bbdbc7c872bb1a1d11a8894474 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 22 Dec 2013 12:20:06 +0400 Subject: [PATCH 214/447] mc_att_control_vector, multirotor_pos_control: fixed --- .../mc_att_control_vector_main.cpp | 2 +- .../multirotor_pos_control.c | 17 +++++++++-------- 2 files changed, 10 insertions(+), 9 deletions(-) diff --git a/src/modules/mc_att_control_vector/mc_att_control_vector_main.cpp b/src/modules/mc_att_control_vector/mc_att_control_vector_main.cpp index 63aece1d95..76f0533723 100644 --- a/src/modules/mc_att_control_vector/mc_att_control_vector_main.cpp +++ b/src/modules/mc_att_control_vector/mc_att_control_vector_main.cpp @@ -630,7 +630,7 @@ MulticopterAttitudeControl::task_main() math::Vector3 rates_sp = _K * e_R; /* feed forward yaw setpoint rate */ - rates_sp(2) += yaw_sp_move_rate; + rates_sp(2) += yaw_sp_move_rate * yaw_w; math::Vector3 control = _K_rate_p * (rates_sp - rates) + _K_rate_d * (_rates_prev - rates) / fmaxf(dt, 0.003f); _rates_prev = rates; diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c index fe9a292b04..74ae5947fc 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c @@ -764,12 +764,13 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) float body_y[3]; float body_z[3]; - if (thrust_abs > 0.0f) { + if (thrust_abs > SIGMA) { body_z[0] = -thrust_sp[0] / thrust_abs; body_z[1] = -thrust_sp[1] / thrust_abs; body_z[2] = -thrust_sp[2] / thrust_abs; } else { + /* no thrust, set Z axis to safe value */ body_z[0] = 0.0f; body_z[1] = 0.0f; body_z[2] = 1.0f; @@ -781,13 +782,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) y_C[1] = cosf(att_sp.yaw_body); y_C[2] = 0; - if (fabsf(body_z[2]) < SIGMA) { - /* desired thrust is in XY plane, set X downside to construct correct matrix, - * but yaw component will not be used actually */ - body_x[0] = -body_x[0]; - body_x[1] = -body_x[1]; - body_x[2] = -body_x[2]; - } else { + if (fabsf(body_z[2]) > SIGMA) { /* desired body_x axis = cross(y_C, body_z) */ cross3(y_C, body_z, body_x); @@ -798,6 +793,12 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) body_x[2] = -body_x[2]; } normalize3(body_x); + } else { + /* desired thrust is in XY plane, set X downside to construct correct matrix, + * but yaw component will not be used actually */ + body_x[0] = 0.0f; + body_x[1] = 0.0f; + body_x[2] = 1.0f; } /* desired body_y axis = cross(body_z, body_x) */ From 4c6f6ed12cc90a7a0365fc22b56c59dfa3d55028 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 22 Dec 2013 22:41:46 +0400 Subject: [PATCH 215/447] multirotor_pos_control: remove unused parameters, use ellipsoid for velocity setpoint limiting --- .../multirotor_pos_control.c | 49 ++++++++----------- .../multirotor_pos_control_params.c | 12 ----- .../multirotor_pos_control_params.h | 8 --- 3 files changed, 20 insertions(+), 49 deletions(-) diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c index 74ae5947fc..378d3fda7d 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c @@ -173,6 +173,11 @@ static float norm(float x, float y) return sqrtf(x * x + y * y); } +static float norm3(float x, float y, float z) +{ + return sqrtf(x * x + y * y + z * z); +} + static void cross3(float a[3], float b[3], float res[3]) { res[0] = a[1] * b[2] - a[2] * b[1]; @@ -298,8 +303,6 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) hrt_abstime ref_alt_t = 0; uint64_t local_ref_timestamp = 0; - PID_t xy_pos_pids[2]; - PID_t z_pos_pid; float thrust_int[3] = { 0.0f, 0.0f, 0.0f }; thread_running = true; @@ -309,12 +312,6 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) parameters_init(¶ms_h); parameters_update(¶ms_h, ¶ms); - for (int i = 0; i < 2; i++) { - pid_init(&(xy_pos_pids[i]), PID_MODE_DERIVATIV_SET, 0.02f); - } - - pid_init(&z_pos_pid, PID_MODE_DERIVATIV_SET, 0.02f); - bool param_updated = true; while (!thread_should_exit) { @@ -329,12 +326,6 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) /* update params */ parameters_update(¶ms_h, ¶ms); - - for (int i = 0; i < 2; i++) { - pid_set_parameters(&(xy_pos_pids[i]), params.xy_p, 0.0f, params.xy_d, 0.0f, 0.0f); - } - - pid_set_parameters(&z_pos_pid, params.z_p, 0.0f, params.z_d, 1.0f, params.z_vel_max); } bool updated; @@ -362,6 +353,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) } else { dt = 0.0f; } + t_prev = t; if (control_mode.flag_armed && !was_armed) { /* reset setpoints and integrals on arming */ @@ -376,8 +368,6 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) was_armed = control_mode.flag_armed; - t_prev = t; - if (control_mode.flag_control_altitude_enabled || control_mode.flag_control_position_enabled || control_mode.flag_control_climb_rate_enabled || control_mode.flag_control_velocity_enabled) { orb_copy(ORB_ID(manual_control_setpoint), manual_sub, &manual); orb_copy(ORB_ID(vehicle_attitude), att_sub, &att); @@ -612,7 +602,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) /* run position & altitude controllers, calculate velocity setpoint */ if (control_mode.flag_control_altitude_enabled) { - global_vel_sp.vz = pid_calculate(&z_pos_pid, local_pos_sp.z, local_pos.z, local_pos.vz - sp_move_rate[2], dt) + sp_move_rate[2] * params.z_ff; + global_vel_sp.vz = (local_pos_sp.z - local_pos.z) * params.z_p + sp_move_rate[2] * params.z_ff; } else { reset_man_sp_z = true; @@ -621,18 +611,8 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) if (control_mode.flag_control_position_enabled) { /* calculate velocity set point in NED frame */ - global_vel_sp.vx = pid_calculate(&xy_pos_pids[0], local_pos_sp.x, local_pos.x, local_pos.vx - sp_move_rate[0], dt) + sp_move_rate[0] * params.xy_ff; - global_vel_sp.vy = pid_calculate(&xy_pos_pids[1], local_pos_sp.y, local_pos.y, local_pos.vy - sp_move_rate[1], dt) + sp_move_rate[1] * params.xy_ff; - - if (!control_mode.flag_control_manual_enabled) { - /* limit horizontal speed only in AUTO mode */ - float xy_vel_sp_norm = norm(global_vel_sp.vx, global_vel_sp.vy) / params.xy_vel_max; - - if (xy_vel_sp_norm > 1.0f) { - global_vel_sp.vx /= xy_vel_sp_norm; - global_vel_sp.vy /= xy_vel_sp_norm; - } - } + global_vel_sp.vx = (local_pos_sp.x - local_pos.x) * params.xy_p + sp_move_rate[0] * params.xy_ff; + global_vel_sp.vy = (local_pos_sp.y - local_pos.y) * params.xy_p + sp_move_rate[1] * params.xy_ff; } else { reset_man_sp_xy = true; @@ -640,6 +620,17 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) global_vel_sp.vy = 0.0f; } + if (!control_mode.flag_control_manual_enabled) { + /* limit 3D speed only in AUTO mode */ + float vel_sp_norm = norm3(global_vel_sp.vx / params.xy_vel_max, global_vel_sp.vy / params.xy_vel_max, global_vel_sp.vz / params.z_vel_max); + + if (vel_sp_norm > 1.0f) { + global_vel_sp.vx /= vel_sp_norm; + global_vel_sp.vy /= vel_sp_norm; + global_vel_sp.vz /= vel_sp_norm; + } + } + /* publish new velocity setpoint */ orb_publish(ORB_ID(vehicle_global_velocity_setpoint), global_vel_sp_pub, &global_vel_sp); // TODO subscribe to velocity setpoint if altitude/position control disabled diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control_params.c b/src/modules/multirotor_pos_control/multirotor_pos_control_params.c index 1c798b0075..bebcdb3f5a 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control_params.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control_params.c @@ -44,17 +44,13 @@ PARAM_DEFINE_FLOAT(MPC_THR_MIN, 0.2f); PARAM_DEFINE_FLOAT(MPC_THR_MAX, 0.8f); PARAM_DEFINE_FLOAT(MPC_Z_P, 1.0f); -PARAM_DEFINE_FLOAT(MPC_Z_D, 0.0f); PARAM_DEFINE_FLOAT(MPC_Z_VEL_P, 0.2f); PARAM_DEFINE_FLOAT(MPC_Z_VEL_I, 0.1f); -PARAM_DEFINE_FLOAT(MPC_Z_VEL_D, 0.0f); PARAM_DEFINE_FLOAT(MPC_Z_VEL_MAX, 3.0f); PARAM_DEFINE_FLOAT(MPC_Z_FF, 0.5f); PARAM_DEFINE_FLOAT(MPC_XY_P, 0.5f); -PARAM_DEFINE_FLOAT(MPC_XY_D, 0.0f); PARAM_DEFINE_FLOAT(MPC_XY_VEL_P, 0.1f); PARAM_DEFINE_FLOAT(MPC_XY_VEL_I, 0.05f); -PARAM_DEFINE_FLOAT(MPC_XY_VEL_D, 0.0f); PARAM_DEFINE_FLOAT(MPC_XY_VEL_MAX, 5.0f); PARAM_DEFINE_FLOAT(MPC_XY_FF, 0.5f); PARAM_DEFINE_FLOAT(MPC_TILT_MAX, 0.5f); @@ -66,17 +62,13 @@ int parameters_init(struct multirotor_position_control_param_handles *h) h->thr_min = param_find("MPC_THR_MIN"); h->thr_max = param_find("MPC_THR_MAX"); h->z_p = param_find("MPC_Z_P"); - h->z_d = param_find("MPC_Z_D"); h->z_vel_p = param_find("MPC_Z_VEL_P"); h->z_vel_i = param_find("MPC_Z_VEL_I"); - h->z_vel_d = param_find("MPC_Z_VEL_D"); h->z_vel_max = param_find("MPC_Z_VEL_MAX"); h->z_ff = param_find("MPC_Z_FF"); h->xy_p = param_find("MPC_XY_P"); - h->xy_d = param_find("MPC_XY_D"); h->xy_vel_p = param_find("MPC_XY_VEL_P"); h->xy_vel_i = param_find("MPC_XY_VEL_I"); - h->xy_vel_d = param_find("MPC_XY_VEL_D"); h->xy_vel_max = param_find("MPC_XY_VEL_MAX"); h->xy_ff = param_find("MPC_XY_FF"); h->tilt_max = param_find("MPC_TILT_MAX"); @@ -95,17 +87,13 @@ int parameters_update(const struct multirotor_position_control_param_handles *h, param_get(h->thr_min, &(p->thr_min)); param_get(h->thr_max, &(p->thr_max)); param_get(h->z_p, &(p->z_p)); - param_get(h->z_d, &(p->z_d)); param_get(h->z_vel_p, &(p->z_vel_p)); param_get(h->z_vel_i, &(p->z_vel_i)); - param_get(h->z_vel_d, &(p->z_vel_d)); param_get(h->z_vel_max, &(p->z_vel_max)); param_get(h->z_ff, &(p->z_ff)); param_get(h->xy_p, &(p->xy_p)); - param_get(h->xy_d, &(p->xy_d)); param_get(h->xy_vel_p, &(p->xy_vel_p)); param_get(h->xy_vel_i, &(p->xy_vel_i)); - param_get(h->xy_vel_d, &(p->xy_vel_d)); param_get(h->xy_vel_max, &(p->xy_vel_max)); param_get(h->xy_ff, &(p->xy_ff)); param_get(h->tilt_max, &(p->tilt_max)); diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control_params.h b/src/modules/multirotor_pos_control/multirotor_pos_control_params.h index 804716d11a..37a925dcc8 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control_params.h +++ b/src/modules/multirotor_pos_control/multirotor_pos_control_params.h @@ -46,17 +46,13 @@ struct multirotor_position_control_params { float thr_min; float thr_max; float z_p; - float z_d; float z_vel_p; float z_vel_i; - float z_vel_d; float z_vel_max; float z_ff; float xy_p; - float xy_d; float xy_vel_p; float xy_vel_i; - float xy_vel_d; float xy_vel_max; float xy_ff; float tilt_max; @@ -72,17 +68,13 @@ struct multirotor_position_control_param_handles { param_t thr_min; param_t thr_max; param_t z_p; - param_t z_d; param_t z_vel_p; param_t z_vel_i; - param_t z_vel_d; param_t z_vel_max; param_t z_ff; param_t xy_p; - param_t xy_d; param_t xy_vel_p; param_t xy_vel_i; - param_t xy_vel_d; param_t xy_vel_max; param_t xy_ff; param_t tilt_max; From c9f785ff7f64960b79129e4e79fd0db4863192ae Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 23 Dec 2013 10:37:52 +0100 Subject: [PATCH 216/447] Added missing export keywords --- src/lib/mathlib/math/Matrix.hpp | 10 +++++----- src/lib/mathlib/math/Vector.hpp | 13 +++++++------ 2 files changed, 12 insertions(+), 11 deletions(-) diff --git a/src/lib/mathlib/math/Matrix.hpp b/src/lib/mathlib/math/Matrix.hpp index 47929ffcb9..31b03b54b7 100644 --- a/src/lib/mathlib/math/Matrix.hpp +++ b/src/lib/mathlib/math/Matrix.hpp @@ -49,13 +49,13 @@ namespace math { template -class Matrix; +class __EXPORT Matrix; -class Quaternion; +class __EXPORT Quaternion; // MxN matrix with float elements template -class MatrixBase { +class __EXPORT MatrixBase { public: /** * matrix data[row][col] @@ -259,7 +259,7 @@ public: }; template -class Matrix : public MatrixBase { +class __EXPORT Matrix : public MatrixBase { public: using MatrixBase::operator *; @@ -292,7 +292,7 @@ public: namespace math { template <> -class Matrix<3, 3> : public MatrixBase<3, 3> { +class __EXPORT Matrix<3, 3> : public MatrixBase<3, 3> { public: using MatrixBase<3, 3>::operator *; diff --git a/src/lib/mathlib/math/Vector.hpp b/src/lib/mathlib/math/Vector.hpp index 8d754a3214..c865bab422 100644 --- a/src/lib/mathlib/math/Vector.hpp +++ b/src/lib/mathlib/math/Vector.hpp @@ -42,6 +42,7 @@ #ifndef VECTOR_HPP #define VECTOR_HPP +#include #include #include #include "../CMSIS/Include/arm_math.h" @@ -50,10 +51,10 @@ namespace math { template -class Vector; +class __EXPORT Vector; template -class VectorBase { +class __EXPORT VectorBase { public: /** * vector data @@ -267,7 +268,7 @@ public: }; template -class Vector : public VectorBase { +class __EXPORT Vector : public VectorBase { public: using VectorBase::operator *; @@ -294,7 +295,7 @@ public: }; template <> -class Vector<2> : public VectorBase<2> { +class __EXPORT Vector<2> : public VectorBase<2> { public: Vector() : VectorBase<2>() { } @@ -334,7 +335,7 @@ public: }; template <> -class Vector<3> : public VectorBase<3> { +class __EXPORT Vector<3> : public VectorBase<3> { public: Vector() { arm_col = {3, 1, &this->data[0]}; @@ -375,7 +376,7 @@ public: }; template <> -class Vector<4> : public VectorBase<4> { +class __EXPORT Vector<4> : public VectorBase<4> { public: Vector() : VectorBase() {} From 411428b8e448f773d104704449b08261197a4b27 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 23 Dec 2013 11:45:03 +0100 Subject: [PATCH 217/447] remove dataman from tests (as in upstream master) --- src/systemcmds/tests/tests_main.c | 1 - 1 file changed, 1 deletion(-) diff --git a/src/systemcmds/tests/tests_main.c b/src/systemcmds/tests/tests_main.c index 30ae5b4ecc..1088a44076 100644 --- a/src/systemcmds/tests/tests_main.c +++ b/src/systemcmds/tests/tests_main.c @@ -107,7 +107,6 @@ const struct { {"mixer", test_mixer, OPT_NOJIGTEST | OPT_NOALLTEST}, {"rc", test_rc, OPT_NOJIGTEST | OPT_NOALLTEST}, {"help", test_help, OPT_NOALLTEST | OPT_NOHELP | OPT_NOJIGTEST}, - {"dataman", test_dataman, OPT_NOALLTEST}, {NULL, NULL, 0} }; From 24e2676131ac688eaedcbc9f65a06446d066820d Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 23 Dec 2013 12:13:25 +0100 Subject: [PATCH 218/447] add dataman to config_px4fmu-v2_logging.mk --- makefiles/config_px4fmu-v2_logging.mk | 1 + 1 file changed, 1 insertion(+) diff --git a/makefiles/config_px4fmu-v2_logging.mk b/makefiles/config_px4fmu-v2_logging.mk index ed90f6464c..4e3cae5eec 100644 --- a/makefiles/config_px4fmu-v2_logging.mk +++ b/makefiles/config_px4fmu-v2_logging.mk @@ -115,6 +115,7 @@ MODULES += lib/ecl MODULES += lib/external_lgpl MODULES += lib/geo MODULES += lib/conversion +MODULES += modules/dataman # # Demo apps From afbf4223398407a5734aa0741ef24b5fab5b6dca Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 23 Dec 2013 13:18:05 +0100 Subject: [PATCH 219/447] tecs: change pitch on climbout #559 (ported from ardupilot) --- src/lib/external_lgpl/tecs/tecs.cpp | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/src/lib/external_lgpl/tecs/tecs.cpp b/src/lib/external_lgpl/tecs/tecs.cpp index a733ef1d2a..1d5c85699a 100644 --- a/src/lib/external_lgpl/tecs/tecs.cpp +++ b/src/lib/external_lgpl/tecs/tecs.cpp @@ -404,10 +404,18 @@ void TECS::_update_pitch(void) // Apply max and min values for integrator state that will allow for no more than // 5deg of saturation. This allows for some pitch variation due to gusts before the // integrator is clipped. Otherwise the effectiveness of the integrator will be reduced in turbulence + // During climbout/takeoff, bias the demanded pitch angle so that zero speed error produces a pitch angle + // demand equal to the minimum value (which is )set by the mission plan during this mode). Otherwise the + // integrator has to catch up before the nose can be raised to reduce speed during climbout. float gainInv = (_integ5_state * _timeConst * CONSTANTS_ONE_G); float temp = SEB_error + SEBdot_error * _ptchDamp + SEBdot_dem * _timeConst; + if (_climbOutDem) + { + temp += _PITCHminf * gainInv; + } _integ7_state = constrain(_integ7_state, (gainInv * (_PITCHminf - 0.0783f)) - temp, (gainInv * (_PITCHmaxf + 0.0783f)) - temp); + // Calculate pitch demand from specific energy balance signals _pitch_dem_unc = (temp + _integ7_state) / gainInv; From b7652986d9cc0fe3edc765e3485b696b4b639b03 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 23 Dec 2013 13:38:13 +0100 Subject: [PATCH 220/447] add minimal pitch field to mission item --- src/modules/mavlink/waypoints.c | 11 ++++++++++- src/modules/uORB/topics/mission.h | 1 + 2 files changed, 11 insertions(+), 1 deletion(-) diff --git a/src/modules/mavlink/waypoints.c b/src/modules/mavlink/waypoints.c index 52a580d5be..b72086a7fe 100644 --- a/src/modules/mavlink/waypoints.c +++ b/src/modules/mavlink/waypoints.c @@ -103,11 +103,20 @@ int map_mavlink_mission_item_to_mission_item(const mavlink_mission_item_t *mavli return MAV_MISSION_ERROR; } + switch (mavlink_mission_item->command) { + case MAV_CMD_NAV_TAKEOFF: + mission_item->pitch_min = mavlink_mission_item->param1; + break; + default: + mission_item->radius = mavlink_mission_item->param1; + break; + } + mission_item->yaw = _wrap_pi(mavlink_mission_item->param4*M_DEG_TO_RAD_F); mission_item->loiter_radius = fabsf(mavlink_mission_item->param3); mission_item->loiter_direction = (mavlink_mission_item->param3 > 0) ? 1 : -1; /* 1 if positive CW, -1 if negative CCW */ mission_item->nav_cmd = mavlink_mission_item->command; - mission_item->radius = mavlink_mission_item->param1; + mission_item->time_inside = mavlink_mission_item->param2 / 1e3f; /* from milliseconds to seconds */ mission_item->autocontinue = mavlink_mission_item->autocontinue; mission_item->index = mavlink_mission_item->seq; diff --git a/src/modules/uORB/topics/mission.h b/src/modules/uORB/topics/mission.h index 3702420076..9697835cf6 100644 --- a/src/modules/uORB/topics/mission.h +++ b/src/modules/uORB/topics/mission.h @@ -89,6 +89,7 @@ struct mission_item_s enum NAV_CMD nav_cmd; /**< navigation command */ float radius; /**< radius in which the mission is accepted as reached in meters */ float time_inside; /**< time that the MAV should stay inside the radius before advancing in seconds */ + float pitch_min; /**< minimal pitch angle for fixed wing takeoff waypoints */ bool autocontinue; /**< true if next waypoint should follow after this one */ int index; /**< index matching the mavlink waypoint */ enum ORIGIN origin; /**< where the waypoint has been generated */ From 546964332d5eb3c9e402e39a136fc6c6994c00f6 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 23 Dec 2013 13:41:39 +0100 Subject: [PATCH 221/447] use minimal pitch field introduced in b7652986d9cc0fe3edc765e3485b696b4b639b03 for fw takeoff --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index cdd41d16a2..eb84c49d1a 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -961,7 +961,7 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio /* enforce a minimum of 10 degrees pitch up on takeoff, or take parameter */ _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _mission_item_triplet.current.altitude, calculate_target_airspeed(1.3f * _parameters.airspeed_min), _airspeed.indicated_airspeed_m_s, eas2tas, - true, math::max(math::radians(mission_item_triplet.current.radius), math::radians(10.0f)), + true, math::max(math::radians(mission_item_triplet.current.pitch_min), math::radians(10.0f)), _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max)); From 94b68aa1cc2f6fedd625d2b5ac7edc04b67d3749 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 23 Dec 2013 16:58:27 +0100 Subject: [PATCH 222/447] add missing conversion from mission item to mavlink mission item after changes from b7652986d9cc0fe3edc765e3485b696b4b639b03 --- src/modules/mavlink/waypoints.c | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/src/modules/mavlink/waypoints.c b/src/modules/mavlink/waypoints.c index b72086a7fe..0998cd5eb3 100644 --- a/src/modules/mavlink/waypoints.c +++ b/src/modules/mavlink/waypoints.c @@ -133,6 +133,15 @@ int map_mission_item_to_mavlink_mission_item(const struct mission_item_s *missio mavlink_mission_item->frame = MAV_FRAME_GLOBAL_RELATIVE_ALT; } + switch (mission_item->nav_cmd) { + case NAV_CMD_TAKEOFF: + mavlink_mission_item->param1 = mission_item->pitch_min; + break; + default: + mavlink_mission_item->param1 = mission_item->radius; + break; + } + mavlink_mission_item->x = (float)mission_item->lat; mavlink_mission_item->y = (float)mission_item->lon; mavlink_mission_item->z = mission_item->altitude; @@ -140,7 +149,6 @@ int map_mission_item_to_mavlink_mission_item(const struct mission_item_s *missio mavlink_mission_item->param4 = mission_item->yaw*M_RAD_TO_DEG_F; mavlink_mission_item->param3 = mission_item->loiter_radius*(float)mission_item->loiter_direction; mavlink_mission_item->command = mission_item->nav_cmd; - mavlink_mission_item->param1 = mission_item->radius; mavlink_mission_item->param2 = mission_item->time_inside * 1e3f; /* from seconds to milliseconds */ mavlink_mission_item->autocontinue = mission_item->autocontinue; mavlink_mission_item->seq = mission_item->index; From a26e46bede186c36b475e0b5a9cf3ef758cc1c9b Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 23 Dec 2013 22:34:11 +0400 Subject: [PATCH 223/447] att_pos_estimator_ekf: fixes, mathlib: minor changes --- src/lib/mathlib/math/Matrix.hpp | 15 +++++++++++--- src/lib/mathlib/math/Vector.hpp | 7 +++++++ .../att_pos_estimator_ekf/KalmanNav.cpp | 20 +++++++++++++------ .../att_pos_estimator_ekf/kalman_main.cpp | 2 +- 4 files changed, 34 insertions(+), 10 deletions(-) diff --git a/src/lib/mathlib/math/Matrix.hpp b/src/lib/mathlib/math/Matrix.hpp index 31b03b54b7..7ed8879a77 100644 --- a/src/lib/mathlib/math/Matrix.hpp +++ b/src/lib/mathlib/math/Matrix.hpp @@ -241,19 +241,28 @@ public: } /** - * setup the identity matrix + * set zero matrix + */ + void zero(void) { + memset(data, 0, sizeof(data)); + } + + /** + * set identity matrix */ void identity(void) { memset(data, 0, sizeof(data)); - for (unsigned int i = 0; i < M; i++) + unsigned int n = (M < N) ? M : N; + for (unsigned int i = 0; i < n; i++) data[i][i] = 1; } void print(void) { for (unsigned int i = 0; i < M; i++) { + printf("[ "); for (unsigned int j = 0; j < N; j++) printf("%.3f\t", data[i][j]); - printf("\n"); + printf(" ]\n"); } } }; diff --git a/src/lib/mathlib/math/Vector.hpp b/src/lib/mathlib/math/Vector.hpp index c865bab422..744402e212 100644 --- a/src/lib/mathlib/math/Vector.hpp +++ b/src/lib/mathlib/math/Vector.hpp @@ -259,6 +259,13 @@ public: return *this / length(); } + /** + * set zero vector + */ + void zero(void) { + memset(data, 0, sizeof(data)); + } + void print(void) { printf("[ "); for (unsigned int i = 0; i < N; i++) diff --git a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp b/src/modules/att_pos_estimator_ekf/KalmanNav.cpp index 4ab41c6cee..9d3ef07f29 100644 --- a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp +++ b/src/modules/att_pos_estimator_ekf/KalmanNav.cpp @@ -97,6 +97,14 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) : { using namespace math; + F.zero(); + G.zero(); + V.zero(); + HAtt.zero(); + RAtt.zero(); + HPos.zero(); + RPos.zero(); + // initial state covariance matrix P0.identity(); P0 *= 0.01f; @@ -214,8 +222,8 @@ void KalmanNav::update() if (correctAtt() == ret_ok) _attitudeInitCounter++; if (_attitudeInitCounter > 100) { - warnx("initialized EKF attitude\n"); - warnx("phi: %8.4f, theta: %8.4f, psi: %8.4f\n", + warnx("initialized EKF attitude"); + warnx("phi: %8.4f, theta: %8.4f, psi: %8.4f", double(phi), double(theta), double(psi)); _attitudeInitialized = true; } @@ -245,8 +253,8 @@ void KalmanNav::update() // lat/lon and not have init map_projection_init(lat0, lon0); _positionInitialized = true; - warnx("initialized EKF state with GPS\n"); - warnx("vN: %8.4f, vE: %8.4f, vD: %8.4f, lat: %8.4f, lon: %8.4f, alt: %8.4f\n", + warnx("initialized EKF state with GPS"); + warnx("vN: %8.4f, vE: %8.4f, vD: %8.4f, lat: %8.4f, lon: %8.4f, alt: %8.4f", double(vN), double(vE), double(vD), lat, lon, double(alt)); } @@ -625,7 +633,7 @@ int KalmanNav::correctAtt() if (isnan(val) || isinf(val)) { // abort correction and return - warnx("numerical failure in att correction\n"); + warnx("numerical failure in att correction"); // reset P matrix to P0 P = P0; return ret_error; @@ -691,7 +699,7 @@ int KalmanNav::correctPos() if (!isfinite(val)) { // abort correction and return - warnx("numerical failure in gps correction\n"); + warnx("numerical failure in gps correction"); // fallback to GPS vN = _gps.vel_n_m_s; vE = _gps.vel_e_m_s; diff --git a/src/modules/att_pos_estimator_ekf/kalman_main.cpp b/src/modules/att_pos_estimator_ekf/kalman_main.cpp index 372b2d162c..70ba628f33 100644 --- a/src/modules/att_pos_estimator_ekf/kalman_main.cpp +++ b/src/modules/att_pos_estimator_ekf/kalman_main.cpp @@ -107,7 +107,7 @@ int att_pos_estimator_ekf_main(int argc, char *argv[]) daemon_task = task_spawn_cmd("att_pos_estimator_ekf", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 30, - 4096, + 8096, kalman_demo_thread_main, (argv) ? (const char **)&argv[2] : (const char **)NULL); exit(0); From 96debedcc8747de0bb797366bed34760a9c6ba58 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 24 Dec 2013 11:41:04 +0100 Subject: [PATCH 224/447] prevent dataman from blocking startup when no sd card is present --- src/modules/dataman/dataman.c | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/modules/dataman/dataman.c b/src/modules/dataman/dataman.c index 874a47be7f..14112fc0d4 100644 --- a/src/modules/dataman/dataman.c +++ b/src/modules/dataman/dataman.c @@ -572,11 +572,13 @@ task_main(int argc, char *argv[]) g_task_fd = open(k_data_manager_device_path, O_RDWR | O_CREAT | O_BINARY); if (g_task_fd < 0) { warnx("Could not open data manager file %s", k_data_manager_device_path); + sem_post(&g_init_sema); return -1; } if (lseek(g_task_fd, max_offset, SEEK_SET) != max_offset) { close(g_task_fd); warnx("Could not seek data manager file %s", k_data_manager_device_path); + sem_post(&g_init_sema); return -1; } fsync(g_task_fd); @@ -720,7 +722,7 @@ dataman_main(int argc, char *argv[]) if (g_fd < 0) errx(1, "start failed"); - return 0; + exit(0); } if (g_fd < 0) @@ -733,6 +735,6 @@ dataman_main(int argc, char *argv[]) else usage(); - return 0; + exit(1); } From 6c7ae211b1c0fc7c327de706255811d297eb4917 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 24 Dec 2013 14:12:48 +0100 Subject: [PATCH 225/447] flight termination: fix missing initialization of actuators_1_pub in fw_att_control --- src/modules/fw_att_control/fw_att_control_main.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index 1651cb3993..26777f737c 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -288,6 +288,7 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : _rate_sp_pub(-1), _actuators_0_pub(-1), _attitude_sp_pub(-1), + _actuators_1_pub(-1), /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "fw att control")), From d1f35cc1107f3f07965073862150a5e4c7ea612c Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 24 Dec 2013 15:04:11 +0100 Subject: [PATCH 226/447] HIL: only listen to first 8 actuator outputs --- src/modules/mavlink/orb_listener.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/mavlink/orb_listener.c b/src/modules/mavlink/orb_listener.c index c37c35a63f..9e43467cc3 100644 --- a/src/modules/mavlink/orb_listener.c +++ b/src/modules/mavlink/orb_listener.c @@ -499,8 +499,8 @@ l_actuator_outputs(const struct listener *l) act_outputs.output[6], act_outputs.output[7]); - /* only send in HIL mode */ - if (mavlink_hil_enabled && armed.armed) { + /* only send in HIL mode and only send first group for HIL */ + if (mavlink_hil_enabled && armed.armed && ids[l->arg] == ORB_ID(actuator_outputs_0)) { /* translate the current syste state to mavlink state and mode */ uint8_t mavlink_state = 0; From 9dfe366e908ce0100875996c3ea0d4cfdfcc24bf Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 24 Dec 2013 23:56:28 +0400 Subject: [PATCH 227/447] mathlib: Vector class major cleanup --- src/lib/mathlib/math/Matrix.hpp | 27 +-- src/lib/mathlib/math/Quaternion.hpp | 48 ++++- src/lib/mathlib/math/Vector.hpp | 197 ++++++++---------- .../att_pos_estimator_ekf/KalmanNav.cpp | 8 +- src/modules/mavlink/mavlink_receiver.cpp | 3 +- src/modules/sensors/sensors.cpp | 6 +- src/systemcmds/tests/test_mathlib.cpp | 114 ++++++---- 7 files changed, 210 insertions(+), 193 deletions(-) diff --git a/src/lib/mathlib/math/Matrix.hpp b/src/lib/mathlib/math/Matrix.hpp index 7ed8879a77..9896a16d05 100644 --- a/src/lib/mathlib/math/Matrix.hpp +++ b/src/lib/mathlib/math/Matrix.hpp @@ -94,11 +94,11 @@ public: return data[row][col]; } - unsigned int getRows() { + unsigned int get_rows() { return M; } - unsigned int getCols() { + unsigned int get_cols() { return N; } @@ -295,11 +295,7 @@ public: return res; } }; -} -#include "Quaternion.hpp" - -namespace math { template <> class __EXPORT Matrix<3, 3> : public MatrixBase<3, 3> { public: @@ -329,25 +325,6 @@ public: return res; } - /** - * create a rotation matrix from given quaternion - */ - void from_quaternion(const Quaternion &q) { - float aSq = q.data[0] * q.data[0]; - float bSq = q.data[1] * q.data[1]; - float cSq = q.data[2] * q.data[2]; - float dSq = q.data[3] * q.data[3]; - data[0][0] = aSq + bSq - cSq - dSq; - data[0][1] = 2.0f * (q.data[1] * q.data[2] - q.data[0] * q.data[3]); - data[0][2] = 2.0f * (q.data[0] * q.data[2] + q.data[1] * q.data[3]); - data[1][0] = 2.0f * (q.data[1] * q.data[2] + q.data[0] * q.data[3]); - data[1][1] = aSq - bSq + cSq - dSq; - data[1][2] = 2.0f * (q.data[2] * q.data[3] - q.data[0] * q.data[1]); - data[2][0] = 2.0f * (q.data[1] * q.data[3] - q.data[0] * q.data[2]); - data[2][1] = 2.0f * (q.data[0] * q.data[1] + q.data[2] * q.data[3]); - data[2][2] = aSq - bSq - cSq + dSq; - } - /** * create a rotation matrix from given euler angles * based on http://gentlenav.googlecode.com/files/EulerAngles.pdf diff --git a/src/lib/mathlib/math/Quaternion.hpp b/src/lib/mathlib/math/Quaternion.hpp index 3735fb3d33..c19dbd29c9 100644 --- a/src/lib/mathlib/math/Quaternion.hpp +++ b/src/lib/mathlib/math/Quaternion.hpp @@ -58,7 +58,7 @@ public: /** * trivial ctor */ - Quaternion() { + Quaternion() : Vector() { } /** @@ -70,10 +70,29 @@ public: Quaternion(const Vector<4> &v) : Vector(v) { } - Quaternion(const float *v) : Vector(v) { + Quaternion(const Quaternion &q) : Vector(q) { } - Quaternion derivative(const Vector<3> &w) { + Quaternion(const float v[4]) : Vector(v) { + } + + using Vector<4>::operator *; + + /** + * multiplication + */ + const Quaternion operator *(const Quaternion &q) const { + return Quaternion( + data[0] * q.data[0] - data[1] * q.data[1] - data[2] * q.data[2] - data[3] * q.data[3], + data[0] * q.data[1] + data[1] * q.data[0] + data[2] * q.data[3] - data[3] * q.data[2], + data[0] * q.data[2] - data[1] * q.data[3] + data[2] * q.data[0] + data[3] * q.data[1], + data[0] * q.data[3] + data[1] * q.data[2] - data[2] * q.data[1] + data[3] * q.data[0]); + } + + /** + * derivative + */ + const Quaternion derivative(const Vector<3> &w) { float dataQ[] = { data[0], -data[1], -data[2], -data[3], data[1], data[0], -data[3], data[2], @@ -85,6 +104,9 @@ public: return Q * v * 0.5f; } + /** + * set quaternion to rotation defined by euler angles + */ void from_euler(float roll, float pitch, float yaw) { double cosPhi_2 = cos(double(roll) / 2.0); double sinPhi_2 = sin(double(roll) / 2.0); @@ -97,6 +119,26 @@ public: data[2] = cosPhi_2 * sinTheta_2 * cosPsi_2 + sinPhi_2 * cosTheta_2 * sinPsi_2; data[3] = cosPhi_2 * cosTheta_2 * sinPsi_2 - sinPhi_2 * sinTheta_2 * cosPsi_2; } + + /** + * create rotation matrix for the quaternion + */ + Matrix<3, 3> to_dcm(void) const { + Matrix<3, 3> R; + float aSq = data[0] * data[0]; + float bSq = data[1] * data[1]; + float cSq = data[2] * data[2]; + float dSq = data[3] * data[3]; + R.data[0][0] = aSq + bSq - cSq - dSq; + R.data[0][1] = 2.0f * (data[1] * data[2] - data[0] * data[3]); + R.data[0][2] = 2.0f * (data[0] * data[2] + data[1] * data[3]); + R.data[1][0] = 2.0f * (data[1] * data[2] + data[0] * data[3]); + R.data[1][1] = aSq - bSq + cSq - dSq; + R.data[1][2] = 2.0f * (data[2] * data[3] - data[0] * data[1]); + R.data[2][0] = 2.0f * (data[1] * data[3] - data[0] * data[2]); + R.data[2][1] = 2.0f * (data[0] * data[1] + data[2] * data[3]); + R.data[2][2] = aSq - bSq - cSq + dSq; + } }; } diff --git a/src/lib/mathlib/math/Vector.hpp b/src/lib/mathlib/math/Vector.hpp index 744402e212..d579ecf734 100644 --- a/src/lib/mathlib/math/Vector.hpp +++ b/src/lib/mathlib/math/Vector.hpp @@ -1,8 +1,9 @@ /**************************************************************************** * * Copyright (C) 2013 PX4 Development Team. All rights reserved. - * Author: Will Perone - * Anton Babushkin + * Author: Anton Babushkin + * Pavel Kirienko + * Lorenz Meier * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -36,13 +37,12 @@ /** * @file Vector.hpp * - * Generic Vector + * Vector class */ #ifndef VECTOR_HPP #define VECTOR_HPP -#include #include #include #include "../CMSIS/Include/arm_math.h" @@ -84,7 +84,7 @@ public: /** * setting ctor */ - VectorBase(const float *d) { + VectorBase(const float d[N]) { arm_col = {N, 1, &data[0]}; memcpy(data, d, sizeof(data)); } @@ -92,31 +92,30 @@ public: /** * access to elements by index */ - inline float &operator ()(unsigned int i) { + float &operator ()(const unsigned int i) { return data[i]; } /** * access to elements by index */ - inline const float &operator ()(unsigned int i) const { + float operator ()(const unsigned int i) const { return data[i]; } - unsigned int getRows() { + /** + * get rows number + */ + unsigned int get_size() const { return N; } - unsigned int getCols() { - return 1; - } - /** * test for equality */ bool operator ==(const Vector &v) { for (unsigned int i = 0; i < N; i++) - if (data[i] != v(i)) + if (data[i] != v.data[i]) return false; return true; } @@ -126,7 +125,7 @@ public: */ bool operator !=(const Vector &v) { for (unsigned int i = 0; i < N; i++) - if (data[i] != v(i)) + if (data[i] != v.data[i]) return true; return false; } @@ -155,7 +154,7 @@ public: const Vector operator +(const Vector &v) const { Vector res; for (unsigned int i = 0; i < N; i++) - res.data[i] = data[i] + v(i); + res.data[i] = data[i] + v.data[i]; return res; } @@ -165,7 +164,7 @@ public: const Vector operator -(const Vector &v) const { Vector res; for (unsigned int i = 0; i < N; i++) - res.data[i] = data[i] - v(i); + res.data[i] = data[i] - v.data[i]; return res; } @@ -173,16 +172,20 @@ public: * uniform scaling */ const Vector operator *(const float num) const { - Vector temp(*this); - return temp *= num; + Vector res; + for (unsigned int i = 0; i < N; i++) + res.data[i] = data[i] * num; + return res; } /** * uniform scaling */ const Vector operator /(const float num) const { - Vector temp(*static_cast*>(this)); - return temp /= num; + Vector res; + for (unsigned int i = 0; i < N; i++) + res.data[i] = data[i] / num; + return res; } /** @@ -190,7 +193,7 @@ public: */ const Vector &operator +=(const Vector &v) { for (unsigned int i = 0; i < N; i++) - data[i] += v(i); + data[i] += v.data[i]; return *static_cast*>(this); } @@ -199,7 +202,7 @@ public: */ const Vector &operator -=(const Vector &v) { for (unsigned int i = 0; i < N; i++) - data[i] -= v(i); + data[i] -= v.data[i]; return *static_cast*>(this); } @@ -227,7 +230,7 @@ public: float operator *(const Vector &v) const { float res = 0.0f; for (unsigned int i = 0; i < N; i++) - res += data[i] * v(i); + res += data[i] * v.data[i]; return res; } @@ -235,14 +238,20 @@ public: * gets the length of this vector squared */ float length_squared() const { - return (*this * *this); + float res = 0.0f; + for (unsigned int i = 0; i < N; i++) + res += data[i] * data[i]; + return res; } /** * gets the length of this vector */ float length() const { - return sqrtf(*this * *static_cast*>(this)); + float res = 0.0f; + for (unsigned int i = 0; i < N; i++) + res += data[i] * data[i]; + return sqrtf(res); } /** @@ -277,25 +286,17 @@ public: template class __EXPORT Vector : public VectorBase { public: - using VectorBase::operator *; + //using VectorBase::operator *; + Vector() : VectorBase() {} - Vector() : VectorBase() { - } + Vector(const Vector &v) : VectorBase(v) {} - Vector(const float d[]) : VectorBase(d) { - } - - Vector(const Vector &v) : VectorBase(v) { - } - - Vector(const VectorBase &v) : VectorBase(v) { - } + Vector(const float d[N]) : VectorBase(d) {} /** * set to value */ const Vector &operator =(const Vector &v) { - this->arm_col = {N, 1, &this->data[0]}; memcpy(this->data, v.data, sizeof(this->data)); return *this; } @@ -304,24 +305,24 @@ public: template <> class __EXPORT Vector<2> : public VectorBase<2> { public: - Vector() : VectorBase<2>() { + Vector() : VectorBase<2>() {} + + /* simple copy is 1.6 times faster than memcpy */ + Vector(const Vector &v) : VectorBase<2>() { + data[0] = v.data[0]; + data[1] = v.data[1]; } - Vector(const float x, const float y) : VectorBase() { + Vector(const float d[2]) : VectorBase<2>() { + data[0] = d[0]; + data[1] = d[1]; + } + + Vector(const float x, const float y) : VectorBase<2>() { data[0] = x; data[1] = y; } - Vector(const Vector<2> &v) : VectorBase() { - data[0] = v.data[0]; - data[1] = v.data[1]; - } - - Vector(const VectorBase<2> &v) : VectorBase() { - data[0] = v.data[0]; - data[1] = v.data[1]; - } - /** * set to value */ @@ -331,55 +332,49 @@ public: return *this; } - float cross(const Vector<2> &b) const { - return data[0]*b.data[1] - data[1]*b.data[0]; - } - float operator %(const Vector<2> &v) const { - return cross(v); + return data[0] * v.data[1] - data[1] * v.data[0]; } - }; template <> class __EXPORT Vector<3> : public VectorBase<3> { public: - Vector() { - arm_col = {3, 1, &this->data[0]}; + Vector() : VectorBase<3>() {} + + /* simple copy is 1.6 times faster than memcpy */ + Vector(const Vector &v) : VectorBase<3>() { + for (unsigned int i = 0; i < 3; i++) + data[i] = v.data[i]; } - Vector(const float x, const float y, const float z) { - arm_col = {3, 1, &this->data[0]}; + Vector(const float d[3]) : VectorBase<3>() { + for (unsigned int i = 0; i < 3; i++) + data[i] = d[i]; + } + + Vector(const float x, const float y, const float z) : VectorBase<3>() { data[0] = x; data[1] = y; data[2] = z; } - Vector(const Vector<3> &v) : VectorBase<3>() { - data[0] = v.data[0]; - data[1] = v.data[1]; - data[2] = v.data[2]; - } - - /** - * setting ctor - */ - Vector(const float d[]) { - arm_col = {3, 1, &this->data[0]}; - data[0] = d[0]; - data[1] = d[1]; - data[2] = d[2]; - } - /** * set to value */ const Vector<3> &operator =(const Vector<3> &v) { - data[0] = v.data[0]; - data[1] = v.data[1]; - data[2] = v.data[2]; + for (unsigned int i = 0; i < 3; i++) + data[i] = v.data[i]; return *this; } + + Vector<3> operator %(const Vector<3> &v) const { + return Vector<3>( + data[1] * v.data[2] - data[2] * v.data[1], + data[2] * v.data[0] - data[0] * v.data[2], + data[0] * v.data[1] - data[1] * v.data[0] + ); + } }; template <> @@ -387,49 +382,31 @@ class __EXPORT Vector<4> : public VectorBase<4> { public: Vector() : VectorBase() {} - Vector(const float x, const float y, const float z, const float t) : VectorBase() { - data[0] = x; - data[1] = y; - data[2] = z; - data[3] = t; + Vector(const Vector &v) : VectorBase<4>() { + for (unsigned int i = 0; i < 4; i++) + data[i] = v.data[i]; } - Vector(const Vector<4> &v) : VectorBase() { - data[0] = v.data[0]; - data[1] = v.data[1]; - data[2] = v.data[2]; - data[3] = v.data[3]; + Vector(const float d[4]) : VectorBase<4>() { + for (unsigned int i = 0; i < 4; i++) + data[i] = d[i]; } - Vector(const VectorBase<4> &v) : VectorBase() { - data[0] = v.data[0]; - data[1] = v.data[1]; - data[2] = v.data[2]; - data[3] = v.data[3]; + Vector(const float x0, const float x1, const float x2, const float x3) : VectorBase() { + data[0] = x0; + data[1] = x1; + data[2] = x2; + data[3] = x3; } - /** - * setting ctor - */ - /* - Vector(const float d[]) { - arm_col = {3, 1, &this->data[0]}; - data[0] = d[0]; - data[1] = d[1]; - data[2] = d[2]; - } -*/ /** * set to value */ - /* - const Vector<3> &operator =(const Vector<3> &v) { - data[0] = v.data[0]; - data[1] = v.data[1]; - data[2] = v.data[2]; + const Vector<4> &operator =(const Vector<4> &v) { + for (unsigned int i = 0; i < 4; i++) + data[i] = v.data[i]; return *this; } - */ }; } diff --git a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp b/src/modules/att_pos_estimator_ekf/KalmanNav.cpp index 9d3ef07f29..aca3fe7b67 100644 --- a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp +++ b/src/modules/att_pos_estimator_ekf/KalmanNav.cpp @@ -132,7 +132,7 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) : _sensors.magnetometer_ga[2]); // initialize dcm - C_nb.from_quaternion(q); + C_nb = q.to_dcm(); // HPos is constant HPos(0, 3) = 1.0f; @@ -409,7 +409,7 @@ int KalmanNav::predictState(float dt) } // C_nb update - C_nb.from_quaternion(q); + C_nb = q.to_dcm(); // euler update Vector<3> euler = C_nb.to_euler(); @@ -628,7 +628,7 @@ int KalmanNav::correctAtt() Vector<9> xCorrect = K * y; // check correciton is sane - for (size_t i = 0; i < xCorrect.getRows(); i++) { + for (size_t i = 0; i < xCorrect.get_size(); i++) { float val = xCorrect(i); if (isnan(val) || isinf(val)) { @@ -694,7 +694,7 @@ int KalmanNav::correctPos() Vector<9> xCorrect = K * y; // check correction is sane - for (size_t i = 0; i < xCorrect.getRows(); i++) { + for (size_t i = 0; i < xCorrect.get_size(); i++) { float val = xCorrect(i); if (!isfinite(val)) { diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 653d4b6b38..b4f7f2dfef 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -683,8 +683,7 @@ handle_message(mavlink_message_t *msg) /* Calculate Rotation Matrix */ math::Quaternion q(hil_state.attitude_quaternion); - math::Matrix<3,3> C_nb; - C_nb.from_quaternion(q); + math::Matrix<3,3> C_nb = q.to_dcm(); math::Vector<3> euler = C_nb.to_euler(); /* set rotation matrix */ diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index eea9438f77..9e2eeafa47 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -465,8 +465,6 @@ Sensors::Sensors() : /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "sensor task update")), - _board_rotation(), - _external_mag_rotation(), _mag_is_external(false) { @@ -920,7 +918,7 @@ Sensors::accel_poll(struct sensor_combined_s &raw) orb_copy(ORB_ID(sensor_accel), _accel_sub, &accel_report); - math::Vector<3> vect = {accel_report.x, accel_report.y, accel_report.z}; + math::Vector<3> vect(accel_report.x, accel_report.y, accel_report.z); vect = _board_rotation * vect; raw.accelerometer_m_s2[0] = vect(0); @@ -946,7 +944,7 @@ Sensors::gyro_poll(struct sensor_combined_s &raw) orb_copy(ORB_ID(sensor_gyro), _gyro_sub, &gyro_report); - math::Vector<3> vect = {gyro_report.x, gyro_report.y, gyro_report.z}; + math::Vector<3> vect(gyro_report.x, gyro_report.y, gyro_report.z); vect = _board_rotation * vect; raw.gyro_rad_s[0] = vect(0); diff --git a/src/systemcmds/tests/test_mathlib.cpp b/src/systemcmds/tests/test_mathlib.cpp index d2e1a93e3b..e654e0f81e 100644 --- a/src/systemcmds/tests/test_mathlib.cpp +++ b/src/systemcmds/tests/test_mathlib.cpp @@ -48,6 +48,8 @@ #include "tests.h" +#define TEST_OP(_title, _op) { unsigned int n = 60000; hrt_abstime t0, t1; t0 = hrt_absolute_time(); for (unsigned int j = 0; j < n; j++) { _op; }; t1 = hrt_absolute_time(); warnx(_title ": %.6fus", (double)(t1 - t0) / n); } + using namespace math; const char* formatResult(bool res) { @@ -58,60 +60,82 @@ int test_mathlib(int argc, char *argv[]) { warnx("testing mathlib"); - Matrix<3,3> m3; - m3.identity(); - Matrix<4,4> m4; - m4.identity(); - Vector<3> v3; - v3(0) = 1.0f; - v3(1) = 2.0f; - v3(2) = 3.0f; - Vector<4> v4; - v4(0) = 1.0f; - v4(1) = 2.0f; - v4(2) = 3.0f; - v4(3) = 4.0f; - Vector<3> vres3; - Matrix<3,3> mres3; - Matrix<4,4> mres4; + Vector<2> v2(1.0f, 2.0f); + Vector<3> v3(1.0f, 2.0f, 3.0f); + Vector<4> v4(1.0f, 2.0f, 3.0f, 4.0f); + Vector<10> v10; + v10.zero(); - unsigned int n = 60000; + float data2[2] = {1.0f, 2.0f}; + float data3[3] = {1.0f, 2.0f, 3.0f}; + float data4[4] = {1.0f, 2.0f, 3.0f, 4.0f}; + float data10[10]; - hrt_abstime t0, t1; - - t0 = hrt_absolute_time(); - for (unsigned int j = 0; j < n; j++) { - vres3 = m3 * v3; + { + Vector<2> v; + Vector<2> v1(1.0f, 2.0f); + Vector<2> v2(1.0f, -1.0f); + TEST_OP("Constructor Vector<2>()", Vector<2> v); + TEST_OP("Constructor Vector<2>(Vector<2>)", Vector<2> v(v2)); + TEST_OP("Constructor Vector<2>(float[])", Vector<2> v(data2)); + TEST_OP("Constructor Vector<2>(float, float)", Vector<2> v(1.0f, 2.0f)); + TEST_OP("Vector<2> = Vector<2>", v = v1); + TEST_OP("Vector<2> + Vector<2>", v + v1); + TEST_OP("Vector<2> - Vector<2>", v - v1); + TEST_OP("Vector<2> += Vector<2>", v += v1); + TEST_OP("Vector<2> -= Vector<2>", v -= v1); + TEST_OP("Vector<2> * Vector<2>", v * v1); + TEST_OP("Vector<2> %% Vector<2>", v1 % v2); } - t1 = hrt_absolute_time(); - warnx("Matrix3 * Vector3: %s %.6fus", formatResult(vres3 == v3), (double)(t1 - t0) / n); - t0 = hrt_absolute_time(); - for (unsigned int j = 0; j < n; j++) { - mres3 = m3 * m3; + { + Vector<3> v; + Vector<3> v1(1.0f, 2.0f, 0.0f); + Vector<3> v2(1.0f, -1.0f, 2.0f); + TEST_OP("Constructor Vector<3>()", Vector<3> v); + TEST_OP("Constructor Vector<3>(Vector<3>)", Vector<3> v(v3)); + TEST_OP("Constructor Vector<3>(float[])", Vector<3> v(data3)); + TEST_OP("Constructor Vector<3>(float, float, float)", Vector<3> v(1.0f, 2.0f, 3.0f)); + TEST_OP("Vector<3> = Vector<3>", v = v1); + TEST_OP("Vector<3> + Vector<3>", v + v1); + TEST_OP("Vector<3> - Vector<3>", v - v1); + TEST_OP("Vector<3> += Vector<3>", v += v1); + TEST_OP("Vector<3> -= Vector<3>", v -= v1); + TEST_OP("Vector<3> * float", v1 * 2.0f); + TEST_OP("Vector<3> / float", v1 / 2.0f); + TEST_OP("Vector<3> *= float", v1 *= 2.0f); + TEST_OP("Vector<3> /= float", v1 /= 2.0f); + TEST_OP("Vector<3> * Vector<3>", v * v1); + TEST_OP("Vector<3> %% Vector<3>", v1 % v2); + TEST_OP("Vector<3> length", v1.length()); + TEST_OP("Vector<3> length squared", v1.length_squared()); + TEST_OP("Vector<3> element read", volatile float a = v1(0)); + TEST_OP("Vector<3> element read direct", volatile float a = v1.data[0]); + TEST_OP("Vector<3> element write", v1(0) = 1.0f); + TEST_OP("Vector<3> element write direct", v1.data[0] = 1.0f); } - t1 = hrt_absolute_time(); - warnx("Matrix3 * Matrix3: %s %.6fus", formatResult(mres3 == m3), (double)(t1 - t0) / n); - t0 = hrt_absolute_time(); - for (unsigned int j = 0; j < n; j++) { - mres4 = m4 * m4; + { + Vector<4> v; + Vector<4> v1(1.0f, 2.0f, 0.0f, -1.0f); + Vector<4> v2(1.0f, -1.0f, 2.0f, 0.0f); + TEST_OP("Constructor Vector<4>()", Vector<4> v); + TEST_OP("Constructor Vector<4>(Vector<4>)", Vector<4> v(v4)); + TEST_OP("Constructor Vector<4>(float[])", Vector<4> v(data4)); + TEST_OP("Constructor Vector<4>(float, float, float, float)", Vector<4> v(1.0f, 2.0f, 3.0f, 4.0f)); + TEST_OP("Vector<4> = Vector<4>", v = v1); + TEST_OP("Vector<4> + Vector<4>", v + v1); + TEST_OP("Vector<4> - Vector<4>", v - v1); + TEST_OP("Vector<4> += Vector<4>", v += v1); + TEST_OP("Vector<4> -= Vector<4>", v -= v1); + TEST_OP("Vector<4> * Vector<4>", v * v1); } - t1 = hrt_absolute_time(); - warnx("Matrix4 * Matrix4: %s %.6fus", formatResult(mres4 == m4), (double)(t1 - t0) / n); - t0 = hrt_absolute_time(); - for (unsigned int j = 0; j < n; j++) { - mres3 = m3.transposed(); + { + TEST_OP("Constructor Vector<10>()", Vector<10> v); + TEST_OP("Constructor Vector<10>(Vector<10>)", Vector<10> v(v10)); + TEST_OP("Constructor Vector<10>(float[])", Vector<10> v(data10)); } - t1 = hrt_absolute_time(); - warnx("Matrix3 Transpose: %s %.6fus", formatResult(mres3 == m3), (double)(t1 - t0) / n); - t0 = hrt_absolute_time(); - for (unsigned int j = 0; j < n; j++) { - mres3 = m3.inversed(); - } - t1 = hrt_absolute_time(); - warnx("Matrix3 Invert: %s %.6fus", formatResult(mres3 == m3), (double)(t1 - t0) / n); return 0; } From 5c33aeeb430a984d0802f1af73063afca793a98a Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 25 Dec 2013 02:16:52 +0100 Subject: [PATCH 228/447] move landing slope calculations into own class --- .../fw_pos_control_l1_main.cpp | 38 +++----- .../fw_pos_control_l1/landingslope.cpp | 78 +++++++++++++++++ src/modules/fw_pos_control_l1/landingslope.h | 86 +++++++++++++++++++ src/modules/fw_pos_control_l1/module.mk | 3 +- 4 files changed, 180 insertions(+), 25 deletions(-) create mode 100644 src/modules/fw_pos_control_l1/landingslope.cpp create mode 100644 src/modules/fw_pos_control_l1/landingslope.h diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index eb84c49d1a..ae0f8c0ea7 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -87,6 +87,7 @@ #include #include +#include "landingslope.h" /** * L1 control app start / stop handling function @@ -168,6 +169,9 @@ private: bool land_motor_lim; bool land_onslope; + /* Landingslope object */ + Landingslope landingslope; + float flare_curve_alt_last; /* heading hold */ float target_bearing; @@ -307,11 +311,6 @@ private: */ void vehicle_setpoint_poll(); - /** - * Get Altitude on the landing glide slope - */ - float getLandingSlopeAbsoluteAltitude(float wp_distance, float wp_altitude, float landing_slope_angle_rad, float horizontal_displacement); - /** * Control position. */ @@ -536,6 +535,9 @@ FixedwingPositionControl::parameters_update() return 1; } + /* Update the landing slope */ + landingslope.update(math::radians(_parameters.land_slope_angle), _parameters.land_flare_alt_relative, _parameters.land_thrust_lim_horizontal_distance, _parameters.land_H1_virt); + return OK; } @@ -707,11 +709,6 @@ FixedwingPositionControl::calculate_gndspeed_undershoot(const math::Vector2f &cu } } -float FixedwingPositionControl::getLandingSlopeAbsoluteAltitude(float wp_distance, float wp_altitude, float landing_slope_angle_rad, float horizontal_displacement) -{ - return (wp_distance - horizontal_displacement) * tanf(landing_slope_angle_rad) + wp_altitude; //flare_relative_alt is negative -} - bool FixedwingPositionControl::control_position(const math::Vector2f ¤t_position, const math::Vector2f &ground_speed, const struct mission_item_triplet_s &mission_item_triplet) @@ -854,20 +851,13 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio float airspeed_land = 1.3f * _parameters.airspeed_min; float airspeed_approach = 1.3f * _parameters.airspeed_min; - float landing_slope_angle_rad = math::radians(_parameters.land_slope_angle); - float flare_relative_alt = _parameters.land_flare_alt_relative; - float motor_lim_horizontal_distance = _parameters.land_thrust_lim_horizontal_distance;//be generous here as we currently have to rely on the user input for the waypoint float L_wp_distance = get_distance_to_next_waypoint(prev_wp.getX(), prev_wp.getY(), curr_wp.getX(), curr_wp.getY()) * _parameters.land_slope_length; - float H1 = _parameters.land_H1_virt; - float H0 = flare_relative_alt + H1; - float d1 = flare_relative_alt/tanf(landing_slope_angle_rad); - float flare_constant = (H0 * d1)/flare_relative_alt;//-flare_length/(logf(H1/H0)); - float flare_length = - logf(H1/H0) * flare_constant;//d1+20.0f;//-logf(0.01f/flare_relative_alt); - float horizontal_slope_displacement = (flare_length - d1); - float L_altitude = getLandingSlopeAbsoluteAltitude(L_wp_distance, _mission_item_triplet.current.altitude, landing_slope_angle_rad, horizontal_slope_displacement); - float landing_slope_alt_desired = getLandingSlopeAbsoluteAltitude(wp_distance, _mission_item_triplet.current.altitude, landing_slope_angle_rad, horizontal_slope_displacement); + float L_altitude = landingslope.getLandingSlopeAbsoluteAltitude(L_wp_distance, _mission_item_triplet.current.altitude);//getLandingSlopeAbsoluteAltitude(L_wp_distance, _mission_item_triplet.current.altitude, landing_slope_angle_rad, horizontal_slope_displacement); + float landing_slope_alt_desired = landingslope.getLandingSlopeAbsoluteAltitude(wp_distance, _mission_item_triplet.current.altitude);//getLandingSlopeAbsoluteAltitude(wp_distance, _mission_item_triplet.current.altitude, landing_slope_angle_rad, horizontal_slope_displacement); - if ( (_global_pos.alt < _mission_item_triplet.current.altitude + flare_relative_alt) || land_noreturn_vertical) { //checking for land_noreturn to avoid unwanted climb out + + + if ( (_global_pos.alt < _mission_item_triplet.current.altitude + landingslope.flare_relative_alt()) || land_noreturn_vertical) { //checking for land_noreturn to avoid unwanted climb out /* land with minimal speed */ @@ -877,7 +867,7 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio /* kill the throttle if param requests it */ throttle_max = _parameters.throttle_max; - if (wp_distance < motor_lim_horizontal_distance || land_motor_lim) { + if (wp_distance < landingslope.motor_lim_horizontal_distance() || land_motor_lim) { throttle_max = math::min(throttle_max, _parameters.throttle_land_max); if (!land_motor_lim) { land_motor_lim = true; @@ -886,7 +876,7 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio } - float flare_curve_alt = _mission_item_triplet.current.altitude + H0 * expf(-math::max(0.0f, flare_length - wp_distance)/flare_constant) - H1; + float flare_curve_alt = _mission_item_triplet.current.altitude + landingslope.H0() * expf(-math::max(0.0f, landingslope.flare_length() - wp_distance)/landingslope.flare_constant()) - landingslope.H1_virt(); /* avoid climbout */ if (flare_curve_alt_last < flare_curve_alt && land_noreturn_vertical || land_stayonground) diff --git a/src/modules/fw_pos_control_l1/landingslope.cpp b/src/modules/fw_pos_control_l1/landingslope.cpp new file mode 100644 index 0000000000..ecf51b7401 --- /dev/null +++ b/src/modules/fw_pos_control_l1/landingslope.cpp @@ -0,0 +1,78 @@ +/**************************************************************************** + * + * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. + * Author: @author Lorenz Meier + * @author Thomas Gubler + * @author Julian Oes + * + * 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: landingslope.cpp + * + * @author: Thomas Gubler + */ + +#include "landingslope.h" + +#include +#include +#include +#include +#include + +void Landingslope::update(float landing_slope_angle_rad, + float flare_relative_alt, + float motor_lim_horizontal_distance, + float H1_virt) +{ + + _landing_slope_angle_rad = landing_slope_angle_rad; + _flare_relative_alt = flare_relative_alt; + _motor_lim_horizontal_distance = motor_lim_horizontal_distance; + _H1_virt = H1_virt; + + calculateSlopeValues(); +} + +void Landingslope::calculateSlopeValues() +{ + _H0 = _flare_relative_alt + _H1_virt; + _d1 = _flare_relative_alt/tanf(_landing_slope_angle_rad); + _flare_constant = (_H0 * _d1)/_flare_relative_alt; + _flare_length = - logf(_H1_virt/_H0) * _flare_constant; + _horizontal_slope_displacement = (_flare_length - _d1); +} + +float Landingslope::getLandingSlopeAbsoluteAltitude(float wp_distance, float wp_altitude) +{ + return (wp_distance - _horizontal_slope_displacement) * tanf(_landing_slope_angle_rad) + wp_altitude; //flare_relative_alt is negative +} + diff --git a/src/modules/fw_pos_control_l1/landingslope.h b/src/modules/fw_pos_control_l1/landingslope.h new file mode 100644 index 0000000000..f855b796fa --- /dev/null +++ b/src/modules/fw_pos_control_l1/landingslope.h @@ -0,0 +1,86 @@ +/**************************************************************************** + * + * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. + * Author: @author Lorenz Meier + * @author Thomas Gubler + * @author Julian Oes + * + * 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: landingslope.h + * + * @author: Thomas Gubler + */ + +#ifndef LANDINGSLOPE_H_ +#define LANDINGSLOPE_H_ + +class Landingslope +{ +private: + float _landing_slope_angle_rad; + float _flare_relative_alt; + float _motor_lim_horizontal_distance; + float _H1_virt; + float _H0; + float _d1; + float _flare_constant; + float _flare_length; + float _horizontal_slope_displacement; + + void calculateSlopeValues(); + +public: + Landingslope() {} + ~Landingslope() {} + + float getLandingSlopeAbsoluteAltitude(float wp_distance, float wp_altitude); + + void update(float landing_slope_angle_rad, + float flare_relative_alt, + float motor_lim_horizontal_distance, + float H1_virt); + + + inline float landing_slope_angle_rad() {return _landing_slope_angle_rad;} + inline float flare_relative_alt() {return _flare_relative_alt;} + inline float motor_lim_horizontal_distance() {return _motor_lim_horizontal_distance;} + inline float H1_virt() {return _H1_virt;} + inline float H0() {return _H0;} + inline float d1() {return _d1;} + inline float flare_constant() {return _flare_constant;} + inline float flare_length() {return _flare_length;} + inline float horizontal_slope_displacement() {return _horizontal_slope_displacement;} + +}; + + +#endif /* LANDINGSLOPE_H_ */ diff --git a/src/modules/fw_pos_control_l1/module.mk b/src/modules/fw_pos_control_l1/module.mk index b00b9aa5a5..cf419ec7ea 100644 --- a/src/modules/fw_pos_control_l1/module.mk +++ b/src/modules/fw_pos_control_l1/module.mk @@ -38,4 +38,5 @@ MODULE_COMMAND = fw_pos_control_l1 SRCS = fw_pos_control_l1_main.cpp \ - fw_pos_control_l1_params.c + fw_pos_control_l1_params.c \ + landingslope.cpp From 8ed193d1159dd64e3bd44668e75aac8b71fa3fa2 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 25 Dec 2013 13:03:36 +0400 Subject: [PATCH 229/447] mathlib: Matrix and Quaternion cleanup and bugfixes. Copyright updated. --- src/lib/mathlib/math/Matrix.hpp | 57 +++++++++------ src/lib/mathlib/math/Quaternion.hpp | 42 ++++++----- src/lib/mathlib/math/Vector.hpp | 20 +++--- .../att_pos_estimator_ekf/kalman_main.cpp | 2 +- src/systemcmds/tests/test_mathlib.cpp | 70 ++++++++++++------- 5 files changed, 115 insertions(+), 76 deletions(-) diff --git a/src/lib/mathlib/math/Matrix.hpp b/src/lib/mathlib/math/Matrix.hpp index 9896a16d05..584e5e81bf 100644 --- a/src/lib/mathlib/math/Matrix.hpp +++ b/src/lib/mathlib/math/Matrix.hpp @@ -1,8 +1,9 @@ /**************************************************************************** * * Copyright (C) 2013 PX4 Development Team. All rights reserved. - * Author: Will Perone - * Anton Babushkin + * Author: Anton Babushkin + * Pavel Kirienko + * Lorenz Meier * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -36,7 +37,7 @@ /** * @file Matrix.hpp * - * Generic Matrix + * Matrix class */ #ifndef MATRIX_HPP @@ -51,8 +52,6 @@ namespace math template class __EXPORT Matrix; -class __EXPORT Quaternion; - // MxN matrix with float elements template class __EXPORT MatrixBase { @@ -75,6 +74,14 @@ public: arm_mat = {M, N, &data[0][0]}; } + /** + * copyt ctor + */ + MatrixBase(const MatrixBase &m) { + arm_mat = {M, N, &data[0][0]}; + memcpy(data, m.data, sizeof(data)); + } + MatrixBase(const float *d) { arm_mat = {M, N, &data[0][0]}; memcpy(data, d, sizeof(data)); @@ -83,29 +90,35 @@ public: /** * access by index */ - inline float &operator ()(unsigned int row, unsigned int col) { + float &operator ()(const unsigned int row, const unsigned int col) { return data[row][col]; } /** * access by index */ - inline const float &operator ()(unsigned int row, unsigned int col) const { + float operator ()(const unsigned int row, const unsigned int col) const { return data[row][col]; } - unsigned int get_rows() { + /** + * get rows number + */ + unsigned int get_rows() const { return M; } - unsigned int get_cols() { + /** + * get columns number + */ + unsigned int get_cols() const { return N; } /** * test for equality */ - bool operator ==(const Matrix &m) { + bool operator ==(const Matrix &m) const { for (unsigned int i = 0; i < M; i++) for (unsigned int j = 0; j < N; j++) if (data[i][j] != m.data[i][j]) @@ -116,7 +129,7 @@ public: /** * test for inequality */ - bool operator !=(const Matrix &m) { + bool operator !=(const Matrix &m) const { for (unsigned int i = 0; i < M; i++) for (unsigned int j = 0; j < N; j++) if (data[i][j] != m.data[i][j]) @@ -209,7 +222,7 @@ public: for (unsigned int i = 0; i < M; i++) for (unsigned int j = 0; j < N; j++) data[i][j] /= num; - return *this; + return *static_cast*>(this); } /** @@ -272,11 +285,11 @@ class __EXPORT Matrix : public MatrixBase { public: using MatrixBase::operator *; - Matrix() : MatrixBase() { - } + Matrix() : MatrixBase() {} - Matrix(const float *d) : MatrixBase(d) { - } + Matrix(const Matrix &m) : MatrixBase(m) {} + + Matrix(const float *d) : MatrixBase(d) {} /** * set to value @@ -301,11 +314,11 @@ class __EXPORT Matrix<3, 3> : public MatrixBase<3, 3> { public: using MatrixBase<3, 3>::operator *; - Matrix() : MatrixBase<3, 3>() { - } + Matrix() : MatrixBase<3, 3>() {} - Matrix(const float *d) : MatrixBase<3, 3>(d) { - } + Matrix(const Matrix<3, 3> &m) : MatrixBase<3, 3>(m) {} + + Matrix(const float *d) : MatrixBase<3, 3>(d) {} /** * set to value @@ -348,6 +361,9 @@ public: data[2][2] = cr * cp; } + /** + * get euler angles from rotation matrix + */ Vector<3> to_euler(void) const { Vector<3> euler; euler.data[1] = asinf(-data[2][0]); @@ -364,6 +380,7 @@ public: euler.data[0] = atan2f(data[2][1], data[2][2]); euler.data[2] = atan2f(data[1][0], data[0][0]); } + return euler; } }; diff --git a/src/lib/mathlib/math/Quaternion.hpp b/src/lib/mathlib/math/Quaternion.hpp index c19dbd29c9..54d4e72ab9 100644 --- a/src/lib/mathlib/math/Quaternion.hpp +++ b/src/lib/mathlib/math/Quaternion.hpp @@ -1,8 +1,9 @@ /**************************************************************************** * * Copyright (C) 2013 PX4 Development Team. All rights reserved. - * Author: Will Perone - * Anton Babushkin + * Author: Anton Babushkin + * Pavel Kirienko + * Lorenz Meier * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -36,7 +37,7 @@ /** * @file Quaternion.hpp * - * Quaternion + * Quaternion class */ #ifndef QUATERNION_HPP @@ -50,31 +51,32 @@ namespace math { -template -class Matrix; - -class Quaternion : public Vector<4> { +class __EXPORT Quaternion : public Vector<4> { public: /** * trivial ctor */ - Quaternion() : Vector() { - } + Quaternion() : Vector<4>() {} + + /** + * copy ctor + */ + Quaternion(const Quaternion &q) : Vector<4>(q) {} + + /** + * casting from vector + */ + Quaternion(const Vector<4> &v) : Vector<4>(v) {} /** * setting ctor */ - Quaternion(const float a0, const float b0, const float c0, const float d0): Vector(a0, b0, c0, d0) { - } + Quaternion(const float d[4]) : Vector<4>(d) {} - Quaternion(const Vector<4> &v) : Vector(v) { - } - - Quaternion(const Quaternion &q) : Vector(q) { - } - - Quaternion(const float v[4]) : Vector(v) { - } + /** + * setting ctor + */ + Quaternion(const float a0, const float b0, const float c0, const float d0): Vector<4>(a0, b0, c0, d0) {} using Vector<4>::operator *; @@ -138,8 +140,10 @@ public: R.data[2][0] = 2.0f * (data[1] * data[3] - data[0] * data[2]); R.data[2][1] = 2.0f * (data[0] * data[1] + data[2] * data[3]); R.data[2][2] = aSq - bSq - cSq + dSq; + return R; } }; + } #endif // QUATERNION_HPP diff --git a/src/lib/mathlib/math/Vector.hpp b/src/lib/mathlib/math/Vector.hpp index d579ecf734..6bfcc96b60 100644 --- a/src/lib/mathlib/math/Vector.hpp +++ b/src/lib/mathlib/math/Vector.hpp @@ -68,6 +68,7 @@ public: /** * trivial ctor + * note that this ctor will not initialize elements */ VectorBase() { arm_col = {N, 1, &data[0]}; @@ -104,7 +105,7 @@ public: } /** - * get rows number + * get vector size */ unsigned int get_size() const { return N; @@ -113,7 +114,7 @@ public: /** * test for equality */ - bool operator ==(const Vector &v) { + bool operator ==(const Vector &v) const { for (unsigned int i = 0; i < N; i++) if (data[i] != v.data[i]) return false; @@ -123,7 +124,7 @@ public: /** * test for inequality */ - bool operator !=(const Vector &v) { + bool operator !=(const Vector &v) const { for (unsigned int i = 0; i < N; i++) if (data[i] != v.data[i]) return true; @@ -286,10 +287,9 @@ public: template class __EXPORT Vector : public VectorBase { public: - //using VectorBase::operator *; Vector() : VectorBase() {} - Vector(const Vector &v) : VectorBase(v) {} + Vector(const Vector &v) : VectorBase(v) {} Vector(const float d[N]) : VectorBase(d) {} @@ -307,8 +307,8 @@ class __EXPORT Vector<2> : public VectorBase<2> { public: Vector() : VectorBase<2>() {} - /* simple copy is 1.6 times faster than memcpy */ - Vector(const Vector &v) : VectorBase<2>() { + // simple copy is 1.6 times faster than memcpy + Vector(const Vector<2> &v) : VectorBase<2>() { data[0] = v.data[0]; data[1] = v.data[1]; } @@ -342,8 +342,8 @@ class __EXPORT Vector<3> : public VectorBase<3> { public: Vector() : VectorBase<3>() {} - /* simple copy is 1.6 times faster than memcpy */ - Vector(const Vector &v) : VectorBase<3>() { + // simple copy is 1.6 times faster than memcpy + Vector(const Vector<3> &v) : VectorBase<3>() { for (unsigned int i = 0; i < 3; i++) data[i] = v.data[i]; } @@ -382,7 +382,7 @@ class __EXPORT Vector<4> : public VectorBase<4> { public: Vector() : VectorBase() {} - Vector(const Vector &v) : VectorBase<4>() { + Vector(const Vector<4> &v) : VectorBase<4>() { for (unsigned int i = 0; i < 4; i++) data[i] = v.data[i]; } diff --git a/src/modules/att_pos_estimator_ekf/kalman_main.cpp b/src/modules/att_pos_estimator_ekf/kalman_main.cpp index 70ba628f33..3d20d4d2d6 100644 --- a/src/modules/att_pos_estimator_ekf/kalman_main.cpp +++ b/src/modules/att_pos_estimator_ekf/kalman_main.cpp @@ -107,7 +107,7 @@ int att_pos_estimator_ekf_main(int argc, char *argv[]) daemon_task = task_spawn_cmd("att_pos_estimator_ekf", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 30, - 8096, + 8192, kalman_demo_thread_main, (argv) ? (const char **)&argv[2] : (const char **)NULL); exit(0); diff --git a/src/systemcmds/tests/test_mathlib.cpp b/src/systemcmds/tests/test_mathlib.cpp index e654e0f81e..693a208ba7 100644 --- a/src/systemcmds/tests/test_mathlib.cpp +++ b/src/systemcmds/tests/test_mathlib.cpp @@ -60,25 +60,15 @@ int test_mathlib(int argc, char *argv[]) { warnx("testing mathlib"); - Vector<2> v2(1.0f, 2.0f); - Vector<3> v3(1.0f, 2.0f, 3.0f); - Vector<4> v4(1.0f, 2.0f, 3.0f, 4.0f); - Vector<10> v10; - v10.zero(); - - float data2[2] = {1.0f, 2.0f}; - float data3[3] = {1.0f, 2.0f, 3.0f}; - float data4[4] = {1.0f, 2.0f, 3.0f, 4.0f}; - float data10[10]; - { Vector<2> v; Vector<2> v1(1.0f, 2.0f); Vector<2> v2(1.0f, -1.0f); - TEST_OP("Constructor Vector<2>()", Vector<2> v); - TEST_OP("Constructor Vector<2>(Vector<2>)", Vector<2> v(v2)); - TEST_OP("Constructor Vector<2>(float[])", Vector<2> v(data2)); - TEST_OP("Constructor Vector<2>(float, float)", Vector<2> v(1.0f, 2.0f)); + float data[2] = {1.0f, 2.0f}; + TEST_OP("Constructor Vector<2>()", Vector<2> v3); + TEST_OP("Constructor Vector<2>(Vector<2>)", Vector<2> v3(v1)); + TEST_OP("Constructor Vector<2>(float[])", Vector<2> v3(data)); + TEST_OP("Constructor Vector<2>(float, float)", Vector<2> v3(1.0f, 2.0f)); TEST_OP("Vector<2> = Vector<2>", v = v1); TEST_OP("Vector<2> + Vector<2>", v + v1); TEST_OP("Vector<2> - Vector<2>", v - v1); @@ -92,10 +82,11 @@ int test_mathlib(int argc, char *argv[]) Vector<3> v; Vector<3> v1(1.0f, 2.0f, 0.0f); Vector<3> v2(1.0f, -1.0f, 2.0f); - TEST_OP("Constructor Vector<3>()", Vector<3> v); - TEST_OP("Constructor Vector<3>(Vector<3>)", Vector<3> v(v3)); - TEST_OP("Constructor Vector<3>(float[])", Vector<3> v(data3)); - TEST_OP("Constructor Vector<3>(float, float, float)", Vector<3> v(1.0f, 2.0f, 3.0f)); + float data[3] = {1.0f, 2.0f, 3.0f}; + TEST_OP("Constructor Vector<3>()", Vector<3> v3); + TEST_OP("Constructor Vector<3>(Vector<3>)", Vector<3> v3(v1)); + TEST_OP("Constructor Vector<3>(float[])", Vector<3> v3(data)); + TEST_OP("Constructor Vector<3>(float, float, float)", Vector<3> v3(1.0f, 2.0f, 3.0f)); TEST_OP("Vector<3> = Vector<3>", v = v1); TEST_OP("Vector<3> + Vector<3>", v + v1); TEST_OP("Vector<3> - Vector<3>", v - v1); @@ -119,10 +110,11 @@ int test_mathlib(int argc, char *argv[]) Vector<4> v; Vector<4> v1(1.0f, 2.0f, 0.0f, -1.0f); Vector<4> v2(1.0f, -1.0f, 2.0f, 0.0f); - TEST_OP("Constructor Vector<4>()", Vector<4> v); - TEST_OP("Constructor Vector<4>(Vector<4>)", Vector<4> v(v4)); - TEST_OP("Constructor Vector<4>(float[])", Vector<4> v(data4)); - TEST_OP("Constructor Vector<4>(float, float, float, float)", Vector<4> v(1.0f, 2.0f, 3.0f, 4.0f)); + float data[4] = {1.0f, 2.0f, 3.0f, 4.0f}; + TEST_OP("Constructor Vector<4>()", Vector<4> v3); + TEST_OP("Constructor Vector<4>(Vector<4>)", Vector<4> v3(v1)); + TEST_OP("Constructor Vector<4>(float[])", Vector<4> v3(data)); + TEST_OP("Constructor Vector<4>(float, float, float, float)", Vector<4> v3(1.0f, 2.0f, 3.0f, 4.0f)); TEST_OP("Vector<4> = Vector<4>", v = v1); TEST_OP("Vector<4> + Vector<4>", v + v1); TEST_OP("Vector<4> - Vector<4>", v - v1); @@ -132,9 +124,35 @@ int test_mathlib(int argc, char *argv[]) } { - TEST_OP("Constructor Vector<10>()", Vector<10> v); - TEST_OP("Constructor Vector<10>(Vector<10>)", Vector<10> v(v10)); - TEST_OP("Constructor Vector<10>(float[])", Vector<10> v(data10)); + Vector<10> v1; + v1.zero(); + float data[10]; + TEST_OP("Constructor Vector<10>()", Vector<10> v3); + TEST_OP("Constructor Vector<10>(Vector<10>)", Vector<10> v3(v1)); + TEST_OP("Constructor Vector<10>(float[])", Vector<10> v3(data)); + } + + { + Matrix<3, 3> m1; + m1.identity(); + Matrix<3, 3> m2; + m2.identity(); + Vector<3> v1(1.0f, 2.0f, 0.0f); + TEST_OP("Matrix<3, 3> * Vector<3>", m1 * v1); + TEST_OP("Matrix<3, 3> + Matrix<3, 3>", m1 + m2); + TEST_OP("Matrix<3, 3> * Matrix<3, 3>", m1 * m2); + } + + { + Matrix<10, 10> m1; + m1.identity(); + Matrix<10, 10> m2; + m2.identity(); + Vector<10> v1; + v1.zero(); + TEST_OP("Matrix<10, 10> * Vector<10>", m1 * v1); + TEST_OP("Matrix<10, 10> + Matrix<10, 10>", m1 + m2); + TEST_OP("Matrix<10, 10> * Matrix<10, 10>", m1 * m2); } return 0; From bbeb97df6737f89291db4bb97ebf4c821edd9114 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 25 Dec 2013 13:04:52 +0400 Subject: [PATCH 230/447] mathlib: code style fixed --- src/lib/mathlib/math/Matrix.hpp | 133 ++++++++++++++++------------ src/lib/mathlib/math/Quaternion.hpp | 29 +++--- src/lib/mathlib/math/Vector.hpp | 133 +++++++++++++++++----------- 3 files changed, 175 insertions(+), 120 deletions(-) diff --git a/src/lib/mathlib/math/Matrix.hpp b/src/lib/mathlib/math/Matrix.hpp index 584e5e81bf..2a5eac79f0 100644 --- a/src/lib/mathlib/math/Matrix.hpp +++ b/src/lib/mathlib/math/Matrix.hpp @@ -54,7 +54,8 @@ class __EXPORT Matrix; // MxN matrix with float elements template -class __EXPORT MatrixBase { +class __EXPORT MatrixBase +{ public: /** * matrix data[row][col] @@ -90,14 +91,14 @@ public: /** * access by index */ - float &operator ()(const unsigned int row, const unsigned int col) { + float &operator()(const unsigned int row, const unsigned int col) { return data[row][col]; } /** * access by index */ - float operator ()(const unsigned int row, const unsigned int col) const { + float operator()(const unsigned int row, const unsigned int col) const { return data[row][col]; } @@ -119,10 +120,11 @@ public: * test for equality */ bool operator ==(const Matrix &m) const { - for (unsigned int i = 0; i < M; i++) - for (unsigned int j = 0; j < N; j++) - if (data[i][j] != m.data[i][j]) - return false; + for (unsigned int i = 0; i < M; i++) + for (unsigned int j = 0; j < N; j++) + if (data[i][j] != m.data[i][j]) + return false; + return true; } @@ -130,10 +132,11 @@ public: * test for inequality */ bool operator !=(const Matrix &m) const { - for (unsigned int i = 0; i < M; i++) - for (unsigned int j = 0; j < N; j++) - if (data[i][j] != m.data[i][j]) - return true; + for (unsigned int i = 0; i < M; i++) + for (unsigned int j = 0; j < N; j++) + if (data[i][j] != m.data[i][j]) + return true; + return false; } @@ -150,9 +153,11 @@ public: */ Matrix operator -(void) const { Matrix res; - for (unsigned int i = 0; i < N; i++) - for (unsigned int j = 0; j < M; j++) - res.data[i][j] = -data[i][j]; + + for (unsigned int i = 0; i < N; i++) + for (unsigned int j = 0; j < M; j++) + res.data[i][j] = -data[i][j]; + return res; } @@ -161,16 +166,19 @@ public: */ Matrix operator +(const Matrix &m) const { Matrix res; - for (unsigned int i = 0; i < N; i++) - for (unsigned int j = 0; j < M; j++) - res.data[i][j] = data[i][j] + m.data[i][j]; + + for (unsigned int i = 0; i < N; i++) + for (unsigned int j = 0; j < M; j++) + res.data[i][j] = data[i][j] + m.data[i][j]; + return res; } Matrix &operator +=(const Matrix &m) { - for (unsigned int i = 0; i < N; i++) - for (unsigned int j = 0; j < M; j++) - data[i][j] += m.data[i][j]; + for (unsigned int i = 0; i < N; i++) + for (unsigned int j = 0; j < M; j++) + data[i][j] += m.data[i][j]; + return *static_cast*>(this); } @@ -179,16 +187,19 @@ public: */ Matrix operator -(const Matrix &m) const { Matrix res; - for (unsigned int i = 0; i < M; i++) - for (unsigned int j = 0; j < N; j++) - res.data[i][j] = data[i][j] - m.data[i][j]; + + for (unsigned int i = 0; i < M; i++) + for (unsigned int j = 0; j < N; j++) + res.data[i][j] = data[i][j] - m.data[i][j]; + return res; } Matrix &operator -=(const Matrix &m) { - for (unsigned int i = 0; i < N; i++) - for (unsigned int j = 0; j < M; j++) - data[i][j] -= m.data[i][j]; + for (unsigned int i = 0; i < N; i++) + for (unsigned int j = 0; j < M; j++) + data[i][j] -= m.data[i][j]; + return *static_cast*>(this); } @@ -197,31 +208,37 @@ public: */ Matrix operator *(const float num) const { Matrix res; - for (unsigned int i = 0; i < M; i++) - for (unsigned int j = 0; j < N; j++) - res.data[i][j] = data[i][j] * num; + + for (unsigned int i = 0; i < M; i++) + for (unsigned int j = 0; j < N; j++) + res.data[i][j] = data[i][j] * num; + return res; } Matrix &operator *=(const float num) { - for (unsigned int i = 0; i < M; i++) - for (unsigned int j = 0; j < N; j++) - data[i][j] *= num; + for (unsigned int i = 0; i < M; i++) + for (unsigned int j = 0; j < N; j++) + data[i][j] *= num; + return *static_cast*>(this); } Matrix operator /(const float num) const { Matrix res; - for (unsigned int i = 0; i < M; i++) - for (unsigned int j = 0; j < N; j++) - res[i][j] = data[i][j] / num; + + for (unsigned int i = 0; i < M; i++) + for (unsigned int j = 0; j < N; j++) + res[i][j] = data[i][j] / num; + return res; } Matrix &operator /=(const float num) { - for (unsigned int i = 0; i < M; i++) - for (unsigned int j = 0; j < N; j++) - data[i][j] /= num; + for (unsigned int i = 0; i < M; i++) + for (unsigned int j = 0; j < N; j++) + data[i][j] /= num; + return *static_cast*>(this); } @@ -239,18 +256,18 @@ public: * transpose the matrix */ Matrix transposed(void) const { - Matrix res; - arm_mat_trans_f32(&this->arm_mat, &res.arm_mat); - return res; + Matrix res; + arm_mat_trans_f32(&this->arm_mat, &res.arm_mat); + return res; } /** * invert the matrix */ Matrix inversed(void) const { - Matrix res; - arm_mat_inverse_f32(&this->arm_mat, &res.arm_mat); - return res; + Matrix res; + arm_mat_inverse_f32(&this->arm_mat, &res.arm_mat); + return res; } /** @@ -266,22 +283,26 @@ public: void identity(void) { memset(data, 0, sizeof(data)); unsigned int n = (M < N) ? M : N; + for (unsigned int i = 0; i < n; i++) data[i][i] = 1; } void print(void) { - for (unsigned int i = 0; i < M; i++) { + for (unsigned int i = 0; i < M; i++) { printf("[ "); - for (unsigned int j = 0; j < N; j++) + + for (unsigned int j = 0; j < N; j++) printf("%.3f\t", data[i][j]); + printf(" ]\n"); } } }; template -class __EXPORT Matrix : public MatrixBase { +class __EXPORT Matrix : public MatrixBase +{ public: using MatrixBase::operator *; @@ -303,14 +324,15 @@ public: * multiplication by a vector */ Vector operator *(const Vector &v) const { - Vector res; - arm_mat_mult_f32(&this->arm_mat, &v.arm_col, &res.arm_col); - return res; + Vector res; + arm_mat_mult_f32(&this->arm_mat, &v.arm_col, &res.arm_col); + return res; } }; template <> -class __EXPORT Matrix<3, 3> : public MatrixBase<3, 3> { +class __EXPORT Matrix<3, 3> : public MatrixBase<3, 3> +{ public: using MatrixBase<3, 3>::operator *; @@ -332,10 +354,10 @@ public: * multiplication by a vector */ Vector<3> operator *(const Vector<3> &v) const { - Vector<3> res(data[0][0] * v.data[0] + data[0][1] * v.data[1] + data[0][2] * v.data[2], - data[1][0] * v.data[0] + data[1][1] * v.data[1] + data[1][2] * v.data[2], - data[2][0] * v.data[0] + data[2][1] * v.data[1] + data[2][2] * v.data[2]); - return res; + Vector<3> res(data[0][0] * v.data[0] + data[0][1] * v.data[1] + data[0][2] * v.data[2], + data[1][0] * v.data[0] + data[1][1] * v.data[1] + data[1][2] * v.data[2], + data[2][0] * v.data[0] + data[2][1] * v.data[1] + data[2][2] * v.data[2]); + return res; } /** @@ -380,6 +402,7 @@ public: euler.data[0] = atan2f(data[2][1], data[2][2]); euler.data[2] = atan2f(data[1][0], data[0][0]); } + return euler; } }; diff --git a/src/lib/mathlib/math/Quaternion.hpp b/src/lib/mathlib/math/Quaternion.hpp index 54d4e72ab9..77e1c1f1c6 100644 --- a/src/lib/mathlib/math/Quaternion.hpp +++ b/src/lib/mathlib/math/Quaternion.hpp @@ -51,7 +51,8 @@ namespace math { -class __EXPORT Quaternion : public Vector<4> { +class __EXPORT Quaternion : public Vector<4> +{ public: /** * trivial ctor @@ -85,25 +86,25 @@ public: */ const Quaternion operator *(const Quaternion &q) const { return Quaternion( - data[0] * q.data[0] - data[1] * q.data[1] - data[2] * q.data[2] - data[3] * q.data[3], - data[0] * q.data[1] + data[1] * q.data[0] + data[2] * q.data[3] - data[3] * q.data[2], - data[0] * q.data[2] - data[1] * q.data[3] + data[2] * q.data[0] + data[3] * q.data[1], - data[0] * q.data[3] + data[1] * q.data[2] - data[2] * q.data[1] + data[3] * q.data[0]); + data[0] * q.data[0] - data[1] * q.data[1] - data[2] * q.data[2] - data[3] * q.data[3], + data[0] * q.data[1] + data[1] * q.data[0] + data[2] * q.data[3] - data[3] * q.data[2], + data[0] * q.data[2] - data[1] * q.data[3] + data[2] * q.data[0] + data[3] * q.data[1], + data[0] * q.data[3] + data[1] * q.data[2] - data[2] * q.data[1] + data[3] * q.data[0]); } /** * derivative */ const Quaternion derivative(const Vector<3> &w) { - float dataQ[] = { - data[0], -data[1], -data[2], -data[3], - data[1], data[0], -data[3], data[2], - data[2], data[3], data[0], -data[1], - data[3], -data[2], data[1], data[0] - }; - Matrix<4,4> Q(dataQ); - Vector<4> v(0.0f, w.data[0], w.data[1], w.data[2]); - return Q * v * 0.5f; + float dataQ[] = { + data[0], -data[1], -data[2], -data[3], + data[1], data[0], -data[3], data[2], + data[2], data[3], data[0], -data[1], + data[3], -data[2], data[1], data[0] + }; + Matrix<4, 4> Q(dataQ); + Vector<4> v(0.0f, w.data[0], w.data[1], w.data[2]); + return Q * v * 0.5f; } /** diff --git a/src/lib/mathlib/math/Vector.hpp b/src/lib/mathlib/math/Vector.hpp index 6bfcc96b60..d75a05c980 100644 --- a/src/lib/mathlib/math/Vector.hpp +++ b/src/lib/mathlib/math/Vector.hpp @@ -54,7 +54,8 @@ template class __EXPORT Vector; template -class __EXPORT VectorBase { +class __EXPORT VectorBase +{ public: /** * vector data @@ -93,14 +94,14 @@ public: /** * access to elements by index */ - float &operator ()(const unsigned int i) { + float &operator()(const unsigned int i) { return data[i]; } /** * access to elements by index */ - float operator ()(const unsigned int i) const { + float operator()(const unsigned int i) const { return data[i]; } @@ -115,20 +116,22 @@ public: * test for equality */ bool operator ==(const Vector &v) const { - for (unsigned int i = 0; i < N; i++) - if (data[i] != v.data[i]) - return false; - return true; + for (unsigned int i = 0; i < N; i++) + if (data[i] != v.data[i]) + return false; + + return true; } /** * test for inequality */ bool operator !=(const Vector &v) const { - for (unsigned int i = 0; i < N; i++) - if (data[i] != v.data[i]) - return true; - return false; + for (unsigned int i = 0; i < N; i++) + if (data[i] != v.data[i]) + return true; + + return false; } /** @@ -144,9 +147,11 @@ public: */ const Vector operator -(void) const { Vector res; - for (unsigned int i = 0; i < N; i++) - res.data[i] = -data[i]; - return res; + + for (unsigned int i = 0; i < N; i++) + res.data[i] = -data[i]; + + return res; } /** @@ -154,9 +159,11 @@ public: */ const Vector operator +(const Vector &v) const { Vector res; - for (unsigned int i = 0; i < N; i++) - res.data[i] = data[i] + v.data[i]; - return res; + + for (unsigned int i = 0; i < N; i++) + res.data[i] = data[i] + v.data[i]; + + return res; } /** @@ -164,9 +171,11 @@ public: */ const Vector operator -(const Vector &v) const { Vector res; - for (unsigned int i = 0; i < N; i++) - res.data[i] = data[i] - v.data[i]; - return res; + + for (unsigned int i = 0; i < N; i++) + res.data[i] = data[i] - v.data[i]; + + return res; } /** @@ -174,8 +183,10 @@ public: */ const Vector operator *(const float num) const { Vector res; - for (unsigned int i = 0; i < N; i++) - res.data[i] = data[i] * num; + + for (unsigned int i = 0; i < N; i++) + res.data[i] = data[i] * num; + return res; } @@ -184,8 +195,10 @@ public: */ const Vector operator /(const float num) const { Vector res; - for (unsigned int i = 0; i < N; i++) - res.data[i] = data[i] / num; + + for (unsigned int i = 0; i < N; i++) + res.data[i] = data[i] / num; + return res; } @@ -193,8 +206,9 @@ public: * addition */ const Vector &operator +=(const Vector &v) { - for (unsigned int i = 0; i < N; i++) - data[i] += v.data[i]; + for (unsigned int i = 0; i < N; i++) + data[i] += v.data[i]; + return *static_cast*>(this); } @@ -202,8 +216,9 @@ public: * subtraction */ const Vector &operator -=(const Vector &v) { - for (unsigned int i = 0; i < N; i++) - data[i] -= v.data[i]; + for (unsigned int i = 0; i < N; i++) + data[i] -= v.data[i]; + return *static_cast*>(this); } @@ -211,8 +226,9 @@ public: * uniform scaling */ const Vector &operator *=(const float num) { - for (unsigned int i = 0; i < N; i++) - data[i] *= num; + for (unsigned int i = 0; i < N; i++) + data[i] *= num; + return *static_cast*>(this); } @@ -220,8 +236,9 @@ public: * uniform scaling */ const Vector &operator /=(const float num) { - for (unsigned int i = 0; i < N; i++) - data[i] /= num; + for (unsigned int i = 0; i < N; i++) + data[i] /= num; + return *static_cast*>(this); } @@ -230,9 +247,11 @@ public: */ float operator *(const Vector &v) const { float res = 0.0f; - for (unsigned int i = 0; i < N; i++) - res += data[i] * v.data[i]; - return res; + + for (unsigned int i = 0; i < N; i++) + res += data[i] * v.data[i]; + + return res; } /** @@ -240,9 +259,11 @@ public: */ float length_squared() const { float res = 0.0f; - for (unsigned int i = 0; i < N; i++) - res += data[i] * data[i]; - return res; + + for (unsigned int i = 0; i < N; i++) + res += data[i] * data[i]; + + return res; } /** @@ -250,9 +271,11 @@ public: */ float length() const { float res = 0.0f; - for (unsigned int i = 0; i < N; i++) - res += data[i] * data[i]; - return sqrtf(res); + + for (unsigned int i = 0; i < N; i++) + res += data[i] * data[i]; + + return sqrtf(res); } /** @@ -278,14 +301,17 @@ public: void print(void) { printf("[ "); - for (unsigned int i = 0; i < N; i++) + + for (unsigned int i = 0; i < N; i++) printf("%.3f\t", data[i]); + printf("]\n"); } }; template -class __EXPORT Vector : public VectorBase { +class __EXPORT Vector : public VectorBase +{ public: Vector() : VectorBase() {} @@ -303,7 +329,8 @@ public: }; template <> -class __EXPORT Vector<2> : public VectorBase<2> { +class __EXPORT Vector<2> : public VectorBase<2> +{ public: Vector() : VectorBase<2>() {} @@ -333,12 +360,13 @@ public: } float operator %(const Vector<2> &v) const { - return data[0] * v.data[1] - data[1] * v.data[0]; + return data[0] * v.data[1] - data[1] * v.data[0]; } }; template <> -class __EXPORT Vector<3> : public VectorBase<3> { +class __EXPORT Vector<3> : public VectorBase<3> +{ public: Vector() : VectorBase<3>() {} @@ -365,20 +393,22 @@ public: const Vector<3> &operator =(const Vector<3> &v) { for (unsigned int i = 0; i < 3; i++) data[i] = v.data[i]; + return *this; } Vector<3> operator %(const Vector<3> &v) const { return Vector<3>( - data[1] * v.data[2] - data[2] * v.data[1], - data[2] * v.data[0] - data[0] * v.data[2], - data[0] * v.data[1] - data[1] * v.data[0] - ); + data[1] * v.data[2] - data[2] * v.data[1], + data[2] * v.data[0] - data[0] * v.data[2], + data[0] * v.data[1] - data[1] * v.data[0] + ); } }; template <> -class __EXPORT Vector<4> : public VectorBase<4> { +class __EXPORT Vector<4> : public VectorBase<4> +{ public: Vector() : VectorBase() {} @@ -405,6 +435,7 @@ public: const Vector<4> &operator =(const Vector<4> &v) { for (unsigned int i = 0; i < 4; i++) data[i] = v.data[i]; + return *this; } }; From 8b0125fc3fe878d390d9ebdcfceade9ae191681b Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 25 Dec 2013 10:51:13 +0100 Subject: [PATCH 231/447] fw landing: move more functionality to the landingslope class --- .../fw_pos_control_l1/fw_pos_control_l1_main.cpp | 11 +++++------ src/modules/fw_pos_control_l1/landingslope.cpp | 6 ++++++ src/modules/fw_pos_control_l1/landingslope.h | 1 + 3 files changed, 12 insertions(+), 6 deletions(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index ae0f8c0ea7..2be25517d4 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -845,8 +845,7 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio /* apply minimum pitch (flare) and limit roll if close to touch down, altitude error is negative (going down) */ // XXX this could make a great param - float flare_angle_rad = -math::radians(5.0f);//math::radians(mission_item_triplet.current.param1) - float land_pitch_min = math::radians(5.0f); + float flare_pitch_angle_rad = -math::radians(5.0f);//math::radians(mission_item_triplet.current.param1) float throttle_land = _parameters.throttle_min + (_parameters.throttle_max - _parameters.throttle_min) * 0.1f; float airspeed_land = 1.3f * _parameters.airspeed_min; float airspeed_approach = 1.3f * _parameters.airspeed_min; @@ -876,7 +875,7 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio } - float flare_curve_alt = _mission_item_triplet.current.altitude + landingslope.H0() * expf(-math::max(0.0f, landingslope.flare_length() - wp_distance)/landingslope.flare_constant()) - landingslope.H1_virt(); + float flare_curve_alt = landingslope.getFlarceCurveAltitude(wp_distance, _mission_item_triplet.current.altitude); /* avoid climbout */ if (flare_curve_alt_last < flare_curve_alt && land_noreturn_vertical || land_stayonground) @@ -887,9 +886,9 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, flare_curve_alt, calculate_target_airspeed(airspeed_land), _airspeed.indicated_airspeed_m_s, eas2tas, - false, flare_angle_rad, + false, flare_pitch_angle_rad, 0.0f, throttle_max, throttle_land, - flare_angle_rad, math::radians(15.0f)); + flare_pitch_angle_rad, math::radians(15.0f)); if (!land_noreturn_vertical) { mavlink_log_info(_mavlink_fd, "#audio: Landing, flaring"); @@ -904,7 +903,7 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio /* minimize speed to approach speed, stay on landing slope */ _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, landing_slope_alt_desired, calculate_target_airspeed(airspeed_approach), _airspeed.indicated_airspeed_m_s, eas2tas, - false, flare_angle_rad, + false, flare_pitch_angle_rad, _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max)); //warnx("Landing: stay on slope, alt_desired: %.1f (wp_distance: %.1f), calculate_target_airspeed(airspeed_land) %.1f, horizontal_slope_displacement %.1f, d1 %.1f, flare_length %.1f", landing_slope_alt_desired, wp_distance, calculate_target_airspeed(airspeed_land), horizontal_slope_displacement, d1, flare_length); diff --git a/src/modules/fw_pos_control_l1/landingslope.cpp b/src/modules/fw_pos_control_l1/landingslope.cpp index ecf51b7401..a2d8525b9d 100644 --- a/src/modules/fw_pos_control_l1/landingslope.cpp +++ b/src/modules/fw_pos_control_l1/landingslope.cpp @@ -47,6 +47,7 @@ #include #include #include +#include void Landingslope::update(float landing_slope_angle_rad, float flare_relative_alt, @@ -76,3 +77,8 @@ float Landingslope::getLandingSlopeAbsoluteAltitude(float wp_distance, float wp_ return (wp_distance - _horizontal_slope_displacement) * tanf(_landing_slope_angle_rad) + wp_altitude; //flare_relative_alt is negative } +float Landingslope::getFlarceCurveAltitude(float wp_distance, float wp_altitude) +{ + return wp_altitude + _H0 * expf(-math::max(0.0f, _flare_length - wp_distance)/_flare_constant) - _H1_virt; +} + diff --git a/src/modules/fw_pos_control_l1/landingslope.h b/src/modules/fw_pos_control_l1/landingslope.h index f855b796fa..b77252e6e1 100644 --- a/src/modules/fw_pos_control_l1/landingslope.h +++ b/src/modules/fw_pos_control_l1/landingslope.h @@ -63,6 +63,7 @@ public: ~Landingslope() {} float getLandingSlopeAbsoluteAltitude(float wp_distance, float wp_altitude); + float getFlarceCurveAltitude(float wp_distance, float wp_altitude); void update(float landing_slope_angle_rad, float flare_relative_alt, From b10fa3d0476ce6af977fe7e54bc361c44c27e9c4 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 25 Dec 2013 11:23:58 +0100 Subject: [PATCH 232/447] Waypoints/Navigator: Use two different dataman storage places, keep old waypoints until all new ones are written --- src/modules/dataman/dataman.c | 3 +- src/modules/dataman/dataman.h | 6 ++-- src/modules/mavlink/waypoints.c | 34 ++++++++++++++++----- src/modules/mavlink/waypoints.h | 1 + src/modules/navigator/navigator_main.cpp | 1 + src/modules/navigator/navigator_mission.cpp | 29 ++++++++++++++++-- src/modules/navigator/navigator_mission.h | 2 ++ src/modules/uORB/topics/mission.h | 1 + 8 files changed, 64 insertions(+), 13 deletions(-) diff --git a/src/modules/dataman/dataman.c b/src/modules/dataman/dataman.c index 14112fc0d4..dc2d6c312a 100644 --- a/src/modules/dataman/dataman.c +++ b/src/modules/dataman/dataman.c @@ -111,7 +111,8 @@ static unsigned g_func_counts[dm_number_of_funcs]; static const unsigned g_per_item_max_index[DM_KEY_NUM_KEYS] = { DM_KEY_SAFE_POINTS_MAX, DM_KEY_FENCE_POINTS_MAX, - DM_KEY_WAYPOINTS_OFFBOARD_MAX, + DM_KEY_WAYPOINTS_OFFBOARD_0_MAX, + DM_KEY_WAYPOINTS_OFFBOARD_1_MAX, DM_KEY_WAYPOINTS_ONBOARD_MAX }; diff --git a/src/modules/dataman/dataman.h b/src/modules/dataman/dataman.h index 2a781405aa..a70638cccc 100644 --- a/src/modules/dataman/dataman.h +++ b/src/modules/dataman/dataman.h @@ -50,7 +50,8 @@ extern "C" { typedef enum { DM_KEY_SAFE_POINTS = 0, /* Safe points coordinates, safe point 0 is home point */ DM_KEY_FENCE_POINTS, /* Fence vertex coordinates */ - DM_KEY_WAYPOINTS_OFFBOARD, /* Mission way point coordinates sent over mavlink */ + DM_KEY_WAYPOINTS_OFFBOARD_0, /* Mission way point coordinates sent over mavlink */ + DM_KEY_WAYPOINTS_OFFBOARD_1, /* (alernate between 0 and 1) */ DM_KEY_WAYPOINTS_ONBOARD, /* Mission way point coordinates generated onboard */ DM_KEY_NUM_KEYS /* Total number of item types defined */ } dm_item_t; @@ -59,7 +60,8 @@ extern "C" { enum { DM_KEY_SAFE_POINTS_MAX = 8, DM_KEY_FENCE_POINTS_MAX = GEOFENCE_MAX_VERTICES, - DM_KEY_WAYPOINTS_OFFBOARD_MAX = NUM_MISSIONS_SUPPORTED, + DM_KEY_WAYPOINTS_OFFBOARD_0_MAX = NUM_MISSIONS_SUPPORTED, + DM_KEY_WAYPOINTS_OFFBOARD_1_MAX = NUM_MISSIONS_SUPPORTED, DM_KEY_WAYPOINTS_ONBOARD_MAX = NUM_MISSIONS_SUPPORTED }; diff --git a/src/modules/mavlink/waypoints.c b/src/modules/mavlink/waypoints.c index 0998cd5eb3..45e8914343 100644 --- a/src/modules/mavlink/waypoints.c +++ b/src/modules/mavlink/waypoints.c @@ -169,6 +169,8 @@ void mavlink_wpm_init(mavlink_wpm_storage *state) state->timestamp_lastaction = 0; // state->timestamp_last_send_setpoint = 0; state->timeout = MAVLINK_WPM_PROTOCOL_TIMEOUT_DEFAULT; + + state->current_dataman_id = 0; // state->delay_setpoint = MAVLINK_WPM_SETPOINT_DELAY_DEFAULT; // state->idle = false; ///< indicates if the system is following the waypoints or is waiting // state->current_active_wp_id = -1; ///< id of current waypoint @@ -612,6 +614,7 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi // wpm->yaw_reached = false; // wpm->pos_reached = false; + mission.current_index = wpc.seq; publish_mission(); @@ -718,7 +721,15 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi struct mission_item_s mission_item; ssize_t len = sizeof(struct mission_item_s); - if (dm_read(DM_KEY_WAYPOINTS_OFFBOARD, wpr.seq, &mission_item, len) == len) { + dm_item_t dm_current; + + if (wpm->current_dataman_id == 0) { + dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0; + } else { + dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1; + } + + if (dm_read(dm_current, wpr.seq, &mission_item, len) == len) { if (mission.current_index == wpr.seq) { wp.current = true; @@ -855,10 +866,6 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi if (wpc.count > NUM_MISSIONS_SUPPORTED) { warnx("Too many waypoints: %d, supported: %d", wpc.count, NUM_MISSIONS_SUPPORTED); - } else { - /* set count to 0 while copying */ - mission.count = 0; - publish_mission(); } #ifdef MAVLINK_WPM_NO_PRINTF @@ -992,7 +999,17 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi size_t len = sizeof(struct mission_item_s); - if (dm_write(DM_KEY_WAYPOINTS_OFFBOARD, wp.seq, DM_PERSIST_IN_FLIGHT_RESET, &mission_item, len) != len) { + dm_item_t dm_next; + + if (wpm->current_dataman_id == 0) { + dm_next = DM_KEY_WAYPOINTS_OFFBOARD_1; + mission.dataman_id = 1; + } else { + dm_next = DM_KEY_WAYPOINTS_OFFBOARD_0; + mission.dataman_id = 0; + } + + if (dm_write(dm_next, wp.seq, DM_PERSIST_IN_FLIGHT_RESET, &mission_item, len) != len) { mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_ERROR); wpm->current_state = MAVLINK_WPM_STATE_IDLE; break; @@ -1038,6 +1055,7 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi publish_mission(); + wpm->current_dataman_id = mission.dataman_id; wpm->size = wpm->current_count; //get the new current waypoint @@ -1132,9 +1150,11 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi // wpm->pos_reached = false; /* prepare mission topic */ + mission.dataman_id = -1; mission.count = 0; + mission.current_index = -1; - if (dm_clear(DM_KEY_WAYPOINTS_OFFBOARD) == OK) { + if (dm_clear(DM_KEY_WAYPOINTS_OFFBOARD_0) == OK && dm_clear(DM_KEY_WAYPOINTS_OFFBOARD_1) == OK) { mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_ACCEPTED); } else { mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_ERROR); diff --git a/src/modules/mavlink/waypoints.h b/src/modules/mavlink/waypoints.h index fc68285e93..801bc0bcf1 100644 --- a/src/modules/mavlink/waypoints.h +++ b/src/modules/mavlink/waypoints.h @@ -110,6 +110,7 @@ struct mavlink_wpm_storage { // uint64_t timestamp_firstinside_orbit; // uint64_t timestamp_lastoutside_orbit; uint32_t timeout; + int current_dataman_id; // uint32_t delay_setpoint; // float accept_range_yaw; // float accept_range_distance; diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index d93ecc7cd3..48f828ff7b 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -440,6 +440,7 @@ Navigator::offboard_mission_update() struct mission_s offboard_mission; if (orb_copy(ORB_ID(mission), _offboard_mission_sub, &offboard_mission) == OK) { + _mission.set_offboard_dataman_id(offboard_mission.dataman_id); _mission.set_current_offboard_mission_index(offboard_mission.current_index); _mission.set_offboard_mission_count(offboard_mission.count); diff --git a/src/modules/navigator/navigator_mission.cpp b/src/modules/navigator/navigator_mission.cpp index 993f8f1331..6576aae707 100644 --- a/src/modules/navigator/navigator_mission.cpp +++ b/src/modules/navigator/navigator_mission.cpp @@ -53,7 +53,8 @@ static const int ERROR = -1; Mission::Mission() : - + + _offboard_dataman_id(-1), _current_offboard_mission_index(0), _current_onboard_mission_index(0), _offboard_mission_item_count(0), @@ -67,6 +68,12 @@ Mission::~Mission() } +void +Mission::set_offboard_dataman_id(int new_id) +{ + _offboard_dataman_id = new_id; +} + void Mission::set_current_offboard_mission_index(int new_index) { @@ -132,8 +139,16 @@ Mission::get_current_mission_item(struct mission_item_s *new_mission_item, bool /* otherwise fallback to offboard */ } else if (current_offboard_mission_available()) { + dm_item_t dm_current; + + if (_offboard_dataman_id == 0) { + dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0; + } else { + dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1; + } + const ssize_t len = sizeof(struct mission_item_s); - if (dm_read(DM_KEY_WAYPOINTS_OFFBOARD, _current_offboard_mission_index, new_mission_item, len) != len) { + if (dm_read(dm_current, _current_offboard_mission_index, new_mission_item, len) != len) { /* not supposed to happen unless the datamanager can't access the SD card, etc. */ _current_mission_type = MISSION_TYPE_NONE; return ERROR; @@ -166,8 +181,16 @@ Mission::get_next_mission_item(struct mission_item_s *new_mission_item) /* otherwise fallback to offboard */ } else if (next_offboard_mission_available()) { + dm_item_t dm_current; + + if (_offboard_dataman_id == 0) { + dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0; + } else { + dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1; + } + const ssize_t len = sizeof(struct mission_item_s); - if (dm_read(DM_KEY_WAYPOINTS_OFFBOARD, _current_offboard_mission_index + 1, new_mission_item, len) != len) { + if (dm_read(dm_current, _current_offboard_mission_index + 1, new_mission_item, len) != len) { /* not supposed to happen unless the datamanager can't access the SD card, etc. */ return ERROR; } diff --git a/src/modules/navigator/navigator_mission.h b/src/modules/navigator/navigator_mission.h index e8e4763825..15d4e86bfb 100644 --- a/src/modules/navigator/navigator_mission.h +++ b/src/modules/navigator/navigator_mission.h @@ -55,6 +55,7 @@ public: */ ~Mission(); + void set_offboard_dataman_id(int new_id); void set_current_offboard_mission_index(int new_index); void set_current_onboard_mission_index(int new_index); void set_offboard_mission_count(unsigned new_count); @@ -78,6 +79,7 @@ private: bool next_onboard_mission_available(); bool next_offboard_mission_available(); + int _offboard_dataman_id; unsigned _current_offboard_mission_index; unsigned _current_onboard_mission_index; unsigned _offboard_mission_item_count; /** number of offboard mission items available */ diff --git a/src/modules/uORB/topics/mission.h b/src/modules/uORB/topics/mission.h index 9697835cf6..6d4b50a9b5 100644 --- a/src/modules/uORB/topics/mission.h +++ b/src/modules/uORB/topics/mission.h @@ -97,6 +97,7 @@ struct mission_item_s struct mission_s { + int dataman_id; /**< default -1, there are two offboard storage places in the dataman: 0 or 1 */ unsigned count; /**< count of the missions stored in the datamanager */ int current_index; /**< default -1, start at the one changed latest */ }; From b02b48290fdb5464020ea49209144ab8d5d045af Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 25 Dec 2013 17:10:38 +0100 Subject: [PATCH 233/447] Navigator: add MissionFeasibilityChecker class; performs validation of landing waypoint set-up for fixed wing for now, but can be extended for other checks (e.g. check mission against geofence) --- .../fw_pos_control_l1_main.cpp | 29 ++- .../fw_pos_control_l1/landingslope.cpp | 12 +- src/modules/fw_pos_control_l1/landingslope.h | 34 +++- src/modules/mavlink/waypoints.c | 2 +- .../navigator/mission_feasibility_checker.cpp | 184 ++++++++++++++++++ .../navigator/mission_feasibility_checker.h | 82 ++++++++ src/modules/navigator/module.mk | 3 +- src/modules/navigator/navigator_main.cpp | 26 ++- .../uORB/topics/navigation_capabilities.h | 5 + 9 files changed, 350 insertions(+), 27 deletions(-) create mode 100644 src/modules/navigator/mission_feasibility_checker.cpp create mode 100644 src/modules/navigator/mission_feasibility_checker.h diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 2be25517d4..3784337ac5 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -311,6 +311,11 @@ private: */ void vehicle_setpoint_poll(); + /** + * Publish navigation capabilities + */ + void navigation_capabilities_publish(); + /** * Control position. */ @@ -538,6 +543,12 @@ FixedwingPositionControl::parameters_update() /* Update the landing slope */ landingslope.update(math::radians(_parameters.land_slope_angle), _parameters.land_flare_alt_relative, _parameters.land_thrust_lim_horizontal_distance, _parameters.land_H1_virt); + /* Update and publish the navigation capabilities */ + _nav_capabilities.landing_slope_angle_rad = landingslope.landing_slope_angle_rad(); + _nav_capabilities.landing_horizontal_slope_displacement = landingslope.horizontal_slope_displacement(); + _nav_capabilities.landing_flare_length = landingslope.flare_length(); + navigation_capabilities_publish(); + return OK; } @@ -709,6 +720,15 @@ FixedwingPositionControl::calculate_gndspeed_undershoot(const math::Vector2f &cu } } +void FixedwingPositionControl::navigation_capabilities_publish() +{ + if (_nav_capabilities_pub > 0) { + orb_publish(ORB_ID(navigation_capabilities), _nav_capabilities_pub, &_nav_capabilities); + } else { + _nav_capabilities_pub = orb_advertise(ORB_ID(navigation_capabilities), &_nav_capabilities); + } +} + bool FixedwingPositionControl::control_position(const math::Vector2f ¤t_position, const math::Vector2f &ground_speed, const struct mission_item_triplet_s &mission_item_triplet) @@ -875,7 +895,7 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio } - float flare_curve_alt = landingslope.getFlarceCurveAltitude(wp_distance, _mission_item_triplet.current.altitude); + float flare_curve_alt = landingslope.getFlareCurveAltitude(wp_distance, _mission_item_triplet.current.altitude); /* avoid climbout */ if (flare_curve_alt_last < flare_curve_alt && land_noreturn_vertical || land_stayonground) @@ -1214,11 +1234,8 @@ FixedwingPositionControl::task_main() /* set new turn distance */ _nav_capabilities.turn_distance = turn_distance; - if (_nav_capabilities_pub > 0) { - orb_publish(ORB_ID(navigation_capabilities), _nav_capabilities_pub, &_nav_capabilities); - } else { - _nav_capabilities_pub = orb_advertise(ORB_ID(navigation_capabilities), &_nav_capabilities); - } + navigation_capabilities_publish(); + } } diff --git a/src/modules/fw_pos_control_l1/landingslope.cpp b/src/modules/fw_pos_control_l1/landingslope.cpp index a2d8525b9d..b139a63978 100644 --- a/src/modules/fw_pos_control_l1/landingslope.cpp +++ b/src/modules/fw_pos_control_l1/landingslope.cpp @@ -1,9 +1,7 @@ /**************************************************************************** * * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. - * Author: @author Lorenz Meier - * @author Thomas Gubler - * @author Julian Oes + * Author: @author Thomas Gubler * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -37,7 +35,6 @@ /* * @file: landingslope.cpp * - * @author: Thomas Gubler */ #include "landingslope.h" @@ -74,11 +71,12 @@ void Landingslope::calculateSlopeValues() float Landingslope::getLandingSlopeAbsoluteAltitude(float wp_distance, float wp_altitude) { - return (wp_distance - _horizontal_slope_displacement) * tanf(_landing_slope_angle_rad) + wp_altitude; //flare_relative_alt is negative + return Landingslope::getLandingSlopeAbsoluteAltitude(wp_distance, wp_altitude, _horizontal_slope_displacement, _landing_slope_angle_rad); } -float Landingslope::getFlarceCurveAltitude(float wp_distance, float wp_altitude) +float Landingslope::getFlareCurveAltitude(float wp_landing_distance, float wp_landing_altitude) { - return wp_altitude + _H0 * expf(-math::max(0.0f, _flare_length - wp_distance)/_flare_constant) - _H1_virt; + return wp_landing_altitude + _H0 * expf(-math::max(0.0f, _flare_length - wp_landing_distance)/_flare_constant) - _H1_virt; + } diff --git a/src/modules/fw_pos_control_l1/landingslope.h b/src/modules/fw_pos_control_l1/landingslope.h index b77252e6e1..8ff4315097 100644 --- a/src/modules/fw_pos_control_l1/landingslope.h +++ b/src/modules/fw_pos_control_l1/landingslope.h @@ -1,9 +1,7 @@ /**************************************************************************** * * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. - * Author: @author Lorenz Meier - * @author Thomas Gubler - * @author Julian Oes + * Author: @author Thomas Gubler * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -37,15 +35,18 @@ /* * @file: landingslope.h * - * @author: Thomas Gubler */ #ifndef LANDINGSLOPE_H_ #define LANDINGSLOPE_H_ +#include +#include + class Landingslope { private: + //xxx: documentation of landing pending float _landing_slope_angle_rad; float _flare_relative_alt; float _motor_lim_horizontal_distance; @@ -62,8 +63,29 @@ public: Landingslope() {} ~Landingslope() {} - float getLandingSlopeAbsoluteAltitude(float wp_distance, float wp_altitude); - float getFlarceCurveAltitude(float wp_distance, float wp_altitude); + float getLandingSlopeAbsoluteAltitude(float wp_landing_distance, float wp_landing_altitude); + + /** + * + * @return Absolute altitude of point on landing slope at distance to landing waypoint=wp_landing_distance + */ + __EXPORT static float getLandingSlopeAbsoluteAltitude(float wp_landing_distance, float wp_landing_altitude, float horizontal_slope_displacement, float landing_slope_angle_rad) + { + return (wp_landing_distance - horizontal_slope_displacement) * tanf(landing_slope_angle_rad) + wp_landing_altitude; //flare_relative_alt is negative + } + + /** + * + * @return distance to landing waypoint of point on landing slope at altitude=slope_altitude + */ + __EXPORT static float getLandingSlopeWPDistance(float slope_altitude, float wp_landing_altitude, float horizontal_slope_displacement, float landing_slope_angle_rad) + { + return (slope_altitude - wp_landing_altitude)/tanf(landing_slope_angle_rad) + horizontal_slope_displacement; + + } + + + float getFlareCurveAltitude(float wp_distance, float wp_altitude); void update(float landing_slope_angle_rad, float flare_relative_alt, diff --git a/src/modules/mavlink/waypoints.c b/src/modules/mavlink/waypoints.c index 45e8914343..2ac05039fc 100644 --- a/src/modules/mavlink/waypoints.c +++ b/src/modules/mavlink/waypoints.c @@ -62,7 +62,7 @@ orb_advert_t mission_pub = -1; struct mission_s mission; //#define MAVLINK_WPM_NO_PRINTF -#define MAVLINK_WPM_VERBOSE 1 +#define MAVLINK_WPM_VERBOSE 0 uint8_t mavlink_wpm_comp_id = MAV_COMP_ID_MISSIONPLANNER; diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp new file mode 100644 index 0000000000..25b2636bb8 --- /dev/null +++ b/src/modules/navigator/mission_feasibility_checker.cpp @@ -0,0 +1,184 @@ +/**************************************************************************** + * + * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * Author: @author Lorenz Meier + * @author Thomas Gubler + * + * 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 mission_feasibility_checker.cpp + * Provides checks if mission is feasible given the navigation capabilities + */ + +#include "mission_feasibility_checker.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; + +MissionFeasibilityChecker::MissionFeasibilityChecker() : _mavlink_fd(-1), _capabilities_sub(-1), _initDone(false) +{ + _nav_caps = {0}; +} + + +bool MissionFeasibilityChecker::checkMissionFeasible(bool isRotarywing, dm_item_t dm_current, size_t nItems) +{ + /* Init if not done yet */ + init(); + + /* Open mavlink fd */ + if (_mavlink_fd < 0) { + /* try to open the mavlink log device every once in a while */ + _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); + } + + + if (isRotarywing) + return checkMissionFeasibleRotarywing(dm_current, nItems); + else + return checkMissionFeasibleFixedwing(dm_current, nItems); +} + +bool MissionFeasibilityChecker::checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nItems) +{ + + return checkGeofence(dm_current, nItems); +} + +bool MissionFeasibilityChecker::checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nItems) +{ + /* Update fixed wing navigation capabilites */ + updateNavigationCapabilities(); +// warnx("_nav_caps.landing_slope_angle_rad %.4f, _nav_caps.landing_horizontal_slope_displacement %.4f", _nav_caps.landing_slope_angle_rad, _nav_caps.landing_horizontal_slope_displacement); + + return (checkFixedWingLanding(dm_current, nItems) && checkGeofence(dm_current, nItems)); +} + +bool MissionFeasibilityChecker::checkGeofence(dm_item_t dm_current, size_t nItems) +{ + //xxx: check geofence + return true; +} + +bool MissionFeasibilityChecker::checkFixedWingLanding(dm_item_t dm_current, size_t nItems) +{ + /* Go through all mission items and search for a landing waypoint + * if landing waypoint is found: the previous waypoint is checked to be at a feasible distance and altitude given the landing slope */ + + + for (size_t i = 0; i < nItems; i++) { + static struct mission_item_s missionitem; + const ssize_t len = sizeof(struct mission_item_s); + if (dm_read(dm_current, i, &missionitem, len) != len) { + /* not supposed to happen unless the datamanager can't access the SD card, etc. */ + return false; + } + + if (missionitem.nav_cmd == NAV_CMD_LAND) { + struct mission_item_s missionitem_previous; + if (i != 0) { + if (dm_read(dm_current, i-1, &missionitem_previous, len) != len) { + /* not supposed to happen unless the datamanager can't access the SD card, etc. */ + return false; + } + + float wp_distance = get_distance_to_next_waypoint(missionitem_previous.lat , missionitem_previous.lon, missionitem.lat, missionitem.lon); + float slope_alt_req = Landingslope::getLandingSlopeAbsoluteAltitude(wp_distance, missionitem.altitude, _nav_caps.landing_horizontal_slope_displacement, _nav_caps.landing_slope_angle_rad); + float wp_distance_req = Landingslope::getLandingSlopeWPDistance(missionitem_previous.altitude, missionitem.altitude, _nav_caps.landing_horizontal_slope_displacement, _nav_caps.landing_slope_angle_rad); + float delta_altitude = missionitem.altitude - missionitem_previous.altitude; +// warnx("wp_distance %.2f, delta_altitude %.2f, missionitem_previous.altitude %.2f, missionitem.altitude %.2f, slope_alt_req %.2f, wp_distance_req %.2f", +// wp_distance, delta_altitude, missionitem_previous.altitude, missionitem.altitude, slope_alt_req, wp_distance_req); +// warnx("_nav_caps.landing_horizontal_slope_displacement %.4f, _nav_caps.landing_slope_angle_rad %.4f, _nav_caps.landing_flare_length %.4f", +// _nav_caps.landing_horizontal_slope_displacement, _nav_caps.landing_slope_angle_rad, _nav_caps.landing_flare_length); + + if (wp_distance > _nav_caps.landing_flare_length) { + /* Last wp is before flare region */ + + if (delta_altitude < 0) { + if (missionitem_previous.altitude <= slope_alt_req) { + /* Landing waypoint is at or below altitude of slope at the given waypoint distance: this is ok, aircraft will intersect the slope */ + return true; + } else { + /* Landing waypoint is above altitude of slope at the given waypoint distance */ + mavlink_log_info(_mavlink_fd, "#audio: Landing: last waypoint too high/too close"); + mavlink_log_info(_mavlink_fd, "Move down to %.1fm or move further away by %.1fm", + (double)(slope_alt_req), + (double)(wp_distance_req - wp_distance)); + return false; + } + } else { + /* Landing waypoint is above last waypoint */ + mavlink_log_info(_mavlink_fd, "#audio: Landing waypoint above last nav waypoint"); + return false; + } + } else { + /* Last wp is in flare region */ + //xxx give recommendations + mavlink_log_info(_mavlink_fd, "#audio: Warning: Landing: last waypoint in flare region"); + return false; + } + } else { + mavlink_log_info(_mavlink_fd, "#audio: Warning: starting with land waypoint"); + return false; + } + } + } + + +// float slope_alt = wp_altitude + _H0 * expf(-math::max(0.0f, _flare_length - wp_distance)/_flare_constant) - _H1_virt; +} + +void MissionFeasibilityChecker::updateNavigationCapabilities() +{ + int res = orb_copy(ORB_ID(navigation_capabilities), _capabilities_sub, &_nav_caps); +} + +void MissionFeasibilityChecker::init() +{ + if (!_initDone) { + + _capabilities_sub = orb_subscribe(ORB_ID(navigation_capabilities)); + + _initDone = true; + } +} diff --git a/src/modules/navigator/mission_feasibility_checker.h b/src/modules/navigator/mission_feasibility_checker.h new file mode 100644 index 0000000000..7d1cc2f8a9 --- /dev/null +++ b/src/modules/navigator/mission_feasibility_checker.h @@ -0,0 +1,82 @@ +/**************************************************************************** + * + * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * Author: @author Lorenz Meier + * @author Thomas Gubler + * + * 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 mission_feasibility_checker.h + * Provides checks if mission is feasible given the navigation capabilities + */ +#ifndef MISSION_FEASIBILITY_CHECKER_H_ +#define MISSION_FEASIBILITY_CHECKER_H_ + +#include +#include +#include +#include + + +class MissionFeasibilityChecker +{ +private: + int _mavlink_fd; + + int _capabilities_sub; + struct navigation_capabilities_s _nav_caps; + + bool _initDone; + void init(); + + /* Checks for all airframes */ + bool checkGeofence(dm_item_t dm_current, size_t nItems); + + /* Checks specific to fixedwing airframes */ + bool checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nItems); + bool checkFixedWingLanding(dm_item_t dm_current, size_t nItems); + void updateNavigationCapabilities(); + + /* Checks specific to rotarywing airframes */ + bool checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nItems); +public: + + MissionFeasibilityChecker(); + ~MissionFeasibilityChecker() {} + + /* + * Returns true if mission is feasible and false otherwise + */ + bool checkMissionFeasible(bool isRotarywing, dm_item_t dm_current, size_t nItems); + +}; + + +#endif /* MISSION_FEASIBILITY_CHECKER_H_ */ diff --git a/src/modules/navigator/module.mk b/src/modules/navigator/module.mk index fc59c956a3..6be4e87a05 100644 --- a/src/modules/navigator/module.mk +++ b/src/modules/navigator/module.mk @@ -39,6 +39,7 @@ MODULE_COMMAND = navigator SRCS = navigator_main.cpp \ navigator_params.c \ - navigator_mission.cpp + navigator_mission.cpp \ + mission_feasibility_checker.cpp INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 48f828ff7b..c7ac885b48 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -76,6 +76,7 @@ #include #include "navigator_mission.h" +#include "mission_feasibility_checker.h" /* oddly, ERROR is not defined for c++ */ @@ -165,6 +166,8 @@ private: bool _waypoint_yaw_reached; uint64_t _time_first_inside_orbit; + MissionFeasibilityChecker missionFeasiblityChecker; + struct { float min_altitude; float loiter_radius; @@ -228,7 +231,7 @@ private: /** * Retrieve offboard mission. */ - void offboard_mission_update(); + void offboard_mission_update(bool isrotaryWing); /** * Retrieve onboard mission. @@ -435,11 +438,21 @@ Navigator::navigation_capabilities_update() void -Navigator::offboard_mission_update() +Navigator::offboard_mission_update(bool isrotaryWing) { struct mission_s offboard_mission; if (orb_copy(ORB_ID(mission), _offboard_mission_sub, &offboard_mission) == OK) { + /* Check mission feasibility, for now do not handle the return value, + * however warnings are issued to the gcs via mavlink from inside the MissionFeasiblityChecker */ + dm_item_t dm_current; + if (offboard_mission.dataman_id == 0) { + dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0; + } else { + dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1; + } + missionFeasiblityChecker.checkMissionFeasible(isrotaryWing, dm_current, (size_t)offboard_mission.count); + _mission.set_offboard_dataman_id(offboard_mission.dataman_id); _mission.set_current_offboard_mission_index(offboard_mission.current_index); _mission.set_offboard_mission_count(offboard_mission.count); @@ -503,13 +516,14 @@ Navigator::task_main() /* copy all topics first time */ + vehicle_status_update(); parameters_update(); global_position_update(); home_position_update(); navigation_capabilities_update(); - offboard_mission_update(); + offboard_mission_update(_vstatus.is_rotary_wing); onboard_mission_update(); - vehicle_status_update(); + /* rate limit vehicle status updates to 5Hz */ orb_set_interval(_vstatus_sub, 200); @@ -632,7 +646,7 @@ Navigator::task_main() } if (fds[4].revents & POLLIN) { - offboard_mission_update(); + offboard_mission_update(_vstatus.is_rotary_wing); // XXX check if mission really changed dispatch(EVENT_MISSION_CHANGED); } @@ -1311,4 +1325,4 @@ Navigator::add_home_pos_to_rtl(struct mission_item_s *new_mission_item) new_mission_item->loiter_radius = _parameters.loiter_radius; // TODO: get rid of magic number new_mission_item->radius = 50.0f; // TODO: get rid of magic number } -} \ No newline at end of file +} diff --git a/src/modules/uORB/topics/navigation_capabilities.h b/src/modules/uORB/topics/navigation_capabilities.h index 6a3e811e3f..391756f99f 100644 --- a/src/modules/uORB/topics/navigation_capabilities.h +++ b/src/modules/uORB/topics/navigation_capabilities.h @@ -53,6 +53,11 @@ */ struct navigation_capabilities_s { float turn_distance; /**< the optimal distance to a waypoint to switch to the next */ + + /* Landing parameters: see fw_pos_control_l1/landingslope.h */ + float landing_horizontal_slope_displacement; + float landing_slope_angle_rad; + float landing_flare_length; }; /** From 5c85fa2c5f4bf1b9d8fe12043f56ebc7dbe53d13 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 26 Dec 2013 15:16:32 +0100 Subject: [PATCH 234/447] Missionlib: deactivate functions now implemented in navigator --- src/modules/mavlink/missionlib.c | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/src/modules/mavlink/missionlib.c b/src/modules/mavlink/missionlib.c index 5845429dbc..318dcf08c4 100644 --- a/src/modules/mavlink/missionlib.c +++ b/src/modules/mavlink/missionlib.c @@ -73,11 +73,15 @@ #include "waypoints.h" #include "mavlink_parameters.h" + + static uint8_t missionlib_msg_buf[MAVLINK_MAX_PACKET_LEN]; static uint64_t loiter_start_time; +#if 0 static bool set_special_fields(float param1, float param2, float param3, float param4, uint16_t command, struct vehicle_global_position_setpoint_s *sp); +#endif int mavlink_missionlib_send_message(mavlink_message_t *msg) @@ -88,6 +92,8 @@ mavlink_missionlib_send_message(mavlink_message_t *msg) return 0; } + + int mavlink_missionlib_send_gcs_string(const char *string) { @@ -127,6 +133,7 @@ uint64_t mavlink_missionlib_get_system_timestamp() return hrt_absolute_time(); } +#if 0 /** * Set special vehicle setpoint fields based on current mission item. * @@ -388,3 +395,5 @@ void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1, printf("%s\n", buf); //printf("[mavlink mp] new setpoint\n");//: frame: %d, lat: %d, lon: %d, alt: %d, yaw: %d\n", frame, param5_lat_x*1000, param6_lon_y*1000, param7_alt_z*1000, param4*1000); } + +#endif \ No newline at end of file From 409fa12c4e095e2ec4ddfa1deb7176f0b3b52c0d Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 26 Dec 2013 15:17:44 +0100 Subject: [PATCH 235/447] Mission topic: corrected comment --- src/modules/uORB/topics/mission.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/uORB/topics/mission.h b/src/modules/uORB/topics/mission.h index 6d4b50a9b5..dcdb234fa4 100644 --- a/src/modules/uORB/topics/mission.h +++ b/src/modules/uORB/topics/mission.h @@ -80,8 +80,8 @@ enum ORIGIN { struct mission_item_s { bool altitude_is_relative; /**< true if altitude is relative from start point */ - double lat; /**< latitude in degrees * 1E7 */ - double lon; /**< longitude in degrees * 1E7 */ + double lat; /**< latitude in degrees */ + double lon; /**< longitude in degrees */ float altitude; /**< altitude in meters */ float yaw; /**< in radians NED -PI..+PI */ float loiter_radius; /**< loiter radius in meters, 0 for a VTOL to hover */ From e103729de308c113d561942335a4bcf20c68a255 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 26 Dec 2013 23:02:09 +0400 Subject: [PATCH 236/447] mc_att_control_vector: fixes, parameters added --- .../mc_att_control_vector_main.cpp | 5 +++-- .../mc_att_control_vector_params.c | 20 +++++++++++++++++++ src/modules/mc_att_control_vector/module.mk | 3 ++- 3 files changed, 25 insertions(+), 3 deletions(-) create mode 100644 src/modules/mc_att_control_vector/mc_att_control_vector_params.c diff --git a/src/modules/mc_att_control_vector/mc_att_control_vector_main.cpp b/src/modules/mc_att_control_vector/mc_att_control_vector_main.cpp index 30c073c293..ef6a46810e 100644 --- a/src/modules/mc_att_control_vector/mc_att_control_vector_main.cpp +++ b/src/modules/mc_att_control_vector/mc_att_control_vector_main.cpp @@ -616,12 +616,13 @@ MulticopterAttitudeControl::task_main() math::Vector<3> R_rp_x(R_rp(0, 0), R_rp(1, 0), R_rp(2, 0)); e_R(2) = atan2f((R_rp_x % R_sp_x) * R_sp_z, R_rp_x * R_sp_x) * yaw_w; - if (e_R_z_cos < 0.0) { + if (e_R_z_cos < 0.0f) { /* for large thrust vector rotations use another rotation method: * calculate angle and axis for R -> R_sp rotation directly */ q.from_dcm(R.transposed() * R_sp); math::Vector<3> e_R_d = q.imag(); - float angle_d = 2.0f * atan2f(e_R_d.length(), q(0)); + e_R_d.normalize(); + e_R_d *= 2.0f * atan2f(e_R_d.length(), q(0)); /* use fusion of Z axis based rotation and direct rotation */ float direct_w = e_R_z_cos * e_R_z_cos * yaw_w; diff --git a/src/modules/mc_att_control_vector/mc_att_control_vector_params.c b/src/modules/mc_att_control_vector/mc_att_control_vector_params.c new file mode 100644 index 0000000000..613d1945b6 --- /dev/null +++ b/src/modules/mc_att_control_vector/mc_att_control_vector_params.c @@ -0,0 +1,20 @@ +/* + * mc_att_control_vector_params.c + * + * Created on: 26.12.2013 + * Author: ton + */ + +#include + +/* multicopter attitude controller parameters */ +PARAM_DEFINE_FLOAT(MC_YAWPOS_P, 2.0f); +PARAM_DEFINE_FLOAT(MC_YAWPOS_I, 0.0f); +PARAM_DEFINE_FLOAT(MC_ATT_P, 6.0f); +PARAM_DEFINE_FLOAT(MC_ATT_I, 0.0f); +PARAM_DEFINE_FLOAT(MC_YAWRATE_P, 0.3f); +PARAM_DEFINE_FLOAT(MC_YAWRATE_D, 0.0f); +PARAM_DEFINE_FLOAT(MC_YAWRATE_I, 0.0f); +PARAM_DEFINE_FLOAT(MC_ATTRATE_P, 0.1f); +PARAM_DEFINE_FLOAT(MC_ATTRATE_D, 0.002f); +PARAM_DEFINE_FLOAT(MC_ATTRATE_I, 0.0f); diff --git a/src/modules/mc_att_control_vector/module.mk b/src/modules/mc_att_control_vector/module.mk index 8d380d719f..de0675df86 100644 --- a/src/modules/mc_att_control_vector/module.mk +++ b/src/modules/mc_att_control_vector/module.mk @@ -37,4 +37,5 @@ MODULE_COMMAND = mc_att_control_vector -SRCS = mc_att_control_vector_main.cpp +SRCS = mc_att_control_vector_main.cpp \ + mc_att_control_vector_params.c From 20c9ce9d6dc119547bc81b9034cebc69a364b565 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 26 Dec 2013 23:03:19 +0400 Subject: [PATCH 237/447] mc_pos_control: replacement for multirotor_pos_control, rewritten to C++ and new mathlib --- src/lib/mathlib/math/Vector.hpp | 24 + .../mc_pos_control/mc_pos_control_main.cpp | 956 ++++++++++++++++++ .../mc_pos_control/mc_pos_control_params.c | 25 + .../module.mk | 9 +- .../multirotor_pos_control.c | 864 ---------------- .../multirotor_pos_control_params.c | 106 -- .../multirotor_pos_control_params.h | 97 -- .../multirotor_pos_control/thrust_pid.c | 172 ---- .../multirotor_pos_control/thrust_pid.h | 69 -- 9 files changed, 1009 insertions(+), 1313 deletions(-) create mode 100644 src/modules/mc_pos_control/mc_pos_control_main.cpp create mode 100644 src/modules/mc_pos_control/mc_pos_control_params.c rename src/modules/{multirotor_pos_control => mc_pos_control}/module.mk (91%) delete mode 100644 src/modules/multirotor_pos_control/multirotor_pos_control.c delete mode 100644 src/modules/multirotor_pos_control/multirotor_pos_control_params.c delete mode 100644 src/modules/multirotor_pos_control/multirotor_pos_control_params.h delete mode 100644 src/modules/multirotor_pos_control/thrust_pid.c delete mode 100644 src/modules/multirotor_pos_control/thrust_pid.h diff --git a/src/lib/mathlib/math/Vector.hpp b/src/lib/mathlib/math/Vector.hpp index b7840170ce..c7323c2153 100644 --- a/src/lib/mathlib/math/Vector.hpp +++ b/src/lib/mathlib/math/Vector.hpp @@ -261,6 +261,30 @@ public: return res; } + /** + * element by element multiplication + */ + const Vector emult(const Vector &v) const { + Vector res; + + for (unsigned int i = 0; i < N; i++) + res.data[i] = data[i] * v.data[i]; + + return res; + } + + /** + * element by element division + */ + const Vector edivide(const Vector &v) const { + Vector res; + + for (unsigned int i = 0; i < N; i++) + res.data[i] = data[i] / v.data[i]; + + return res; + } + /** * gets the length of this vector squared */ diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp new file mode 100644 index 0000000000..c06caff1ee --- /dev/null +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -0,0 +1,956 @@ +/**************************************************************************** + * + * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * Author: Anton Babushkin + * + * 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 mc_pos_control_main.cpp + * + * Multirotor position controller. + */ + +#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 +#include +#include +#include +#include + +#define TILT_COS_MAX 0.7f +#define SIGMA 0.000001f + +/** + * Multicopter position control app start / stop handling function + * + * @ingroup apps + */ +extern "C" __EXPORT int mc_pos_control_main(int argc, char *argv[]); + +class MulticopterPositionControl +{ +public: + /** + * Constructor + */ + MulticopterPositionControl(); + + /** + * Destructor, also kills task. + */ + ~MulticopterPositionControl(); + + /** + * Start task. + * + * @return OK on success. + */ + int start(); + +private: + + bool _task_should_exit; /**< if true, task should exit */ + int _control_task; /**< task handle for task */ + + int _att_sub; /**< vehicle attitude subscription */ + int _att_sp_sub; /**< vehicle attitude setpoint */ + int _control_mode_sub; /**< vehicle control mode subscription */ + int _params_sub; /**< notification of parameter updates */ + int _manual_sub; /**< notification of manual control updates */ + int _arming_sub; /**< arming status of outputs */ + int _local_pos_sub; /**< vehicle local position */ + int _global_pos_sp_sub; /**< vehicle global position setpoint */ + + orb_advert_t _local_pos_sp_pub; /**< local position setpoint publication */ + orb_advert_t _att_sp_pub; /**< attitude setpoint publication */ + orb_advert_t _global_vel_sp_pub; /**< vehicle global velocity setpoint */ + + struct vehicle_attitude_s _att; /**< vehicle attitude */ + struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */ + struct manual_control_setpoint_s _manual; /**< r/c channel data */ + struct vehicle_control_mode_s _control_mode; /**< vehicle control mode */ + struct actuator_armed_s _arming; /**< actuator arming status */ + struct vehicle_local_position_s _local_pos; /**< vehicle local position */ + struct vehicle_local_position_setpoint_s _local_pos_sp; /**< vehicle local position */ + struct vehicle_global_position_setpoint_s _global_pos_sp; /**< vehicle global position setpoint */ + struct vehicle_global_velocity_setpoint_s _global_vel_sp; /**< vehicle global velocity setpoint */ + + struct { + param_t takeoff_alt; + param_t takeoff_gap; + param_t thr_min; + param_t thr_max; + param_t z_p; + param_t z_vel_p; + param_t z_vel_i; + param_t z_vel_d; + param_t z_vel_max; + param_t z_ff; + param_t xy_p; + param_t xy_vel_p; + param_t xy_vel_i; + param_t xy_vel_d; + param_t xy_vel_max; + param_t xy_ff; + param_t tilt_max; + + param_t rc_scale_pitch; + param_t rc_scale_roll; + param_t rc_scale_yaw; + } _params_handles; /**< handles for interesting parameters */ + + struct { + float takeoff_alt; + float takeoff_gap; + float thr_min; + float thr_max; + float tilt_max; + + float rc_scale_pitch; + float rc_scale_roll; + float rc_scale_yaw; + + math::Vector<3> pos_p; + math::Vector<3> vel_p; + math::Vector<3> vel_i; + math::Vector<3> vel_d; + math::Vector<3> vel_ff; + math::Vector<3> vel_max; + math::Vector<3> sp_offs_max; + } _params; + + math::Vector<3> _pos; + math::Vector<3> _vel; + math::Vector<3> _pos_sp; + math::Vector<3> _vel_sp; + math::Vector<3> _vel_err_prev; /**< velocity on previous step */ + + hrt_abstime _time_prev; + + bool _was_armed; + + /** + * Update our local parameter cache. + */ + int parameters_update(bool force); + + /** + * Update control outputs + */ + void control_update(); + + /** + * Check for changes in subscribed topics. + */ + void poll_subscriptions(); + + static float scale_control(float ctl, float end, float dz); + + /** + * Shim for calling task_main from task_create. + */ + static void task_main_trampoline(int argc, char *argv[]); + + /** + * Main sensor collection task. + */ + void task_main() __attribute__((noreturn)); +}; + +namespace pos_control +{ + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; + +MulticopterPositionControl *g_control; +} + +MulticopterPositionControl::MulticopterPositionControl() : + + _task_should_exit(false), + _control_task(-1), + +/* subscriptions */ + _att_sub(-1), + _att_sp_sub(-1), + _control_mode_sub(-1), + _params_sub(-1), + _manual_sub(-1), + _arming_sub(-1), + _local_pos_sub(-1), + _global_pos_sp_sub(-1), + +/* publications */ + _local_pos_sp_pub(-1), + _att_sp_pub(-1), + _global_vel_sp_pub(-1), + + _time_prev(0), + _was_armed(false) +{ + memset(&_att, 0, sizeof(_att)); + memset(&_att_sp, 0, sizeof(_att_sp)); + memset(&_manual, 0, sizeof(_manual)); + memset(&_control_mode, 0, sizeof(_control_mode)); + memset(&_arming, 0, sizeof(_arming)); + memset(&_local_pos, 0, sizeof(_local_pos)); + memset(&_local_pos_sp, 0, sizeof(_local_pos_sp)); + memset(&_global_pos_sp, 0, sizeof(_global_pos_sp)); + memset(&_global_vel_sp, 0, sizeof(_global_vel_sp)); + + _params.pos_p.zero(); + _params.vel_p.zero(); + _params.vel_i.zero(); + _params.vel_d.zero(); + _params.vel_max.zero(); + _params.vel_ff.zero(); + _params.sp_offs_max.zero(); + + _pos.zero(); + _vel.zero(); + _pos_sp.zero(); + _vel_sp.zero(); + _vel_err_prev.zero(); + + _params_handles.takeoff_alt = param_find("NAV_TAKEOFF_ALT"); + _params_handles.takeoff_gap = param_find("NAV_TAKEOFF_GAP"); + _params_handles.thr_min = param_find("MPC_THR_MIN"); + _params_handles.thr_max = param_find("MPC_THR_MAX"); + _params_handles.z_p = param_find("MPC_Z_P"); + _params_handles.z_vel_p = param_find("MPC_Z_VEL_P"); + _params_handles.z_vel_i = param_find("MPC_Z_VEL_I"); + _params_handles.z_vel_d = param_find("MPC_Z_VEL_D"); + _params_handles.z_vel_max = param_find("MPC_Z_VEL_MAX"); + _params_handles.z_ff = param_find("MPC_Z_FF"); + _params_handles.xy_p = param_find("MPC_XY_P"); + _params_handles.xy_vel_p = param_find("MPC_XY_VEL_P"); + _params_handles.xy_vel_i = param_find("MPC_XY_VEL_I"); + _params_handles.xy_vel_d = param_find("MPC_XY_VEL_D"); + _params_handles.xy_vel_max = param_find("MPC_XY_VEL_MAX"); + _params_handles.xy_ff = param_find("MPC_XY_FF"); + _params_handles.tilt_max = param_find("MPC_TILT_MAX"); + _params_handles.rc_scale_pitch = param_find("RC_SCALE_PITCH"); + _params_handles.rc_scale_roll = param_find("RC_SCALE_ROLL"); + _params_handles.rc_scale_yaw = param_find("RC_SCALE_YAW"); + + /* fetch initial parameter values */ + parameters_update(true); +} + +MulticopterPositionControl::~MulticopterPositionControl() +{ + if (_control_task != -1) { + /* task wakes up every 100ms or so at the longest */ + _task_should_exit = true; + + /* wait for a second for the task to quit at our request */ + unsigned i = 0; + + do { + /* wait 20ms */ + usleep(20000); + + /* if we have given up, kill it */ + if (++i > 50) { + task_delete(_control_task); + break; + } + } while (_control_task != -1); + } + + pos_control::g_control = nullptr; +} + +int +MulticopterPositionControl::parameters_update(bool force) +{ + bool updated; + struct parameter_update_s param_upd; + + orb_check(_params_sub, &updated); + if (updated) + orb_copy(ORB_ID(parameter_update), _params_sub, ¶m_upd); + + if (updated || force) { + param_get(_params_handles.takeoff_alt, &_params.takeoff_alt); + param_get(_params_handles.takeoff_gap, &_params.takeoff_gap); + param_get(_params_handles.thr_min, &_params.thr_min); + param_get(_params_handles.thr_max, &_params.thr_max); + param_get(_params_handles.tilt_max, &_params.tilt_max); + param_get(_params_handles.rc_scale_pitch, &_params.rc_scale_pitch); + param_get(_params_handles.rc_scale_roll, &_params.rc_scale_roll); + param_get(_params_handles.rc_scale_yaw, &_params.rc_scale_yaw); + + float v; + param_get(_params_handles.xy_p, &v); + _params.pos_p(0) = v; + _params.pos_p(1) = v; + param_get(_params_handles.z_p, &v); + _params.pos_p(2) = v; + param_get(_params_handles.xy_vel_p, &v); + _params.vel_p(0) = v; + _params.vel_p(1) = v; + param_get(_params_handles.z_vel_p, &v); + _params.vel_p(2) = v; + param_get(_params_handles.xy_vel_i, &v); + _params.vel_i(0) = v; + _params.vel_i(1) = v; + param_get(_params_handles.z_vel_i, &v); + _params.vel_i(2) = v; + param_get(_params_handles.xy_vel_d, &v); + _params.vel_d(0) = v; + _params.vel_d(1) = v; + param_get(_params_handles.z_vel_d, &v); + _params.vel_d(2) = v; + param_get(_params_handles.xy_vel_max, &v); + _params.vel_max(0) = v; + _params.vel_max(1) = v; + param_get(_params_handles.z_vel_max, &v); + _params.vel_max(2) = v; + param_get(_params_handles.xy_ff, &v); + _params.vel_ff(0) = v; + _params.vel_ff(1) = v; + param_get(_params_handles.z_ff, &v); + _params.vel_ff(2) = v; + + _params.sp_offs_max = _params.vel_max.edivide(_params.pos_p) * 2.0f; + } + + return OK; +} + +void +MulticopterPositionControl::poll_subscriptions() +{ + bool updated; + + orb_check(_att_sub, &updated); + if (updated) + orb_copy(ORB_ID(vehicle_attitude), _att_sub, &_att); + + orb_check(_att_sp_sub, &updated); + if (updated) + orb_copy(ORB_ID(vehicle_attitude_setpoint), _att_sp_sub, &_att_sp); + + orb_check(_control_mode_sub, &updated); + if (updated) + orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode); + + orb_check(_manual_sub, &updated); + if (updated) + orb_copy(ORB_ID(manual_control_setpoint), _manual_sub, &_manual); + + orb_check(_arming_sub, &updated); + if (updated) + orb_copy(ORB_ID(actuator_armed), _arming_sub, &_arming); + + orb_check(_global_pos_sp_sub, &updated); + if (updated) + orb_copy(ORB_ID(vehicle_global_position_setpoint), _global_pos_sp_sub, &_global_pos_sp); +} + +float +MulticopterPositionControl::scale_control(float ctl, float end, float dz) +{ + if (ctl > dz) { + return (ctl - dz) / (end - dz); + + } else if (ctl < -dz) { + return (ctl + dz) / (end - dz); + + } else { + return 0.0f; + } +} + +void +MulticopterPositionControl::task_main_trampoline(int argc, char *argv[]) +{ + pos_control::g_control->task_main(); +} + +void +MulticopterPositionControl::task_main() +{ + warnx("started"); + + static int mavlink_fd; + mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); + mavlink_log_info(mavlink_fd, "[mpc] started"); + + /* + * do subscriptions + */ + _att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); + _att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); + _control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); + _params_sub = orb_subscribe(ORB_ID(parameter_update)); + _manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); + _arming_sub = orb_subscribe(ORB_ID(actuator_armed)); + _local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position)); + _global_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_global_position_setpoint)); + + parameters_update(true); + + /* initialize values of critical structs until first regular update */ + _arming.armed = false; + + /* get an initial update for all sensor and status data */ + poll_subscriptions(); + + bool reset_mission_sp = false; + bool global_pos_sp_valid = false; + bool reset_man_sp_z = true; + bool reset_man_sp_xy = true; + bool reset_int_z = true; + bool reset_int_z_manual = false; + bool reset_int_xy = true; + bool was_armed = false; + bool reset_auto_sp_xy = true; + bool reset_auto_sp_z = true; + bool reset_takeoff_sp = true; + + hrt_abstime t_prev = 0; + const float alt_ctl_dz = 0.2f; + const float pos_ctl_dz = 0.05f; + + float ref_alt = 0.0f; + hrt_abstime ref_alt_t = 0; + hrt_abstime local_ref_timestamp = 0; + + math::Vector<3> sp_move_rate; + sp_move_rate.zero(); + math::Vector<3> thrust_int; + thrust_int.zero(); + math::Matrix<3, 3> R; + R.identity(); + + /* wakeup source */ + struct pollfd fds[1]; + + /* Setup of loop */ + fds[0].fd = _local_pos_sub; + fds[0].events = POLLIN; + + while (!_task_should_exit) { + /* wait for up to 500ms for data */ + int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 500); + + /* timed out - periodic check for _task_should_exit */ + if (pret == 0) + continue; + + /* this is undesirable but not much we can do */ + if (pret < 0) { + warn("poll error %d, %d", pret, errno); + continue; + } + + orb_copy(ORB_ID(vehicle_local_position), _local_pos_sub, &_local_pos); + + poll_subscriptions(); + parameters_update(false); + + hrt_abstime t = hrt_absolute_time(); + float dt = _time_prev != 0 ? (t - _time_prev) * 0.000001f : 0.0f; + _time_prev = t; + + if (_control_mode.flag_armed && !_was_armed) { + /* reset setpoints and integrals on arming */ + reset_man_sp_z = true; + reset_man_sp_xy = true; + reset_auto_sp_z = true; + reset_auto_sp_xy = true; + reset_takeoff_sp = true; + reset_int_z = true; + reset_int_xy = true; + } + _was_armed = _control_mode.flag_armed; + + if (_control_mode.flag_control_altitude_enabled || + _control_mode.flag_control_position_enabled || + _control_mode.flag_control_climb_rate_enabled || + _control_mode.flag_control_velocity_enabled) { + + _pos(0) = _local_pos.x; + _pos(1) = _local_pos.y; + _pos(2) = _local_pos.z; + _vel(0) = _local_pos.vx; + _vel(1) = _local_pos.vy; + _vel(2) = _local_pos.vz; + + sp_move_rate.zero(); + + if (_control_mode.flag_control_manual_enabled) { + /* manual control */ + /* check for reference point updates and correct setpoint */ + if (_local_pos.ref_timestamp != ref_alt_t) { + if (ref_alt_t != 0) { + /* home alt changed, don't follow large ground level changes in manual flight */ + _pos_sp(2) += _local_pos.ref_alt - ref_alt; + } + + ref_alt_t = _local_pos.ref_timestamp; + ref_alt = _local_pos.ref_alt; + // TODO also correct XY setpoint + } + + /* reset setpoints to current position if needed */ + if (_control_mode.flag_control_altitude_enabled) { + if (reset_man_sp_z) { + reset_man_sp_z = false; + _pos_sp(2) = _pos(2); + mavlink_log_info(mavlink_fd, "[mpc] reset alt sp: %.2f", (double) - _pos_sp(2)); + } + + /* move altitude setpoint with throttle stick */ + sp_move_rate(2) = -scale_control(_manual.throttle - 0.5f, 0.5f, alt_ctl_dz); + } + + if (_control_mode.flag_control_position_enabled) { + if (reset_man_sp_xy) { + reset_man_sp_xy = false; + _pos_sp(0) = _pos(0); + _pos_sp(1) = _pos(1); + mavlink_log_info(mavlink_fd, "[mpc] reset pos sp: %.2f, %.2f", (double)_pos_sp(0), (double)_pos_sp(1)); + } + + /* move position setpoint with roll/pitch stick */ + sp_move_rate(0) = scale_control(-_manual.pitch / _params.rc_scale_pitch, 1.0f, pos_ctl_dz); + sp_move_rate(1) = scale_control(_manual.roll / _params.rc_scale_roll, 1.0f, pos_ctl_dz); + } + + /* limit setpoint move rate */ + float sp_move_norm = sp_move_rate.length(); + if (sp_move_norm > 1.0f) { + sp_move_rate /= sp_move_norm; + } + + /* scale to max speed and rotate around yaw */ + math::Matrix<3, 3> R_yaw_sp; + R_yaw_sp.from_euler(0.0f, 0.0f, _att_sp.yaw_body); + sp_move_rate = R_yaw_sp * sp_move_rate.emult(_params.vel_max); + + /* move position setpoint */ + _pos_sp += sp_move_rate * dt; + + /* check if position setpoint is too far from actual position */ + math::Vector<3> pos_sp_offs = (_pos_sp - _pos).edivide(_params.vel_max); + float pos_sp_offs_norm = pos_sp_offs.length(); + if (pos_sp_offs_norm > 1.0f) { + pos_sp_offs /= pos_sp_offs_norm; + _pos_sp = _pos + pos_sp_offs.emult(_params.vel_max); + } + + /* copy yaw setpoint to vehicle_local_position_setpoint topic */ + _local_pos_sp.yaw = _att_sp.yaw_body; + + /* local position setpoint is valid and can be used for auto loiter after position controlled mode */ + reset_auto_sp_xy = !_control_mode.flag_control_position_enabled; + reset_auto_sp_z = !_control_mode.flag_control_altitude_enabled; + reset_takeoff_sp = true; + + /* force reprojection of global setpoint after manual mode */ + reset_mission_sp = true; + } else { + // TODO AUTO + _pos_sp = _pos; + } + + _local_pos_sp.x = _pos_sp(0); + _local_pos_sp.y = _pos_sp(1); + _local_pos_sp.z = _pos_sp(2); + + /* publish local position setpoint */ + if (_local_pos_sp_pub > 0) { + orb_publish(ORB_ID(vehicle_local_position_setpoint), _local_pos_sp_pub, &_local_pos_sp); + + } else { + _local_pos_sp_pub = orb_advertise(ORB_ID(vehicle_local_position_setpoint), &_local_pos_sp); + } + + /* run position & altitude controllers, calculate velocity setpoint */ + math::Vector<3> pos_err = _pos_sp - _pos; + _vel_sp = pos_err.emult(_params.pos_p) + sp_move_rate.emult(_params.vel_ff); + + if (!_control_mode.flag_control_altitude_enabled) { + reset_man_sp_z = true; + _vel_sp(2) = 0.0f; + } + + if (!_control_mode.flag_control_position_enabled) { + reset_man_sp_xy = true; + _vel_sp(0) = 0.0f; + _vel_sp(1) = 0.0f; + } + + if (!_control_mode.flag_control_manual_enabled) { + /* limit 3D speed only in AUTO mode */ + float vel_sp_norm = _vel_sp.edivide(_params.vel_max).length(); + + if (vel_sp_norm > 1.0f) { + _vel_sp /= vel_sp_norm; + } + } + + _global_vel_sp.vx = _vel_sp(0); + _global_vel_sp.vy = _vel_sp(1); + _global_vel_sp.vz = _vel_sp(2); + + /* publish velocity setpoint */ + if (_global_vel_sp_pub > 0) { + orb_publish(ORB_ID(vehicle_global_velocity_setpoint), _global_vel_sp_pub, &_global_vel_sp); + + } else { + _global_vel_sp_pub = orb_advertise(ORB_ID(vehicle_global_velocity_setpoint), &_global_vel_sp); + } + + if (_control_mode.flag_control_climb_rate_enabled || _control_mode.flag_control_velocity_enabled) { + /* reset integrals if needed */ + if (_control_mode.flag_control_climb_rate_enabled) { + if (reset_int_z) { + reset_int_z = false; + float i = _params.thr_min; + + if (reset_int_z_manual) { + i = _manual.throttle; + + if (i < _params.thr_min) { + i = _params.thr_min; + + } else if (i > _params.thr_max) { + i = _params.thr_max; + } + } + + thrust_int(2) = -i; + mavlink_log_info(mavlink_fd, "[mpc] reset hovering thrust: %.2f", (double)i); + } + } else { + reset_int_z = true; + } + + if (_control_mode.flag_control_velocity_enabled) { + if (reset_int_xy) { + reset_int_xy = false; + thrust_int(0) = 0.0f; + thrust_int(1) = 0.0f; + mavlink_log_info(mavlink_fd, "[mpc] reset xy vel integral"); + } + } else { + reset_int_xy = true; + } + + /* calculate desired thrust vector in NED frame */ + math::Vector<3> vel_err = _vel_sp - _vel; + math::Vector<3> thrust_sp = vel_err.emult(_params.vel_p) + (vel_err - _vel_err_prev).emult(_params.vel_d) / dt + thrust_int; + + _vel_err_prev = vel_err; + + if (!_control_mode.flag_control_velocity_enabled) { + thrust_sp(0) = 0.0f; + thrust_sp(1) = 0.0f; + } + if (!_control_mode.flag_control_climb_rate_enabled) { + thrust_sp(2) = 0.0f; + } + + /* limit thrust vector and check for saturation */ + bool saturation_xy = false; + bool saturation_z = false; + + /* limit min lift */ + float thr_min = _params.thr_min; + if (!_control_mode.flag_control_velocity_enabled && thr_min < 0.0f) { + /* don't allow downside thrust direction in manual attitude mode */ + thr_min = 0.0f; + } + + if (-thrust_sp(2) < thr_min) { + thrust_sp(2) = -thr_min; + saturation_z = true; + } + + /* limit max tilt */ + float tilt = atan2f(math::Vector<2>(thrust_sp(0), thrust_sp(1)).length(), -thrust_sp(2)); + + if (tilt > _params.tilt_max && _params.thr_min >= 0.0f) { + /* crop horizontal component */ + float k = tanf(_params.tilt_max) / tanf(tilt); + thrust_sp(0) *= k; + thrust_sp(1) *= k; + saturation_xy = true; + } + + /* limit max thrust */ + float thrust_abs = thrust_sp.length(); + + if (thrust_abs > _params.thr_max) { + if (thrust_sp(2) < 0.0f) { + if (-thrust_sp(2) > _params.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; + 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_abs = math::Vector<2>(thrust_sp(0), thrust_sp(1)).length(); + float k = thrust_xy_max / thrust_xy_abs; + thrust_sp(0) *= k; + thrust_sp(1) *= k; + saturation_xy = true; + } + + } else { + /* Z component is negative, going down, simply limit thrust vector */ + float k = _params.thr_max / thrust_abs; + thrust_sp *= k; + saturation_xy = true; + saturation_z = true; + } + + thrust_abs = _params.thr_max; + } + + /* update integrals */ + math::Vector<3> m; + m(0) = (_control_mode.flag_control_velocity_enabled && !saturation_xy) ? 1.0f : 0.0f; + m(1) = m(0); + m(2) = (_control_mode.flag_control_climb_rate_enabled && !saturation_z) ? 1.0f : 0.0f; + + thrust_int += vel_err.emult(_params.vel_i) * dt; + + /* calculate attitude and thrust from thrust vector */ + if (_control_mode.flag_control_velocity_enabled) { + /* desired body_z axis = -normalize(thrust_vector) */ + math::Vector<3> body_x; + math::Vector<3> body_y; + math::Vector<3> body_z; + + if (thrust_abs > SIGMA) { + body_z = -thrust_sp / thrust_abs; + + } else { + /* no thrust, set Z axis to safe value */ + body_z.zero(); + body_z(2) = 1.0f; + } + + /* vector of desired yaw direction in XY plane, rotated by PI/2 */ + math::Vector<3> y_C(-sinf(_att_sp.yaw_body), cosf(_att_sp.yaw_body), 0.0f); + + if (fabsf(body_z(2)) > SIGMA) { + /* desired body_x axis, orthogonal to body_z */ + body_x = y_C % body_z; + + /* keep nose to front while inverted upside down */ + if (body_z(2) < 0.0f) { + body_x = -body_x; + } + body_x.normalize(); + } else { + /* desired thrust is in XY plane, set X downside to construct correct matrix, + * but yaw component will not be used actually */ + body_x.zero(); + body_x(2) = 1.0f; + } + + /* desired body_y axis */ + body_y = body_z % body_x; + + /* fill rotation matrix */ + for (int i = 0; i < 3; i++) { + R(i, 0) = body_x(i); + R(i, 1) = body_y(i); + R(i, 2) = body_z(i); + } + + /* copy rotation matrix to attitude setpoint topic */ + memcpy(&_att_sp.R_body[0][0], R.data, sizeof(_att_sp.R_body)); + _att_sp.R_valid = true; + + /* calculate euler angles, for logging only, must not be used for control */ + math::Vector<3> euler = R.to_euler(); + _att_sp.roll_body = euler(0); + _att_sp.pitch_body = euler(1); + /* yaw already used to construct rot matrix, but actual rotation matrix can have different yaw near singularity */ + + } else { + /* thrust compensation for altitude only control mode */ + float att_comp; + + if (_att.R[2][2] > TILT_COS_MAX) + att_comp = 1.0f / _att.R[2][2]; + else if (_att.R[2][2] > 0.0f) + att_comp = ((1.0f / TILT_COS_MAX - 1.0f) / TILT_COS_MAX) * _att.R[2][2] + 1.0f; + else + att_comp = 1.0f; + + thrust_abs *= att_comp; + } + + _att_sp.thrust = thrust_abs; + + _att_sp.timestamp = hrt_absolute_time(); + + /* publish attitude setpoint */ + if (_att_sp_pub > 0) { + orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &_att_sp); + + } else { + _att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_att_sp); + } + + } else { + reset_int_z = true; + } + } else { + /* position controller disabled, reset setpoints */ + reset_man_sp_z = true; + reset_man_sp_xy = true; + reset_int_z = true; + reset_int_xy = true; + reset_mission_sp = true; + reset_auto_sp_xy = true; + reset_auto_sp_z = true; + } + + /* reset altitude controller integral (hovering throttle) to manual throttle after manual throttle control */ + reset_int_z_manual = _control_mode.flag_armed && _control_mode.flag_control_manual_enabled && !_control_mode.flag_control_climb_rate_enabled; + } + + warnx("stopped"); + mavlink_log_info(mavlink_fd, "[mpc] stopped"); + + _control_task = -1; + _exit(0); +} + +int +MulticopterPositionControl::start() +{ + ASSERT(_control_task == -1); + + /* start the task */ + _control_task = task_spawn_cmd("mc_pos_control", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 2048, + (main_t)&MulticopterPositionControl::task_main_trampoline, + nullptr); + + if (_control_task < 0) { + warn("task start failed"); + return -errno; + } + + return OK; +} + +int mc_pos_control_main(int argc, char *argv[]) +{ + if (argc < 1) + errx(1, "usage: mc_pos_control {start|stop|status}"); + + if (!strcmp(argv[1], "start")) { + + if (pos_control::g_control != nullptr) + errx(1, "already running"); + + pos_control::g_control = new MulticopterPositionControl; + + if (pos_control::g_control == nullptr) + errx(1, "alloc failed"); + + if (OK != pos_control::g_control->start()) { + delete pos_control::g_control; + pos_control::g_control = nullptr; + err(1, "start failed"); + } + + exit(0); + } + + if (!strcmp(argv[1], "stop")) { + if (pos_control::g_control == nullptr) + errx(1, "not running"); + + delete pos_control::g_control; + pos_control::g_control = nullptr; + exit(0); + } + + if (!strcmp(argv[1], "status")) { + if (pos_control::g_control) { + errx(0, "running"); + + } else { + errx(1, "not running"); + } + } + + warnx("unrecognized command"); + return 1; +} diff --git a/src/modules/mc_pos_control/mc_pos_control_params.c b/src/modules/mc_pos_control/mc_pos_control_params.c new file mode 100644 index 0000000000..2fe70698fc --- /dev/null +++ b/src/modules/mc_pos_control/mc_pos_control_params.c @@ -0,0 +1,25 @@ +/* + * mc_pos_control_params.c + * + * Created on: 26.12.2013 + * Author: ton + */ + +#include + +/* multicopter position controller parameters */ +PARAM_DEFINE_FLOAT(MPC_THR_MIN, 0.0f); +PARAM_DEFINE_FLOAT(MPC_THR_MAX, 1.0f); +PARAM_DEFINE_FLOAT(MPC_Z_P, 1.0f); +PARAM_DEFINE_FLOAT(MPC_Z_VEL_P, 0.1f); +PARAM_DEFINE_FLOAT(MPC_Z_VEL_I, 0.05f); +PARAM_DEFINE_FLOAT(MPC_Z_VEL_D, 0.0f); +PARAM_DEFINE_FLOAT(MPC_Z_VEL_MAX, 5.0f); +PARAM_DEFINE_FLOAT(MPC_Z_FF, 0.5f); +PARAM_DEFINE_FLOAT(MPC_XY_P, 1.0f); +PARAM_DEFINE_FLOAT(MPC_XY_VEL_P, 0.1f); +PARAM_DEFINE_FLOAT(MPC_XY_VEL_I, 0.01f); +PARAM_DEFINE_FLOAT(MPC_XY_VEL_D, 0.01f); +PARAM_DEFINE_FLOAT(MPC_XY_VEL_MAX, 5.0f); +PARAM_DEFINE_FLOAT(MPC_XY_FF, 0.5f); +PARAM_DEFINE_FLOAT(MPC_TILT_MAX, 1.0f); diff --git a/src/modules/multirotor_pos_control/module.mk b/src/modules/mc_pos_control/module.mk similarity index 91% rename from src/modules/multirotor_pos_control/module.mk rename to src/modules/mc_pos_control/module.mk index bc4b48fb40..1d98173fa3 100644 --- a/src/modules/multirotor_pos_control/module.mk +++ b/src/modules/mc_pos_control/module.mk @@ -32,11 +32,10 @@ ############################################################################ # -# Build multirotor position control +# Build multicopter position controller # -MODULE_COMMAND = multirotor_pos_control +MODULE_COMMAND = mc_pos_control -SRCS = multirotor_pos_control.c \ - multirotor_pos_control_params.c \ - thrust_pid.c +SRCS = mc_pos_control_main.cpp \ + mc_pos_control_params.c diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c deleted file mode 100644 index 378d3fda7d..0000000000 --- a/src/modules/multirotor_pos_control/multirotor_pos_control.c +++ /dev/null @@ -1,864 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. - * Author: Anton Babushkin - * - * 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 multirotor_pos_control.c - * - * Multirotor position controller - */ - -#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 - -#include "multirotor_pos_control_params.h" -#include "thrust_pid.h" - -#define TILT_COS_MAX 0.7f -#define SIGMA 0.000001f - -static bool thread_should_exit = false; /**< Deamon exit flag */ -static bool thread_running = false; /**< Deamon status flag */ -static int deamon_task; /**< Handle of deamon task / thread */ - -__EXPORT int multirotor_pos_control_main(int argc, char *argv[]); - -/** - * Mainloop of position controller. - */ -static int multirotor_pos_control_thread_main(int argc, char *argv[]); - -/** - * Print the correct usage. - */ -static void usage(const char *reason); - -static float scale_control(float ctl, float end, float dz); - -static float norm(float x, float y); - -static void usage(const char *reason) -{ - if (reason) - fprintf(stderr, "%s\n", reason); - - fprintf(stderr, "usage: multirotor_pos_control {start|stop|status}\n\n"); - exit(1); -} - -/** - * The deamon app only briefly exists to start - * the background job. The stack size assigned in the - * Makefile does only apply to this management task. - * - * The actual stack size should be set in the call - * to task_spawn(). - */ -int multirotor_pos_control_main(int argc, char *argv[]) -{ - if (argc < 1) - usage("missing command"); - - if (!strcmp(argv[1], "start")) { - - if (thread_running) { - warnx("already running"); - /* this is not an error */ - exit(0); - } - - warnx("start"); - thread_should_exit = false; - deamon_task = task_spawn_cmd("multirotor_pos_control", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 60, - 4096, - multirotor_pos_control_thread_main, - (argv) ? (const char **)&argv[2] : (const char **)NULL); - exit(0); - } - - if (!strcmp(argv[1], "stop")) { - warnx("stop"); - thread_should_exit = true; - exit(0); - } - - if (!strcmp(argv[1], "status")) { - if (thread_running) { - warnx("app is running"); - - } else { - warnx("app not started"); - } - - exit(0); - } - - usage("unrecognized command"); - exit(1); -} - -static float scale_control(float ctl, float end, float dz) -{ - if (ctl > dz) { - return (ctl - dz) / (end - dz); - - } else if (ctl < -dz) { - return (ctl + dz) / (end - dz); - - } else { - return 0.0f; - } -} - -static float norm(float x, float y) -{ - return sqrtf(x * x + y * y); -} - -static float norm3(float x, float y, float z) -{ - return sqrtf(x * x + y * y + z * z); -} - -static void cross3(float a[3], float b[3], float res[3]) -{ - res[0] = a[1] * b[2] - a[2] * b[1]; - res[1] = a[2] * b[0] - a[0] * b[2]; - res[2] = a[0] * b[1] - a[1] * b[0]; -} - -static float normalize3(float x[3]) -{ - float n = sqrtf(x[0] * x[0] + x[1] * x[1] + x[2] * x[2]); - - if (n > 0.0f) { - x[0] /= n; - x[1] /= n; - x[2] /= n; - } - - return n; -} - -static float rt_atan2f_snf(float u0, float u1) -{ - float y; - int32_t b_u0; - int32_t b_u1; - - if (isnanf(u0) || isnanf(u1)) { - y = NAN; - - } else if (isinff(u0) && isinff(u1)) { - if (u0 > 0.0f) { - b_u0 = 1; - - } else { - b_u0 = -1; - } - - if (u1 > 0.0f) { - b_u1 = 1; - - } else { - b_u1 = -1; - } - - y = atan2f((float)b_u0, (float)b_u1); - - } else if (u1 == 0.0f) { - if (u0 > 0.0f) { - y = M_PI_F / 2.0f; - - } else if (u0 < 0.0f) { - y = -(M_PI_F / 2.0f); - - } else { - y = 0.0F; - } - - } else { - y = atan2f(u0, u1); - } - - return y; -} - -static int multirotor_pos_control_thread_main(int argc, char *argv[]) -{ - /* welcome user */ - warnx("started"); - static int mavlink_fd; - mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); - mavlink_log_info(mavlink_fd, "[mpc] started"); - - /* structures */ - struct vehicle_control_mode_s control_mode; - memset(&control_mode, 0, sizeof(control_mode)); - struct vehicle_attitude_s att; - memset(&att, 0, sizeof(att)); - struct vehicle_attitude_setpoint_s att_sp; - memset(&att_sp, 0, sizeof(att_sp)); - struct manual_control_setpoint_s manual; - memset(&manual, 0, sizeof(manual)); - struct vehicle_local_position_s local_pos; - memset(&local_pos, 0, sizeof(local_pos)); - struct vehicle_local_position_setpoint_s local_pos_sp; - memset(&local_pos_sp, 0, sizeof(local_pos_sp)); - struct vehicle_global_position_setpoint_s global_pos_sp; - memset(&global_pos_sp, 0, sizeof(global_pos_sp)); - struct vehicle_global_velocity_setpoint_s global_vel_sp; - memset(&global_vel_sp, 0, sizeof(global_vel_sp)); - - /* subscribe to attitude, motor setpoints and system state */ - int param_sub = orb_subscribe(ORB_ID(parameter_update)); - int control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); - int att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); - int att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); - int manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); - int local_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint)); - int local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position)); - int global_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_global_position_setpoint)); - - /* publish setpoint */ - orb_advert_t local_pos_sp_pub = orb_advertise(ORB_ID(vehicle_local_position_setpoint), &local_pos_sp); - orb_advert_t global_vel_sp_pub = orb_advertise(ORB_ID(vehicle_global_velocity_setpoint), &global_vel_sp); - orb_advert_t att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp); - - bool reset_mission_sp = false; - bool global_pos_sp_valid = false; - bool reset_man_sp_z = true; - bool reset_man_sp_xy = true; - bool reset_int_z = true; - bool reset_int_z_manual = false; - bool reset_int_xy = true; - bool was_armed = false; - bool reset_auto_sp_xy = true; - bool reset_auto_sp_z = true; - bool reset_takeoff_sp = true; - - hrt_abstime t_prev = 0; - const float alt_ctl_dz = 0.2f; - const float pos_ctl_dz = 0.05f; - - float ref_alt = 0.0f; - hrt_abstime ref_alt_t = 0; - uint64_t local_ref_timestamp = 0; - - float thrust_int[3] = { 0.0f, 0.0f, 0.0f }; - - thread_running = true; - - struct multirotor_position_control_params params; - struct multirotor_position_control_param_handles params_h; - parameters_init(¶ms_h); - parameters_update(¶ms_h, ¶ms); - - bool param_updated = true; - - while (!thread_should_exit) { - - if (!param_updated) - orb_check(param_sub, ¶m_updated); - - if (param_updated) { - /* clear updated flag */ - struct parameter_update_s ps; - orb_copy(ORB_ID(parameter_update), param_sub, &ps); - - /* update params */ - parameters_update(¶ms_h, ¶ms); - } - - bool updated; - - orb_check(control_mode_sub, &updated); - - if (updated) { - orb_copy(ORB_ID(vehicle_control_mode), control_mode_sub, &control_mode); - } - - orb_check(global_pos_sp_sub, &updated); - - if (updated) { - orb_copy(ORB_ID(vehicle_global_position_setpoint), global_pos_sp_sub, &global_pos_sp); - global_pos_sp_valid = true; - reset_mission_sp = true; - } - - hrt_abstime t = hrt_absolute_time(); - float dt; - - if (t_prev != 0) { - dt = (t - t_prev) * 0.000001f; - - } else { - dt = 0.0f; - } - t_prev = t; - - if (control_mode.flag_armed && !was_armed) { - /* reset setpoints and integrals on arming */ - reset_man_sp_z = true; - reset_man_sp_xy = true; - reset_auto_sp_z = true; - reset_auto_sp_xy = true; - reset_takeoff_sp = true; - reset_int_z = true; - reset_int_xy = true; - } - - was_armed = control_mode.flag_armed; - - if (control_mode.flag_control_altitude_enabled || control_mode.flag_control_position_enabled || control_mode.flag_control_climb_rate_enabled || control_mode.flag_control_velocity_enabled) { - orb_copy(ORB_ID(manual_control_setpoint), manual_sub, &manual); - orb_copy(ORB_ID(vehicle_attitude), att_sub, &att); - orb_copy(ORB_ID(vehicle_attitude_setpoint), att_sp_sub, &att_sp); - orb_copy(ORB_ID(vehicle_local_position), local_pos_sub, &local_pos); - - float z_sp_offs_max = params.z_vel_max / params.z_p * 2.0f; - float xy_sp_offs_max = params.xy_vel_max / params.xy_p * 2.0f; - float sp_move_rate[3] = { 0.0f, 0.0f, 0.0f }; - - if (control_mode.flag_control_manual_enabled) { - /* manual control */ - /* check for reference point updates and correct setpoint */ - if (local_pos.ref_timestamp != ref_alt_t) { - if (ref_alt_t != 0) { - /* home alt changed, don't follow large ground level changes in manual flight */ - local_pos_sp.z += local_pos.ref_alt - ref_alt; - } - - ref_alt_t = local_pos.ref_timestamp; - ref_alt = local_pos.ref_alt; - // TODO also correct XY setpoint - } - - /* reset setpoints to current position if needed */ - if (control_mode.flag_control_altitude_enabled) { - if (reset_man_sp_z) { - reset_man_sp_z = false; - local_pos_sp.z = local_pos.z; - mavlink_log_info(mavlink_fd, "[mpc] reset alt sp: %.2f", (double) - local_pos_sp.z); - } - - /* move altitude setpoint with throttle stick */ - float z_sp_ctl = scale_control(manual.throttle - 0.5f, 0.5f, alt_ctl_dz); - - if (z_sp_ctl != 0.0f) { - sp_move_rate[2] = -z_sp_ctl * params.z_vel_max; - local_pos_sp.z += sp_move_rate[2] * dt; - - if (local_pos_sp.z > local_pos.z + z_sp_offs_max) { - local_pos_sp.z = local_pos.z + z_sp_offs_max; - - } else if (local_pos_sp.z < local_pos.z - z_sp_offs_max) { - local_pos_sp.z = local_pos.z - z_sp_offs_max; - } - } - } - - if (control_mode.flag_control_position_enabled) { - if (reset_man_sp_xy) { - reset_man_sp_xy = false; - local_pos_sp.x = local_pos.x; - local_pos_sp.y = local_pos.y; - mavlink_log_info(mavlink_fd, "[mpc] reset pos sp: %.2f, %.2f", (double)local_pos_sp.x, (double)local_pos_sp.y); - } - - /* move position setpoint with roll/pitch stick */ - float pos_pitch_sp_ctl = scale_control(-manual.pitch / params.rc_scale_pitch, 1.0f, pos_ctl_dz); - float pos_roll_sp_ctl = scale_control(manual.roll / params.rc_scale_roll, 1.0f, pos_ctl_dz); - - if (pos_pitch_sp_ctl != 0.0f || pos_roll_sp_ctl != 0.0f) { - /* calculate direction and increment of control in NED frame */ - float xy_sp_ctl_dir = att_sp.yaw_body + atan2f(pos_roll_sp_ctl, pos_pitch_sp_ctl); - float xy_sp_ctl_speed = norm(pos_pitch_sp_ctl, pos_roll_sp_ctl) * params.xy_vel_max; - sp_move_rate[0] = cosf(xy_sp_ctl_dir) * xy_sp_ctl_speed; - sp_move_rate[1] = sinf(xy_sp_ctl_dir) * xy_sp_ctl_speed; - local_pos_sp.x += sp_move_rate[0] * dt; - local_pos_sp.y += sp_move_rate[1] * dt; - /* limit maximum setpoint from position offset and preserve direction - * fail safe, should not happen in normal operation */ - float pos_vec_x = local_pos_sp.x - local_pos.x; - float pos_vec_y = local_pos_sp.y - local_pos.y; - float pos_vec_norm = norm(pos_vec_x, pos_vec_y) / xy_sp_offs_max; - - if (pos_vec_norm > 1.0f) { - local_pos_sp.x = local_pos.x + pos_vec_x / pos_vec_norm; - local_pos_sp.y = local_pos.y + pos_vec_y / pos_vec_norm; - } - } - } - - /* copy yaw setpoint to vehicle_local_position_setpoint topic */ - local_pos_sp.yaw = att_sp.yaw_body; - - /* local position setpoint is valid and can be used for auto loiter after position controlled mode */ - reset_auto_sp_xy = !control_mode.flag_control_position_enabled; - reset_auto_sp_z = !control_mode.flag_control_altitude_enabled; - reset_takeoff_sp = true; - - /* force reprojection of global setpoint after manual mode */ - reset_mission_sp = true; - - } else if (control_mode.flag_control_auto_enabled) { - /* AUTO mode, use global setpoint */ - if (control_mode.auto_state == NAVIGATION_STATE_AUTO_READY) { - reset_auto_sp_xy = true; - reset_auto_sp_z = true; - - } else if (control_mode.auto_state == NAVIGATION_STATE_AUTO_TAKEOFF) { - if (reset_takeoff_sp) { - reset_takeoff_sp = false; - local_pos_sp.x = local_pos.x; - local_pos_sp.y = local_pos.y; - local_pos_sp.z = - params.takeoff_alt - params.takeoff_gap; - att_sp.yaw_body = att.yaw; - mavlink_log_info(mavlink_fd, "[mpc] takeoff sp: %.2f %.2f %.2f", (double)local_pos_sp.x, (double)local_pos_sp.y, (double) - local_pos_sp.z); - } - - reset_auto_sp_xy = false; - reset_auto_sp_z = true; - - } else if (control_mode.auto_state == NAVIGATION_STATE_AUTO_RTL) { - // TODO - reset_auto_sp_xy = true; - reset_auto_sp_z = true; - - } else if (control_mode.auto_state == NAVIGATION_STATE_AUTO_MISSION) { - /* init local projection using local position ref */ - if (local_pos.ref_timestamp != local_ref_timestamp) { - reset_mission_sp = true; - local_ref_timestamp = local_pos.ref_timestamp; - double lat_home = local_pos.ref_lat * 1e-7; - double lon_home = local_pos.ref_lon * 1e-7; - map_projection_init(lat_home, lon_home); - mavlink_log_info(mavlink_fd, "[mpc] local pos ref: %.7f, %.7f", (double)lat_home, (double)lon_home); - } - - if (reset_mission_sp) { - reset_mission_sp = false; - /* update global setpoint projection */ - - if (global_pos_sp_valid) { - /* global position setpoint valid, use it */ - double sp_lat = global_pos_sp.lat * 1e-7; - double sp_lon = global_pos_sp.lon * 1e-7; - /* project global setpoint to local setpoint */ - map_projection_project(sp_lat, sp_lon, &(local_pos_sp.x), &(local_pos_sp.y)); - - if (global_pos_sp.altitude_is_relative) { - local_pos_sp.z = -global_pos_sp.altitude; - - } else { - local_pos_sp.z = local_pos.ref_alt - global_pos_sp.altitude; - } - - /* update yaw setpoint only if value is valid */ - if (isfinite(global_pos_sp.yaw) && fabsf(global_pos_sp.yaw) < M_TWOPI_F) { - att_sp.yaw_body = global_pos_sp.yaw; - } - - mavlink_log_info(mavlink_fd, "[mpc] new sp: %.7f, %.7f (%.2f, %.2f)", (double)sp_lat, sp_lon, (double)local_pos_sp.x, (double)local_pos_sp.y); - - } else { - if (reset_auto_sp_xy) { - reset_auto_sp_xy = false; - /* local position setpoint is invalid, - * use current position as setpoint for loiter */ - local_pos_sp.x = local_pos.x; - local_pos_sp.y = local_pos.y; - local_pos_sp.yaw = att.yaw; - } - - if (reset_auto_sp_z) { - reset_auto_sp_z = false; - local_pos_sp.z = local_pos.z; - } - - mavlink_log_info(mavlink_fd, "[mpc] no global pos sp, loiter: %.2f, %.2f", (double)local_pos_sp.x, (double)local_pos_sp.y); - } - } - - reset_auto_sp_xy = true; - reset_auto_sp_z = true; - } - - if (control_mode.auto_state != NAVIGATION_STATE_AUTO_TAKEOFF) { - reset_takeoff_sp = true; - } - - if (control_mode.auto_state != NAVIGATION_STATE_AUTO_MISSION) { - reset_mission_sp = true; - } - - /* copy yaw setpoint to vehicle_local_position_setpoint topic */ - local_pos_sp.yaw = att_sp.yaw_body; - - /* reset setpoints after AUTO mode */ - reset_man_sp_xy = true; - reset_man_sp_z = true; - - } else { - /* no control (failsafe), loiter or stay on ground */ - if (local_pos.landed) { - /* landed: move setpoint down */ - /* in air: hold altitude */ - if (local_pos_sp.z < 5.0f) { - /* set altitude setpoint to 5m under ground, - * don't set it too deep to avoid unexpected landing in case of false "landed" signal */ - local_pos_sp.z = 5.0f; - mavlink_log_info(mavlink_fd, "[mpc] landed, set alt: %.2f", (double) - local_pos_sp.z); - } - - reset_man_sp_z = true; - - } else { - /* in air: hold altitude */ - if (reset_man_sp_z) { - reset_man_sp_z = false; - local_pos_sp.z = local_pos.z; - mavlink_log_info(mavlink_fd, "[mpc] set loiter alt: %.2f", (double) - local_pos_sp.z); - } - - reset_auto_sp_z = false; - } - - if (control_mode.flag_control_position_enabled) { - if (reset_man_sp_xy) { - reset_man_sp_xy = false; - local_pos_sp.x = local_pos.x; - local_pos_sp.y = local_pos.y; - local_pos_sp.yaw = att.yaw; - att_sp.yaw_body = att.yaw; - mavlink_log_info(mavlink_fd, "[mpc] set loiter pos: %.2f %.2f", (double)local_pos_sp.x, (double)local_pos_sp.y); - } - - reset_auto_sp_xy = false; - } - } - - /* publish local position setpoint */ - orb_publish(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_pub, &local_pos_sp); - - /* run position & altitude controllers, calculate velocity setpoint */ - if (control_mode.flag_control_altitude_enabled) { - global_vel_sp.vz = (local_pos_sp.z - local_pos.z) * params.z_p + sp_move_rate[2] * params.z_ff; - - } else { - reset_man_sp_z = true; - global_vel_sp.vz = 0.0f; - } - - if (control_mode.flag_control_position_enabled) { - /* calculate velocity set point in NED frame */ - global_vel_sp.vx = (local_pos_sp.x - local_pos.x) * params.xy_p + sp_move_rate[0] * params.xy_ff; - global_vel_sp.vy = (local_pos_sp.y - local_pos.y) * params.xy_p + sp_move_rate[1] * params.xy_ff; - - } else { - reset_man_sp_xy = true; - global_vel_sp.vx = 0.0f; - global_vel_sp.vy = 0.0f; - } - - if (!control_mode.flag_control_manual_enabled) { - /* limit 3D speed only in AUTO mode */ - float vel_sp_norm = norm3(global_vel_sp.vx / params.xy_vel_max, global_vel_sp.vy / params.xy_vel_max, global_vel_sp.vz / params.z_vel_max); - - if (vel_sp_norm > 1.0f) { - global_vel_sp.vx /= vel_sp_norm; - global_vel_sp.vy /= vel_sp_norm; - global_vel_sp.vz /= vel_sp_norm; - } - } - - /* publish new velocity setpoint */ - orb_publish(ORB_ID(vehicle_global_velocity_setpoint), global_vel_sp_pub, &global_vel_sp); - // TODO subscribe to velocity setpoint if altitude/position control disabled - - if (control_mode.flag_control_climb_rate_enabled || control_mode.flag_control_velocity_enabled) { - /* calculate desired thrust vector in NED frame */ - float thrust_sp[3] = { 0.0f, 0.0f, 0.0f }; - - if (reset_int_z) { - reset_int_z = false; - float i = params.thr_min; - - if (reset_int_z_manual) { - i = manual.throttle; - - if (i < params.thr_min) { - i = params.thr_min; - - } else if (i > params.thr_max) { - i = params.thr_max; - } - } - - thrust_int[2] = -i; - mavlink_log_info(mavlink_fd, "[mpc] reset hovering thrust: %.2f", (double)i); - } - - thrust_sp[2] = (global_vel_sp.vz - local_pos.vz) * params.z_vel_p + thrust_int[2]; - - if (control_mode.flag_control_velocity_enabled) { - if (reset_int_xy) { - reset_int_xy = false; - thrust_int[0] = 0.0f; - thrust_int[1] = 0.0f; - mavlink_log_info(mavlink_fd, "[mpc] reset xy vel integral"); - } - - thrust_sp[0] = (global_vel_sp.vx - local_pos.vx) * params.xy_vel_p + thrust_int[0]; - thrust_sp[1] = (global_vel_sp.vy - local_pos.vy) * params.xy_vel_p + thrust_int[1]; - - } else { - reset_int_xy = true; - } - - /* limit thrust vector and check for saturation */ - bool saturation_xy = false; - bool saturation_z = false; - - /* limit min lift */ - float thr_min = params.thr_min; - if (!control_mode.flag_control_velocity_enabled && thr_min < 0.0f) { - /* don't allow downside thrust direction in manual attitude mode */ - thr_min = 0.0f; - } - - if (-thrust_sp[2] < thr_min) { - thrust_sp[2] = -thr_min; - saturation_z = true; - } - - /* limit max tilt */ - float tilt = atan2f(norm(thrust_sp[0], thrust_sp[1]), -thrust_sp[2]); - - if (tilt > params.tilt_max && params.thr_min > 0.0f) { - /* crop horizontal component */ - float k = tanf(params.tilt_max) / tanf(tilt); - thrust_sp[0] *= k; - thrust_sp[1] *= k; - saturation_xy = true; - } - - /* limit max thrust */ - float thrust_abs = sqrtf(thrust_sp[0] * thrust_sp[0] + thrust_sp[1] * thrust_sp[1] + thrust_sp[2] * thrust_sp[2]); - - if (thrust_abs > params.thr_max) { - if (thrust_sp[2] < 0.0f) { - if (-thrust_sp[2] > params.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; - 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_abs = norm(thrust_sp[0], thrust_sp[1]); - float k = thrust_xy_max / thrust_xy_abs; - thrust_sp[0] *= k; - thrust_sp[1] *= k; - saturation_xy = true; - } - - } else { - /* Z component is negative, going down, simply limit thrust vector */ - float k = params.thr_max / thrust_abs; - thrust_sp[0] *= k; - thrust_sp[1] *= k; - thrust_sp[2] *= k; - saturation_xy = true; - saturation_z = true; - } - - thrust_abs = params.thr_max; - } - - /* update integrals */ - if (control_mode.flag_control_velocity_enabled && !saturation_xy) { - thrust_int[0] += (global_vel_sp.vx - local_pos.vx) * params.xy_vel_i * dt; - thrust_int[1] += (global_vel_sp.vy - local_pos.vy) * params.xy_vel_i * dt; - } - - if (control_mode.flag_control_climb_rate_enabled && !saturation_z) { - thrust_int[2] += (global_vel_sp.vz - local_pos.vz) * params.z_vel_i * dt; - } - - /* calculate attitude and thrust from thrust vector */ - if (control_mode.flag_control_velocity_enabled) { - /* desired body_z axis = -normalize(thrust_vector) */ - float body_x[3]; - float body_y[3]; - float body_z[3]; - - if (thrust_abs > SIGMA) { - body_z[0] = -thrust_sp[0] / thrust_abs; - body_z[1] = -thrust_sp[1] / thrust_abs; - body_z[2] = -thrust_sp[2] / thrust_abs; - - } else { - /* no thrust, set Z axis to safe value */ - body_z[0] = 0.0f; - body_z[1] = 0.0f; - body_z[2] = 1.0f; - } - - /* vector of desired yaw direction in XY plane, rotated by PI/2 */ - float y_C[3]; - y_C[0] = -sinf(att_sp.yaw_body); - y_C[1] = cosf(att_sp.yaw_body); - y_C[2] = 0; - - if (fabsf(body_z[2]) > SIGMA) { - /* desired body_x axis = cross(y_C, body_z) */ - cross3(y_C, body_z, body_x); - - /* keep nose to front while inverted upside down */ - if (body_z[2] < 0.0f) { - body_x[0] = -body_x[0]; - body_x[1] = -body_x[1]; - body_x[2] = -body_x[2]; - } - normalize3(body_x); - } else { - /* desired thrust is in XY plane, set X downside to construct correct matrix, - * but yaw component will not be used actually */ - body_x[0] = 0.0f; - body_x[1] = 0.0f; - body_x[2] = 1.0f; - } - - /* desired body_y axis = cross(body_z, body_x) */ - cross3(body_z, body_x, body_y); - - /* fill rotation matrix */ - for (int i = 0; i < 3; i++) { - att_sp.R_body[i][0] = body_x[i]; - att_sp.R_body[i][1] = body_y[i]; - att_sp.R_body[i][2] = body_z[i]; - } - - att_sp.R_valid = true; - - /* calculate roll, pitch from rotation matrix */ - att_sp.roll_body = rt_atan2f_snf(att_sp.R_body[2][1], att_sp.R_body[2][2]); - att_sp.pitch_body = -asinf(att_sp.R_body[2][0]); - /* yaw already used to construct rot matrix, but actual rotation matrix can have different yaw near singularity */ - //att_sp.yaw_body = rt_atan2f_snf(att_sp.R_body[1][0], att_sp.R_body[0][0]); - - } else { - /* thrust compensation for altitude only control mode */ - float att_comp; - - if (att.R[2][2] > TILT_COS_MAX) - att_comp = 1.0f / att.R[2][2]; - else if (att.R[2][2] > 0.0f) - att_comp = ((1.0f / TILT_COS_MAX - 1.0f) / TILT_COS_MAX) * att.R[2][2] + 1.0f; - else - att_comp = 1.0f; - - thrust_abs *= att_comp; - } - - att_sp.thrust = thrust_abs; - - att_sp.timestamp = hrt_absolute_time(); - - /* publish new attitude setpoint */ - orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); - - } else { - reset_int_z = true; - } - - } else { - /* position controller disabled, reset setpoints */ - reset_man_sp_z = true; - reset_man_sp_xy = true; - reset_int_z = true; - reset_int_xy = true; - reset_mission_sp = true; - reset_auto_sp_xy = true; - reset_auto_sp_z = true; - } - - /* reset altitude controller integral (hovering throttle) to manual throttle after manual throttle control */ - reset_int_z_manual = control_mode.flag_armed && control_mode.flag_control_manual_enabled && !control_mode.flag_control_climb_rate_enabled; - - /* run at approximately 50 Hz */ - usleep(20000); - } - - warnx("stopped"); - mavlink_log_info(mavlink_fd, "[mpc] stopped"); - - thread_running = false; - - fflush(stdout); - return 0; -} - diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control_params.c b/src/modules/multirotor_pos_control/multirotor_pos_control_params.c deleted file mode 100644 index bebcdb3f5a..0000000000 --- a/src/modules/multirotor_pos_control/multirotor_pos_control_params.c +++ /dev/null @@ -1,106 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. - * Author: Anton Babushkin - * - * 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 multirotor_pos_control_params.c - * - * Parameters for multirotor_pos_control - */ - -#include "multirotor_pos_control_params.h" - -/* controller parameters */ -PARAM_DEFINE_FLOAT(MPC_THR_MIN, 0.2f); -PARAM_DEFINE_FLOAT(MPC_THR_MAX, 0.8f); -PARAM_DEFINE_FLOAT(MPC_Z_P, 1.0f); -PARAM_DEFINE_FLOAT(MPC_Z_VEL_P, 0.2f); -PARAM_DEFINE_FLOAT(MPC_Z_VEL_I, 0.1f); -PARAM_DEFINE_FLOAT(MPC_Z_VEL_MAX, 3.0f); -PARAM_DEFINE_FLOAT(MPC_Z_FF, 0.5f); -PARAM_DEFINE_FLOAT(MPC_XY_P, 0.5f); -PARAM_DEFINE_FLOAT(MPC_XY_VEL_P, 0.1f); -PARAM_DEFINE_FLOAT(MPC_XY_VEL_I, 0.05f); -PARAM_DEFINE_FLOAT(MPC_XY_VEL_MAX, 5.0f); -PARAM_DEFINE_FLOAT(MPC_XY_FF, 0.5f); -PARAM_DEFINE_FLOAT(MPC_TILT_MAX, 0.5f); - -int parameters_init(struct multirotor_position_control_param_handles *h) -{ - h->takeoff_alt = param_find("NAV_TAKEOFF_ALT"); - h->takeoff_gap = param_find("NAV_TAKEOFF_GAP"); - h->thr_min = param_find("MPC_THR_MIN"); - h->thr_max = param_find("MPC_THR_MAX"); - h->z_p = param_find("MPC_Z_P"); - h->z_vel_p = param_find("MPC_Z_VEL_P"); - h->z_vel_i = param_find("MPC_Z_VEL_I"); - h->z_vel_max = param_find("MPC_Z_VEL_MAX"); - h->z_ff = param_find("MPC_Z_FF"); - h->xy_p = param_find("MPC_XY_P"); - h->xy_vel_p = param_find("MPC_XY_VEL_P"); - h->xy_vel_i = param_find("MPC_XY_VEL_I"); - h->xy_vel_max = param_find("MPC_XY_VEL_MAX"); - h->xy_ff = param_find("MPC_XY_FF"); - h->tilt_max = param_find("MPC_TILT_MAX"); - - h->rc_scale_pitch = param_find("RC_SCALE_PITCH"); - h->rc_scale_roll = param_find("RC_SCALE_ROLL"); - h->rc_scale_yaw = param_find("RC_SCALE_YAW"); - - return OK; -} - -int parameters_update(const struct multirotor_position_control_param_handles *h, struct multirotor_position_control_params *p) -{ - param_get(h->takeoff_alt, &(p->takeoff_alt)); - param_get(h->takeoff_gap, &(p->takeoff_gap)); - param_get(h->thr_min, &(p->thr_min)); - param_get(h->thr_max, &(p->thr_max)); - param_get(h->z_p, &(p->z_p)); - param_get(h->z_vel_p, &(p->z_vel_p)); - param_get(h->z_vel_i, &(p->z_vel_i)); - param_get(h->z_vel_max, &(p->z_vel_max)); - param_get(h->z_ff, &(p->z_ff)); - param_get(h->xy_p, &(p->xy_p)); - param_get(h->xy_vel_p, &(p->xy_vel_p)); - param_get(h->xy_vel_i, &(p->xy_vel_i)); - param_get(h->xy_vel_max, &(p->xy_vel_max)); - param_get(h->xy_ff, &(p->xy_ff)); - param_get(h->tilt_max, &(p->tilt_max)); - - param_get(h->rc_scale_pitch, &(p->rc_scale_pitch)); - param_get(h->rc_scale_roll, &(p->rc_scale_roll)); - param_get(h->rc_scale_yaw, &(p->rc_scale_yaw)); - - return OK; -} diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control_params.h b/src/modules/multirotor_pos_control/multirotor_pos_control_params.h deleted file mode 100644 index 37a925dcc8..0000000000 --- a/src/modules/multirotor_pos_control/multirotor_pos_control_params.h +++ /dev/null @@ -1,97 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. - * Author: Anton Babushkin - * - * 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 multirotor_pos_control_params.h - * - * Parameters for multirotor_pos_control - */ - -#include - -struct multirotor_position_control_params { - float takeoff_alt; - float takeoff_gap; - float thr_min; - float thr_max; - float z_p; - float z_vel_p; - float z_vel_i; - float z_vel_max; - float z_ff; - float xy_p; - float xy_vel_p; - float xy_vel_i; - float xy_vel_max; - float xy_ff; - float tilt_max; - - float rc_scale_pitch; - float rc_scale_roll; - float rc_scale_yaw; -}; - -struct multirotor_position_control_param_handles { - param_t takeoff_alt; - param_t takeoff_gap; - param_t thr_min; - param_t thr_max; - param_t z_p; - param_t z_vel_p; - param_t z_vel_i; - param_t z_vel_max; - param_t z_ff; - param_t xy_p; - param_t xy_vel_p; - param_t xy_vel_i; - param_t xy_vel_max; - param_t xy_ff; - param_t tilt_max; - - param_t rc_scale_pitch; - param_t rc_scale_roll; - param_t rc_scale_yaw; -}; - -/** - * Initialize all parameter handles and values - * - */ -int parameters_init(struct multirotor_position_control_param_handles *h); - -/** - * Update all parameters - * - */ -int parameters_update(const struct multirotor_position_control_param_handles *h, struct multirotor_position_control_params *p); diff --git a/src/modules/multirotor_pos_control/thrust_pid.c b/src/modules/multirotor_pos_control/thrust_pid.c deleted file mode 100644 index 39a7153229..0000000000 --- a/src/modules/multirotor_pos_control/thrust_pid.c +++ /dev/null @@ -1,172 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. - * Author: Anton Babushkin - * - * 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 thrust_pid.c - * - * Implementation of thrust control PID. - * - * @author Anton Babushkin - */ - -#include "thrust_pid.h" -#include - -#define COS_TILT_MAX 0.7f - -__EXPORT void thrust_pid_init(thrust_pid_t *pid, float dt_min) -{ - pid->dt_min = dt_min; - pid->kp = 0.0f; - pid->ki = 0.0f; - pid->kd = 0.0f; - pid->integral = 0.0f; - pid->output_min = 0.0f; - pid->output_max = 0.0f; - pid->error_previous = 0.0f; - pid->last_output = 0.0f; -} - -__EXPORT int thrust_pid_set_parameters(thrust_pid_t *pid, float kp, float ki, float kd, float output_min, float output_max) -{ - int ret = 0; - - if (isfinite(kp)) { - pid->kp = kp; - - } else { - ret = 1; - } - - if (isfinite(ki)) { - pid->ki = ki; - - } else { - ret = 1; - } - - if (isfinite(kd)) { - pid->kd = kd; - - } else { - ret = 1; - } - - if (isfinite(output_min)) { - pid->output_min = output_min; - - } else { - ret = 1; - } - - if (isfinite(output_max)) { - pid->output_max = output_max; - - } else { - ret = 1; - } - - return ret; -} - -__EXPORT float thrust_pid_calculate(thrust_pid_t *pid, float sp, float val, float dt, float r22) -{ - if (!isfinite(sp) || !isfinite(val) || !isfinite(dt)) { - return pid->last_output; - } - - float i, d; - - /* error value */ - float error = sp - val; - - /* error derivative */ - d = (-val - pid->error_previous) / fmaxf(dt, pid->dt_min); - pid->error_previous = -val; - - if (!isfinite(d)) { - d = 0.0f; - } - - /* calculate the error integral */ - i = pid->integral + (pid->ki * error * dt); - - /* attitude-thrust compensation - * r22 is (2, 2) component of rotation matrix for current attitude */ - float att_comp; - - if (r22 > COS_TILT_MAX) { - att_comp = 1.0f / r22; - - } else if (r22 > 0.0f) { - att_comp = ((1.0f / COS_TILT_MAX - 1.0f) / COS_TILT_MAX) * r22 + 1.0f; - - } else { - att_comp = 1.0f; - } - - /* calculate PD output */ - float output = ((error * pid->kp) + (d * pid->kd)) * att_comp; - - /* check for saturation */ - if (isfinite(i)) { - float i_comp = i * att_comp; - if ((output + i_comp) >= pid->output_min || (output + i_comp) <= pid->output_max) { - /* not saturated, use new integral value */ - pid->integral = i; - } - } - - /* add I component to output */ - output += pid->integral * att_comp; - - /* limit output */ - if (isfinite(output)) { - if (output > pid->output_max) { - output = pid->output_max; - - } else if (output < pid->output_min) { - output = pid->output_min; - } - - pid->last_output = output; - } - - return pid->last_output; -} - -__EXPORT void thrust_pid_set_integral(thrust_pid_t *pid, float i) -{ - pid->integral = i; -} diff --git a/src/modules/multirotor_pos_control/thrust_pid.h b/src/modules/multirotor_pos_control/thrust_pid.h deleted file mode 100644 index 71c3704d01..0000000000 --- a/src/modules/multirotor_pos_control/thrust_pid.h +++ /dev/null @@ -1,69 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. - * Author: Anton Babushkin - * - * 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 thrust_pid.h - * - * Definition of thrust control PID interface. - * - * @author Anton Babushkin - */ - -#ifndef THRUST_PID_H_ -#define THRUST_PID_H_ - -#include - -__BEGIN_DECLS - -typedef struct { - float dt_min; - float kp; - float ki; - float kd; - float integral; - float output_min; - float output_max; - float error_previous; - float last_output; -} thrust_pid_t; - -__EXPORT void thrust_pid_init(thrust_pid_t *pid, float dt_min); -__EXPORT int thrust_pid_set_parameters(thrust_pid_t *pid, float kp, float ki, float kd, float output_min, float output_max); -__EXPORT float thrust_pid_calculate(thrust_pid_t *pid, float sp, float val, float dt, float r22); -__EXPORT void thrust_pid_set_integral(thrust_pid_t *pid, float i); - -__END_DECLS - -#endif /* THRUST_PID_H_ */ From e1f949163b1c1affecef8a2fea83cf9f5a53b68c Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 26 Dec 2013 23:06:36 +0400 Subject: [PATCH 238/447] makefiles and rc scripts fixed to use new attitude and position controllers --- ROMFS/px4fmu_common/init.d/09_ardrone_flow | 2 +- ROMFS/px4fmu_common/init.d/1001_rc_quad.hil | 2 +- ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil | 2 +- ROMFS/px4fmu_common/init.d/rc.multirotor | 2 +- makefiles/config_px4fmu-v1_default.mk | 3 +-- makefiles/config_px4fmu-v2_default.mk | 3 +-- 6 files changed, 6 insertions(+), 8 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/09_ardrone_flow b/ROMFS/px4fmu_common/init.d/09_ardrone_flow index 9b739f2455..e4561eee31 100644 --- a/ROMFS/px4fmu_common/init.d/09_ardrone_flow +++ b/ROMFS/px4fmu_common/init.d/09_ardrone_flow @@ -72,7 +72,7 @@ flow_position_estimator start # # Fire up the multi rotor attitude controller # -multirotor_att_control start +mc_att_control_vector start # # Fire up the flow position controller diff --git a/ROMFS/px4fmu_common/init.d/1001_rc_quad.hil b/ROMFS/px4fmu_common/init.d/1001_rc_quad.hil index 6dd7d460b8..c15e5d7c50 100644 --- a/ROMFS/px4fmu_common/init.d/1001_rc_quad.hil +++ b/ROMFS/px4fmu_common/init.d/1001_rc_quad.hil @@ -91,7 +91,7 @@ mc_att_control_vector start # # Start position control # -multirotor_pos_control start +mc_pos_control start echo "[HIL] setup done, running" diff --git a/ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil b/ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil index c295ede1e5..ad36716cf2 100644 --- a/ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil +++ b/ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil @@ -91,7 +91,7 @@ mc_att_control_vector start # # Start position control # -multirotor_pos_control start +mc_pos_control start echo "[HIL] setup done, running" diff --git a/ROMFS/px4fmu_common/init.d/rc.multirotor b/ROMFS/px4fmu_common/init.d/rc.multirotor index c996e3ff90..f6ac58632e 100644 --- a/ROMFS/px4fmu_common/init.d/rc.multirotor +++ b/ROMFS/px4fmu_common/init.d/rc.multirotor @@ -36,4 +36,4 @@ mc_att_control_vector start # # Start position control # -multirotor_pos_control start +mc_pos_control start diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk index be24100fc6..eaf8ba5b07 100644 --- a/makefiles/config_px4fmu-v1_default.mk +++ b/makefiles/config_px4fmu-v1_default.mk @@ -83,9 +83,8 @@ MODULES += examples/flow_position_estimator #MODULES += modules/segway # XXX Needs GCC 4.7 fix MODULES += modules/fw_pos_control_l1 MODULES += modules/fw_att_control -MODULES += modules/multirotor_att_control MODULES += modules/mc_att_control_vector -MODULES += modules/multirotor_pos_control +MODULES += modules/mc_pos_control MODULES += examples/flow_position_control MODULES += examples/flow_speed_control diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index d6b9146684..f9573b6058 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -83,9 +83,8 @@ MODULES += examples/flow_position_estimator #MODULES += modules/segway # XXX Needs GCC 4.7 fix MODULES += modules/fw_pos_control_l1 MODULES += modules/fw_att_control -MODULES += modules/multirotor_att_control MODULES += modules/mc_att_control_vector -MODULES += modules/multirotor_pos_control +MODULES += modules/mc_pos_control # # Logging From 1c7e07d8d7256e62dfc49fc9f2f1d494c3da4cb4 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 26 Dec 2013 21:41:54 +0100 Subject: [PATCH 239/447] Topics: Move from global_position_setpoint to mission_item_triplet --- .../flow_position_control_main.c | 1 - src/modules/controllib/uorb/blocks.hpp | 1 - src/modules/mavlink/orb_listener.c | 27 +++--- src/modules/mavlink/orb_topics.h | 4 +- src/modules/mavlink_onboard/orb_topics.h | 2 +- .../multirotor_pos_control.c | 41 ++++----- src/modules/sdlog2/sdlog2.c | 37 ++++---- src/modules/sdlog2/sdlog2_messages.h | 11 ++- src/modules/uORB/objects_common.cpp | 3 - .../topics/vehicle_global_position_setpoint.h | 86 ------------------- 10 files changed, 59 insertions(+), 154 deletions(-) delete mode 100644 src/modules/uORB/topics/vehicle_global_position_setpoint.h diff --git a/src/examples/flow_position_control/flow_position_control_main.c b/src/examples/flow_position_control/flow_position_control_main.c index 3125ce2460..391e40ac13 100644 --- a/src/examples/flow_position_control/flow_position_control_main.c +++ b/src/examples/flow_position_control/flow_position_control_main.c @@ -61,7 +61,6 @@ #include #include #include -#include #include #include #include diff --git a/src/modules/controllib/uorb/blocks.hpp b/src/modules/controllib/uorb/blocks.hpp index 335439fb77..8cc0d77d47 100644 --- a/src/modules/controllib/uorb/blocks.hpp +++ b/src/modules/controllib/uorb/blocks.hpp @@ -43,7 +43,6 @@ #include #include #include -#include #include #include #include diff --git a/src/modules/mavlink/orb_listener.c b/src/modules/mavlink/orb_listener.c index 9e43467cc3..de902f3da0 100644 --- a/src/modules/mavlink/orb_listener.c +++ b/src/modules/mavlink/orb_listener.c @@ -136,7 +136,7 @@ static const struct listener listeners[] = { {l_input_rc, &mavlink_subs.input_rc_sub, 0}, {l_global_position, &mavlink_subs.global_pos_sub, 0}, {l_local_position, &mavlink_subs.local_pos_sub, 0}, - {l_global_position_setpoint, &mavlink_subs.spg_sub, 0}, + {l_global_position_setpoint, &mavlink_subs.triplet_sub, 0}, {l_local_position_setpoint, &mavlink_subs.spl_sub, 0}, {l_attitude_setpoint, &mavlink_subs.spa_sub, 0}, {l_actuator_outputs, &mavlink_subs.act_0_sub, 0}, @@ -402,23 +402,24 @@ l_local_position(const struct listener *l) void l_global_position_setpoint(const struct listener *l) { - struct vehicle_global_position_setpoint_s global_sp; - - /* copy local position data into local buffer */ - orb_copy(ORB_ID(vehicle_global_position_setpoint), mavlink_subs.spg_sub, &global_sp); + struct mission_item_triplet_s triplet; + orb_copy(ORB_ID(mission_item_triplet), mavlink_subs.triplet_sub, &triplet); uint8_t coordinate_frame = MAV_FRAME_GLOBAL; + + if (!triplet.current_valid) + return; - if (global_sp.altitude_is_relative) + if (triplet.current.altitude_is_relative) coordinate_frame = MAV_FRAME_GLOBAL_RELATIVE_ALT; if (gcs_link) mavlink_msg_global_position_setpoint_int_send(MAVLINK_COMM_0, coordinate_frame, - global_sp.lat, - global_sp.lon, - global_sp.altitude * 1000.0f, - global_sp.yaw * M_RAD_TO_DEG_F * 100.0f); + (int32_t)(triplet.current.lat * 1e7f), + (int32_t)(triplet.current.lon * 1e7f), + (int32_t)(triplet.current.altitude * 1e3f), + (int16_t)(triplet.current.yaw * M_RAD_TO_DEG_F * 1e2f)); } void @@ -770,9 +771,9 @@ uorb_receive_start(void) orb_set_interval(mavlink_subs.local_pos_sub, 1000); /* 1Hz active updates */ /* --- GLOBAL SETPOINT VALUE --- */ - mavlink_subs.spg_sub = orb_subscribe(ORB_ID(vehicle_global_position_setpoint)); - orb_set_interval(mavlink_subs.spg_sub, 2000); /* 0.5 Hz updates */ - + mavlink_subs.triplet_sub = orb_subscribe(ORB_ID(mission_item_triplet)); + orb_set_interval(mavlink_subs.triplet_sub, 2000); /* 0.5 Hz updates */ + /* --- LOCAL SETPOINT VALUE --- */ mavlink_subs.spl_sub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint)); orb_set_interval(mavlink_subs.spl_sub, 2000); /* 0.5 Hz updates */ diff --git a/src/modules/mavlink/orb_topics.h b/src/modules/mavlink/orb_topics.h index 2cba25338b..7d24b8f932 100644 --- a/src/modules/mavlink/orb_topics.h +++ b/src/modules/mavlink/orb_topics.h @@ -50,8 +50,8 @@ #include #include #include +#include #include -#include #include #include #include @@ -86,7 +86,7 @@ struct mavlink_subscriptions { int local_pos_sub; int spa_sub; int spl_sub; - int spg_sub; + int triplet_sub; int debug_key_value; int input_rc_sub; int optical_flow; diff --git a/src/modules/mavlink_onboard/orb_topics.h b/src/modules/mavlink_onboard/orb_topics.h index 1b49c9ce4e..86bfa26f25 100644 --- a/src/modules/mavlink_onboard/orb_topics.h +++ b/src/modules/mavlink_onboard/orb_topics.h @@ -50,7 +50,7 @@ #include #include #include -#include +#include #include #include #include diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c index 3d23d0c096..5af7e2a829 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c @@ -61,8 +61,8 @@ #include #include #include -#include #include +#include #include #include #include @@ -190,12 +190,12 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) memset(&manual, 0, sizeof(manual)); struct vehicle_local_position_s local_pos; memset(&local_pos, 0, sizeof(local_pos)); - struct vehicle_local_position_setpoint_s local_pos_sp; - memset(&local_pos_sp, 0, sizeof(local_pos_sp)); - struct vehicle_global_position_setpoint_s global_pos_sp; - memset(&global_pos_sp, 0, sizeof(global_pos_sp)); + struct mission_item_triplet_s triplet; + memset(&triplet, 0, sizeof(triplet)); struct vehicle_global_velocity_setpoint_s global_vel_sp; memset(&global_vel_sp, 0, sizeof(global_vel_sp)); + struct vehicle_local_position_setpoint_s local_pos_sp; + memset(&local_pos_sp, 0, sizeof(local_pos_sp)); /* subscribe to attitude, motor setpoints and system state */ int param_sub = orb_subscribe(ORB_ID(parameter_update)); @@ -203,9 +203,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) int att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); int att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); int manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); - int local_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint)); - int local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position)); - int global_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_global_position_setpoint)); + int mission_triplet_sub = orb_subscribe(ORB_ID(mission_item_triplet)); /* publish setpoint */ orb_advert_t local_pos_sp_pub = orb_advertise(ORB_ID(vehicle_local_position_setpoint), &local_pos_sp); @@ -292,11 +290,11 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(vehicle_control_mode), control_mode_sub, &control_mode); } - orb_check(global_pos_sp_sub, &updated); + orb_check(mission_triplet_sub, &updated); if (updated) { - orb_copy(ORB_ID(vehicle_global_position_setpoint), global_pos_sp_sub, &global_pos_sp); - global_pos_sp_valid = true; + orb_copy(ORB_ID(mission_item_triplet), mission_triplet_sub, &triplet); + global_pos_sp_valid = triplet.current_valid; reset_mission_sp = true; } @@ -329,7 +327,6 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(manual_control_setpoint), manual_sub, &manual); orb_copy(ORB_ID(vehicle_attitude), att_sub, &att); orb_copy(ORB_ID(vehicle_attitude_setpoint), att_sp_sub, &att_sp); - orb_copy(ORB_ID(vehicle_local_position), local_pos_sub, &local_pos); float z_sp_offs_max = params.z_vel_max / params.z_p * 2.0f; float xy_sp_offs_max = params.xy_vel_max / params.xy_p * 2.0f; @@ -459,24 +456,22 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) /* update global setpoint projection */ if (global_pos_sp_valid) { - /* global position setpoint valid, use it */ - double sp_lat = global_pos_sp.lat * 1e-7; - double sp_lon = global_pos_sp.lon * 1e-7; - /* project global setpoint to local setpoint */ - map_projection_project(sp_lat, sp_lon, &(local_pos_sp.x), &(local_pos_sp.y)); - if (global_pos_sp.altitude_is_relative) { - local_pos_sp.z = -global_pos_sp.altitude; + /* project global setpoint to local setpoint */ + map_projection_project(triplet.current.lat, triplet.current.lon, &(local_pos_sp.x), &(local_pos_sp.y)); + + if (triplet.current.altitude_is_relative) { + local_pos_sp.z = -triplet.current.altitude; } else { - local_pos_sp.z = local_pos.ref_alt - global_pos_sp.altitude; + local_pos_sp.z = local_pos.ref_alt - triplet.current.lat; } /* update yaw setpoint only if value is valid */ - if (isfinite(global_pos_sp.yaw) && fabsf(global_pos_sp.yaw) < M_TWOPI) { - att_sp.yaw_body = global_pos_sp.yaw; + if (isfinite(triplet.current.yaw) && fabsf(triplet.current.yaw) < M_TWOPI) { + att_sp.yaw_body = triplet.current.yaw; } - mavlink_log_info(mavlink_fd, "[mpc] new sp: %.7f, %.7f (%.2f, %.2f)", (double)sp_lat, sp_lon, (double)local_pos_sp.x, (double)local_pos_sp.y); + mavlink_log_info(mavlink_fd, "[mpc] new sp: %.7f, %.7f (%.2f, %.2f)", triplet.current.lat, triplet.current.lon, (double)local_pos_sp.x, (double)local_pos_sp.y); } else { if (reset_auto_sp_xy) { diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index e94b1e13cb..5dc325a05e 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -72,7 +72,7 @@ #include #include #include -#include +#include #include #include #include @@ -693,7 +693,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct vehicle_local_position_s local_pos; struct vehicle_local_position_setpoint_s local_pos_sp; struct vehicle_global_position_s global_pos; - struct vehicle_global_position_setpoint_s global_pos_sp; + struct mission_item_triplet_s triplet; struct vehicle_gps_position_s gps_pos; struct vehicle_vicon_position_s vicon_pos; struct optical_flow_s flow; @@ -718,7 +718,7 @@ int sdlog2_thread_main(int argc, char *argv[]) int local_pos_sub; int local_pos_sp_sub; int global_pos_sub; - int global_pos_sp_sub; + int triplet_sub; int gps_pos_sub; int vicon_pos_sub; int flow_sub; @@ -840,8 +840,8 @@ int sdlog2_thread_main(int argc, char *argv[]) fdsc_count++; /* --- GLOBAL POSITION SETPOINT--- */ - subs.global_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_global_position_setpoint)); - fds[fdsc_count].fd = subs.global_pos_sp_sub; + subs.triplet_sub = orb_subscribe(ORB_ID(mission_item_triplet)); + fds[fdsc_count].fd = subs.triplet_sub; fds[fdsc_count].events = POLLIN; fdsc_count++; @@ -1150,20 +1150,21 @@ int sdlog2_thread_main(int argc, char *argv[]) /* --- GLOBAL POSITION SETPOINT --- */ if (fds[ifds++].revents & POLLIN) { - orb_copy(ORB_ID(vehicle_global_position_setpoint), subs.global_pos_sp_sub, &buf.global_pos_sp); + orb_copy(ORB_ID(mission_item_triplet), subs.triplet_sub, &buf.triplet); log_msg.msg_type = LOG_GPSP_MSG; - log_msg.body.log_GPSP.altitude_is_relative = buf.global_pos_sp.altitude_is_relative; - log_msg.body.log_GPSP.lat = buf.global_pos_sp.lat; - log_msg.body.log_GPSP.lon = buf.global_pos_sp.lon; - log_msg.body.log_GPSP.altitude = buf.global_pos_sp.altitude; - log_msg.body.log_GPSP.yaw = buf.global_pos_sp.yaw; - log_msg.body.log_GPSP.loiter_radius = buf.global_pos_sp.loiter_radius; - log_msg.body.log_GPSP.loiter_direction = buf.global_pos_sp.loiter_direction; - log_msg.body.log_GPSP.nav_cmd = buf.global_pos_sp.nav_cmd; - log_msg.body.log_GPSP.param1 = buf.global_pos_sp.param1; - log_msg.body.log_GPSP.param2 = buf.global_pos_sp.param2; - log_msg.body.log_GPSP.param3 = buf.global_pos_sp.param3; - log_msg.body.log_GPSP.param4 = buf.global_pos_sp.param4; + log_msg.body.log_GPSP.altitude_is_relative = buf.triplet.current.altitude_is_relative; + log_msg.body.log_GPSP.lat = (int32_t)(buf.triplet.current.lat * 1e7); + log_msg.body.log_GPSP.lon = (int32_t)(buf.triplet.current.lon * 1e7); + log_msg.body.log_GPSP.altitude = buf.triplet.current.altitude; + log_msg.body.log_GPSP.yaw = buf.triplet.current.yaw; + log_msg.body.log_GPSP.nav_cmd = buf.triplet.current.nav_cmd; + log_msg.body.log_GPSP.loiter_radius = buf.triplet.current.loiter_radius; + log_msg.body.log_GPSP.loiter_direction = buf.triplet.current.loiter_direction; + log_msg.body.log_GPSP.loiter_radius = buf.triplet.current.loiter_radius; + log_msg.body.log_GPSP.loiter_direction = buf.triplet.current.loiter_direction; + log_msg.body.log_GPSP.radius = buf.triplet.current.radius; + log_msg.body.log_GPSP.time_inside = buf.triplet.current.time_inside; + log_msg.body.log_GPSP.pitch_min = buf.triplet.current.pitch_min; LOGBUFFER_WRITE_AND_COUNT(GPSP); } diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index ab4dc9b00d..cb1393f1f7 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -214,13 +214,12 @@ struct log_GPSP_s { int32_t lon; float altitude; float yaw; + uint8_t nav_cmd; float loiter_radius; int8_t loiter_direction; - uint8_t nav_cmd; - float param1; - float param2; - float param3; - float param4; + float radius; + float time_inside; + float pitch_min; }; /* --- ESC - ESC STATE --- */ @@ -288,7 +287,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(ARSP, "fff", "RollRateSP,PitchRateSP,YawRateSP"), LOG_FORMAT(FLOW, "hhfffBB", "RawX,RawY,CompX,CompY,Dist,Q,SensID"), LOG_FORMAT(GPOS, "LLffff", "Lat,Lon,Alt,VelN,VelE,VelD"), - LOG_FORMAT(GPSP, "BLLfffbBffff", "AltRel,Lat,Lon,Alt,Yaw,LoiterR,LoiterDir,NavCmd,P1,P2,P3,P4"), + LOG_FORMAT(GPSP, "BLLffBfbfff", "AltRel,Lat,Lon,Alt,Yaw,NavCmd,LoitRad,LoitDir,Rad,TimeIn,pitMin"), LOG_FORMAT(ESC, "HBBBHHHHHHfH", "Counter,NumESC,Conn,N,Ver,Adr,Volt,Amp,RPM,Temp,SetP,SetPRAW"), LOG_FORMAT(GVSP, "fff", "VX,VY,VZ"), /* system-level messages, ID >= 0x80 */ diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index 7434df1c33..79a820c066 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -117,9 +117,6 @@ ORB_DEFINE(vehicle_local_position_setpoint, struct vehicle_local_position_setpoi #include "topics/vehicle_bodyframe_speed_setpoint.h" ORB_DEFINE(vehicle_bodyframe_speed_setpoint, struct vehicle_bodyframe_speed_setpoint_s); -#include "topics/vehicle_global_position_setpoint.h" -ORB_DEFINE(vehicle_global_position_setpoint, struct vehicle_global_position_setpoint_s); - #include "topics/mission_item_triplet.h" ORB_DEFINE(mission_item_triplet, struct mission_item_triplet_s); diff --git a/src/modules/uORB/topics/vehicle_global_position_setpoint.h b/src/modules/uORB/topics/vehicle_global_position_setpoint.h deleted file mode 100644 index a56434d3b0..0000000000 --- a/src/modules/uORB/topics/vehicle_global_position_setpoint.h +++ /dev/null @@ -1,86 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: @author Thomas Gubler - * @author Julian Oes - * @author Lorenz Meier - * - * 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 vehicle_global_position_setpoint.h - * Definition of the global WGS84 position setpoint uORB topic. - */ - -#ifndef TOPIC_VEHICLE_GLOBAL_POSITION_SETPOINT_H_ -#define TOPIC_VEHICLE_GLOBAL_POSITION_SETPOINT_H_ - -#include -#include -#include "../uORB.h" -#include "mission.h" - -/** - * @addtogroup topics - * @{ - */ - -/** - * Global position setpoint in WGS84 coordinates. - * - * This is the position the MAV is heading towards. If it of type loiter, - * the MAV is circling around it with the given loiter radius in meters. - */ -struct vehicle_global_position_setpoint_s -{ - bool altitude_is_relative; /**< true if altitude is relative from start point */ - int32_t lat; /**< latitude in degrees * 1E7 */ - int32_t lon; /**< longitude in degrees * 1E7 */ - float altitude; /**< altitude in meters */ - float yaw; /**< in radians NED -PI..+PI */ - float loiter_radius; /**< loiter radius in meters, 0 for a VTOL to hover */ - int8_t loiter_direction; /**< 1: positive / clockwise, -1, negative. */ - enum NAV_CMD nav_cmd; /**< true if loitering is enabled */ - float param1; - float param2; - float param3; - float param4; - float turn_distance_xy; /**< The distance on the plane which will mark this as reached */ - float turn_distance_z; /**< The distance in Z direction which will mark this as reached */ -}; - -/** - * @} - */ - -/* register this as object request broker structure */ -ORB_DECLARE(vehicle_global_position_setpoint); - -#endif From d3a71d1e420c595a9a242d12264d553759dd9e2a Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 26 Dec 2013 22:41:05 +0100 Subject: [PATCH 240/447] Waypoints: reverse param1 and param2 --- src/modules/mavlink/waypoints.c | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/modules/mavlink/waypoints.c b/src/modules/mavlink/waypoints.c index 45e8914343..741ea8aa45 100644 --- a/src/modules/mavlink/waypoints.c +++ b/src/modules/mavlink/waypoints.c @@ -105,10 +105,10 @@ int map_mavlink_mission_item_to_mission_item(const mavlink_mission_item_t *mavli switch (mavlink_mission_item->command) { case MAV_CMD_NAV_TAKEOFF: - mission_item->pitch_min = mavlink_mission_item->param1; + mission_item->pitch_min = mavlink_mission_item->param2; break; default: - mission_item->radius = mavlink_mission_item->param1; + mission_item->radius = mavlink_mission_item->param2; break; } @@ -117,7 +117,7 @@ int map_mavlink_mission_item_to_mission_item(const mavlink_mission_item_t *mavli mission_item->loiter_direction = (mavlink_mission_item->param3 > 0) ? 1 : -1; /* 1 if positive CW, -1 if negative CCW */ mission_item->nav_cmd = mavlink_mission_item->command; - mission_item->time_inside = mavlink_mission_item->param2 / 1e3f; /* from milliseconds to seconds */ + mission_item->time_inside = mavlink_mission_item->param1 / 1e3f; /* from milliseconds to seconds */ mission_item->autocontinue = mavlink_mission_item->autocontinue; mission_item->index = mavlink_mission_item->seq; mission_item->origin = ORIGIN_MAVLINK; @@ -135,10 +135,10 @@ int map_mission_item_to_mavlink_mission_item(const struct mission_item_s *missio switch (mission_item->nav_cmd) { case NAV_CMD_TAKEOFF: - mavlink_mission_item->param1 = mission_item->pitch_min; + mavlink_mission_item->param2 = mission_item->pitch_min; break; default: - mavlink_mission_item->param1 = mission_item->radius; + mavlink_mission_item->param2 = mission_item->radius; break; } @@ -149,7 +149,7 @@ int map_mission_item_to_mavlink_mission_item(const struct mission_item_s *missio mavlink_mission_item->param4 = mission_item->yaw*M_RAD_TO_DEG_F; mavlink_mission_item->param3 = mission_item->loiter_radius*(float)mission_item->loiter_direction; mavlink_mission_item->command = mission_item->nav_cmd; - mavlink_mission_item->param2 = mission_item->time_inside * 1e3f; /* from seconds to milliseconds */ + mavlink_mission_item->param1 = mission_item->time_inside * 1e3f; /* from seconds to milliseconds */ mavlink_mission_item->autocontinue = mission_item->autocontinue; mavlink_mission_item->seq = mission_item->index; From 6a04d13e733700e7e2e90c20399889a79eae293a Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 27 Dec 2013 10:50:40 +0400 Subject: [PATCH 241/447] mc_pos_control: minor reorganizing --- .../mc_pos_control/mc_pos_control_main.cpp | 22 +++++++++---------- 1 file changed, 10 insertions(+), 12 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 c06caff1ee..f7d50f45da 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -179,10 +179,6 @@ private: math::Vector<3> _vel_sp; math::Vector<3> _vel_err_prev; /**< velocity on previous step */ - hrt_abstime _time_prev; - - bool _was_armed; - /** * Update our local parameter cache. */ @@ -241,10 +237,7 @@ MulticopterPositionControl::MulticopterPositionControl() : /* publications */ _local_pos_sp_pub(-1), _att_sp_pub(-1), - _global_vel_sp_pub(-1), - - _time_prev(0), - _was_armed(false) + _global_vel_sp_pub(-1) { memset(&_att, 0, sizeof(_att)); memset(&_att_sp, 0, sizeof(_att_sp)); @@ -469,6 +462,7 @@ MulticopterPositionControl::task_main() bool reset_takeoff_sp = true; hrt_abstime t_prev = 0; + const float alt_ctl_dz = 0.2f; const float pos_ctl_dz = 0.05f; @@ -510,10 +504,10 @@ MulticopterPositionControl::task_main() parameters_update(false); hrt_abstime t = hrt_absolute_time(); - float dt = _time_prev != 0 ? (t - _time_prev) * 0.000001f : 0.0f; - _time_prev = t; + float dt = t_prev != 0 ? (t - t_prev) * 0.000001f : 0.0f; + t_prev = t; - if (_control_mode.flag_armed && !_was_armed) { + if (_control_mode.flag_armed && !was_armed) { /* reset setpoints and integrals on arming */ reset_man_sp_z = true; reset_man_sp_xy = true; @@ -523,7 +517,7 @@ MulticopterPositionControl::task_main() reset_int_z = true; reset_int_xy = true; } - _was_armed = _control_mode.flag_armed; + was_armed = _control_mode.flag_armed; if (_control_mode.flag_control_altitude_enabled || _control_mode.flag_control_position_enabled || @@ -782,6 +776,10 @@ MulticopterPositionControl::task_main() thrust_int += vel_err.emult(_params.vel_i) * dt; + /* protection against flipping on ground when landing */ + if (thrust_int(2) > 0.0f) + thrust_int(2) = 0.0f; + /* calculate attitude and thrust from thrust vector */ if (_control_mode.flag_control_velocity_enabled) { /* desired body_z axis = -normalize(thrust_vector) */ From 32c7aea2a6a0c355d2cae362884e40e4f3573311 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 27 Dec 2013 11:07:45 +0100 Subject: [PATCH 242/447] Home position: use double for lat/lon and float for altitude, set home position to global position instead of GPS position once we have a fix --- src/drivers/hott/messages.cpp | 4 ++-- src/modules/commander/commander.cpp | 20 +++++++------------- src/modules/mavlink/orb_listener.c | 2 +- src/modules/navigator/navigator_main.cpp | 21 ++++++++++----------- src/modules/uORB/topics/home_position.h | 20 +++++++++----------- 5 files changed, 29 insertions(+), 38 deletions(-) diff --git a/src/drivers/hott/messages.cpp b/src/drivers/hott/messages.cpp index bb8d45beab..90a7440159 100644 --- a/src/drivers/hott/messages.cpp +++ b/src/drivers/hott/messages.cpp @@ -295,8 +295,8 @@ build_gps_response(uint8_t *buffer, size_t *size) memset(&home, 0, sizeof(home)); orb_copy(ORB_ID(home_position), _home_sub, &home); - _home_lat = ((double)(home.lat))*1e-7d; - _home_lon = ((double)(home.lon))*1e-7d; + _home_lat = home.lat; + _home_lon = home.lon; _home_position_set = true; } diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 3fc1d2c191..442f16a58f 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1048,23 +1048,17 @@ int commander_thread_main(int argc, char *argv[]) if (!home_position_set && gps_position.fix_type >= 3 && (gps_position.eph_m < hdop_threshold_m) && (gps_position.epv_m < vdop_threshold_m) && // XXX note that vdop is 0 for mtk - (hrt_absolute_time() < gps_position.timestamp_position + POSITION_TIMEOUT) && !armed.armed) { + (hrt_absolute_time() < gps_position.timestamp_position + POSITION_TIMEOUT) && !armed.armed + && global_position.valid) { /* copy position data to uORB home message, store it locally as well */ - // TODO use global position estimate - home.lat = gps_position.lat; - home.lon = gps_position.lon; - home.alt = gps_position.alt; - home.eph_m = gps_position.eph_m; - home.epv_m = gps_position.epv_m; - home.s_variance_m_s = gps_position.s_variance_m_s; - home.p_variance_m = gps_position.p_variance_m; + home.lat = (double)global_position.lat / 1e7d; + home.lon = (double)global_position.lon / 1e7d; + home.altitude = (float)global_position.alt / 1e3f; - double home_lat_d = home.lat * 1e-7; - double home_lon_d = home.lon * 1e-7; - warnx("home: lat = %.7f, lon = %.7f", home_lat_d, home_lon_d); - mavlink_log_info(mavlink_fd, "[cmd] home: %.7f, %.7f", home_lat_d, home_lon_d); + warnx("home: lat = %.7f, lon = %.7f, alt = %.4f ", home.lat, home.lon, (double)home.altitude); + mavlink_log_info(mavlink_fd, "[cmd] home: %.7f, %.7f, %.4f", home.lat, home.lon, (double)home.altitude); /* announce new home position */ if (home_pub > 0) { diff --git a/src/modules/mavlink/orb_listener.c b/src/modules/mavlink/orb_listener.c index de902f3da0..96888f06a0 100644 --- a/src/modules/mavlink/orb_listener.c +++ b/src/modules/mavlink/orb_listener.c @@ -657,7 +657,7 @@ l_home(const struct listener *l) orb_copy(ORB_ID(home_position), mavlink_subs.home_sub, &home); - mavlink_msg_gps_global_origin_send(MAVLINK_COMM_0, home.lat, home.lon, home.alt); + mavlink_msg_gps_global_origin_send(MAVLINK_COMM_0, (int32_t)(home.lat*1e7d), (int32_t)(home.lon*1e7d), (int32_t)(home.altitude)*1e3f); } void diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index c7ac885b48..c6fe93e9e1 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -953,8 +953,7 @@ Navigator::start_loiter() get_loiter_item(&_mission_item_triplet.current); - /* XXX get rid of ugly conversion for home position altitude */ - float global_min_alt = _parameters.min_altitude + (float)_home_pos.alt/1e3f; + float global_min_alt = _parameters.min_altitude + _home_pos.altitude; /* Use current altitude if above min altitude set by parameter */ if (_global_pos.alt < global_min_alt) { @@ -1080,9 +1079,9 @@ Navigator::start_rtl() _mission_item_triplet.current_valid = true; _mission_item_triplet.next_valid = false; - _mission_item_triplet.current.lat = (double)_home_pos.lat / 1e7; - _mission_item_triplet.current.lon = (double)_home_pos.lon / 1e7; - _mission_item_triplet.current.altitude = (float)_home_pos.alt / 1e3f + _parameters.min_altitude; + _mission_item_triplet.current.lat = _home_pos.lat; + _mission_item_triplet.current.lon = _home_pos.lon; + _mission_item_triplet.current.altitude = _home_pos.altitude + _parameters.min_altitude; _mission_item_triplet.current.yaw = 0.0f; _mission_item_triplet.current.nav_cmd = NAV_CMD_RETURN_TO_LAUNCH; _mission_item_triplet.current.loiter_direction = 1; @@ -1104,9 +1103,9 @@ Navigator::start_rtl_loiter() _mission_item_triplet.current_valid = true; _mission_item_triplet.next_valid = false; - _mission_item_triplet.current.lat = (double)_home_pos.lat / 1e7; - _mission_item_triplet.current.lon = (double)_home_pos.lon / 1e7; - _mission_item_triplet.current.altitude = _home_pos.alt / 1e3f + _parameters.min_altitude; + _mission_item_triplet.current.lat = _home_pos.lat; + _mission_item_triplet.current.lon = _home_pos.lon; + _mission_item_triplet.current.altitude = _home_pos.altitude + _parameters.min_altitude; get_loiter_item(&_mission_item_triplet.current); @@ -1319,9 +1318,9 @@ Navigator::add_home_pos_to_rtl(struct mission_item_s *new_mission_item) { if (new_mission_item->nav_cmd == NAV_CMD_RETURN_TO_LAUNCH) { /* if it is a RTL waypoint, append the home position */ - new_mission_item->lat = (double)_home_pos.lat / 1e7; - new_mission_item->lon = (double)_home_pos.lon / 1e7; - new_mission_item->altitude = (float)_home_pos.alt / 1e3f + _parameters.min_altitude; + new_mission_item->lat = _home_pos.lat; + new_mission_item->lon = _home_pos.lon; + new_mission_item->altitude = _home_pos.altitude + _parameters.min_altitude; new_mission_item->loiter_radius = _parameters.loiter_radius; // TODO: get rid of magic number new_mission_item->radius = 50.0f; // TODO: get rid of magic number } diff --git a/src/modules/uORB/topics/home_position.h b/src/modules/uORB/topics/home_position.h index 7e1b82a0fb..1cf37e29c0 100644 --- a/src/modules/uORB/topics/home_position.h +++ b/src/modules/uORB/topics/home_position.h @@ -2,6 +2,7 @@ * * Copyright (C) 2012-2013 PX4 Development Team. All rights reserved. * Author: Lorenz Meier + * Julian Oes * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -34,9 +35,10 @@ /** * @file home_position.h - * Definition of the GPS home position uORB topic. + * Definition of the home position uORB topic. * * @author Lorenz Meier + * @author Julian Oes */ #ifndef TOPIC_HOME_POSITION_H_ @@ -55,16 +57,12 @@ */ struct home_position_s { - uint64_t timestamp; /**< Timestamp (microseconds since system boot) */ - uint64_t time_gps_usec; /**< Timestamp (microseconds in GPS format), this is the timestamp from the gps module */ - - int32_t lat; /**< Latitude in 1E7 degrees */ - int32_t lon; /**< Longitude in 1E7 degrees */ - int32_t alt; /**< Altitude in 1E3 meters (millimeters) above MSL */ - float eph_m; /**< GPS HDOP horizontal dilution of position in m */ - float epv_m; /**< GPS VDOP horizontal dilution of position in m */ - float s_variance_m_s; /**< speed accuracy estimate m/s */ - float p_variance_m; /**< position accuracy estimate m */ + uint64_t timestamp; /**< Timestamp (microseconds since system boot) */ + + bool altitude_is_relative; + double lat; /**< Latitude in degrees */ + double lon; /**< Longitude in degrees */ + float altitude; /**< Altitude in meters */ }; /** From effa62937fbaab0e446a95ac1ac9447a2895594e Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 27 Dec 2013 11:14:08 +0100 Subject: [PATCH 243/447] Prevent some warnings for lat/lon double conversions --- src/lib/geo/geo.c | 4 ++-- src/modules/mavlink/orb_listener.c | 4 ++-- src/modules/navigator/navigator_main.cpp | 10 +++++----- .../position_estimator_inav_main.c | 4 ++-- src/modules/sdlog2/sdlog2.c | 4 ++-- 5 files changed, 13 insertions(+), 13 deletions(-) diff --git a/src/lib/geo/geo.c b/src/lib/geo/geo.c index 85b17f9ae7..f64bfb41a4 100644 --- a/src/lib/geo/geo.c +++ b/src/lib/geo/geo.c @@ -513,8 +513,8 @@ __EXPORT bool inside_geofence(const struct vehicle_global_position_s *vehicle, c unsigned int i, j, vertices = fence->count; bool c = false; - double lat = vehicle->lat / 1e7; - double lon = vehicle->lon / 1e7; + double lat = vehicle->lat / 1e7d; + double lon = vehicle->lon / 1e7d; // skip vertex 0 (return point) for (i = 0, j = vertices - 1; i < vertices; j = i++) diff --git a/src/modules/mavlink/orb_listener.c b/src/modules/mavlink/orb_listener.c index 96888f06a0..17978615f0 100644 --- a/src/modules/mavlink/orb_listener.c +++ b/src/modules/mavlink/orb_listener.c @@ -416,8 +416,8 @@ l_global_position_setpoint(const struct listener *l) if (gcs_link) mavlink_msg_global_position_setpoint_int_send(MAVLINK_COMM_0, coordinate_frame, - (int32_t)(triplet.current.lat * 1e7f), - (int32_t)(triplet.current.lon * 1e7f), + (int32_t)(triplet.current.lat * 1e7d), + (int32_t)(triplet.current.lon * 1e7d), (int32_t)(triplet.current.altitude * 1e3f), (int16_t)(triplet.current.yaw * M_RAD_TO_DEG_F * 1e2f)); } diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index c6fe93e9e1..7be9229c9b 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -723,7 +723,7 @@ Navigator::status() { warnx("Global position is %svalid", _global_pos.valid ? "" : "in"); if (_global_pos.valid) { - warnx("Longitude %5.5f degrees, latitude %5.5f degrees", _global_pos.lon / 1e7, _global_pos.lat / 1e7); + warnx("Longitude %5.5f degrees, latitude %5.5f degrees", _global_pos.lon / 1e7d, _global_pos.lat / 1e7d); warnx("Altitude %5.5f meters, altitude above home %5.5f meters", (double)_global_pos.alt, (double)_global_pos.relative_alt); warnx("Ground velocity in m/s, x %5.5f, y %5.5f, z %5.5f", @@ -947,8 +947,8 @@ Navigator::start_loiter() _mission_item_triplet.current_valid = true; _mission_item_triplet.next_valid = false; - _mission_item_triplet.current.lat = (double)_global_pos.lat / 1e7; - _mission_item_triplet.current.lon = (double)_global_pos.lon / 1e7; + _mission_item_triplet.current.lat = (double)_global_pos.lat / 1e7d; + _mission_item_triplet.current.lon = (double)_global_pos.lon / 1e7d; _mission_item_triplet.current.yaw = 0.0f; get_loiter_item(&_mission_item_triplet.current); @@ -1162,11 +1162,11 @@ Navigator::mission_item_reached() // if (coordinate_frame == (int)MAV_FRAME_GLOBAL) { dist = get_distance_to_point_global_wgs84(_mission_item_triplet.current.lat, _mission_item_triplet.current.lon, _mission_item_triplet.current.altitude, - (double)_global_pos.lat / 1e7, (double)_global_pos.lon / 1e7, _global_pos.alt, + (double)_global_pos.lat / 1e7d, (double)_global_pos.lon / 1e7d, _global_pos.alt, &dist_xy, &dist_z); // warnx("1 lat: %2.2f, lon: %2.2f, alt: %2.2f", _mission_item_triplet.current.lat, _mission_item_triplet.current.lon, _mission_item_triplet.current.altitude); - // warnx("2 lat: %2.2f, lon: %2.2f, alt: %2.2f", (double)_global_pos.lat / 1e7, (double)_global_pos.lon / 1e7, _global_pos.alt); + // warnx("2 lat: %2.2f, lon: %2.2f, alt: %2.2f", (double)_global_pos.lat / 1e7d, (double)_global_pos.lon / 1e7d, _global_pos.alt); // warnx("Dist: %4.4f", dist); diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index 3084b6d928..5bf0fba30e 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -614,8 +614,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (local_pos.xy_global) { double est_lat, est_lon; map_projection_reproject(local_pos.x, local_pos.y, &est_lat, &est_lon); - global_pos.lat = (int32_t)(est_lat * 1e7); - global_pos.lon = (int32_t)(est_lon * 1e7); + global_pos.lat = (int32_t)(est_lat * 1e7d); + global_pos.lon = (int32_t)(est_lon * 1e7d); global_pos.time_gps_usec = gps.time_gps_usec; } diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 5dc325a05e..9f634d9e4e 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1153,8 +1153,8 @@ int sdlog2_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(mission_item_triplet), subs.triplet_sub, &buf.triplet); log_msg.msg_type = LOG_GPSP_MSG; log_msg.body.log_GPSP.altitude_is_relative = buf.triplet.current.altitude_is_relative; - log_msg.body.log_GPSP.lat = (int32_t)(buf.triplet.current.lat * 1e7); - log_msg.body.log_GPSP.lon = (int32_t)(buf.triplet.current.lon * 1e7); + log_msg.body.log_GPSP.lat = (int32_t)(buf.triplet.current.lat * 1e7d); + log_msg.body.log_GPSP.lon = (int32_t)(buf.triplet.current.lon * 1e7d); log_msg.body.log_GPSP.altitude = buf.triplet.current.altitude; log_msg.body.log_GPSP.yaw = buf.triplet.current.yaw; log_msg.body.log_GPSP.nav_cmd = buf.triplet.current.nav_cmd; From 61a3177979838649af2a6e8e50bea7d15e2765f4 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 27 Dec 2013 14:39:59 +0400 Subject: [PATCH 244/447] attitude_estimator_ekf: acc compensation and magnetic declination fixes --- .../attitude_estimator_ekf_main.cpp | 42 ++++++++++++------- 1 file changed, 27 insertions(+), 15 deletions(-) diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp index efa6d97fdd..36e82db43b 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp @@ -62,6 +62,8 @@ #include #include +#include + #include #include #include @@ -274,6 +276,13 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds float gyro_offsets[3] = { 0.0f, 0.0f, 0.0f }; unsigned offset_count = 0; + /* actual acceleration (by GPS velocity) in body frame */ + float acc[3] = { 0.0f, 0.0f, 0.0f }; + + /* rotation matrix for magnetic declination */ + math::Matrix<3, 3> R_decl; + R_decl.identity(); + /* register the perf counter */ perf_counter_t ekf_loop_perf = perf_alloc(PC_ELAPSED, "attitude_estimator_ekf"); @@ -308,6 +317,9 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds /* update parameters */ parameters_update(&ekf_param_handles, &ekf_params); + + /* update mag declination rotation matrix */ + R_decl.from_euler(0.0f, 0.0f, ekf_params.mag_decl); } /* only run filter if sensor values changed */ @@ -362,7 +374,6 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds sensor_last_timestamp[1] = raw.timestamp; } - float acc[3]; if (gps.fix_type >= 3 && gps.eph_m < 10.0f && gps.vel_ned_valid && hrt_absolute_time() < gps.timestamp_velocity + 500000) { if (last_gps != 0 && gps.timestamp_velocity != last_gps) { float gps_dt = (gps.timestamp_velocity - last_gps) / 1000000.0f; @@ -379,20 +390,18 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds acc[i] += att.R[j][i] * acc_NED[j]; } } + + vel_prev[0] = gps.vel_n_m_s; + vel_prev[1] = gps.vel_e_m_s; + vel_prev[2] = gps.vel_d_m_s; } + last_gps = gps.timestamp_velocity; } else { acc[0] = 0.0f; acc[1] = 0.0f; acc[2] = 0.0f; - } - if (gps.vel_ned_valid) { - vel_prev[0] = gps.vel_n_m_s; - vel_prev[1] = gps.vel_e_m_s; - vel_prev[2] = gps.vel_d_m_s; - last_gps = gps.timestamp_velocity; - } else { vel_prev[0] = 0.0f; vel_prev[1] = 0.0f; vel_prev[2] = 0.0f; @@ -472,7 +481,7 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds continue; } - if (last_data > 0 && raw.timestamp - last_data > 12000) + if (last_data > 0 && raw.timestamp - last_data > 30000) printf("[attitude estimator ekf] sensor data missed! (%llu)\n", raw.timestamp - last_data); last_data = raw.timestamp; @@ -480,10 +489,9 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds /* send out */ att.timestamp = raw.timestamp; - // XXX Apply the same transformation to the rotation matrix - att.roll = euler[0] - ekf_params.roll_off; - att.pitch = euler[1] - ekf_params.pitch_off; - att.yaw = euler[2] - ekf_params.yaw_off; + att.roll = euler[0]; + att.pitch = euler[1]; + att.yaw = euler[2] + ekf_params.mag_decl; att.rollspeed = x_aposteriori[0]; att.pitchspeed = x_aposteriori[1]; @@ -492,12 +500,16 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds att.pitchacc = x_aposteriori[4]; att.yawacc = x_aposteriori[5]; - //att.yawspeed =z_k[2] ; /* copy offsets */ memcpy(&att.rate_offsets, &(x_aposteriori[3]), sizeof(att.rate_offsets)); + /* magnetic declination */ + + math::Matrix<3, 3> R_body = (&Rot_matrix[0]); + math::Matrix<3, 3> R = R_decl * R_body; + /* copy rotation matrix */ - memcpy(&att.R, Rot_matrix, sizeof(Rot_matrix)); + memcpy(&att.R[0][0], &R.data[0][0], sizeof(att.R)); att.R_valid = true; if (isfinite(att.roll) && isfinite(att.pitch) && isfinite(att.yaw)) { From 464df9c5e8cac88c24ee080337970df74edcd239 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 27 Dec 2013 14:40:24 +0400 Subject: [PATCH 245/447] mavlink: HIL GPS velocity fix --- src/modules/mavlink/mavlink_receiver.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 7b6fad6589..e8e02e1e3f 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -579,6 +579,7 @@ handle_message(mavlink_message_t *msg) hil_gps.alt = gps.alt; hil_gps.eph_m = (float)gps.eph * 1e-2f; // from cm to m hil_gps.epv_m = (float)gps.epv * 1e-2f; // from cm to m + hil_gps.timestamp_variance = gps.time_usec; hil_gps.s_variance_m_s = 5.0f; hil_gps.p_variance_m = hil_gps.eph_m * hil_gps.eph_m; hil_gps.vel_m_s = (float)gps.vel * 1e-2f; // from cm/s to m/s @@ -590,6 +591,7 @@ handle_message(mavlink_message_t *msg) if (heading_rad > M_PI_F) heading_rad -= 2.0f * M_PI_F; + hil_gps.timestamp_velocity = gps.time_usec; hil_gps.vel_n_m_s = gps.vn * 1e-2f; // from cm to m hil_gps.vel_e_m_s = gps.ve * 1e-2f; // from cm to m hil_gps.vel_d_m_s = gps.vd * 1e-2f; // from cm to m From 72d9c80ce954d2289282f5df01aef7e5e8914acc Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 27 Dec 2013 14:00:27 +0100 Subject: [PATCH 246/447] Home position: corrected wrong conversion --- 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 442f16a58f..28118b9904 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1055,7 +1055,7 @@ int commander_thread_main(int argc, char *argv[]) home.lat = (double)global_position.lat / 1e7d; home.lon = (double)global_position.lon / 1e7d; - home.altitude = (float)global_position.alt / 1e3f; + home.altitude = (float)global_position.alt; warnx("home: lat = %.7f, lon = %.7f, alt = %.4f ", home.lat, home.lon, (double)home.altitude); mavlink_log_info(mavlink_fd, "[cmd] home: %.7f, %.7f, %.4f", home.lat, home.lon, (double)home.altitude); From b98984e1ffa924d0888efa47af046d1127e9addf Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 27 Dec 2013 20:15:54 +0100 Subject: [PATCH 247/447] fw autoland: add parameter for heading hold distance, fix heading hold --- .../fw_pos_control_l1_main.cpp | 24 +++++++++++-------- .../fw_pos_control_l1_params.c | 1 + 2 files changed, 15 insertions(+), 10 deletions(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 3784337ac5..ee0aca69e3 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -49,6 +49,7 @@ * More details and acknowledgements in the referenced library headers. * * @author Lorenz Meier + * @author Thomas Gubler */ #include @@ -227,6 +228,7 @@ private: float land_H1_virt; float land_flare_alt_relative; float land_thrust_lim_horizontal_distance; + float land_heading_hold_horizontal_distance; } _parameters; /**< local copies of interesting parameters */ @@ -271,6 +273,7 @@ private: param_t land_H1_virt; param_t land_flare_alt_relative; param_t land_thrust_lim_horizontal_distance; + param_t land_heading_hold_horizontal_distance; } _parameter_handles; /**< handles for interesting parameters */ @@ -420,6 +423,7 @@ FixedwingPositionControl::FixedwingPositionControl() : _parameter_handles.land_H1_virt = param_find("FW_LND_HVIRT"); _parameter_handles.land_flare_alt_relative = param_find("FW_LND_FLALT"); _parameter_handles.land_thrust_lim_horizontal_distance = param_find("FW_LND_TLDIST"); + _parameter_handles.land_heading_hold_horizontal_distance = param_find("FW_LND_HHDIST"); _parameter_handles.time_const = param_find("FW_T_TIME_CONST"); _parameter_handles.min_sink_rate = param_find("FW_T_SINK_MIN"); @@ -508,6 +512,7 @@ FixedwingPositionControl::parameters_update() param_get(_parameter_handles.land_H1_virt, &(_parameters.land_H1_virt)); param_get(_parameter_handles.land_flare_alt_relative, &(_parameters.land_flare_alt_relative)); param_get(_parameter_handles.land_thrust_lim_horizontal_distance, &(_parameters.land_thrust_lim_horizontal_distance)); + param_get(_parameter_handles.land_heading_hold_horizontal_distance, &(_parameters.land_heading_hold_horizontal_distance)); _l1_control.set_l1_damping(_parameters.l1_damping); _l1_control.set_l1_period(_parameters.l1_period); @@ -822,19 +827,18 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio /* switch to heading hold for the last meters, continue heading hold after */ float wp_distance = get_distance_to_next_waypoint(current_position.getX(), current_position.getY(), curr_wp.getX(), curr_wp.getY()); //warnx("wp dist: %d, alt err: %d, noret: %s", (int)wp_distance, (int)altitude_error, (land_noreturn) ? "YES" : "NO"); - const float heading_hold_distance = 15.0f; - if (wp_distance < heading_hold_distance || land_noreturn_horizontal) { + if (wp_distance < _parameters.land_heading_hold_horizontal_distance || land_noreturn_horizontal) { /* heading hold, along the line connecting this and the last waypoint */ - - // if (mission_item_triplet.previous_valid) { - // target_bearing = get_bearing_to_next_waypoint(prev_wp.getX(), prev_wp.getY(), next_wp.getX(), next_wp.getY()); - // } else { - - if (!land_noreturn_horizontal) //set target_bearing in first occurrence - target_bearing = _att.yaw; - //} + if (!land_noreturn_horizontal) {//set target_bearing in first occurrence + if (mission_item_triplet.previous_valid) { + target_bearing = get_bearing_to_next_waypoint(prev_wp.getX(), prev_wp.getY(), curr_wp.getX(), curr_wp.getY()); + } else { + target_bearing = _att.yaw; + } + mavlink_log_info(_mavlink_fd, "#audio: Landing, heading hold"); + } // warnx("NORET: %d, target_bearing: %d, yaw: %d", (int)land_noreturn_horizontal, (int)math::degrees(target_bearing), (int)math::degrees(_att.yaw)); 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 10a0c00fc9..9f46b51706 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 @@ -120,3 +120,4 @@ PARAM_DEFINE_FLOAT(FW_LND_SLLR, 0.9f); PARAM_DEFINE_FLOAT(FW_LND_HVIRT, 10.0f); PARAM_DEFINE_FLOAT(FW_LND_FLALT, 15.0f); PARAM_DEFINE_FLOAT(FW_LND_TLDIST, 30.0f); +PARAM_DEFINE_FLOAT(FW_LND_HHDIST, 15.0f); From 7a34089dee9a3f96cdcdf0a3e94619cf22c2053c Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 27 Dec 2013 23:49:24 +0400 Subject: [PATCH 248/447] mc_pos_control: default parameters updated --- src/modules/mc_pos_control/mc_pos_control_params.c | 4 ++-- 1 file changed, 2 insertions(+), 2 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 2fe70698fc..92fa94dcd1 100644 --- a/src/modules/mc_pos_control/mc_pos_control_params.c +++ b/src/modules/mc_pos_control/mc_pos_control_params.c @@ -12,13 +12,13 @@ PARAM_DEFINE_FLOAT(MPC_THR_MIN, 0.0f); PARAM_DEFINE_FLOAT(MPC_THR_MAX, 1.0f); PARAM_DEFINE_FLOAT(MPC_Z_P, 1.0f); PARAM_DEFINE_FLOAT(MPC_Z_VEL_P, 0.1f); -PARAM_DEFINE_FLOAT(MPC_Z_VEL_I, 0.05f); +PARAM_DEFINE_FLOAT(MPC_Z_VEL_I, 0.02f); PARAM_DEFINE_FLOAT(MPC_Z_VEL_D, 0.0f); PARAM_DEFINE_FLOAT(MPC_Z_VEL_MAX, 5.0f); PARAM_DEFINE_FLOAT(MPC_Z_FF, 0.5f); PARAM_DEFINE_FLOAT(MPC_XY_P, 1.0f); PARAM_DEFINE_FLOAT(MPC_XY_VEL_P, 0.1f); -PARAM_DEFINE_FLOAT(MPC_XY_VEL_I, 0.01f); +PARAM_DEFINE_FLOAT(MPC_XY_VEL_I, 0.02f); PARAM_DEFINE_FLOAT(MPC_XY_VEL_D, 0.01f); PARAM_DEFINE_FLOAT(MPC_XY_VEL_MAX, 5.0f); PARAM_DEFINE_FLOAT(MPC_XY_FF, 0.5f); From 153114aec8571a9105541b1fef473d36c4099519 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 28 Dec 2013 00:16:10 +0400 Subject: [PATCH 249/447] mc_pos_control: calculate velocity error derivative without setpoint acceleration to get more clean output --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 14 +++++++++----- 1 file changed, 9 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 f7d50f45da..948459c43b 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -177,7 +177,7 @@ private: math::Vector<3> _vel; math::Vector<3> _pos_sp; math::Vector<3> _vel_sp; - math::Vector<3> _vel_err_prev; /**< velocity on previous step */ + math::Vector<3> _vel_prev; /**< velocity on previous step */ /** * Update our local parameter cache. @@ -261,7 +261,7 @@ MulticopterPositionControl::MulticopterPositionControl() : _vel.zero(); _pos_sp.zero(); _vel_sp.zero(); - _vel_err_prev.zero(); + _vel_prev.zero(); _params_handles.takeoff_alt = param_find("NAV_TAKEOFF_ALT"); _params_handles.takeoff_gap = param_find("NAV_TAKEOFF_GAP"); @@ -693,11 +693,15 @@ MulticopterPositionControl::task_main() reset_int_xy = true; } - /* calculate desired thrust vector in NED frame */ + /* velocity error */ math::Vector<3> vel_err = _vel_sp - _vel; - math::Vector<3> thrust_sp = vel_err.emult(_params.vel_p) + (vel_err - _vel_err_prev).emult(_params.vel_d) / dt + thrust_int; - _vel_err_prev = vel_err; + /* derivative of velocity error, not includes setpoint acceleration */ + math::Vector<3> vel_err_d = (sp_move_rate - _vel).emult(_params.pos_p) - (_vel - _vel_prev) / dt; + _vel_prev = _vel; + + /* thrust vector in NED frame */ + math::Vector<3> thrust_sp = vel_err.emult(_params.vel_p) + vel_err_d.emult(_params.vel_d) + thrust_int; if (!_control_mode.flag_control_velocity_enabled) { thrust_sp(0) = 0.0f; From f084ddfeb05715efee2488e7bd9b51939b4142b8 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 28 Dec 2013 11:47:25 +0400 Subject: [PATCH 250/447] mc_pos_control: AUTO implemented, fixes --- .../fw_pos_control_l1_main.cpp | 36 +- .../mc_pos_control/mc_pos_control_main.cpp | 95 ++- .../multirotor_pos_control.c | 685 ------------------ 3 files changed, 79 insertions(+), 737 deletions(-) delete mode 100644 src/modules/multirotor_pos_control/multirotor_pos_control.c diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 40cb05b7d4..349c10118b 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -319,11 +319,11 @@ private: /** * Control position. */ - bool control_position(const math::Vector2f &global_pos, const math::Vector2f &ground_speed, + bool control_position(const math::Vector<2> &global_pos, const math::Vector<2> &ground_speed, const struct mission_item_triplet_s &_mission_item_triplet); float calculate_target_airspeed(float airspeed_demand); - void calculate_gndspeed_undershoot(const math::Vector2f ¤t_position, const math::Vector2f &ground_speed, const struct mission_item_triplet_s &mission_item_triplet); + void calculate_gndspeed_undershoot(const math::Vector<2> ¤t_position, const math::Vector<2> &ground_speed, const struct mission_item_triplet_s &mission_item_triplet); /** * Shim for calling task_main from task_create. @@ -680,13 +680,13 @@ FixedwingPositionControl::calculate_target_airspeed(float airspeed_demand) } void -FixedwingPositionControl::calculate_gndspeed_undershoot(const math::Vector2f ¤t_position, const math::Vector2f &ground_speed, const struct mission_item_triplet_s &mission_item_triplet) +FixedwingPositionControl::calculate_gndspeed_undershoot(const math::Vector<2> ¤t_position, const math::Vector<2> &ground_speed, const struct mission_item_triplet_s &mission_item_triplet) { if (_global_pos_valid) { /* rotate ground speed vector with current attitude */ - math::Vector2f yaw_vector(_R_nb(0, 0), _R_nb(1, 0)); + math::Vector<2> yaw_vector(_R_nb(0, 0), _R_nb(1, 0)); yaw_vector.normalize(); float ground_speed_body = yaw_vector * ground_speed; @@ -697,7 +697,7 @@ FixedwingPositionControl::calculate_gndspeed_undershoot(const math::Vector2f &cu distance = get_distance_to_next_waypoint(mission_item_triplet.previous.lat, mission_item_triplet.previous.lon, mission_item_triplet.current.lat, mission_item_triplet.current.lon); delta_altitude = mission_item_triplet.current.altitude - mission_item_triplet.previous.altitude; } else { - distance = get_distance_to_next_waypoint(current_position.getX(), current_position.getY(), mission_item_triplet.current.lat, mission_item_triplet.current.lon); + distance = get_distance_to_next_waypoint(current_position(0), current_position(1), mission_item_triplet.current.lat, mission_item_triplet.current.lon); delta_altitude = mission_item_triplet.current.altitude - _global_pos.alt; } @@ -730,7 +730,7 @@ void FixedwingPositionControl::navigation_capabilities_publish() } bool -FixedwingPositionControl::control_position(const math::Vector2f ¤t_position, const math::Vector2f &ground_speed, +FixedwingPositionControl::control_position(const math::Vector<2> ¤t_position, const math::Vector<2> &ground_speed, const struct mission_item_triplet_s &mission_item_triplet) { bool setpoint = true; @@ -765,26 +765,26 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio _tecs.set_speed_weight(_parameters.speed_weight); /* current waypoint (the one currently heading for) */ - math::Vector2f next_wp(mission_item_triplet.current.lat, mission_item_triplet.current.lon); + math::Vector<2> next_wp(mission_item_triplet.current.lat, mission_item_triplet.current.lon); /* current waypoint (the one currently heading for) */ - math::Vector2f curr_wp(mission_item_triplet.current.lat, mission_item_triplet.current.lon); + math::Vector<2> curr_wp(mission_item_triplet.current.lat, mission_item_triplet.current.lon); /* previous waypoint */ - math::Vector2f prev_wp; + math::Vector<2> prev_wp; if (mission_item_triplet.previous_valid) { - prev_wp.setX(mission_item_triplet.previous.lat); - prev_wp.setY(mission_item_triplet.previous.lon); + prev_wp(0) = mission_item_triplet.previous.lat; + prev_wp(1) = mission_item_triplet.previous.lon; } else { /* * No valid previous waypoint, go for the current wp. * This is automatically handled by the L1 library. */ - prev_wp.setX(mission_item_triplet.current.lat); - prev_wp.setY(mission_item_triplet.current.lon); + prev_wp(0) = mission_item_triplet.current.lat; + prev_wp(1) = mission_item_triplet.current.lon; } @@ -820,7 +820,7 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio /* Horizontal landing control */ /* switch to heading hold for the last meters, continue heading hold after */ - float wp_distance = get_distance_to_next_waypoint(current_position.getX(), current_position.getY(), curr_wp.getX(), curr_wp.getY()); + float wp_distance = get_distance_to_next_waypoint(current_position(0), current_position(1), curr_wp(0), curr_wp(1)); //warnx("wp dist: %d, alt err: %d, noret: %s", (int)wp_distance, (int)altitude_error, (land_noreturn) ? "YES" : "NO"); const float heading_hold_distance = 15.0f; if (wp_distance < heading_hold_distance || land_noreturn_horizontal) { @@ -829,7 +829,7 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio // if (mission_item_triplet.previous_valid) { - // target_bearing = get_bearing_to_next_waypoint(prev_wp.getX(), prev_wp.getY(), next_wp.getX(), next_wp.getY()); + // target_bearing = get_bearing_to_next_waypoint(prev_wp(0), prev_wp(1), next_wp(0), next_wp(1)); // } else { if (!land_noreturn_horizontal) //set target_bearing in first occurrence @@ -870,7 +870,7 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio float airspeed_land = 1.3f * _parameters.airspeed_min; float airspeed_approach = 1.3f * _parameters.airspeed_min; - float L_wp_distance = get_distance_to_next_waypoint(prev_wp.getX(), prev_wp.getY(), curr_wp.getX(), curr_wp.getY()) * _parameters.land_slope_length; + float L_wp_distance = get_distance_to_next_waypoint(prev_wp(0), prev_wp(1), curr_wp(0), curr_wp(1)) * _parameters.land_slope_length; float L_altitude = landingslope.getLandingSlopeAbsoluteAltitude(L_wp_distance, _mission_item_triplet.current.altitude);//getLandingSlopeAbsoluteAltitude(L_wp_distance, _mission_item_triplet.current.altitude, landing_slope_angle_rad, horizontal_slope_displacement); float landing_slope_alt_desired = landingslope.getLandingSlopeAbsoluteAltitude(wp_distance, _mission_item_triplet.current.altitude);//getLandingSlopeAbsoluteAltitude(wp_distance, _mission_item_triplet.current.altitude, landing_slope_angle_rad, horizontal_slope_displacement); @@ -989,8 +989,8 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio // warnx("nav bearing: %8.4f bearing err: %8.4f target bearing: %8.4f", (double)_l1_control.nav_bearing(), // (double)_l1_control.bearing_error(), (double)_l1_control.target_bearing()); - // warnx("prev wp: %8.4f/%8.4f, next wp: %8.4f/%8.4f prev:%s", (double)prev_wp.getX(), (double)prev_wp.getY(), - // (double)next_wp.getX(), (double)next_wp.getY(), (mission_item_triplet.previous_valid) ? "valid" : "invalid"); + // warnx("prev wp: %8.4f/%8.4f, next wp: %8.4f/%8.4f prev:%s", (double)prev_wp(0), (double)prev_wp(1), + // (double)next_wp(0), (double)next_wp(1), (mission_item_triplet.previous_valid) ? "valid" : "invalid"); // XXX at this point we always want no loiter hold if a // mission is active 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 948459c43b..e2a5a5def2 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -61,7 +61,7 @@ #include #include #include -#include +#include #include #include #include @@ -113,7 +113,7 @@ private: int _manual_sub; /**< notification of manual control updates */ int _arming_sub; /**< arming status of outputs */ int _local_pos_sub; /**< vehicle local position */ - int _global_pos_sp_sub; /**< vehicle global position setpoint */ + int _mission_items_sub; /**< mission item triplet */ orb_advert_t _local_pos_sp_pub; /**< local position setpoint publication */ orb_advert_t _att_sp_pub; /**< attitude setpoint publication */ @@ -126,7 +126,7 @@ private: struct actuator_armed_s _arming; /**< actuator arming status */ struct vehicle_local_position_s _local_pos; /**< vehicle local position */ struct vehicle_local_position_setpoint_s _local_pos_sp; /**< vehicle local position */ - struct vehicle_global_position_setpoint_s _global_pos_sp; /**< vehicle global position setpoint */ + struct mission_item_triplet_s _mission_items; /**< vehicle global position setpoint */ struct vehicle_global_velocity_setpoint_s _global_vel_sp; /**< vehicle global velocity setpoint */ struct { @@ -232,7 +232,7 @@ MulticopterPositionControl::MulticopterPositionControl() : _manual_sub(-1), _arming_sub(-1), _local_pos_sub(-1), - _global_pos_sp_sub(-1), + _mission_items_sub(-1), /* publications */ _local_pos_sp_pub(-1), @@ -246,7 +246,7 @@ MulticopterPositionControl::MulticopterPositionControl() : memset(&_arming, 0, sizeof(_arming)); memset(&_local_pos, 0, sizeof(_local_pos)); memset(&_local_pos_sp, 0, sizeof(_local_pos_sp)); - memset(&_global_pos_sp, 0, sizeof(_global_pos_sp)); + memset(&_mission_items, 0, sizeof(_mission_items)); memset(&_global_vel_sp, 0, sizeof(_global_vel_sp)); _params.pos_p.zero(); @@ -395,9 +395,9 @@ MulticopterPositionControl::poll_subscriptions() if (updated) orb_copy(ORB_ID(actuator_armed), _arming_sub, &_arming); - orb_check(_global_pos_sp_sub, &updated); + orb_check(_mission_items_sub, &updated); if (updated) - orb_copy(ORB_ID(vehicle_global_position_setpoint), _global_pos_sp_sub, &_global_pos_sp); + orb_copy(ORB_ID(mission_item_triplet), _mission_items_sub, &_mission_items); } float @@ -439,7 +439,7 @@ MulticopterPositionControl::task_main() _manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); _arming_sub = orb_subscribe(ORB_ID(actuator_armed)); _local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position)); - _global_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_global_position_setpoint)); + _mission_items_sub = orb_subscribe(ORB_ID(mission_item_triplet)); parameters_update(true); @@ -449,8 +449,6 @@ MulticopterPositionControl::task_main() /* get an initial update for all sensor and status data */ poll_subscriptions(); - bool reset_mission_sp = false; - bool global_pos_sp_valid = false; bool reset_man_sp_z = true; bool reset_man_sp_xy = true; bool reset_int_z = true; @@ -466,9 +464,10 @@ MulticopterPositionControl::task_main() const float alt_ctl_dz = 0.2f; const float pos_ctl_dz = 0.05f; + hrt_abstime ref_timestamp = 0; + int32_t ref_lat = 0.0f; + int32_t ref_lon = 0.0f; float ref_alt = 0.0f; - hrt_abstime ref_alt_t = 0; - hrt_abstime local_ref_timestamp = 0; math::Vector<3> sp_move_rate; sp_move_rate.zero(); @@ -533,22 +532,32 @@ MulticopterPositionControl::task_main() sp_move_rate.zero(); + if (_local_pos.ref_timestamp != ref_timestamp) { + /* initialize local projection with new reference */ + double lat_home = _local_pos.ref_lat * 1e-7; + double lon_home = _local_pos.ref_lon * 1e-7; + map_projection_init(lat_home, lon_home); + mavlink_log_info(mavlink_fd, "[mpc] local pos ref: %.7f, %.7f", (double)lat_home, (double)lon_home); + + if (_control_mode.flag_control_manual_enabled && ref_timestamp != 0) { + /* correct setpoint in manual mode to stay in the same point */ + float ref_change_x = 0.0f; + float ref_change_y = 0.0f; + map_projection_project(ref_lat, ref_lon, &ref_change_x, &ref_change_y); + _pos_sp(0) += ref_change_x; + _pos_sp(1) += ref_change_y; + _pos_sp(2) += _local_pos.ref_alt - ref_alt; + } + ref_timestamp = _local_pos.ref_timestamp; + ref_lat = _local_pos.ref_lat; + ref_lon = _local_pos.ref_lon; + ref_alt = _local_pos.ref_alt; + } + if (_control_mode.flag_control_manual_enabled) { /* manual control */ - /* check for reference point updates and correct setpoint */ - if (_local_pos.ref_timestamp != ref_alt_t) { - if (ref_alt_t != 0) { - /* home alt changed, don't follow large ground level changes in manual flight */ - _pos_sp(2) += _local_pos.ref_alt - ref_alt; - } - - ref_alt_t = _local_pos.ref_timestamp; - ref_alt = _local_pos.ref_alt; - // TODO also correct XY setpoint - } - - /* reset setpoints to current position if needed */ if (_control_mode.flag_control_altitude_enabled) { + /* reset setpoint Z to current altitude if needed */ if (reset_man_sp_z) { reset_man_sp_z = false; _pos_sp(2) = _pos(2); @@ -560,6 +569,7 @@ MulticopterPositionControl::task_main() } if (_control_mode.flag_control_position_enabled) { + /* reset setpoint XY to current position if needed */ if (reset_man_sp_xy) { reset_man_sp_xy = false; _pos_sp(0) = _pos(0); @@ -594,21 +604,39 @@ MulticopterPositionControl::task_main() _pos_sp = _pos + pos_sp_offs.emult(_params.vel_max); } - /* copy yaw setpoint to vehicle_local_position_setpoint topic */ - _local_pos_sp.yaw = _att_sp.yaw_body; - /* local position setpoint is valid and can be used for auto loiter after position controlled mode */ reset_auto_sp_xy = !_control_mode.flag_control_position_enabled; reset_auto_sp_z = !_control_mode.flag_control_altitude_enabled; reset_takeoff_sp = true; - - /* force reprojection of global setpoint after manual mode */ - reset_mission_sp = true; } else { - // TODO AUTO - _pos_sp = _pos; + /* AUTO */ + if (_mission_items.current_valid) { + struct mission_item_s item = _mission_items.current; + map_projection_project(item.lat, item.lon, &_pos_sp(0), &_pos_sp(1)); + + // TODO home altitude can be != ref_alt, check home_position topic + _pos_sp(2) = -(item.altitude_is_relative ? item.altitude : item.altitude - ref_alt); + + /* in case of interrupted mission don't go to waypoint but stop */ + reset_auto_sp_xy = true; + reset_auto_sp_z = true; + + } else { + /* no waypoint, loiter, reset position setpoint if needed */ + if (reset_auto_sp_xy) { + reset_auto_sp_xy = false; + _pos_sp(0) = _pos(0); + _pos_sp(1) = _pos(1); + } + if (reset_auto_sp_z) { + reset_auto_sp_z = false; + _pos_sp(2) = _pos(2); + } + } } + /* copy resulting setpoint to vehicle_local_position_setpoint topic for logging */ + _local_pos_sp.yaw = _att_sp.yaw_body; _local_pos_sp.x = _pos_sp(0); _local_pos_sp.y = _pos_sp(1); _local_pos_sp.z = _pos_sp(2); @@ -874,7 +902,6 @@ MulticopterPositionControl::task_main() reset_man_sp_xy = true; reset_int_z = true; reset_int_xy = true; - reset_mission_sp = true; reset_auto_sp_xy = true; reset_auto_sp_z = true; } diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c deleted file mode 100644 index 5af7e2a829..0000000000 --- a/src/modules/multirotor_pos_control/multirotor_pos_control.c +++ /dev/null @@ -1,685 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. - * Author: Anton Babushkin - * - * 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 multirotor_pos_control.c - * - * Multirotor position controller - */ - -#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 - -#include "multirotor_pos_control_params.h" -#include "thrust_pid.h" - - -static bool thread_should_exit = false; /**< Deamon exit flag */ -static bool thread_running = false; /**< Deamon status flag */ -static int deamon_task; /**< Handle of deamon task / thread */ - -__EXPORT int multirotor_pos_control_main(int argc, char *argv[]); - -/** - * Mainloop of position controller. - */ -static int multirotor_pos_control_thread_main(int argc, char *argv[]); - -/** - * Print the correct usage. - */ -static void usage(const char *reason); - -static float scale_control(float ctl, float end, float dz); - -static float norm(float x, float y); - -static void usage(const char *reason) -{ - if (reason) - fprintf(stderr, "%s\n", reason); - - fprintf(stderr, "usage: multirotor_pos_control {start|stop|status}\n\n"); - exit(1); -} - -/** - * The deamon app only briefly exists to start - * the background job. The stack size assigned in the - * Makefile does only apply to this management task. - * - * The actual stack size should be set in the call - * to task_spawn(). - */ -int multirotor_pos_control_main(int argc, char *argv[]) -{ - if (argc < 1) - usage("missing command"); - - if (!strcmp(argv[1], "start")) { - - if (thread_running) { - warnx("already running"); - /* this is not an error */ - exit(0); - } - - warnx("start"); - thread_should_exit = false; - deamon_task = task_spawn_cmd("multirotor_pos_control", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 60, - 4096, - multirotor_pos_control_thread_main, - (argv) ? (const char **)&argv[2] : (const char **)NULL); - exit(0); - } - - if (!strcmp(argv[1], "stop")) { - warnx("stop"); - thread_should_exit = true; - exit(0); - } - - if (!strcmp(argv[1], "status")) { - if (thread_running) { - warnx("app is running"); - - } else { - warnx("app not started"); - } - - exit(0); - } - - usage("unrecognized command"); - exit(1); -} - -static float scale_control(float ctl, float end, float dz) -{ - if (ctl > dz) { - return (ctl - dz) / (end - dz); - - } else if (ctl < -dz) { - return (ctl + dz) / (end - dz); - - } else { - return 0.0f; - } -} - -static float norm(float x, float y) -{ - return sqrtf(x * x + y * y); -} - -static int multirotor_pos_control_thread_main(int argc, char *argv[]) -{ - /* welcome user */ - warnx("started"); - static int mavlink_fd; - mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); - mavlink_log_info(mavlink_fd, "[mpc] started"); - - /* structures */ - struct vehicle_control_mode_s control_mode; - memset(&control_mode, 0, sizeof(control_mode)); - struct vehicle_attitude_s att; - memset(&att, 0, sizeof(att)); - struct vehicle_attitude_setpoint_s att_sp; - memset(&att_sp, 0, sizeof(att_sp)); - struct manual_control_setpoint_s manual; - memset(&manual, 0, sizeof(manual)); - struct vehicle_local_position_s local_pos; - memset(&local_pos, 0, sizeof(local_pos)); - struct mission_item_triplet_s triplet; - memset(&triplet, 0, sizeof(triplet)); - struct vehicle_global_velocity_setpoint_s global_vel_sp; - memset(&global_vel_sp, 0, sizeof(global_vel_sp)); - struct vehicle_local_position_setpoint_s local_pos_sp; - memset(&local_pos_sp, 0, sizeof(local_pos_sp)); - - /* subscribe to attitude, motor setpoints and system state */ - int param_sub = orb_subscribe(ORB_ID(parameter_update)); - int control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); - int att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); - int att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); - int manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); - int mission_triplet_sub = orb_subscribe(ORB_ID(mission_item_triplet)); - - /* publish setpoint */ - orb_advert_t local_pos_sp_pub = orb_advertise(ORB_ID(vehicle_local_position_setpoint), &local_pos_sp); - orb_advert_t global_vel_sp_pub = orb_advertise(ORB_ID(vehicle_global_velocity_setpoint), &global_vel_sp); - orb_advert_t att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp); - - bool reset_mission_sp = false; - bool global_pos_sp_valid = false; - bool reset_man_sp_z = true; - bool reset_man_sp_xy = true; - bool reset_int_z = true; - bool reset_int_z_manual = false; - bool reset_int_xy = true; - bool was_armed = false; - bool reset_auto_sp_xy = true; - bool reset_auto_sp_z = true; - bool reset_takeoff_sp = true; - - hrt_abstime t_prev = 0; - const float alt_ctl_dz = 0.2f; - const float pos_ctl_dz = 0.05f; - - float ref_alt = 0.0f; - hrt_abstime ref_alt_t = 0; - uint64_t local_ref_timestamp = 0; - - PID_t xy_pos_pids[2]; - PID_t xy_vel_pids[2]; - PID_t z_pos_pid; - thrust_pid_t z_vel_pid; - - thread_running = true; - - struct multirotor_position_control_params params; - struct multirotor_position_control_param_handles params_h; - parameters_init(¶ms_h); - parameters_update(¶ms_h, ¶ms); - - - for (int i = 0; i < 2; i++) { - pid_init(&(xy_pos_pids[i]), params.xy_p, 0.0f, params.xy_d, 1.0f, 0.0f, PID_MODE_DERIVATIV_SET, 0.02f); - pid_init(&(xy_vel_pids[i]), params.xy_vel_p, params.xy_vel_i, params.xy_vel_d, 1.0f, params.tilt_max, PID_MODE_DERIVATIV_CALC_NO_SP, 0.02f); - } - - pid_init(&z_pos_pid, params.z_p, 0.0f, params.z_d, 1.0f, params.z_vel_max, PID_MODE_DERIVATIV_SET, 0.02f); - thrust_pid_init(&z_vel_pid, params.z_vel_p, params.z_vel_i, params.z_vel_d, -params.thr_max, -params.thr_min, PID_MODE_DERIVATIV_CALC_NO_SP, 0.02f); - - while (!thread_should_exit) { - - bool param_updated; - orb_check(param_sub, ¶m_updated); - - if (param_updated) { - /* clear updated flag */ - struct parameter_update_s ps; - orb_copy(ORB_ID(parameter_update), param_sub, &ps); - /* update params */ - parameters_update(¶ms_h, ¶ms); - - for (int i = 0; i < 2; i++) { - pid_set_parameters(&(xy_pos_pids[i]), params.xy_p, 0.0f, params.xy_d, 1.0f, 0.0f); - /* use integral_limit_out = tilt_max / 2 */ - float i_limit; - - if (params.xy_vel_i > 0.0f) { - i_limit = params.tilt_max / params.xy_vel_i / 2.0f; - - } else { - i_limit = 0.0f; // not used - } - - pid_set_parameters(&(xy_vel_pids[i]), params.xy_vel_p, params.xy_vel_i, params.xy_vel_d, i_limit, params.tilt_max); - } - - pid_set_parameters(&z_pos_pid, params.z_p, 0.0f, params.z_d, 1.0f, params.z_vel_max); - thrust_pid_set_parameters(&z_vel_pid, params.z_vel_p, params.z_vel_i, params.z_vel_d, -params.thr_max, -params.thr_min); - } - - bool updated; - - orb_check(control_mode_sub, &updated); - - if (updated) { - orb_copy(ORB_ID(vehicle_control_mode), control_mode_sub, &control_mode); - } - - orb_check(mission_triplet_sub, &updated); - - if (updated) { - orb_copy(ORB_ID(mission_item_triplet), mission_triplet_sub, &triplet); - global_pos_sp_valid = triplet.current_valid; - reset_mission_sp = true; - } - - hrt_abstime t = hrt_absolute_time(); - float dt; - - if (t_prev != 0) { - dt = (t - t_prev) * 0.000001f; - - } else { - dt = 0.0f; - } - - if (control_mode.flag_armed && !was_armed) { - /* reset setpoints and integrals on arming */ - reset_man_sp_z = true; - reset_man_sp_xy = true; - reset_auto_sp_z = true; - reset_auto_sp_xy = true; - reset_takeoff_sp = true; - reset_int_z = true; - reset_int_xy = true; - } - - was_armed = control_mode.flag_armed; - - t_prev = t; - - if (control_mode.flag_control_altitude_enabled || control_mode.flag_control_velocity_enabled || control_mode.flag_control_position_enabled) { - orb_copy(ORB_ID(manual_control_setpoint), manual_sub, &manual); - orb_copy(ORB_ID(vehicle_attitude), att_sub, &att); - orb_copy(ORB_ID(vehicle_attitude_setpoint), att_sp_sub, &att_sp); - - float z_sp_offs_max = params.z_vel_max / params.z_p * 2.0f; - float xy_sp_offs_max = params.xy_vel_max / params.xy_p * 2.0f; - float sp_move_rate[3] = { 0.0f, 0.0f, 0.0f }; - - if (control_mode.flag_control_manual_enabled) { - /* manual control */ - /* check for reference point updates and correct setpoint */ - if (local_pos.ref_timestamp != ref_alt_t) { - if (ref_alt_t != 0) { - /* home alt changed, don't follow large ground level changes in manual flight */ - local_pos_sp.z += local_pos.ref_alt - ref_alt; - } - - ref_alt_t = local_pos.ref_timestamp; - ref_alt = local_pos.ref_alt; - // TODO also correct XY setpoint - } - - /* reset setpoints to current position if needed */ - if (control_mode.flag_control_altitude_enabled) { - if (reset_man_sp_z) { - reset_man_sp_z = false; - local_pos_sp.z = local_pos.z; - mavlink_log_info(mavlink_fd, "[mpc] reset alt sp: %.2f", (double) - local_pos_sp.z); - } - - /* move altitude setpoint with throttle stick */ - float z_sp_ctl = scale_control(manual.throttle - 0.5f, 0.5f, alt_ctl_dz); - - if (z_sp_ctl != 0.0f) { - sp_move_rate[2] = -z_sp_ctl * params.z_vel_max; - local_pos_sp.z += sp_move_rate[2] * dt; - - if (local_pos_sp.z > local_pos.z + z_sp_offs_max) { - local_pos_sp.z = local_pos.z + z_sp_offs_max; - - } else if (local_pos_sp.z < local_pos.z - z_sp_offs_max) { - local_pos_sp.z = local_pos.z - z_sp_offs_max; - } - } - } - - if (control_mode.flag_control_position_enabled) { - if (reset_man_sp_xy) { - reset_man_sp_xy = false; - local_pos_sp.x = local_pos.x; - local_pos_sp.y = local_pos.y; - pid_reset_integral(&xy_vel_pids[0]); - pid_reset_integral(&xy_vel_pids[1]); - mavlink_log_info(mavlink_fd, "[mpc] reset pos sp: %.2f, %.2f", (double)local_pos_sp.x, (double)local_pos_sp.y); - } - - /* move position setpoint with roll/pitch stick */ - float pos_pitch_sp_ctl = scale_control(-manual.pitch / params.rc_scale_pitch, 1.0f, pos_ctl_dz); - float pos_roll_sp_ctl = scale_control(manual.roll / params.rc_scale_roll, 1.0f, pos_ctl_dz); - - if (pos_pitch_sp_ctl != 0.0f || pos_roll_sp_ctl != 0.0f) { - /* calculate direction and increment of control in NED frame */ - float xy_sp_ctl_dir = att.yaw + atan2f(pos_roll_sp_ctl, pos_pitch_sp_ctl); - float xy_sp_ctl_speed = norm(pos_pitch_sp_ctl, pos_roll_sp_ctl) * params.xy_vel_max; - sp_move_rate[0] = cosf(xy_sp_ctl_dir) * xy_sp_ctl_speed; - sp_move_rate[1] = sinf(xy_sp_ctl_dir) * xy_sp_ctl_speed; - local_pos_sp.x += sp_move_rate[0] * dt; - local_pos_sp.y += sp_move_rate[1] * dt; - /* limit maximum setpoint from position offset and preserve direction - * fail safe, should not happen in normal operation */ - float pos_vec_x = local_pos_sp.x - local_pos.x; - float pos_vec_y = local_pos_sp.y - local_pos.y; - float pos_vec_norm = norm(pos_vec_x, pos_vec_y) / xy_sp_offs_max; - - if (pos_vec_norm > 1.0f) { - local_pos_sp.x = local_pos.x + pos_vec_x / pos_vec_norm; - local_pos_sp.y = local_pos.y + pos_vec_y / pos_vec_norm; - } - } - } - - /* copy yaw setpoint to vehicle_local_position_setpoint topic */ - local_pos_sp.yaw = att_sp.yaw_body; - - /* local position setpoint is valid and can be used for auto loiter after position controlled mode */ - reset_auto_sp_xy = !control_mode.flag_control_position_enabled; - reset_auto_sp_z = !control_mode.flag_control_altitude_enabled; - reset_takeoff_sp = true; - - /* force reprojection of global setpoint after manual mode */ - reset_mission_sp = true; - - } else if (control_mode.flag_control_auto_enabled) { - /* AUTO mode, use global setpoint */ - if (control_mode.auto_state == NAVIGATION_STATE_AUTO_READY) { - reset_auto_sp_xy = true; - reset_auto_sp_z = true; - - } else if (control_mode.auto_state == NAVIGATION_STATE_AUTO_TAKEOFF) { - if (reset_takeoff_sp) { - reset_takeoff_sp = false; - local_pos_sp.x = local_pos.x; - local_pos_sp.y = local_pos.y; - local_pos_sp.z = - params.takeoff_alt - params.takeoff_gap; - att_sp.yaw_body = att.yaw; - mavlink_log_info(mavlink_fd, "[mpc] takeoff sp: %.2f %.2f %.2f", (double)local_pos_sp.x, (double)local_pos_sp.y, (double) - local_pos_sp.z); - } - - reset_auto_sp_xy = false; - reset_auto_sp_z = true; - - } else if (control_mode.auto_state == NAVIGATION_STATE_AUTO_RTL) { - // TODO - reset_auto_sp_xy = true; - reset_auto_sp_z = true; - - } else if (control_mode.auto_state == NAVIGATION_STATE_AUTO_MISSION) { - /* init local projection using local position ref */ - if (local_pos.ref_timestamp != local_ref_timestamp) { - reset_mission_sp = true; - local_ref_timestamp = local_pos.ref_timestamp; - double lat_home = local_pos.ref_lat * 1e-7; - double lon_home = local_pos.ref_lon * 1e-7; - map_projection_init(lat_home, lon_home); - mavlink_log_info(mavlink_fd, "[mpc] local pos ref: %.7f, %.7f", (double)lat_home, (double)lon_home); - } - - if (reset_mission_sp) { - reset_mission_sp = false; - /* update global setpoint projection */ - - if (global_pos_sp_valid) { - - /* project global setpoint to local setpoint */ - map_projection_project(triplet.current.lat, triplet.current.lon, &(local_pos_sp.x), &(local_pos_sp.y)); - - if (triplet.current.altitude_is_relative) { - local_pos_sp.z = -triplet.current.altitude; - - } else { - local_pos_sp.z = local_pos.ref_alt - triplet.current.lat; - } - /* update yaw setpoint only if value is valid */ - if (isfinite(triplet.current.yaw) && fabsf(triplet.current.yaw) < M_TWOPI) { - att_sp.yaw_body = triplet.current.yaw; - } - - mavlink_log_info(mavlink_fd, "[mpc] new sp: %.7f, %.7f (%.2f, %.2f)", triplet.current.lat, triplet.current.lon, (double)local_pos_sp.x, (double)local_pos_sp.y); - - } else { - if (reset_auto_sp_xy) { - reset_auto_sp_xy = false; - /* local position setpoint is invalid, - * use current position as setpoint for loiter */ - local_pos_sp.x = local_pos.x; - local_pos_sp.y = local_pos.y; - local_pos_sp.yaw = att.yaw; - } - - if (reset_auto_sp_z) { - reset_auto_sp_z = false; - local_pos_sp.z = local_pos.z; - } - - mavlink_log_info(mavlink_fd, "[mpc] no global pos sp, loiter: %.2f, %.2f", (double)local_pos_sp.x, (double)local_pos_sp.y); - } - } - - reset_auto_sp_xy = true; - reset_auto_sp_z = true; - } - - if (control_mode.auto_state != NAVIGATION_STATE_AUTO_TAKEOFF) { - reset_takeoff_sp = true; - } - - if (control_mode.auto_state != NAVIGATION_STATE_AUTO_MISSION) { - reset_mission_sp = true; - } - - /* copy yaw setpoint to vehicle_local_position_setpoint topic */ - local_pos_sp.yaw = att_sp.yaw_body; - - /* reset setpoints after AUTO mode */ - reset_man_sp_xy = true; - reset_man_sp_z = true; - - } else { - /* no control (failsafe), loiter or stay on ground */ - if (local_pos.landed) { - /* landed: move setpoint down */ - /* in air: hold altitude */ - if (local_pos_sp.z < 5.0f) { - /* set altitude setpoint to 5m under ground, - * don't set it too deep to avoid unexpected landing in case of false "landed" signal */ - local_pos_sp.z = 5.0f; - mavlink_log_info(mavlink_fd, "[mpc] landed, set alt: %.2f", (double) - local_pos_sp.z); - } - - reset_man_sp_z = true; - - } else { - /* in air: hold altitude */ - if (reset_man_sp_z) { - reset_man_sp_z = false; - local_pos_sp.z = local_pos.z; - mavlink_log_info(mavlink_fd, "[mpc] set loiter alt: %.2f", (double) - local_pos_sp.z); - } - - reset_auto_sp_z = false; - } - - if (control_mode.flag_control_position_enabled) { - if (reset_man_sp_xy) { - reset_man_sp_xy = false; - local_pos_sp.x = local_pos.x; - local_pos_sp.y = local_pos.y; - local_pos_sp.yaw = att.yaw; - att_sp.yaw_body = att.yaw; - mavlink_log_info(mavlink_fd, "[mpc] set loiter pos: %.2f %.2f", (double)local_pos_sp.x, (double)local_pos_sp.y); - } - - reset_auto_sp_xy = false; - } - } - - /* publish local position setpoint */ - orb_publish(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_pub, &local_pos_sp); - - /* run position & altitude controllers, calculate velocity setpoint */ - if (control_mode.flag_control_altitude_enabled) { - global_vel_sp.vz = pid_calculate(&z_pos_pid, local_pos_sp.z, local_pos.z, local_pos.vz - sp_move_rate[2], dt) + sp_move_rate[2]; - - } else { - reset_man_sp_z = true; - global_vel_sp.vz = 0.0f; - } - - if (control_mode.flag_control_position_enabled) { - /* calculate velocity set point in NED frame */ - global_vel_sp.vx = pid_calculate(&xy_pos_pids[0], local_pos_sp.x, local_pos.x, local_pos.vx - sp_move_rate[0], dt) + sp_move_rate[0]; - global_vel_sp.vy = pid_calculate(&xy_pos_pids[1], local_pos_sp.y, local_pos.y, local_pos.vy - sp_move_rate[1], dt) + sp_move_rate[1]; - - /* limit horizontal speed */ - float xy_vel_sp_norm = norm(global_vel_sp.vx, global_vel_sp.vy) / params.xy_vel_max; - - if (xy_vel_sp_norm > 1.0f) { - global_vel_sp.vx /= xy_vel_sp_norm; - global_vel_sp.vy /= xy_vel_sp_norm; - } - - } else { - reset_man_sp_xy = true; - global_vel_sp.vx = 0.0f; - global_vel_sp.vy = 0.0f; - } - - /* publish new velocity setpoint */ - orb_publish(ORB_ID(vehicle_global_velocity_setpoint), global_vel_sp_pub, &global_vel_sp); - // TODO subscribe to velocity setpoint if altitude/position control disabled - - if (control_mode.flag_control_climb_rate_enabled || control_mode.flag_control_velocity_enabled) { - /* run velocity controllers, calculate thrust vector with attitude-thrust compensation */ - float thrust_sp[3] = { 0.0f, 0.0f, 0.0f }; - - if (control_mode.flag_control_climb_rate_enabled) { - if (reset_int_z) { - reset_int_z = false; - float i = params.thr_min; - - if (reset_int_z_manual) { - i = manual.throttle; - - if (i < params.thr_min) { - i = params.thr_min; - - } else if (i > params.thr_max) { - i = params.thr_max; - } - } - - thrust_pid_set_integral(&z_vel_pid, -i); - mavlink_log_info(mavlink_fd, "[mpc] reset hovering thrust: %.2f", (double)i); - } - - thrust_sp[2] = thrust_pid_calculate(&z_vel_pid, global_vel_sp.vz, local_pos.vz, dt, att.R[2][2]); - att_sp.thrust = -thrust_sp[2]; - - } else { - /* reset thrust integral when altitude control enabled */ - reset_int_z = true; - } - - if (control_mode.flag_control_velocity_enabled) { - /* calculate thrust set point in NED frame */ - if (reset_int_xy) { - reset_int_xy = false; - pid_reset_integral(&xy_vel_pids[0]); - pid_reset_integral(&xy_vel_pids[1]); - mavlink_log_info(mavlink_fd, "[mpc] reset pos integral"); - } - - thrust_sp[0] = pid_calculate(&xy_vel_pids[0], global_vel_sp.vx, local_pos.vx, 0.0f, dt); - thrust_sp[1] = pid_calculate(&xy_vel_pids[1], global_vel_sp.vy, local_pos.vy, 0.0f, dt); - - /* thrust_vector now contains desired acceleration (but not in m/s^2) in NED frame */ - /* limit horizontal part of thrust */ - float thrust_xy_dir = atan2f(thrust_sp[1], thrust_sp[0]); - /* assuming that vertical component of thrust is g, - * horizontal component = g * tan(alpha) */ - float tilt = atanf(norm(thrust_sp[0], thrust_sp[1])); - - if (tilt > params.tilt_max) { - tilt = params.tilt_max; - } - - /* convert direction to body frame */ - thrust_xy_dir -= att.yaw; - /* calculate roll and pitch */ - att_sp.roll_body = sinf(thrust_xy_dir) * tilt; - att_sp.pitch_body = -cosf(thrust_xy_dir) * tilt / cosf(att_sp.roll_body); - - } else { - reset_int_xy = true; - } - - att_sp.timestamp = hrt_absolute_time(); - - /* publish new attitude setpoint */ - orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); - } - - } else { - /* position controller disabled, reset setpoints */ - reset_man_sp_z = true; - reset_man_sp_xy = true; - reset_int_z = true; - reset_int_xy = true; - reset_mission_sp = true; - reset_auto_sp_xy = true; - reset_auto_sp_z = true; - } - - /* reset altitude controller integral (hovering throttle) to manual throttle after manual throttle control */ - reset_int_z_manual = control_mode.flag_armed && control_mode.flag_control_manual_enabled && !control_mode.flag_control_climb_rate_enabled; - - /* run at approximately 50 Hz */ - usleep(20000); - } - - warnx("stopped"); - mavlink_log_info(mavlink_fd, "[mpc] stopped"); - - thread_running = false; - - fflush(stdout); - return 0; -} - From 7e29028429fce33b88dd1eeb6d3ac89aa5cb5891 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 29 Dec 2013 12:16:49 +0400 Subject: [PATCH 251/447] Moving nav state from commander to navigator, WIP --- src/modules/commander/commander.cpp | 278 +++-------------- .../commander/state_machine_helper.cpp | 189 +----------- src/modules/commander/state_machine_helper.h | 9 +- src/modules/mavlink/mavlink.c | 3 + src/modules/mavlink_onboard/mavlink.c | 3 +- .../multirotor_att_control_main.c | 2 +- .../multirotor_pos_control.c | 134 +-------- src/modules/navigator/navigator_main.cpp | 283 +++++++++++------- src/modules/sdlog2/sdlog2.c | 3 +- .../uORB/topics/vehicle_control_mode.h | 19 +- src/modules/uORB/topics/vehicle_status.h | 18 +- 11 files changed, 249 insertions(+), 692 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 28118b9904..199bfb0daf 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -69,7 +69,6 @@ #include #include #include -#include #include #include #include @@ -194,7 +193,7 @@ void usage(const char *reason); /** * React to commands that are sent e.g. from the mavlink module. */ -void handle_command(struct vehicle_status_s *status, struct vehicle_control_mode_s *control_mode, struct vehicle_command_s *cmd, struct actuator_armed_s *armed); +void handle_command(struct vehicle_status_s *status, struct vehicle_command_s *cmd, struct actuator_armed_s *armed); /** * Mainloop of commander. @@ -215,8 +214,6 @@ void print_reject_arm(const char *msg); void print_status(); -transition_result_t check_navigation_state_machine(struct vehicle_status_s *status, struct vehicle_control_mode_s *control_mode, struct vehicle_local_position_s *local_pos); - /** * Loop that runs at a lower rate and priority for calibration and parameter tasks. */ @@ -346,13 +343,12 @@ void print_status() warnx("arming: %s", armed_str); } -static orb_advert_t control_mode_pub; static orb_advert_t status_pub; -void handle_command(struct vehicle_status_s *status, const struct safety_s *safety, struct vehicle_control_mode_s *control_mode, struct vehicle_command_s *cmd, struct actuator_armed_s *armed) +void handle_command(struct vehicle_status_s *status, const struct safety_s *safety, struct vehicle_command_s *cmd, struct actuator_armed_s *armed) { /* result of the command */ - uint8_t result = VEHICLE_CMD_RESULT_UNSUPPORTED; + enum VEHICLE_CMD_RESULT result = VEHICLE_CMD_RESULT_UNSUPPORTED; /* only handle high-priority commands here */ @@ -365,12 +361,12 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe /* set HIL state */ hil_state_t new_hil_state = (base_mode & MAV_MODE_FLAG_HIL_ENABLED) ? HIL_STATE_ON : HIL_STATE_OFF; - int hil_ret = hil_state_transition(new_hil_state, status_pub, status, control_mode_pub, control_mode, mavlink_fd); + int hil_ret = hil_state_transition(new_hil_state, status_pub, status, mavlink_fd); /* if HIL got enabled, reset battery status state */ - if (hil_ret == OK && control_mode->flag_system_hil_enabled) { + if (hil_ret == OK && status->hil_state == HIL_STATE_ON) { /* reset the arming mode to disarmed */ - arming_res = arming_state_transition(status, safety, control_mode, ARMING_STATE_STANDBY, armed); + arming_res = arming_state_transition(status, safety, ARMING_STATE_STANDBY, armed); if (arming_res != TRANSITION_DENIED) { mavlink_log_info(mavlink_fd, "[cmd] HIL: Reset ARMED state to standby"); @@ -386,12 +382,12 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe arming_res = TRANSITION_NOT_CHANGED; if (base_mode & MAV_MODE_FLAG_SAFETY_ARMED) { - if ((safety->safety_switch_available && !safety->safety_off) && !control_mode->flag_system_hil_enabled) { + if ((safety->safety_switch_available && !safety->safety_off) && status->hil_state == HIL_STATE_OFF) { print_reject_arm("NOT ARMING: Press safety switch first."); arming_res = TRANSITION_DENIED; } else { - arming_res = arming_state_transition(status, safety, control_mode, ARMING_STATE_ARMED, armed); + arming_res = arming_state_transition(status, safety, ARMING_STATE_ARMED, armed); } if (arming_res == TRANSITION_CHANGED) { @@ -401,7 +397,7 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe } else { if (status->arming_state == ARMING_STATE_ARMED || status->arming_state == ARMING_STATE_ARMED_ERROR) { arming_state_t new_arming_state = (status->arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR); - arming_res = arming_state_transition(status, safety, control_mode, new_arming_state, armed); + arming_res = arming_state_transition(status, safety, new_arming_state, armed); if (arming_res == TRANSITION_CHANGED) { mavlink_log_info(mavlink_fd, "[cmd] DISARMED by command"); @@ -462,29 +458,6 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe break; } - case VEHICLE_CMD_NAV_TAKEOFF: { - if (armed->armed) { - transition_result_t nav_res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_TAKEOFF, control_mode); - - if (nav_res == TRANSITION_CHANGED) { - mavlink_log_info(mavlink_fd, "[cmd] TAKEOFF on command"); - } - - if (nav_res != TRANSITION_DENIED) { - result = VEHICLE_CMD_RESULT_ACCEPTED; - - } else { - result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; - } - - } else { - /* reject TAKEOFF not armed */ - result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; - } - - break; - } - case VEHICLE_CMD_COMPONENT_ARM_DISARM: { transition_result_t arming_res = TRANSITION_NOT_CHANGED; @@ -494,7 +467,7 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe arming_res = TRANSITION_DENIED; } else { - arming_res = arming_state_transition(status, safety, control_mode, ARMING_STATE_ARMED, armed); + arming_res = arming_state_transition(status, safety, ARMING_STATE_ARMED, armed); } if (arming_res == TRANSITION_CHANGED) { @@ -513,7 +486,7 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe case VEHICLE_CMD_DO_SET_SERVO: { //xxx: needs its own mavlink command if (armed->armed && cmd->param3 > 0.5) { //xxx: for safety only for now, param3 is unused by VEHICLE_CMD_DO_SET_SERVO - transition_result_t flighttermination_res = flighttermination_state_transition(status, FLIGHTTERMINATION_STATE_ON, control_mode); + transition_result_t flighttermination_res = flighttermination_state_transition(status, FLIGHTTERMINATION_STATE_ON); result = VEHICLE_CMD_RESULT_ACCEPTED; } else { @@ -524,29 +497,22 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe } break; + case VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN: + case VEHICLE_CMD_PREFLIGHT_CALIBRATION: + case VEHICLE_CMD_PREFLIGHT_SET_SENSOR_OFFSETS: + case VEHICLE_CMD_PREFLIGHT_STORAGE: + /* ignore commands that handled in low prio loop */ + break; + default: + /* warn about unsupported commands */ + answer_command(*cmd, VEHICLE_CMD_RESULT_UNSUPPORTED); break; } - /* supported command handling stop */ - if (result == VEHICLE_CMD_RESULT_ACCEPTED) { - tune_positive(); - - } else if (result == VEHICLE_CMD_RESULT_UNSUPPORTED) { - /* we do not care in the high prio loop about commands we don't know */ - } else { - tune_negative(); - - if (result == VEHICLE_CMD_RESULT_DENIED) { - mavlink_log_critical(mavlink_fd, "#audio: command denied: %u", cmd->command); - - } else if (result == VEHICLE_CMD_RESULT_FAILED) { - mavlink_log_critical(mavlink_fd, "#audio: command failed: %u", cmd->command); - - } else if (result == VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED) { - mavlink_log_critical(mavlink_fd, "#audio: command temporarily rejected: %u", cmd->command); - - } + if (result != VEHICLE_CMD_RESULT_UNSUPPORTED) { + /* already warned about unsupported commands in "default" case */ + answer_command(*cmd, result); } /* send any requested ACKs */ @@ -564,9 +530,6 @@ static struct actuator_armed_s armed; static struct safety_s safety; -/* flags for control apps */ -struct vehicle_control_mode_s control_mode; - int commander_thread_main(int argc, char *argv[]) { /* not yet initialized */ @@ -609,11 +572,9 @@ int commander_thread_main(int argc, char *argv[]) /* Initialize armed with all false */ memset(&armed, 0, sizeof(armed)); - /* Initialize all flags to false */ - memset(&control_mode, 0, sizeof(control_mode)); - status.main_state = MAIN_STATE_MANUAL; - status.navigation_state = NAVIGATION_STATE_DIRECT; + status.set_nav_state = NAV_STATE_INIT; + status.set_nav_state_timestamp = 0; status.arming_state = ARMING_STATE_INIT; status.hil_state = HIL_STATE_OFF; @@ -625,9 +586,6 @@ int commander_thread_main(int argc, char *argv[]) status.rc_signal_lost = true; status.offboard_control_signal_lost = true; - /* allow manual override initially */ - control_mode.flag_external_manual_override_ok = true; - /* set battery warning flag */ status.battery_warning = VEHICLE_BATTERY_WARNING_NONE; status.condition_battery_voltage_valid = false; @@ -635,9 +593,6 @@ int commander_thread_main(int argc, char *argv[]) // XXX for now just set sensors as initialized status.condition_system_sensors_initialized = true; - // XXX just disable offboard control for now - control_mode.flag_control_offboard_enabled = false; - /* advertise to ORB */ status_pub = orb_advertise(ORB_ID(vehicle_status), &status); /* publish current state machine */ @@ -649,8 +604,6 @@ int commander_thread_main(int argc, char *argv[]) armed_pub = orb_advertise(ORB_ID(actuator_armed), &armed); - control_mode_pub = orb_advertise(ORB_ID(vehicle_control_mode), &control_mode); - /* home position */ orb_advert_t home_pub = -1; struct home_position_s home; @@ -803,11 +756,9 @@ int commander_thread_main(int argc, char *argv[]) status.system_type == VEHICLE_TYPE_QUADROTOR || status.system_type == VEHICLE_TYPE_HEXAROTOR || status.system_type == VEHICLE_TYPE_OCTOROTOR) { - control_mode.flag_external_manual_override_ok = false; status.is_rotary_wing = true; } else { - control_mode.flag_external_manual_override_ok = true; status.is_rotary_wing = false; } @@ -859,7 +810,7 @@ int commander_thread_main(int argc, char *argv[]) // XXX this would be the right approach to do it, but do we *WANT* this? // /* disarm if safety is now on and still armed */ // if (safety.safety_switch_available && !safety.safety_off) { - // (void)arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_STANDBY, &armed); + // (void)arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed); // } } @@ -990,10 +941,10 @@ int commander_thread_main(int argc, char *argv[]) battery_tune_played = false; if (armed.armed) { - arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_ARMED_ERROR, &armed); + arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed); } else { - arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_STANDBY_ERROR, &armed); + arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed); } status_changed = true; @@ -1012,7 +963,7 @@ int commander_thread_main(int argc, char *argv[]) /* If in INIT state, try to proceed to STANDBY state */ if (status.arming_state == ARMING_STATE_INIT && low_prio_task == LOW_PRIO_TASK_NONE) { // XXX check for sensors - arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_STANDBY, &armed); + arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed); } else { // XXX: Add emergency stuff if sensors are lost @@ -1102,15 +1053,13 @@ int commander_thread_main(int argc, char *argv[]) * do it only for rotary wings */ if (status.is_rotary_wing && (status.arming_state == ARMING_STATE_ARMED || status.arming_state == ARMING_STATE_ARMED_ERROR) && - (status.main_state == MAIN_STATE_MANUAL || status.navigation_state == NAVIGATION_STATE_AUTO_READY || - (status.condition_landed && ( - status.navigation_state == NAVIGATION_STATE_ALTHOLD || - status.navigation_state == NAVIGATION_STATE_VECTOR - ))) && sp_man.yaw < -STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) { + (status.main_state == MAIN_STATE_MANUAL || status.condition_landed) && + sp_man.yaw < -STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) { + if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { /* disarm to STANDBY if ARMED or to STANDBY_ERROR if ARMED_ERROR */ arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR); - res = arming_state_transition(&status, &safety, &control_mode, new_arming_state, &armed); + res = arming_state_transition(&status, &safety, new_arming_state, &armed); stick_off_counter = 0; } else { @@ -1132,7 +1081,7 @@ int commander_thread_main(int argc, char *argv[]) print_reject_arm("NOT ARMING: Switch to MANUAL mode first."); } else { - res = arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_ARMED, &armed); + res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed); } stick_on_counter = 0; @@ -1185,12 +1134,12 @@ int commander_thread_main(int argc, char *argv[]) /* Flight termination in manual mode if assisted switch is on easy position //xxx hack! */ if (armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.assisted_switch > STICK_ON_OFF_LIMIT) { - transition_result_t flighttermination_res = flighttermination_state_transition(&status, FLIGHTTERMINATION_STATE_ON, &control_mode); + transition_result_t flighttermination_res = flighttermination_state_transition(&status, FLIGHTTERMINATION_STATE_ON); if (flighttermination_res == TRANSITION_CHANGED) { tune_positive(); } } else { - flighttermination_state_transition(&status, FLIGHTTERMINATION_STATE_OFF, &control_mode); + flighttermination_state_transition(&status, FLIGHTTERMINATION_STATE_OFF); } @@ -1202,32 +1151,18 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd); /* handle it */ - handle_command(&status, &safety, &control_mode, &cmd, &armed); - } - - /* evaluate the navigation state machine */ - transition_result_t res = check_navigation_state_machine(&status, &control_mode, &local_position); - - if (res == TRANSITION_DENIED) { - /* DENIED here indicates bug in the commander */ - warnx("ERROR: nav denied: arm %d main %d nav %d", status.arming_state, status.main_state, status.navigation_state); - mavlink_log_critical(mavlink_fd, "#audio: ERROR: nav denied: arm %d main %d nav %d", status.arming_state, status.main_state, status.navigation_state); + handle_command(&status, &safety, &cmd, &armed); } /* check which state machines for changes, clear "changed" flag */ bool arming_state_changed = check_arming_state_changed(); bool main_state_changed = check_main_state_changed(); - bool navigation_state_changed = check_navigation_state_changed(); bool flighttermination_state_changed = check_flighttermination_state_changed(); hrt_abstime t1 = hrt_absolute_time(); - if (navigation_state_changed || arming_state_changed) { - control_mode.flag_armed = armed.armed; // copy armed state to vehicle_control_mode topic - } - - if (arming_state_changed || main_state_changed || navigation_state_changed) { - mavlink_log_info(mavlink_fd, "[cmd] state: arm %d, main %d, nav %d", status.arming_state, status.main_state, status.navigation_state); + if (arming_state_changed || main_state_changed) { + mavlink_log_info(mavlink_fd, "[cmd] state: arm %d, main %d", status.arming_state, status.main_state); status_changed = true; } @@ -1235,8 +1170,6 @@ int commander_thread_main(int argc, char *argv[]) if (counter % (200000 / COMMANDER_MONITORING_INTERVAL) == 0 || status_changed) { status.timestamp = t1; orb_publish(ORB_ID(vehicle_status), status_pub, &status); - control_mode.timestamp = t1; - orb_publish(ORB_ID(vehicle_control_mode), control_mode_pub, &control_mode); armed.timestamp = t1; orb_publish(ORB_ID(actuator_armed), armed_pub, &armed); } @@ -1549,133 +1482,6 @@ print_reject_arm(const char *msg) } } -transition_result_t -check_navigation_state_machine(struct vehicle_status_s *status, struct vehicle_control_mode_s *control_mode, struct vehicle_local_position_s *local_pos) -{ - transition_result_t res = TRANSITION_DENIED; - - if (status->main_state == MAIN_STATE_AUTO) { - if (status->arming_state == ARMING_STATE_ARMED || status->arming_state == ARMING_STATE_ARMED_ERROR) { - // TODO AUTO_LAND handling - if (status->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) { - /* don't switch to other states until takeoff not completed */ - // XXX: only respect the condition_landed when the local position is actually valid - if (status->is_rotary_wing && status->condition_local_altitude_valid && (local_pos->z > -takeoff_alt || status->condition_landed)) { - return TRANSITION_NOT_CHANGED; - } - } - - if (status->navigation_state != NAVIGATION_STATE_AUTO_TAKEOFF && - status->navigation_state != NAVIGATION_STATE_AUTO_LOITER && - status->navigation_state != NAVIGATION_STATE_AUTO_MISSION && - status->navigation_state != NAVIGATION_STATE_AUTO_RTL) { - /* possibly on ground, switch to TAKEOFF if needed */ - if (status->is_rotary_wing && status->condition_local_altitude_valid && (local_pos->z > -takeoff_alt || status->condition_landed)) { - res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_TAKEOFF, control_mode); - return res; - } - } - - /* switch to AUTO mode */ - if (status->rc_signal_found_once && !status->rc_signal_lost) { - /* act depending on switches when manual control enabled */ - if (status->return_switch == RETURN_SWITCH_RETURN) { - /* RTL */ - res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_RTL, control_mode); - - } else { - if (status->mission_switch == MISSION_SWITCH_MISSION) { - /* MISSION */ - res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_MISSION, control_mode); - - } else { - /* LOITER */ - res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_LOITER, control_mode); - } - } - - } else { - /* switch to MISSION when no RC control and first time in some AUTO mode */ - if (status->navigation_state == NAVIGATION_STATE_AUTO_LOITER || - status->navigation_state == NAVIGATION_STATE_AUTO_MISSION || - status->navigation_state == NAVIGATION_STATE_AUTO_RTL || - status->navigation_state == NAVIGATION_STATE_AUTO_LAND) { - res = TRANSITION_NOT_CHANGED; - - } else { - res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_MISSION, control_mode); - } - } - - } else { - /* disarmed, always switch to AUTO_READY */ - res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_READY, control_mode); - } - - } else { - /* manual control modes */ - if (status->rc_signal_lost && (status->arming_state == ARMING_STATE_ARMED || status->arming_state == ARMING_STATE_ARMED_ERROR)) { - /* switch to failsafe mode */ - bool manual_control_old = control_mode->flag_control_manual_enabled; - - if (!status->condition_landed && status->condition_local_position_valid) { - /* in air: try to hold position if possible */ - res = navigation_state_transition(status, NAVIGATION_STATE_VECTOR, control_mode); - - } else { - /* landed: don't try to hold position but land (if taking off) */ - res = TRANSITION_DENIED; - } - - if (res == TRANSITION_DENIED) { - res = navigation_state_transition(status, NAVIGATION_STATE_ALTHOLD, control_mode); - } - - control_mode->flag_control_manual_enabled = false; - - if (res == TRANSITION_NOT_CHANGED && manual_control_old) { - /* mark navigation state as changed to force immediate flag publishing */ - set_navigation_state_changed(); - res = TRANSITION_CHANGED; - } - - if (res == TRANSITION_CHANGED) { - if (control_mode->flag_control_position_enabled) { - mavlink_log_critical(mavlink_fd, "#audio: FAILSAFE: POS HOLD"); - - } else { - if (status->condition_landed) { - mavlink_log_critical(mavlink_fd, "#audio: FAILSAFE: ALT HOLD (LAND)"); - - } else { - mavlink_log_critical(mavlink_fd, "#audio: FAILSAFE: ALT HOLD"); - } - } - } - - } else { - switch (status->main_state) { - case MAIN_STATE_MANUAL: - res = navigation_state_transition(status, status->is_rotary_wing ? NAVIGATION_STATE_STABILIZE : NAVIGATION_STATE_DIRECT, control_mode); - break; - - case MAIN_STATE_SEATBELT: - res = navigation_state_transition(status, NAVIGATION_STATE_ALTHOLD, control_mode); - break; - - case MAIN_STATE_EASY: - res = navigation_state_transition(status, NAVIGATION_STATE_VECTOR, control_mode); - break; - - default: - break; - } - } - } - - return res; -} - void answer_command(struct vehicle_command_s &cmd, enum VEHICLE_CMD_RESULT result) { switch (result) { @@ -1784,7 +1590,7 @@ void *commander_low_prio_loop(void *arg) /* try to go to INIT/PREFLIGHT arming state */ // XXX disable interrupts in arming_state_transition - if (TRANSITION_DENIED == arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_INIT, &armed)) { + if (TRANSITION_DENIED == arming_state_transition(&status, &safety, ARMING_STATE_INIT, &armed)) { answer_command(cmd, VEHICLE_CMD_RESULT_DENIED); break; } @@ -1824,7 +1630,7 @@ void *commander_low_prio_loop(void *arg) else tune_negative(); - arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_STANDBY, &armed); + arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed); break; } @@ -1876,7 +1682,7 @@ void *commander_low_prio_loop(void *arg) } default: - answer_command(cmd, VEHICLE_CMD_RESULT_UNSUPPORTED); + /* don't answer on unsupported commands, it will be done in main loop */ break; } diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index ca3ec94b88..731e0e3ffc 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -46,7 +46,6 @@ #include #include #include -#include #include #include #include @@ -64,12 +63,10 @@ static const int ERROR = -1; static bool arming_state_changed = true; static bool main_state_changed = true; -static bool navigation_state_changed = true; static bool flighttermination_state_changed = true; transition_result_t arming_state_transition(struct vehicle_status_s *status, const struct safety_s *safety, - const struct vehicle_control_mode_s *control_mode, arming_state_t new_arming_state, struct actuator_armed_s *armed) { /* @@ -86,7 +83,7 @@ arming_state_transition(struct vehicle_status_s *status, const struct safety_s * } else { /* enforce lockdown in HIL */ - if (control_mode->flag_system_hil_enabled) { + if (status->hil_state == HIL_STATE_ON) { armed->lockdown = true; } else { armed->lockdown = false; @@ -109,7 +106,7 @@ arming_state_transition(struct vehicle_status_s *status, const struct safety_s * /* allow coming from INIT and disarming from ARMED */ if (status->arming_state == ARMING_STATE_INIT || status->arming_state == ARMING_STATE_ARMED - || control_mode->flag_system_hil_enabled) { + || status->hil_state == HIL_STATE_ON) { /* sensors need to be initialized for STANDBY state */ if (status->condition_system_sensors_initialized) { @@ -126,7 +123,7 @@ arming_state_transition(struct vehicle_status_s *status, const struct safety_s * /* allow arming from STANDBY and IN-AIR-RESTORE */ if ((status->arming_state == ARMING_STATE_STANDBY || status->arming_state == ARMING_STATE_IN_AIR_RESTORE) - && (!safety->safety_switch_available || safety->safety_off || control_mode->flag_system_hil_enabled)) { /* only allow arming if safety is off */ + && (!safety->safety_switch_available || safety->safety_off || status->hil_state == HIL_STATE_ON)) { /* only allow arming if safety is off */ ret = TRANSITION_CHANGED; armed->armed = true; armed->ready_to_arm = true; @@ -289,169 +286,6 @@ check_main_state_changed() } } -transition_result_t -navigation_state_transition(struct vehicle_status_s *status, navigation_state_t new_navigation_state, struct vehicle_control_mode_s *control_mode) -{ - transition_result_t ret = TRANSITION_DENIED; - - /* only check transition if the new state is actually different from the current one */ - if (new_navigation_state == status->navigation_state) { - ret = TRANSITION_NOT_CHANGED; - - } else { - - switch (new_navigation_state) { - case NAVIGATION_STATE_DIRECT: - ret = TRANSITION_CHANGED; - control_mode->flag_control_rates_enabled = true; - control_mode->flag_control_attitude_enabled = false; - control_mode->flag_control_velocity_enabled = false; - control_mode->flag_control_position_enabled = false; - control_mode->flag_control_altitude_enabled = false; - control_mode->flag_control_climb_rate_enabled = false; - control_mode->flag_control_manual_enabled = true; - control_mode->flag_control_auto_enabled = false; - break; - - case NAVIGATION_STATE_STABILIZE: - ret = TRANSITION_CHANGED; - control_mode->flag_control_rates_enabled = true; - control_mode->flag_control_attitude_enabled = true; - control_mode->flag_control_velocity_enabled = false; - control_mode->flag_control_position_enabled = false; - control_mode->flag_control_altitude_enabled = false; - control_mode->flag_control_climb_rate_enabled = false; - control_mode->flag_control_manual_enabled = true; - control_mode->flag_control_auto_enabled = false; - break; - - case NAVIGATION_STATE_ALTHOLD: - ret = TRANSITION_CHANGED; - control_mode->flag_control_rates_enabled = true; - control_mode->flag_control_attitude_enabled = true; - control_mode->flag_control_velocity_enabled = false; - control_mode->flag_control_position_enabled = false; - control_mode->flag_control_altitude_enabled = true; - control_mode->flag_control_climb_rate_enabled = true; - control_mode->flag_control_manual_enabled = true; - control_mode->flag_control_auto_enabled = false; - break; - - case NAVIGATION_STATE_VECTOR: - ret = TRANSITION_CHANGED; - control_mode->flag_control_rates_enabled = true; - control_mode->flag_control_attitude_enabled = true; - control_mode->flag_control_velocity_enabled = true; - control_mode->flag_control_position_enabled = true; - control_mode->flag_control_altitude_enabled = true; - control_mode->flag_control_climb_rate_enabled = true; - control_mode->flag_control_manual_enabled = true; - control_mode->flag_control_auto_enabled = false; - break; - - case NAVIGATION_STATE_AUTO_READY: - ret = TRANSITION_CHANGED; - control_mode->flag_control_rates_enabled = false; - control_mode->flag_control_attitude_enabled = false; - control_mode->flag_control_velocity_enabled = false; - control_mode->flag_control_position_enabled = false; - control_mode->flag_control_altitude_enabled = false; - control_mode->flag_control_climb_rate_enabled = false; - control_mode->flag_control_manual_enabled = false; - control_mode->flag_control_auto_enabled = true; - break; - - case NAVIGATION_STATE_AUTO_TAKEOFF: - ret = TRANSITION_CHANGED; - control_mode->flag_control_rates_enabled = true; - control_mode->flag_control_attitude_enabled = true; - control_mode->flag_control_velocity_enabled = true; - control_mode->flag_control_position_enabled = true; - control_mode->flag_control_altitude_enabled = true; - control_mode->flag_control_climb_rate_enabled = true; - control_mode->flag_control_manual_enabled = false; - control_mode->flag_control_auto_enabled = true; - break; - - case NAVIGATION_STATE_AUTO_LOITER: - ret = TRANSITION_CHANGED; - control_mode->flag_control_rates_enabled = true; - control_mode->flag_control_attitude_enabled = true; - control_mode->flag_control_velocity_enabled = true; - control_mode->flag_control_position_enabled = true; - control_mode->flag_control_altitude_enabled = true; - control_mode->flag_control_climb_rate_enabled = true; - control_mode->flag_control_manual_enabled = false; - control_mode->flag_control_auto_enabled = false; - break; - - case NAVIGATION_STATE_AUTO_MISSION: - ret = TRANSITION_CHANGED; - control_mode->flag_control_rates_enabled = true; - control_mode->flag_control_attitude_enabled = true; - control_mode->flag_control_velocity_enabled = true; - control_mode->flag_control_position_enabled = true; - control_mode->flag_control_altitude_enabled = true; - control_mode->flag_control_climb_rate_enabled = true; - control_mode->flag_control_manual_enabled = false; - control_mode->flag_control_auto_enabled = true; - break; - - case NAVIGATION_STATE_AUTO_RTL: - ret = TRANSITION_CHANGED; - control_mode->flag_control_rates_enabled = true; - control_mode->flag_control_attitude_enabled = true; - control_mode->flag_control_velocity_enabled = true; - control_mode->flag_control_position_enabled = true; - control_mode->flag_control_altitude_enabled = true; - control_mode->flag_control_climb_rate_enabled = true; - control_mode->flag_control_manual_enabled = false; - control_mode->flag_control_auto_enabled = true; - break; - - case NAVIGATION_STATE_AUTO_LAND: - - /* deny transitions from landed state */ - if (status->navigation_state != NAVIGATION_STATE_AUTO_READY) { - ret = TRANSITION_CHANGED; - control_mode->flag_control_rates_enabled = true; - control_mode->flag_control_attitude_enabled = true; - control_mode->flag_control_velocity_enabled = true; - control_mode->flag_control_position_enabled = true; - control_mode->flag_control_altitude_enabled = true; - control_mode->flag_control_climb_rate_enabled = true; - control_mode->flag_control_manual_enabled = false; - control_mode->flag_control_auto_enabled = true; - } - - break; - - default: - break; - } - - if (ret == TRANSITION_CHANGED) { - status->navigation_state = new_navigation_state; - control_mode->auto_state = status->navigation_state; - navigation_state_changed = true; - } - } - - return ret; -} - -bool -check_navigation_state_changed() -{ - if (navigation_state_changed) { - navigation_state_changed = false; - return true; - - } else { - return false; - } -} - bool check_flighttermination_state_changed() { @@ -464,16 +298,10 @@ check_flighttermination_state_changed() } } -void -set_navigation_state_changed() -{ - navigation_state_changed = true; -} - /** * Transition from one hil state to another */ -int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_status, int control_mode_pub, struct vehicle_control_mode_s *current_control_mode, const int mavlink_fd) +int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) { bool valid_transition = false; int ret = ERROR; @@ -502,7 +330,6 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s || current_status->arming_state == ARMING_STATE_STANDBY || current_status->arming_state == ARMING_STATE_STANDBY_ERROR) { - current_control_mode->flag_system_hil_enabled = true; mavlink_log_critical(mavlink_fd, "Switched to ON hil state"); valid_transition = true; } @@ -521,9 +348,6 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s current_status->timestamp = hrt_absolute_time(); orb_publish(ORB_ID(vehicle_status), status_pub, current_status); - current_control_mode->timestamp = hrt_absolute_time(); - orb_publish(ORB_ID(vehicle_control_mode), control_mode_pub, current_control_mode); - // XXX also set lockdown here ret = OK; @@ -539,7 +363,7 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s /** * Transition from one flightermination state to another */ -transition_result_t flighttermination_state_transition(struct vehicle_status_s *status, flighttermination_state_t new_flighttermination_state, struct vehicle_control_mode_s *control_mode) +transition_result_t flighttermination_state_transition(struct vehicle_status_s *status, flighttermination_state_t new_flighttermination_state) { transition_result_t ret = TRANSITION_DENIED; @@ -566,7 +390,8 @@ transition_result_t flighttermination_state_transition(struct vehicle_status_s * if (ret == TRANSITION_CHANGED) { flighttermination_state_changed = true; - control_mode->flag_control_flighttermination_enabled = status->flighttermination_state == FLIGHTTERMINATION_STATE_ON; + // TODO + //control_mode->flag_control_flighttermination_enabled = status->flighttermination_state == FLIGHTTERMINATION_STATE_ON; } } diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h index e1ec9d4adf..e569fb4f37 100644 --- a/src/modules/commander/state_machine_helper.h +++ b/src/modules/commander/state_machine_helper.h @@ -48,7 +48,6 @@ #include #include #include -#include typedef enum { TRANSITION_DENIED = -1, @@ -58,7 +57,7 @@ typedef enum { } transition_result_t; transition_result_t arming_state_transition(struct vehicle_status_s *current_state, const struct safety_s *safety, - const struct vehicle_control_mode_s *control_mode, arming_state_t new_arming_state, struct actuator_armed_s *armed); + arming_state_t new_arming_state, struct actuator_armed_s *armed); bool is_safe(const struct vehicle_status_s *current_state, const struct safety_s *safety, const struct actuator_armed_s *armed); @@ -68,9 +67,7 @@ transition_result_t main_state_transition(struct vehicle_status_s *current_state bool check_main_state_changed(); -transition_result_t navigation_state_transition(struct vehicle_status_s *status, navigation_state_t new_navigation_state, struct vehicle_control_mode_s *control_mode); - -transition_result_t flighttermination_state_transition(struct vehicle_status_s *status, flighttermination_state_t new_flighttermination_state, struct vehicle_control_mode_s *control_mode); +transition_result_t flighttermination_state_transition(struct vehicle_status_s *status, flighttermination_state_t new_flighttermination_state); bool check_navigation_state_changed(); @@ -78,6 +75,6 @@ bool check_flighttermination_state_changed(); void set_navigation_state_changed(); -int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_state, int control_mode_pub, struct vehicle_control_mode_s *current_control_mode, const int mavlink_fd); +int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_state, const int mavlink_fd); #endif /* STATE_MACHINE_HELPER_H_ */ diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c index 2ec00a9bc6..eec6c567c4 100644 --- a/src/modules/mavlink/mavlink.c +++ b/src/modules/mavlink/mavlink.c @@ -221,6 +221,8 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_base_mode, u } else if (v_status.main_state == MAIN_STATE_AUTO) { *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; + // TODO use control_mode topic + /* if (v_status.navigation_state == NAVIGATION_STATE_AUTO_READY) { custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY; } else if (v_status.navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) { @@ -234,6 +236,7 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_base_mode, u } else if (v_status.navigation_state == NAVIGATION_STATE_AUTO_LAND) { custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LAND; } + */ } *mavlink_custom_mode = custom_mode.data; diff --git a/src/modules/mavlink_onboard/mavlink.c b/src/modules/mavlink_onboard/mavlink.c index 0edb53a3e0..ab9ce45f3e 100644 --- a/src/modules/mavlink_onboard/mavlink.c +++ b/src/modules/mavlink_onboard/mavlink.c @@ -441,7 +441,8 @@ int mavlink_thread_main(int argc, char *argv[]) get_mavlink_mode_and_state(&control_mode, &armed, &mavlink_state, &mavlink_mode); /* send heartbeat */ - mavlink_msg_heartbeat_send(chan, mavlink_system.type, MAV_AUTOPILOT_PX4, mavlink_mode, v_status.navigation_state, mavlink_state); + // TODO fix navigation state, use control_mode topic + mavlink_msg_heartbeat_send(chan, mavlink_system.type, MAV_AUTOPILOT_PX4, mavlink_mode, 0, mavlink_state); /* send status (values already copied in the section above) */ mavlink_msg_sys_status_send(chan, diff --git a/src/modules/multirotor_att_control/multirotor_att_control_main.c b/src/modules/multirotor_att_control/multirotor_att_control_main.c index 60a211817c..111e9197f6 100644 --- a/src/modules/multirotor_att_control/multirotor_att_control_main.c +++ b/src/modules/multirotor_att_control/multirotor_att_control_main.c @@ -316,7 +316,7 @@ mc_thread_main(int argc, char *argv[]) } } else { - if (!control_mode.flag_control_auto_enabled) { + if (!control_mode.flag_control_attitude_enabled) { /* no control, try to stay on place */ if (!control_mode.flag_control_velocity_enabled) { /* no velocity control, reset attitude setpoint */ diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c index 5af7e2a829..2ca6504208 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c @@ -415,140 +415,8 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) /* force reprojection of global setpoint after manual mode */ reset_mission_sp = true; - - } else if (control_mode.flag_control_auto_enabled) { - /* AUTO mode, use global setpoint */ - if (control_mode.auto_state == NAVIGATION_STATE_AUTO_READY) { - reset_auto_sp_xy = true; - reset_auto_sp_z = true; - - } else if (control_mode.auto_state == NAVIGATION_STATE_AUTO_TAKEOFF) { - if (reset_takeoff_sp) { - reset_takeoff_sp = false; - local_pos_sp.x = local_pos.x; - local_pos_sp.y = local_pos.y; - local_pos_sp.z = - params.takeoff_alt - params.takeoff_gap; - att_sp.yaw_body = att.yaw; - mavlink_log_info(mavlink_fd, "[mpc] takeoff sp: %.2f %.2f %.2f", (double)local_pos_sp.x, (double)local_pos_sp.y, (double) - local_pos_sp.z); - } - - reset_auto_sp_xy = false; - reset_auto_sp_z = true; - - } else if (control_mode.auto_state == NAVIGATION_STATE_AUTO_RTL) { - // TODO - reset_auto_sp_xy = true; - reset_auto_sp_z = true; - - } else if (control_mode.auto_state == NAVIGATION_STATE_AUTO_MISSION) { - /* init local projection using local position ref */ - if (local_pos.ref_timestamp != local_ref_timestamp) { - reset_mission_sp = true; - local_ref_timestamp = local_pos.ref_timestamp; - double lat_home = local_pos.ref_lat * 1e-7; - double lon_home = local_pos.ref_lon * 1e-7; - map_projection_init(lat_home, lon_home); - mavlink_log_info(mavlink_fd, "[mpc] local pos ref: %.7f, %.7f", (double)lat_home, (double)lon_home); - } - - if (reset_mission_sp) { - reset_mission_sp = false; - /* update global setpoint projection */ - - if (global_pos_sp_valid) { - - /* project global setpoint to local setpoint */ - map_projection_project(triplet.current.lat, triplet.current.lon, &(local_pos_sp.x), &(local_pos_sp.y)); - - if (triplet.current.altitude_is_relative) { - local_pos_sp.z = -triplet.current.altitude; - - } else { - local_pos_sp.z = local_pos.ref_alt - triplet.current.lat; - } - /* update yaw setpoint only if value is valid */ - if (isfinite(triplet.current.yaw) && fabsf(triplet.current.yaw) < M_TWOPI) { - att_sp.yaw_body = triplet.current.yaw; - } - - mavlink_log_info(mavlink_fd, "[mpc] new sp: %.7f, %.7f (%.2f, %.2f)", triplet.current.lat, triplet.current.lon, (double)local_pos_sp.x, (double)local_pos_sp.y); - - } else { - if (reset_auto_sp_xy) { - reset_auto_sp_xy = false; - /* local position setpoint is invalid, - * use current position as setpoint for loiter */ - local_pos_sp.x = local_pos.x; - local_pos_sp.y = local_pos.y; - local_pos_sp.yaw = att.yaw; - } - - if (reset_auto_sp_z) { - reset_auto_sp_z = false; - local_pos_sp.z = local_pos.z; - } - - mavlink_log_info(mavlink_fd, "[mpc] no global pos sp, loiter: %.2f, %.2f", (double)local_pos_sp.x, (double)local_pos_sp.y); - } - } - - reset_auto_sp_xy = true; - reset_auto_sp_z = true; - } - - if (control_mode.auto_state != NAVIGATION_STATE_AUTO_TAKEOFF) { - reset_takeoff_sp = true; - } - - if (control_mode.auto_state != NAVIGATION_STATE_AUTO_MISSION) { - reset_mission_sp = true; - } - - /* copy yaw setpoint to vehicle_local_position_setpoint topic */ - local_pos_sp.yaw = att_sp.yaw_body; - - /* reset setpoints after AUTO mode */ - reset_man_sp_xy = true; - reset_man_sp_z = true; - - } else { - /* no control (failsafe), loiter or stay on ground */ - if (local_pos.landed) { - /* landed: move setpoint down */ - /* in air: hold altitude */ - if (local_pos_sp.z < 5.0f) { - /* set altitude setpoint to 5m under ground, - * don't set it too deep to avoid unexpected landing in case of false "landed" signal */ - local_pos_sp.z = 5.0f; - mavlink_log_info(mavlink_fd, "[mpc] landed, set alt: %.2f", (double) - local_pos_sp.z); - } - - reset_man_sp_z = true; - - } else { - /* in air: hold altitude */ - if (reset_man_sp_z) { - reset_man_sp_z = false; - local_pos_sp.z = local_pos.z; - mavlink_log_info(mavlink_fd, "[mpc] set loiter alt: %.2f", (double) - local_pos_sp.z); - } - - reset_auto_sp_z = false; - } - - if (control_mode.flag_control_position_enabled) { - if (reset_man_sp_xy) { - reset_man_sp_xy = false; - local_pos_sp.x = local_pos.x; - local_pos_sp.y = local_pos.y; - local_pos_sp.yaw = att.yaw; - att_sp.yaw_body = att.yaw; - mavlink_log_info(mavlink_fd, "[mpc] set loiter pos: %.2f %.2f", (double)local_pos_sp.x, (double)local_pos_sp.y); - } - - reset_auto_sp_xy = false; - } } + /* AUTO not implemented */ /* publish local position setpoint */ orb_publish(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_pub, &local_pos_sp); diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 7be9229c9b..37c2a0389f 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -61,6 +61,7 @@ #include #include #include +#include #include #include #include @@ -145,8 +146,10 @@ private: orb_advert_t _triplet_pub; /**< publish position setpoint triplet */ orb_advert_t _fence_pub; /**< publish fence topic */ orb_advert_t _mission_result_pub; /**< publish mission result topic */ + orb_advert_t _control_mode_pub; /**< publish vehicle control mode topic */ struct vehicle_status_s _vstatus; /**< vehicle status */ + struct vehicle_control_mode_s _control_mode; /**< vehicle control mode */ struct vehicle_global_position_s _global_pos; /**< global vehicle position */ struct home_position_s _home_pos; /**< home position for RTL */ struct mission_item_triplet_s _mission_item_triplet; /**< triplet of mission items */ @@ -168,6 +171,8 @@ private: MissionFeasibilityChecker missionFeasiblityChecker; + uint64_t _set_nav_state_timestamp; /**< timestamp of last handled navigation state request */ + struct { float min_altitude; float loiter_radius; @@ -192,21 +197,10 @@ private: MAX_EVENT }; - enum State { - STATE_INIT, - STATE_NONE, - STATE_LOITER, - STATE_MISSION, - STATE_MISSION_LOITER, - STATE_RTL, - STATE_RTL_LOITER, - MAX_STATE - }; - /** * State machine transition table */ - static StateTable::Tran const myTable[MAX_STATE][MAX_EVENT]; + static StateTable::Tran const myTable[NAV_STATE_MAX][MAX_EVENT]; /** * Update our local parameter cache. @@ -300,6 +294,10 @@ private: */ void publish_mission_item_triplet(); + /** + * Publish vehicle_control_mode topic for controllers + */ + void publish_control_mode(); /** @@ -328,7 +326,7 @@ Navigator *g_navigator; Navigator::Navigator() : /* state machine transition table */ - StateTable(&myTable[0][0], MAX_STATE, MAX_EVENT), + StateTable(&myTable[0][0], NAV_STATE_MAX, MAX_EVENT), _task_should_exit(false), _navigator_task(-1), @@ -347,6 +345,7 @@ Navigator::Navigator() : _triplet_pub(-1), _fence_pub(-1), _mission_result_pub(-1), + _control_mode_pub(-1), /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "navigator")), @@ -357,7 +356,8 @@ Navigator::Navigator() : _mission(), _waypoint_position_reached(false), _waypoint_yaw_reached(false), - _time_first_inside_orbit(0) + _time_first_inside_orbit(0), + _set_nav_state_timestamp(0) { memset(&_fence, 0, sizeof(_fence)); @@ -375,7 +375,7 @@ Navigator::Navigator() : memset(&_mission_result, 0, sizeof(struct mission_result_s)); /* Initialize state machine */ - myState = STATE_INIT; + myState = NAV_STATE_INIT; } Navigator::~Navigator() @@ -513,7 +513,6 @@ Navigator::task_main() _vstatus_sub = orb_subscribe(ORB_ID(vehicle_status)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); _home_pos_sub = orb_subscribe(ORB_ID(home_position)); - /* copy all topics first time */ vehicle_status_update(); @@ -524,9 +523,6 @@ Navigator::task_main() offboard_mission_update(_vstatus.is_rotary_wing); onboard_mission_update(); - - /* rate limit vehicle status updates to 5Hz */ - orb_set_interval(_vstatus_sub, 200); /* rate limit position updates to 50 Hz */ orb_set_interval(_global_pos_sub, 20); @@ -575,49 +571,41 @@ Navigator::task_main() /* Evaluate state machine from commander and set the navigator mode accordingly */ if (_vstatus.main_state == MAIN_STATE_AUTO) { + if (_vstatus.set_nav_state_timestamp != _set_nav_state_timestamp) { + /* commander requested new navigation mode, try to set it */ + _set_nav_state_timestamp = _vstatus.set_nav_state_timestamp; - switch (_vstatus.navigation_state) { - case NAVIGATION_STATE_DIRECT: - case NAVIGATION_STATE_STABILIZE: - case NAVIGATION_STATE_ALTHOLD: - case NAVIGATION_STATE_VECTOR: - - /* don't publish a mission item triplet */ - dispatch(EVENT_NONE_REQUESTED); + switch (_vstatus.set_nav_state) { + case NAV_STATE_INIT: + case NAV_STATE_NONE: + /* nothing to do */ break; - case NAVIGATION_STATE_AUTO_READY: - case NAVIGATION_STATE_AUTO_TAKEOFF: - case NAVIGATION_STATE_AUTO_MISSION: - - /* try mission if none is available, fallback to loiter instead */ - if (_mission.current_mission_available()) { - dispatch(EVENT_MISSION_REQUESTED); - } else { - dispatch(EVENT_LOITER_REQUESTED); - } - break; - - case NAVIGATION_STATE_AUTO_LOITER: - + case NAV_STATE_LOITER: dispatch(EVENT_LOITER_REQUESTED); break; + case NAV_STATE_MISSION: + dispatch(EVENT_MISSION_REQUESTED); + break; - case NAVIGATION_STATE_AUTO_RTL: - + case NAV_STATE_RTL: dispatch(EVENT_RTL_REQUESTED); break; - case NAVIGATION_STATE_AUTO_LAND: - - /* TODO add this */ - - break; - default: - warnx("ERROR: Navigation state not supported"); + warnx("ERROR: Requested navigation state not supported"); break; + } + + } else { + /* try mission, if none is available fallback to loiter instead */ + if (_mission.current_mission_available()) { + dispatch(EVENT_MISSION_REQUESTED); + } else { + dispatch(EVENT_LOITER_REQUESTED); + } + break; } } else { @@ -632,10 +620,8 @@ Navigator::task_main() } } - /* only update parameters if it changed */ if (fds[0].revents & POLLIN) { - parameters_update(); /* note that these new parameters won't be in effect until a mission triplet is published again */ } @@ -670,9 +656,7 @@ Navigator::task_main() if (mission_item_reached()) { if (_vstatus.main_state == MAIN_STATE_AUTO && - (_vstatus.navigation_state == NAVIGATION_STATE_AUTO_READY || - _vstatus.navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF || - _vstatus.navigation_state == NAVIGATION_STATE_AUTO_MISSION)) { + (myState == NAV_STATE_MISSION)) { /* advance by one mission item */ _mission.move_to_next(); @@ -688,6 +672,9 @@ Navigator::task_main() } } } + + publish_control_mode(); + perf_end(_loop_perf); } @@ -740,25 +727,25 @@ Navigator::status() } switch (myState) { - case STATE_INIT: + case NAV_STATE_INIT: warnx("State: Init"); break; - case STATE_NONE: + case NAV_STATE_NONE: warnx("State: None"); break; - case STATE_LOITER: + case NAV_STATE_LOITER: warnx("State: Loiter"); break; - case STATE_MISSION: + case NAV_STATE_MISSION: warnx("State: Mission"); break; - case STATE_MISSION_LOITER: + case NAV_STATE_MISSION_LOITER: warnx("State: Loiter after Mission"); break; - case STATE_RTL: + case NAV_STATE_RTL: warnx("State: RTL"); break; - case STATE_RTL_LOITER: + case NAV_STATE_RTL_LOITER: warnx("State: Loiter after RTL"); break; default: @@ -857,76 +844,76 @@ Navigator::fence_point(int argc, char *argv[]) -StateTable::Tran const Navigator::myTable[MAX_STATE][MAX_EVENT] = { +StateTable::Tran const Navigator::myTable[NAV_STATE_MAX][MAX_EVENT] = { { /* STATE_INIT */ - /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), STATE_NONE}, - /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), STATE_LOITER}, - /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), STATE_MISSION}, - /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), STATE_RTL}, - /* EVENT_MISSION_FINISHED */ {NO_ACTION, STATE_INIT}, - /* EVENT_MISSION_CHANGED */ {NO_ACTION, STATE_INIT}, - /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, STATE_INIT}, + /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE}, + /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER}, + /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, + /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, + /* EVENT_MISSION_FINISHED */ {NO_ACTION, NAV_STATE_INIT}, + /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_INIT}, + /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_INIT}, }, { /* STATE_NONE */ - /* EVENT_NONE_REQUESTED */ {NO_ACTION, STATE_NONE}, - /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), STATE_LOITER}, - /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), STATE_MISSION}, - /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), STATE_RTL}, - /* EVENT_MISSION_FINISHED */ {NO_ACTION, STATE_NONE}, - /* EVENT_MISSION_CHANGED */ {NO_ACTION, STATE_NONE}, - /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, STATE_NONE}, + /* EVENT_NONE_REQUESTED */ {NO_ACTION, NAV_STATE_NONE}, + /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER}, + /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, + /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, + /* EVENT_MISSION_FINISHED */ {NO_ACTION, NAV_STATE_NONE}, + /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_NONE}, + /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_NONE}, }, { /* STATE_LOITER */ - /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), STATE_NONE}, - /* EVENT_LOITER_REQUESTED */ {NO_ACTION, STATE_LOITER}, - /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), STATE_MISSION}, - /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), STATE_RTL}, - /* EVENT_MISSION_FINISHED */ {NO_ACTION, STATE_LOITER}, - /* EVENT_MISSION_CHANGED */ {NO_ACTION, STATE_LOITER}, - /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, STATE_LOITER}, + /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE}, + /* EVENT_LOITER_REQUESTED */ {NO_ACTION, NAV_STATE_LOITER}, + /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, + /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, + /* EVENT_MISSION_FINISHED */ {NO_ACTION, NAV_STATE_LOITER}, + /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_LOITER}, + /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_LOITER}, }, { /* STATE_MISSION */ - /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), STATE_NONE}, - /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), STATE_LOITER}, - /* EVENT_MISSION_REQUESTED */ {NO_ACTION, STATE_MISSION}, - /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), STATE_RTL}, - /* EVENT_MISSION_FINISHED */ {ACTION(&Navigator::start_mission_loiter), STATE_MISSION_LOITER}, - /* EVENT_MISSION_CHANGED */ {ACTION(&Navigator::start_mission), STATE_MISSION}, - /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, STATE_MISSION}, + /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE}, + /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER}, + /* EVENT_MISSION_REQUESTED */ {NO_ACTION, NAV_STATE_MISSION}, + /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, + /* EVENT_MISSION_FINISHED */ {ACTION(&Navigator::start_mission_loiter), NAV_STATE_MISSION_LOITER}, + /* EVENT_MISSION_CHANGED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, + /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_MISSION}, }, { /* STATE_MISSION_LOITER */ - /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), STATE_NONE}, - /* EVENT_LOITER_REQUESTED */ {NO_ACTION, STATE_MISSION_LOITER}, - /* EVENT_MISSION_REQUESTED */ {NO_ACTION, STATE_MISSION_LOITER}, - /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), STATE_RTL}, - /* EVENT_MISSION_FINISHED */ {NO_ACTION, STATE_MISSION_LOITER}, - /* EVENT_MISSION_CHANGED */ {ACTION(&Navigator::start_mission), STATE_MISSION}, - /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, STATE_MISSION_LOITER}, + /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE}, + /* EVENT_LOITER_REQUESTED */ {NO_ACTION, NAV_STATE_MISSION_LOITER}, + /* EVENT_MISSION_REQUESTED */ {NO_ACTION, NAV_STATE_MISSION_LOITER}, + /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, + /* EVENT_MISSION_FINISHED */ {NO_ACTION, NAV_STATE_MISSION_LOITER}, + /* EVENT_MISSION_CHANGED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, + /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_MISSION_LOITER}, }, { /* STATE_RTL */ - /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), STATE_NONE}, - /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), STATE_LOITER}, - /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), STATE_MISSION}, - /* EVENT_RTL_REQUESTED */ {NO_ACTION, STATE_RTL}, - /* EVENT_MISSION_FINISHED */ {ACTION(&Navigator::start_rtl_loiter), STATE_RTL_LOITER}, - /* EVENT_MISSION_CHANGED */ {NO_ACTION, STATE_RTL}, - /* EVENT_HOME_POSITION_CHANGED */ {ACTION(&Navigator::start_loiter), STATE_RTL}, + /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE}, + /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER}, + /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, + /* EVENT_RTL_REQUESTED */ {NO_ACTION, NAV_STATE_RTL}, + /* EVENT_MISSION_FINISHED */ {ACTION(&Navigator::start_rtl_loiter), NAV_STATE_RTL_LOITER}, + /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_RTL}, + /* EVENT_HOME_POSITION_CHANGED */ {ACTION(&Navigator::start_loiter), NAV_STATE_RTL}, }, { /* STATE_RTL_LOITER */ - /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), STATE_NONE}, - /* EVENT_LOITER_REQUESTED */ {NO_ACTION, STATE_RTL_LOITER}, - /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), STATE_MISSION}, - /* EVENT_RTL_REQUESTED */ {NO_ACTION, STATE_RTL_LOITER}, - /* EVENT_MISSION_FINISHED */ {NO_ACTION, STATE_RTL_LOITER}, - /* EVENT_MISSION_CHANGED */ {NO_ACTION, STATE_RTL_LOITER}, - /* EVENT_HOME_POSITION_CHANGED */ {ACTION(&Navigator::start_loiter), STATE_LOITER}, + /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE}, + /* EVENT_LOITER_REQUESTED */ {NO_ACTION, NAV_STATE_RTL_LOITER}, + /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, + /* EVENT_RTL_REQUESTED */ {NO_ACTION, NAV_STATE_RTL_LOITER}, + /* EVENT_MISSION_FINISHED */ {NO_ACTION, NAV_STATE_RTL_LOITER}, + /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_RTL_LOITER}, + /* EVENT_HOME_POSITION_CHANGED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER}, }, }; @@ -949,7 +936,7 @@ Navigator::start_loiter() _mission_item_triplet.current.lat = (double)_global_pos.lat / 1e7d; _mission_item_triplet.current.lon = (double)_global_pos.lon / 1e7d; - _mission_item_triplet.current.yaw = 0.0f; + _mission_item_triplet.current.yaw = 0.0f; // TODO use current yaw sp here or set to undefined? get_loiter_item(&_mission_item_triplet.current); @@ -1239,6 +1226,76 @@ Navigator::publish_mission_item_triplet() } } +void +Navigator::publish_control_mode() +{ + /* update vehicle_control_mode topic*/ + _control_mode.main_state = _vstatus.main_state; + _control_mode.nav_state = static_cast(myState); + _control_mode.flag_armed = _vstatus.arming_state == ARMING_STATE_ARMED || _vstatus.arming_state == ARMING_STATE_ARMED_ERROR; + _control_mode.flag_external_manual_override_ok = !_vstatus.is_rotary_wing; + _control_mode.flag_system_hil_enabled = _vstatus.hil_state == HIL_STATE_ON; + + _control_mode.flag_control_offboard_enabled = false; + _control_mode.flag_control_flighttermination_enabled = false; + + switch (_vstatus.main_state) { + case MAIN_STATE_MANUAL: + _control_mode.flag_control_manual_enabled = true; + _control_mode.flag_control_rates_enabled = _vstatus.is_rotary_wing; + _control_mode.flag_control_attitude_enabled = _vstatus.is_rotary_wing; + _control_mode.flag_control_altitude_enabled = false; + _control_mode.flag_control_climb_rate_enabled = false; + _control_mode.flag_control_position_enabled = false; + _control_mode.flag_control_velocity_enabled = false; + break; + + case MAIN_STATE_SEATBELT: + _control_mode.flag_control_manual_enabled = true; + _control_mode.flag_control_rates_enabled = true; + _control_mode.flag_control_attitude_enabled = true; + _control_mode.flag_control_altitude_enabled = true; + _control_mode.flag_control_climb_rate_enabled = true; + _control_mode.flag_control_position_enabled = false; + _control_mode.flag_control_velocity_enabled = false; + break; + + case MAIN_STATE_EASY: + _control_mode.flag_control_manual_enabled = true; + _control_mode.flag_control_rates_enabled = true; + _control_mode.flag_control_attitude_enabled = true; + _control_mode.flag_control_altitude_enabled = true; + _control_mode.flag_control_climb_rate_enabled = true; + _control_mode.flag_control_position_enabled = true; + _control_mode.flag_control_velocity_enabled = true; + break; + + case MAIN_STATE_AUTO: + _control_mode.flag_control_manual_enabled = false; + _control_mode.flag_control_rates_enabled = true; + _control_mode.flag_control_attitude_enabled = true; + _control_mode.flag_control_position_enabled = true; + _control_mode.flag_control_velocity_enabled = true; + _control_mode.flag_control_altitude_enabled = true; + _control_mode.flag_control_climb_rate_enabled = true; + break; + + default: + break; + } + + _control_mode.timestamp = hrt_absolute_time(); + + /* lazily publish the mission triplet only once available */ + if (_control_mode_pub > 0) { + /* publish the mission triplet */ + orb_publish(ORB_ID(vehicle_control_mode), _control_mode_pub, &_control_mode); + + } else { + /* advertise and publish */ + _control_mode_pub = orb_advertise(ORB_ID(vehicle_control_mode), &_control_mode); + } +} bool Navigator::cmp_mission_item_equivalent(const struct mission_item_s a, const struct mission_item_s b) { if (fabsf(a.altitude_is_relative - b.altitude_is_relative) < FLT_EPSILON && diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 9f634d9e4e..78322aff34 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -969,7 +969,8 @@ int sdlog2_thread_main(int argc, char *argv[]) // Don't orb_copy, it's already done few lines above log_msg.msg_type = LOG_STAT_MSG; log_msg.body.log_STAT.main_state = (uint8_t) buf_status.main_state; - log_msg.body.log_STAT.navigation_state = (uint8_t) buf_status.navigation_state; + // TODO use control_mode topic + //log_msg.body.log_STAT.navigation_state = (uint8_t) buf_status.navigation_state; log_msg.body.log_STAT.arming_state = (uint8_t) buf_status.arming_state; log_msg.body.log_STAT.battery_voltage = buf_status.battery_voltage; log_msg.body.log_STAT.battery_current = buf_status.battery_current; diff --git a/src/modules/uORB/topics/vehicle_control_mode.h b/src/modules/uORB/topics/vehicle_control_mode.h index e26fb97c8a..5ce968f675 100644 --- a/src/modules/uORB/topics/vehicle_control_mode.h +++ b/src/modules/uORB/topics/vehicle_control_mode.h @@ -48,6 +48,7 @@ #include #include #include "../uORB.h" +#include "vehicle_status.h" /** * @addtogroup topics @{ @@ -59,10 +60,25 @@ * * Encodes the complete system state and is set by the commander app. */ + +typedef enum { + NAV_STATE_INIT = 0, + NAV_STATE_NONE, + NAV_STATE_LOITER, + NAV_STATE_MISSION, + NAV_STATE_MISSION_LOITER, + NAV_STATE_RTL, + NAV_STATE_RTL_LOITER, + NAV_STATE_MAX +} nav_state_t; + struct vehicle_control_mode_s { uint64_t timestamp; /**< in microseconds since system start, is set whenever the writing thread stores new data */ + main_state_t main_state; + nav_state_t nav_state; + bool flag_armed; bool flag_external_manual_override_ok; /**< external override non-fatal for system. Only true for fixed wing */ @@ -79,9 +95,6 @@ struct vehicle_control_mode_s bool flag_control_altitude_enabled; /**< true if altitude is controlled */ bool flag_control_climb_rate_enabled; /**< true if climb rate is controlled */ bool flag_control_flighttermination_enabled; /**< true if flighttermination is enabled */ - - bool flag_control_auto_enabled; // TEMP - uint8_t auto_state; // TEMP navigation state for AUTO modes }; /** diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index 1c3763924c..ae3a24a1b7 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -66,20 +66,6 @@ typedef enum { MAIN_STATE_AUTO, } main_state_t; -/* navigation state machine */ -typedef enum { - NAVIGATION_STATE_DIRECT = 0, // true manual control, no any stabilization - NAVIGATION_STATE_STABILIZE, // attitude stabilization - NAVIGATION_STATE_ALTHOLD, // attitude + altitude stabilization - NAVIGATION_STATE_VECTOR, // attitude + altitude + position stabilization - NAVIGATION_STATE_AUTO_READY, // AUTO, landed, reeady for takeoff - NAVIGATION_STATE_AUTO_TAKEOFF, // detect takeoff using land detector and switch to desired AUTO mode - NAVIGATION_STATE_AUTO_LOITER, // pause mission - NAVIGATION_STATE_AUTO_MISSION, // fly mission - NAVIGATION_STATE_AUTO_RTL, // Return To Launch, when home position switch to LAND - NAVIGATION_STATE_AUTO_LAND // land and switch to AUTO_READY when landed (detect using land detector) -} navigation_state_t; - typedef enum { ARMING_STATE_INIT = 0, ARMING_STATE_STANDBY, @@ -95,7 +81,6 @@ typedef enum { HIL_STATE_ON } hil_state_t; - typedef enum { FLIGHTTERMINATION_STATE_OFF = 0, FLIGHTTERMINATION_STATE_ON @@ -182,7 +167,8 @@ struct vehicle_status_s uint64_t timestamp; /**< in microseconds since system start, is set whenever the writing thread stores new data */ main_state_t main_state; /**< main state machine */ - navigation_state_t navigation_state; /**< navigation state machine */ + unsigned int set_nav_state; /**< set navigation state machine to specified value */ + uint64_t set_nav_state_timestamp; /**< timestamp of latest change of set_nav_state */ arming_state_t arming_state; /**< current arming state */ hil_state_t hil_state; /**< current hil state */ From ea55527bbb2a0a14b099e9c4d8c69faf7a623196 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sun, 29 Dec 2013 14:50:26 +0100 Subject: [PATCH 252/447] Waypoints and missionlib: lot's of cleanup --- src/modules/mavlink/mavlink.c | 9 +- src/modules/mavlink/mavlink_receiver.cpp | 3 +- src/modules/mavlink/missionlib.c | 399 --------- src/modules/mavlink/missionlib.h | 52 -- src/modules/mavlink/module.mk | 1 - src/modules/mavlink/orb_listener.c | 1 - src/modules/mavlink/orb_topics.h | 1 + src/modules/mavlink/waypoints.c | 1029 ++++++---------------- src/modules/mavlink/waypoints.h | 44 +- src/modules/uORB/topics/mission.h | 1 - 10 files changed, 304 insertions(+), 1236 deletions(-) delete mode 100644 src/modules/mavlink/missionlib.c delete mode 100644 src/modules/mavlink/missionlib.h diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c index eec6c567c4..4c38cf35a1 100644 --- a/src/modules/mavlink/mavlink.c +++ b/src/modules/mavlink/mavlink.c @@ -68,7 +68,6 @@ #include "waypoints.h" #include "orb_topics.h" -#include "missionlib.h" #include "mavlink_hil.h" #include "util.h" #include "waypoints.h" @@ -710,25 +709,25 @@ int mavlink_thread_main(int argc, char *argv[]) } } - mavlink_waypoint_eventloop(mavlink_missionlib_get_system_timestamp(), &global_pos, &local_pos, &nav_cap); + mavlink_waypoint_eventloop(hrt_absolute_time()); /* sleep quarter the time */ usleep(25000); /* check if waypoint has been reached against the last positions */ - mavlink_waypoint_eventloop(mavlink_missionlib_get_system_timestamp(), &global_pos, &local_pos, &nav_cap); + mavlink_waypoint_eventloop(hrt_absolute_time()); /* sleep quarter the time */ usleep(25000); /* send parameters at 20 Hz (if queued for sending) */ mavlink_pm_queued_send(); - mavlink_waypoint_eventloop(mavlink_missionlib_get_system_timestamp(), &global_pos, &local_pos, &nav_cap); + mavlink_waypoint_eventloop(hrt_absolute_time()); /* sleep quarter the time */ usleep(25000); - mavlink_waypoint_eventloop(mavlink_missionlib_get_system_timestamp(), &global_pos, &local_pos, &nav_cap); + mavlink_waypoint_eventloop(hrt_absolute_time()); if (baudrate > 57600) { mavlink_pm_queued_send(); diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 7b6fad6589..771989430a 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -79,7 +79,6 @@ __BEGIN_DECLS #include "mavlink_bridge_header.h" #include "waypoints.h" #include "orb_topics.h" -#include "missionlib.h" #include "mavlink_hil.h" #include "mavlink_parameters.h" #include "util.h" @@ -844,7 +843,7 @@ receive_thread(void *arg) handle_message(&msg); /* handle packet with waypoint component */ - mavlink_wpm_message_handler(&msg, &global_pos, &local_pos); + mavlink_wpm_message_handler(&msg); /* handle packet with parameter component */ mavlink_pm_message_handler(MAVLINK_COMM_0, &msg); diff --git a/src/modules/mavlink/missionlib.c b/src/modules/mavlink/missionlib.c deleted file mode 100644 index 318dcf08c4..0000000000 --- a/src/modules/mavlink/missionlib.c +++ /dev/null @@ -1,399 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: @author Lorenz Meier - * - * 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 missionlib.h - * MAVLink missionlib components - */ - -// XXX trim includes -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include "mavlink_bridge_header.h" -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include - -#include "geo/geo.h" -#include "waypoints.h" -#include "orb_topics.h" -#include "missionlib.h" -#include "mavlink_hil.h" -#include "util.h" -#include "waypoints.h" -#include "mavlink_parameters.h" - - - -static uint8_t missionlib_msg_buf[MAVLINK_MAX_PACKET_LEN]; -static uint64_t loiter_start_time; - -#if 0 -static bool set_special_fields(float param1, float param2, float param3, float param4, uint16_t command, - struct vehicle_global_position_setpoint_s *sp); -#endif - -int -mavlink_missionlib_send_message(mavlink_message_t *msg) -{ - uint16_t len = mavlink_msg_to_send_buffer(missionlib_msg_buf, msg); - - mavlink_send_uart_bytes(chan, missionlib_msg_buf, len); - return 0; -} - - - -int -mavlink_missionlib_send_gcs_string(const char *string) -{ - const int len = MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN; - mavlink_statustext_t statustext; - int i = 0; - - while (i < len - 1) { - statustext.text[i] = string[i]; - - if (string[i] == '\0') - break; - - i++; - } - - if (i > 1) { - /* Enforce null termination */ - statustext.text[i] = '\0'; - mavlink_message_t msg; - - mavlink_msg_statustext_encode(mavlink_system.sysid, mavlink_system.compid, &msg, &statustext); - return mavlink_missionlib_send_message(&msg); - - } else { - return 1; - } -} - -/** - * Get system time since boot in microseconds - * - * @return the system time since boot in microseconds - */ -uint64_t mavlink_missionlib_get_system_timestamp() -{ - return hrt_absolute_time(); -} - -#if 0 -/** - * Set special vehicle setpoint fields based on current mission item. - * - * @return true if the mission item could be interpreted - * successfully, it return false on failure. - */ -bool set_special_fields(float param1, float param2, float param3, float param4, uint16_t command, - struct vehicle_global_position_setpoint_s *sp) -{ - switch (command) { - case MAV_CMD_NAV_LOITER_UNLIM: - sp->nav_cmd = NAV_CMD_LOITER_UNLIMITED; - break; - case MAV_CMD_NAV_LOITER_TIME: - sp->nav_cmd = NAV_CMD_LOITER_TIME_LIMIT; - loiter_start_time = hrt_absolute_time(); - break; - // case MAV_CMD_NAV_LOITER_TURNS: - // sp->nav_cmd = NAV_CMD_LOITER_TURN_COUNT; - // break; - case MAV_CMD_NAV_WAYPOINT: - sp->nav_cmd = NAV_CMD_WAYPOINT; - break; - case MAV_CMD_NAV_RETURN_TO_LAUNCH: - sp->nav_cmd = NAV_CMD_RETURN_TO_LAUNCH; - break; - case MAV_CMD_NAV_LAND: - sp->nav_cmd = NAV_CMD_LAND; - break; - case MAV_CMD_NAV_TAKEOFF: - sp->nav_cmd = NAV_CMD_TAKEOFF; - break; - default: - /* abort */ - return false; - } - - sp->loiter_radius = param3; - sp->loiter_direction = (param3 >= 0) ? 1 : -1; - - sp->param1 = param1; - sp->param2 = param2; - sp->param3 = param3; - sp->param4 = param4; - - - /* define the turn distance */ - float orbit = 15.0f; - - if (command == (int)MAV_CMD_NAV_WAYPOINT) { - - orbit = param2; - - } else if (command == (int)MAV_CMD_NAV_LOITER_TURNS || - command == (int)MAV_CMD_NAV_LOITER_TIME || - command == (int)MAV_CMD_NAV_LOITER_UNLIM) { - - orbit = param3; - } else { - - // XXX set default orbit via param - // 15 initialized above - } - - sp->turn_distance_xy = orbit; - sp->turn_distance_z = orbit; -} - -/** - * This callback is executed each time a waypoint changes. - * - * It publishes the vehicle_global_position_setpoint_s or the - * vehicle_local_position_setpoint_s topic, depending on the type of waypoint - */ -void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1, - float param2, float param3, float param4, float param5_lat_x, - float param6_lon_y, float param7_alt_z, uint8_t frame, uint16_t command) -{ - static orb_advert_t global_position_setpoint_pub = -1; - // static orb_advert_t global_position_set_triplet_pub = -1; - static orb_advert_t local_position_setpoint_pub = -1; - static unsigned last_waypoint_index = -1; - char buf[50] = {0}; - - // XXX include check if WP is supported, jump to next if not - - /* Update controller setpoints */ - if (frame == (int)MAV_FRAME_GLOBAL) { - /* global, absolute waypoint */ - struct vehicle_global_position_setpoint_s sp; - sp.lat = param5_lat_x * 1e7f; - sp.lon = param6_lon_y * 1e7f; - sp.altitude = param7_alt_z; - sp.altitude_is_relative = false; - sp.yaw = _wrap_pi(param4 / 180.0f * M_PI_F); - set_special_fields(param1, param2, param3, param4, command, &sp); - - /* Initialize setpoint publication if necessary */ - if (global_position_setpoint_pub < 0) { - global_position_setpoint_pub = orb_advertise(ORB_ID(vehicle_global_position_setpoint), &sp); - - } else { - orb_publish(ORB_ID(vehicle_global_position_setpoint), global_position_setpoint_pub, &sp); - } - - - /* fill triplet: previous, current, next waypoint */ - // struct vehicle_global_position_set_triplet_s triplet; - - /* current waypoint is same as sp */ - // memcpy(&(triplet.current), &sp, sizeof(sp)); - - /* - * Check if previous WP (in mission, not in execution order) - * is available and identify correct index - */ - int last_setpoint_index = -1; - bool last_setpoint_valid = false; - - if (index > 0) { - last_setpoint_index = index - 1; - } - - while (last_setpoint_index >= 0) { - - if (wpm->waypoints[last_setpoint_index].frame == (int)MAV_FRAME_GLOBAL && - (wpm->waypoints[last_setpoint_index].command == (int)MAV_CMD_NAV_WAYPOINT || - wpm->waypoints[last_setpoint_index].command == (int)MAV_CMD_NAV_LOITER_TURNS || - wpm->waypoints[last_setpoint_index].command == (int)MAV_CMD_NAV_LOITER_TIME || - wpm->waypoints[last_setpoint_index].command == (int)MAV_CMD_NAV_LOITER_UNLIM)) { - last_setpoint_valid = true; - break; - } - - last_setpoint_index--; - } - - /* - * Check if next WP (in mission, not in execution order) - * is available and identify correct index - */ - int next_setpoint_index = -1; - bool next_setpoint_valid = false; - - /* next waypoint */ - if (wpm->size > 1) { - next_setpoint_index = index + 1; - } - - while (next_setpoint_index < wpm->size) { - - if (wpm->waypoints[next_setpoint_index].frame == (int)MAV_FRAME_GLOBAL && (wpm->waypoints[next_setpoint_index].command == (int)MAV_CMD_NAV_WAYPOINT || - wpm->waypoints[next_setpoint_index].command == (int)MAV_CMD_NAV_LOITER_TURNS || - wpm->waypoints[next_setpoint_index].command == (int)MAV_CMD_NAV_LOITER_TIME || - wpm->waypoints[next_setpoint_index].command == (int)MAV_CMD_NAV_LOITER_UNLIM)) { - next_setpoint_valid = true; - break; - } - - next_setpoint_index++; - } - - /* populate last and next */ - - // triplet.previous_valid = false; - // triplet.next_valid = false; - - // if (last_setpoint_valid) { - // triplet.previous_valid = true; - // struct vehicle_global_position_setpoint_s sp; - // sp.lat = wpm->waypoints[last_setpoint_index].x * 1e7f; - // sp.lon = wpm->waypoints[last_setpoint_index].y * 1e7f; - // sp.altitude = wpm->waypoints[last_setpoint_index].z; - // sp.altitude_is_relative = false; - // sp.yaw = _wrap_pi(wpm->waypoints[last_setpoint_index].param4 / 180.0f * M_PI_F); - // set_special_fields(wpm->waypoints[last_setpoint_index].param1, - // wpm->waypoints[last_setpoint_index].param2, - // wpm->waypoints[last_setpoint_index].param3, - // wpm->waypoints[last_setpoint_index].param4, - // wpm->waypoints[last_setpoint_index].command, &sp); - // memcpy(&(triplet.previous), &sp, sizeof(sp)); - // } - - // if (next_setpoint_valid) { - // triplet.next_valid = true; - // struct vehicle_global_position_setpoint_s sp; - // sp.lat = wpm->waypoints[next_setpoint_index].x * 1e7f; - // sp.lon = wpm->waypoints[next_setpoint_index].y * 1e7f; - // sp.altitude = wpm->waypoints[next_setpoint_index].z; - // sp.altitude_is_relative = false; - // sp.yaw = _wrap_pi(wpm->waypoints[next_setpoint_index].param4 / 180.0f * M_PI_F); - // set_special_fields(wpm->waypoints[next_setpoint_index].param1, - // wpm->waypoints[next_setpoint_index].param2, - // wpm->waypoints[next_setpoint_index].param3, - // wpm->waypoints[next_setpoint_index].param4, - // wpm->waypoints[next_setpoint_index].command, &sp); - // memcpy(&(triplet.next), &sp, sizeof(sp)); - // } - - /* Initialize triplet publication if necessary */ - // if (global_position_set_triplet_pub < 0) { - // global_position_set_triplet_pub = orb_advertise(ORB_ID(vehicle_global_position_set_triplet), &triplet); - - // } else { - // orb_publish(ORB_ID(vehicle_global_position_set_triplet), global_position_set_triplet_pub, &triplet); - // } - - sprintf(buf, "[mp] WP#%i lat: % 3.6f/lon % 3.6f/alt % 4.6f/hdg %3.4f\n", (int)index, (double)param5_lat_x, (double)param6_lon_y, (double)param7_alt_z, (double)param4); - - } else if (frame == (int)MAV_FRAME_GLOBAL_RELATIVE_ALT) { - /* global, relative alt (in relation to HOME) waypoint */ - struct vehicle_global_position_setpoint_s sp; - sp.lat = param5_lat_x * 1e7f; - sp.lon = param6_lon_y * 1e7f; - sp.altitude = param7_alt_z; - sp.altitude_is_relative = true; - sp.yaw = _wrap_pi(param4 / 180.0f * M_PI_F); - set_special_fields(param1, param2, param3, param4, command, &sp); - - /* Initialize publication if necessary */ - if (global_position_setpoint_pub < 0) { - global_position_setpoint_pub = orb_advertise(ORB_ID(vehicle_global_position_setpoint), &sp); - - } else { - orb_publish(ORB_ID(vehicle_global_position_setpoint), global_position_setpoint_pub, &sp); - } - - - - sprintf(buf, "[mp] WP#%i (lat: %f/lon %f/rel alt %f/hdg %f\n", (int)index, (double)param5_lat_x, (double)param6_lon_y, (double)param7_alt_z, (double)param4); - - } else if (frame == (int)MAV_FRAME_LOCAL_ENU || frame == (int)MAV_FRAME_LOCAL_NED) { - /* local, absolute waypoint */ - struct vehicle_local_position_setpoint_s sp; - sp.x = param5_lat_x; - sp.y = param6_lon_y; - sp.z = param7_alt_z; - sp.yaw = _wrap_pi(param4 / 180.0f * M_PI_F); - - /* Initialize publication if necessary */ - if (local_position_setpoint_pub < 0) { - local_position_setpoint_pub = orb_advertise(ORB_ID(vehicle_local_position_setpoint), &sp); - - } else { - orb_publish(ORB_ID(vehicle_local_position_setpoint), local_position_setpoint_pub, &sp); - } - - sprintf(buf, "[mp] WP#%i (x: %f/y %f/z %f/hdg %f\n", (int)index, (double)param5_lat_x, (double)param6_lon_y, (double)param7_alt_z, (double)param4); - } else { - warnx("non-navigation WP, ignoring"); - mavlink_missionlib_send_gcs_string("[mp] Unknown waypoint type, ignoring."); - return; - } - - /* only set this for known waypoint types (non-navigation types would have returned earlier) */ - last_waypoint_index = index; - - mavlink_missionlib_send_gcs_string(buf); - printf("%s\n", buf); - //printf("[mavlink mp] new setpoint\n");//: frame: %d, lat: %d, lon: %d, alt: %d, yaw: %d\n", frame, param5_lat_x*1000, param6_lon_y*1000, param7_alt_z*1000, param4*1000); -} - -#endif \ No newline at end of file diff --git a/src/modules/mavlink/missionlib.h b/src/modules/mavlink/missionlib.h deleted file mode 100644 index c7d8f90c53..0000000000 --- a/src/modules/mavlink/missionlib.h +++ /dev/null @@ -1,52 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: @author Lorenz Meier - * - * 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 missionlib.h - * MAVLink mission helper library - */ - -#pragma once - -#include "mavlink_bridge_header.h" - -//extern void mavlink_wpm_send_message(mavlink_message_t *msg); -//extern void mavlink_wpm_send_gcs_string(const char *string); -//extern uint64_t mavlink_wpm_get_system_timestamp(void); -extern int mavlink_missionlib_send_message(mavlink_message_t *msg); -extern int mavlink_missionlib_send_gcs_string(const char *string); -extern uint64_t mavlink_missionlib_get_system_timestamp(void); -extern void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1, - float param2, float param3, float param4, float param5_lat_x, - float param6_lon_y, float param7_alt_z, uint8_t frame, uint16_t command); diff --git a/src/modules/mavlink/module.mk b/src/modules/mavlink/module.mk index 5d3d6a73c1..89a097c245 100644 --- a/src/modules/mavlink/module.mk +++ b/src/modules/mavlink/module.mk @@ -37,7 +37,6 @@ MODULE_COMMAND = mavlink SRCS += mavlink.c \ - missionlib.c \ mavlink_parameters.c \ mavlink_receiver.cpp \ orb_listener.c \ diff --git a/src/modules/mavlink/orb_listener.c b/src/modules/mavlink/orb_listener.c index 17978615f0..28478a8030 100644 --- a/src/modules/mavlink/orb_listener.c +++ b/src/modules/mavlink/orb_listener.c @@ -60,7 +60,6 @@ #include "waypoints.h" #include "orb_topics.h" -#include "missionlib.h" #include "mavlink_hil.h" #include "util.h" diff --git a/src/modules/mavlink/orb_topics.h b/src/modules/mavlink/orb_topics.h index 7d24b8f932..9000728cbc 100644 --- a/src/modules/mavlink/orb_topics.h +++ b/src/modules/mavlink/orb_topics.h @@ -45,6 +45,7 @@ #include #include #include +#include #include #include #include diff --git a/src/modules/mavlink/waypoints.c b/src/modules/mavlink/waypoints.c index 59db898b96..2ff11e813e 100644 --- a/src/modules/mavlink/waypoints.c +++ b/src/modules/mavlink/waypoints.c @@ -44,28 +44,63 @@ #include #include #include - #include "mavlink_bridge_header.h" -#include "missionlib.h" #include "waypoints.h" #include "util.h" #include #include - #include #include +#include +#include -bool debug = false; -bool verbose = false; +bool verbose = true; orb_advert_t mission_pub = -1; struct mission_s mission; -//#define MAVLINK_WPM_NO_PRINTF -#define MAVLINK_WPM_VERBOSE 0 - uint8_t mavlink_wpm_comp_id = MAV_COMP_ID_MISSIONPLANNER; +void +mavlink_missionlib_send_message(mavlink_message_t *msg) +{ + uint16_t len = mavlink_msg_to_send_buffer(missionlib_msg_buf, msg); + + mavlink_send_uart_bytes(chan, missionlib_msg_buf, len); +} + + + +int +mavlink_missionlib_send_gcs_string(const char *string) +{ + const int len = MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN; + mavlink_statustext_t statustext; + int i = 0; + + while (i < len - 1) { + statustext.text[i] = string[i]; + + if (string[i] == '\0') + break; + + i++; + } + + if (i > 1) { + /* Enforce null termination */ + statustext.text[i] = '\0'; + mavlink_message_t msg; + + mavlink_msg_statustext_encode(mavlink_system.sysid, mavlink_system.compid, &msg, &statustext); + mavlink_missionlib_send_message(&msg); + return OK; + + } else { + return 1; + } +} + void publish_mission() { /* Initialize mission publication if necessary */ @@ -119,7 +154,7 @@ int map_mavlink_mission_item_to_mission_item(const mavlink_mission_item_t *mavli mission_item->time_inside = mavlink_mission_item->param1 / 1e3f; /* from milliseconds to seconds */ mission_item->autocontinue = mavlink_mission_item->autocontinue; - mission_item->index = mavlink_mission_item->seq; + // mission_item->index = mavlink_mission_item->seq; mission_item->origin = ORIGIN_MAVLINK; return OK; @@ -151,33 +186,22 @@ int map_mission_item_to_mavlink_mission_item(const struct mission_item_s *missio mavlink_mission_item->command = mission_item->nav_cmd; mavlink_mission_item->param1 = mission_item->time_inside * 1e3f; /* from seconds to milliseconds */ mavlink_mission_item->autocontinue = mission_item->autocontinue; - mavlink_mission_item->seq = mission_item->index; + // mavlink_mission_item->seq = mission_item->index; return OK; } void mavlink_wpm_init(mavlink_wpm_storage *state) { - // Set all waypoints to zero - - // Set count to zero state->size = 0; state->max_size = MAVLINK_WPM_MAX_WP_COUNT; state->current_state = MAVLINK_WPM_STATE_IDLE; state->current_partner_sysid = 0; state->current_partner_compid = 0; state->timestamp_lastaction = 0; - // state->timestamp_last_send_setpoint = 0; + state->timestamp_last_send_setpoint = 0; state->timeout = MAVLINK_WPM_PROTOCOL_TIMEOUT_DEFAULT; - state->current_dataman_id = 0; - // state->delay_setpoint = MAVLINK_WPM_SETPOINT_DELAY_DEFAULT; - // state->idle = false; ///< indicates if the system is following the waypoints or is waiting - // state->current_active_wp_id = -1; ///< id of current waypoint - // state->yaw_reached = false; ///< boolean for yaw attitude reached - // state->pos_reached = false; ///< boolean for position reached - // state->timestamp_lastoutside_orbit = 0;///< timestamp when the MAV was last outside the orbit or had the wrong yaw value - // state->timestamp_firstinside_orbit = 0;///< timestamp when the MAV was the first time after a waypoint change inside the orbit and had the correct yaw value } /* @@ -188,24 +212,14 @@ void mavlink_wpm_send_waypoint_ack(uint8_t sysid, uint8_t compid, uint8_t type) mavlink_message_t msg; mavlink_mission_ack_t wpa; - wpa.target_system = wpm->current_partner_sysid; - wpa.target_component = wpm->current_partner_compid; + wpa.target_system = sysid; + wpa.target_component = compid; wpa.type = type; mavlink_msg_mission_ack_encode(mavlink_system.sysid, mavlink_wpm_comp_id, &msg, &wpa); mavlink_missionlib_send_message(&msg); - // FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY")); - -// if (MAVLINK_WPM_TEXT_FEEDBACK) { -// #ifdef MAVLINK_WPM_NO_PRINTF -// mavlink_missionlib_send_gcs_string("Sent waypoint ACK"); -// #else - -// if (MAVLINK_WPM_VERBOSE) printf("Sent waypoint ack (%u) to ID %u\n", wpa.type, wpa.target_system); - -// #endif -// } + if (verbose) warnx("Sent waypoint ack (%u) to ID %u", wpa.type, wpa.target_system); } /* @@ -220,45 +234,19 @@ void mavlink_wpm_send_waypoint_ack(uint8_t sysid, uint8_t compid, uint8_t type) void mavlink_wpm_send_waypoint_current(uint16_t seq) { if (seq < wpm->size) { - mavlink_mission_item_t *cur = &(wpm->waypoints[seq]); - mavlink_message_t msg; mavlink_mission_current_t wpc; - wpc.seq = cur->seq; + wpc.seq = seq; mavlink_msg_mission_current_encode(mavlink_system.sysid, mavlink_wpm_comp_id, &msg, &wpc); mavlink_missionlib_send_message(&msg); - // FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY")); - - if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_missionlib_send_gcs_string("Set current waypoint\n"); //// printf("Broadcasted new current waypoint %u\n", wpc.seq); + if (verbose) warnx("Broadcasted new current waypoint %u", wpc.seq); } else { - if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_missionlib_send_gcs_string("ERROR: wp index out of bounds\n"); - } -} - -/* - * @brief Directs the MAV to fly to a position - * - * Sends a message to the controller, advising it to fly to the coordinates - * of the waypoint with a given orientation - * - * @param seq The waypoint sequence number the MAV should fly to. - */ -void mavlink_wpm_send_setpoint(uint16_t seq) -{ - if (seq < wpm->size) { - mavlink_mission_item_t *cur = &(wpm->waypoints[seq]); - mavlink_missionlib_current_waypoint_changed(cur->seq, cur->param1, - cur->param2, cur->param3, cur->param4, cur->x, - cur->y, cur->z, cur->frame, cur->command); - - // wpm->timestamp_last_send_setpoint = mavlink_missionlib_get_system_timestamp(); - - } else { - if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_missionlib_send_gcs_string("ERROR: Waypoint index out of bounds\n"); //// if (verbose) // printf("ERROR: index out of bounds\n"); + mavlink_missionlib_send_gcs_string("ERROR: wp index out of bounds"); + if (verbose) warnx("ERROR: index out of bounds"); } } @@ -267,36 +255,48 @@ void mavlink_wpm_send_waypoint_count(uint8_t sysid, uint8_t compid, uint16_t cou mavlink_message_t msg; mavlink_mission_count_t wpc; - wpc.target_system = wpm->current_partner_sysid; - wpc.target_component = wpm->current_partner_compid; + wpc.target_system = sysid; + wpc.target_component = compid; wpc.count = mission.count; mavlink_msg_mission_count_encode(mavlink_system.sysid, mavlink_wpm_comp_id, &msg, &wpc); mavlink_missionlib_send_message(&msg); - if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_missionlib_send_gcs_string("Sent waypoint count"); //// if (verbose) // printf("Sent waypoint count (%u) to ID %u\n", wpc.count, wpc.target_system); - - // FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY")); + if (verbose) warnx("Sent waypoint count (%u) to ID %u", wpc.count, wpc.target_system); } -void mavlink_wpm_send_waypoint(uint8_t sysid, uint8_t compid, mavlink_mission_item_t *wp) +void mavlink_wpm_send_waypoint(uint8_t sysid, uint8_t compid, uint16_t seq) { - // if (seq < wpm->size) { + struct mission_item_s mission_item; + ssize_t len = sizeof(struct mission_item_s); + + dm_item_t dm_current; + + if (wpm->current_dataman_id == 0) { + dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0; + } else { + dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1; + } + + if (dm_read(dm_current, seq, &mission_item, len) == len) { + + /* create mission_item_s from mavlink_mission_item_t */ + mavlink_mission_item_t wp; + map_mission_item_to_mavlink_mission_item(&mission_item, &wp); + mavlink_message_t msg; - // mavlink_mission_item_t *wp = &(wpm->waypoints[seq]); - wp->target_system = wpm->current_partner_sysid; - wp->target_component = wpm->current_partner_compid; - mavlink_msg_mission_item_encode(mavlink_system.sysid, mavlink_wpm_comp_id, &msg, wp); + wp.target_system = sysid; + wp.target_component = compid; + wp.seq = seq; + mavlink_msg_mission_item_encode(mavlink_system.sysid, mavlink_wpm_comp_id, &msg, &wp); mavlink_missionlib_send_message(&msg); - if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_missionlib_send_gcs_string("Sent waypoint"); //// if (verbose) // printf("Sent waypoint %u to ID %u\n", wp->seq, wp->target_system); - - // FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY")); - - // } else { - // if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_missionlib_send_gcs_string("ERROR: Waypoint index out of bounds\n"); - // } + if (verbose) warnx("Sent waypoint %u to ID %u", wp.seq, wp.target_system); + } else { + mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_ERROR); + if (verbose) warnx("ERROR: could not read WP%u", seq); + } } void mavlink_wpm_send_waypoint_request(uint8_t sysid, uint8_t compid, uint16_t seq) @@ -304,18 +304,17 @@ void mavlink_wpm_send_waypoint_request(uint8_t sysid, uint8_t compid, uint16_t s if (seq < wpm->max_size) { mavlink_message_t msg; mavlink_mission_request_t wpr; - wpr.target_system = wpm->current_partner_sysid; - wpr.target_component = wpm->current_partner_compid; + wpr.target_system = sysid; + wpr.target_component = compid; wpr.seq = seq; mavlink_msg_mission_request_encode(mavlink_system.sysid, mavlink_wpm_comp_id, &msg, &wpr); mavlink_missionlib_send_message(&msg); - if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_missionlib_send_gcs_string("Sent waypoint request"); //// if (verbose) // printf("Sent waypoint request %u to ID %u\n", wpr.seq, wpr.target_system); - - // FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY")); + if (verbose) warnx("Sent waypoint request %u to ID %u", wpr.seq, wpr.target_system); } else { - if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_missionlib_send_gcs_string("ERROR: Waypoint index exceeds list capacity\n"); + mavlink_missionlib_send_gcs_string("ERROR: Waypoint index exceeds list capacity"); + if (verbose) warnx("ERROR: Waypoint index exceeds list capacity"); } } @@ -336,234 +335,33 @@ void mavlink_wpm_send_waypoint_reached(uint16_t seq) mavlink_msg_mission_item_reached_encode(mavlink_system.sysid, mavlink_wpm_comp_id, &msg, &wp_reached); mavlink_missionlib_send_message(&msg); - if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_missionlib_send_gcs_string("Sent waypoint reached message"); //// if (verbose) // printf("Sent waypoint %u reached message\n", wp_reached.seq); - - // FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY")); + if (verbose) warnx("Sent waypoint %u reached message", wp_reached.seq); } -// void check_waypoints_reached(uint64_t now, const struct vehicle_global_position_s *global_pos, struct vehicle_local_position_s *local_pos, float turn_distance) -// { -// static uint16_t counter; - -// if ((!global_pos->valid && !local_pos->xy_valid) || -// /* no waypoint */ -// wpm->size == 0) { -// /* nothing to check here, return */ -// return; -// } - -// if (wpm->current_active_wp_id < wpm->size) { - -// float orbit; -// if (wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_WAYPOINT) { - -// orbit = wpm->waypoints[wpm->current_active_wp_id].param2; - -// } else if (wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_TURNS || -// wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_TIME || -// wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_UNLIM) { - -// orbit = wpm->waypoints[wpm->current_active_wp_id].param3; -// } else { - -// // XXX set default orbit via param -// orbit = 15.0f; -// } - -// /* keep vertical orbit */ -// float vertical_switch_distance = orbit; - -// /* Take the larger turn distance - orbit or turn_distance */ -// if (orbit < turn_distance) -// orbit = turn_distance; - -// int coordinate_frame = wpm->waypoints[wpm->current_active_wp_id].frame; -// float dist = -1.0f; - -// float dist_xy = -1.0f; -// float dist_z = -1.0f; - -// if (coordinate_frame == (int)MAV_FRAME_GLOBAL) { -// dist = mavlink_wpm_distance_to_point_global_wgs84(wpm->current_active_wp_id, (float)global_pos->lat * 1e-7f, (float)global_pos->lon * 1e-7f, global_pos->alt, &dist_xy, &dist_z); - -// } else if (coordinate_frame == (int)MAV_FRAME_GLOBAL_RELATIVE_ALT) { -// dist = mavlink_wpm_distance_to_point_global_wgs84(wpm->current_active_wp_id, (float)global_pos->lat * 1e-7f, (float)global_pos->lon * 1e-7f, global_pos->relative_alt, &dist_xy, &dist_z); - -// } else if (coordinate_frame == (int)MAV_FRAME_LOCAL_ENU || coordinate_frame == (int)MAV_FRAME_LOCAL_NED) { -// dist = mavlink_wpm_distance_to_point_local(wpm->current_active_wp_id, local_pos->x, local_pos->y, local_pos->z, &dist_xy, &dist_z); - -// } else if (coordinate_frame == (int)MAV_FRAME_MISSION) { -// /* Check if conditions of mission item are satisfied */ -// // XXX TODO -// } - -// if (dist >= 0.f && dist_xy <= orbit && dist_z >= 0.0f && dist_z <= vertical_switch_distance) { -// wpm->pos_reached = true; -// } - -// // check if required yaw reached -// float yaw_sp = _wrap_pi(wpm->waypoints[wpm->current_active_wp_id].param4 / 180.0f * FM_PI); -// float yaw_err = _wrap_pi(yaw_sp - local_pos->yaw); -// if (fabsf(yaw_err) < 0.05f) { -// wpm->yaw_reached = true; -// } -// } - -// //check if the current waypoint was reached -// if (wpm->pos_reached && /*wpm->yaw_reached &&*/ !wpm->idle) { -// if (wpm->current_active_wp_id < wpm->size) { -// mavlink_mission_item_t *cur_wp = &(wpm->waypoints[wpm->current_active_wp_id]); - -// if (wpm->timestamp_firstinside_orbit == 0) { -// // Announce that last waypoint was reached -// mavlink_wpm_send_waypoint_reached(cur_wp->seq); -// wpm->timestamp_firstinside_orbit = now; -// } - -// // check if the MAV was long enough inside the waypoint orbit -// //if (now-timestamp_lastoutside_orbit > (cur_wp->hold_time*1000)) - -// bool time_elapsed = false; - -// if (now - wpm->timestamp_firstinside_orbit >= cur_wp->param1 * 1000 * 1000) { -// time_elapsed = true; -// } else if (cur_wp->command == (int)MAV_CMD_NAV_TAKEOFF) { -// time_elapsed = true; -// } - -// if (time_elapsed) { - -// /* safeguard against invalid missions with last wp autocontinue on */ -// if (wpm->current_active_wp_id == wpm->size - 1) { -// /* stop handling missions here */ -// cur_wp->autocontinue = false; -// } - -// if (cur_wp->autocontinue) { - -// cur_wp->current = 0; - -// float navigation_lat = -1.0f; -// float navigation_lon = -1.0f; -// float navigation_alt = -1.0f; -// int navigation_frame = -1; - -// /* initialize to current position in case we don't find a suitable navigation waypoint */ -// if (global_pos->valid) { -// navigation_lat = global_pos->lat/1e7; -// navigation_lon = global_pos->lon/1e7; -// navigation_alt = global_pos->alt; -// navigation_frame = MAV_FRAME_GLOBAL; -// } else if (local_pos->xy_valid && local_pos->z_valid) { -// navigation_lat = local_pos->x; -// navigation_lon = local_pos->y; -// navigation_alt = local_pos->z; -// navigation_frame = MAV_FRAME_LOCAL_NED; -// } - -// /* guard against missions without final land waypoint */ -// /* only accept supported navigation waypoints, skip unknown ones */ -// do { - -// /* pick up the last valid navigation waypoint, this will be one we hold on to after the mission */ -// if (wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_WAYPOINT || -// wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_TURNS || -// wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_TIME || -// wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_UNLIM || -// wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_TAKEOFF) { - -// /* this is a navigation waypoint */ -// navigation_frame = cur_wp->frame; -// navigation_lat = cur_wp->x; -// navigation_lon = cur_wp->y; -// navigation_alt = cur_wp->z; -// } - -// if (wpm->current_active_wp_id == wpm->size - 1) { - -// /* if we're not landing at the last nav waypoint, we're falling back to loiter */ -// if (wpm->waypoints[wpm->current_active_wp_id].command != (int)MAV_CMD_NAV_LAND) { -// /* the last waypoint was reached, if auto continue is -// * activated AND it is NOT a land waypoint, keep the system loitering there. -// */ -// cur_wp->command = MAV_CMD_NAV_LOITER_UNLIM; -// cur_wp->param3 = 20.0f; // XXX magic number 20 m loiter radius -// cur_wp->frame = navigation_frame; -// cur_wp->x = navigation_lat; -// cur_wp->y = navigation_lon; -// cur_wp->z = navigation_alt; -// } - -// /* we risk an endless loop for missions without navigation waypoints, abort. */ -// break; - -// } else { -// if ((uint16_t)(wpm->current_active_wp_id + 1) < wpm->size) -// wpm->current_active_wp_id++; -// } - -// } while (!(wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_WAYPOINT || -// wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_TURNS || -// wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_TIME || -// wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_UNLIM)); - -// // Fly to next waypoint -// wpm->timestamp_firstinside_orbit = 0; -// mavlink_wpm_send_waypoint_current(wpm->current_active_wp_id); -// mavlink_wpm_send_setpoint(wpm->current_active_wp_id); -// wpm->waypoints[wpm->current_active_wp_id].current = true; -// wpm->pos_reached = false; -// wpm->yaw_reached = false; -// printf("Set new waypoint (%u)\n", wpm->current_active_wp_id); -// } -// } -// } - -// } else { -// wpm->timestamp_lastoutside_orbit = now; -// } - -// counter++; -// } - - -int mavlink_waypoint_eventloop(uint64_t now, const struct vehicle_global_position_s *global_position, struct vehicle_local_position_s *local_position, struct navigation_capabilities_s *nav_cap) +void mavlink_waypoint_eventloop(uint64_t now) { /* check for timed-out operations */ if (now - wpm->timestamp_lastaction > wpm->timeout && wpm->current_state != MAVLINK_WPM_STATE_IDLE) { -#ifdef MAVLINK_WPM_NO_PRINTF - mavlink_missionlib_send_gcs_string("Operation timeout switching -> IDLE"); -#else + mavlink_missionlib_send_gcs_string("Operation timeout"); - if (MAVLINK_WPM_VERBOSE) printf("Last operation (state=%u) timed out, changing state to MAVLINK_WPM_STATE_IDLE\n", wpm->current_state); + if (verbose) warnx("Last operation (state=%u) timed out, changing state to MAVLINK_WPM_STATE_IDLE", wpm->current_state); -#endif wpm->current_state = MAVLINK_WPM_STATE_IDLE; - // wpm->current_count = 0; wpm->current_partner_sysid = 0; wpm->current_partner_compid = 0; - // wpm->current_wp_id = -1; - - // if (wpm->size == 0) { - // wpm->current_active_wp_id = -1; - // } } - - // check_waypoints_reached(now, global_position, local_position, nav_cap->turn_distance); - - return OK; } -void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehicle_global_position_s *global_pos , struct vehicle_local_position_s *local_pos) +void mavlink_wpm_message_handler(const mavlink_message_t *msg) { - uint64_t now = mavlink_missionlib_get_system_timestamp(); + uint64_t now = hrt_absolute_time(); switch (msg->msgid) { - case MAVLINK_MSG_ID_MISSION_ACK: { + case MAVLINK_MSG_ID_MISSION_ACK: { mavlink_mission_ack_t wpa; mavlink_msg_mission_ack_decode(msg, &wpa); @@ -573,8 +371,6 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi if (wpm->current_state == MAVLINK_WPM_STATE_SENDLIST || wpm->current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS) { if (wpm->current_wp_id == wpm->size - 1) { - // mavlink_missionlib_send_gcs_string("Got last WP ACK state -> IDLE"); - wpm->current_state = MAVLINK_WPM_STATE_IDLE; wpm->current_wp_id = 0; } @@ -582,12 +378,13 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi } else { mavlink_missionlib_send_gcs_string("REJ. WP CMD: curr partner id mismatch"); + if (verbose) warnx("REJ. WP CMD: curr partner id mismatch"); } break; } - case MAVLINK_MSG_ID_MISSION_SET_CURRENT: { + case MAVLINK_MSG_ID_MISSION_SET_CURRENT: { mavlink_mission_set_current_t wpc; mavlink_msg_mission_set_current_decode(msg, &wpc); @@ -596,52 +393,32 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi if (wpm->current_state == MAVLINK_WPM_STATE_IDLE) { if (wpc.seq < wpm->size) { - // if (verbose) // printf("Received MAVLINK_MSG_ID_MISSION_ITEM_SET_CURRENT\n"); - // wpm->current_active_wp_id = wpc.seq; - // uint32_t i; - - // for (i = 0; i < wpm->size; i++) { - // if (i == wpm->current_active_wp_id) { - // wpm->waypoints[i].current = true; - - // } else { - // wpm->waypoints[i].current = false; - // } - // } - - // mavlink_missionlib_send_gcs_string("NEW WP SET"); - - // wpm->yaw_reached = false; - // wpm->pos_reached = false; - mission.current_index = wpc.seq; - publish_mission(); - mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_ACCEPTED); - - //mavlink_wpm_send_waypoint_current(wpc.seq); - // mavlink_wpm_send_setpoint(wpm->current_active_wp_id); - // wpm->timestamp_firstinside_orbit = 0; + mavlink_wpm_send_waypoint_current(wpc.seq); } else { mavlink_missionlib_send_gcs_string("IGN WP CURR CMD: Not in list"); + if (verbose) warnx("IGN WP CURR CMD: Not in list"); } } else { mavlink_missionlib_send_gcs_string("IGN WP CURR CMD: Busy"); + if (verbose) warnx("IGN WP CURR CMD: Busy"); } } else { mavlink_missionlib_send_gcs_string("REJ. WP CMD: target id mismatch"); + if (verbose) warnx("REJ. WP CMD: target id mismatch"); } break; } - case MAVLINK_MSG_ID_MISSION_REQUEST_LIST: { + case MAVLINK_MSG_ID_MISSION_REQUEST_LIST: { mavlink_mission_request_list_t wprl; mavlink_msg_mission_request_list_decode(msg, &wprl); @@ -650,532 +427,304 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi if (wpm->current_state == MAVLINK_WPM_STATE_IDLE || wpm->current_state == MAVLINK_WPM_STATE_SENDLIST) { if (wpm->size > 0) { - //if (verbose && wpm->current_state == MAVLINK_WPM_STATE_IDLE) // printf("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST_LIST from %u changing state to MAVLINK_WPM_STATE_SENDLIST\n", msg->sysid); -// if (verbose && wpm->current_state == MAVLINK_WPM_STATE_SENDLIST) // printf("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST_LIST again from %u staying in state MAVLINK_WPM_STATE_SENDLIST\n", msg->sysid); + wpm->current_state = MAVLINK_WPM_STATE_SENDLIST; wpm->current_wp_id = 0; wpm->current_partner_sysid = msg->sysid; wpm->current_partner_compid = msg->compid; } else { - // if (verbose) // printf("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST_LIST from %u but have no waypoints, staying in \n", msg->sysid); + if (verbose) warnx("No waypoints send"); } wpm->current_count = wpm->size; mavlink_wpm_send_waypoint_count(msg->sysid, msg->compid, wpm->current_count); } else { - // if (verbose) // printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST_LIST because i'm doing something else already (state=%i).\n", wpm->current_state); + mavlink_missionlib_send_gcs_string("IGN REQUEST LIST: Busy"); + if (verbose) warnx("IGN REQUEST LIST: Busy"); } } else { - // if (verbose) // printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT MISMATCH\n"); + mavlink_missionlib_send_gcs_string("REJ. REQUEST LIST: target id mismatch"); + if (verbose) warnx("REJ. REQUEST LIST: target id mismatch"); } break; } - case MAVLINK_MSG_ID_MISSION_REQUEST: { + case MAVLINK_MSG_ID_MISSION_REQUEST: { mavlink_mission_request_t wpr; mavlink_msg_mission_request_decode(msg, &wpr); if (msg->sysid == wpm->current_partner_sysid && msg->compid == wpm->current_partner_compid && wpr.target_system == mavlink_system.sysid /*&& wpr.target_component == mavlink_wpm_comp_id*/) { wpm->timestamp_lastaction = now; - //ensure that we are in the correct state and that the first request has id 0 and the following requests have either the last id (re-send last waypoint) or last_id+1 (next waypoint) - if ((wpm->current_state == MAVLINK_WPM_STATE_SENDLIST && wpr.seq == 0) || (wpm->current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS && (wpr.seq == wpm->current_wp_id || wpr.seq == wpm->current_wp_id + 1) && wpr.seq < wpm->size)) { - if (wpm->current_state == MAVLINK_WPM_STATE_SENDLIST) { -#ifdef MAVLINK_WPM_NO_PRINTF - mavlink_missionlib_send_gcs_string("GOT WP REQ, state -> SEND"); -#else + if (wpr.seq >= wpm->size) { - if (MAVLINK_WPM_VERBOSE) printf("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u from %u changing state to MAVLINK_WPM_STATE_SENDLIST_SENDWPS\n", wpr.seq, msg->sysid); + mavlink_missionlib_send_gcs_string("REJ. WP CMD: Req. WP not in list"); + if (verbose) warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because the requested waypoint ID (%u) was out of bounds.", wpr.seq); + break; + } -#endif - } + /* + * Ensure that we are in the correct state and that the first request has id 0 + * and the following requests have either the last id (re-send last waypoint) or last_id+1 (next waypoint) + */ + if (wpm->current_state == MAVLINK_WPM_STATE_SENDLIST) { - if (wpm->current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS && wpr.seq == wpm->current_wp_id + 1) { -#ifdef MAVLINK_WPM_NO_PRINTF - mavlink_missionlib_send_gcs_string("GOT 2nd WP REQ"); -#else - - if (MAVLINK_WPM_VERBOSE) printf("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u from %u staying in state MAVLINK_WPM_STATE_SENDLIST_SENDWPS\n", wpr.seq, msg->sysid); - -#endif - } - - if (wpm->current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS && wpr.seq == wpm->current_wp_id) { -#ifdef MAVLINK_WPM_NO_PRINTF - mavlink_missionlib_send_gcs_string("GOT 2nd WP REQ"); -#else - - if (MAVLINK_WPM_VERBOSE) printf("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u (again) from %u staying in state MAVLINK_WPM_STATE_SENDLIST_SENDWPS\n", wpr.seq, msg->sysid); - -#endif - } - - wpm->current_state = MAVLINK_WPM_STATE_SENDLIST_SENDWPS; - wpm->current_wp_id = wpr.seq; - - mavlink_mission_item_t wp; - - struct mission_item_s mission_item; - ssize_t len = sizeof(struct mission_item_s); - - dm_item_t dm_current; - - if (wpm->current_dataman_id == 0) { - dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0; + if (wpr.seq == 0) { + if (verbose) warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u from %u changing state to MAVLINK_WPM_STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid); + wpm->current_state = MAVLINK_WPM_STATE_SENDLIST_SENDWPS; } else { - dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1; + mavlink_missionlib_send_gcs_string("REJ. WP CMD: First id != 0"); + if (verbose) warnx("REJ. WP CMD: First id != 0"); + break; } - if (dm_read(dm_current, wpr.seq, &mission_item, len) == len) { + } else if (wpm->current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS) { + + if (wpr.seq == wpm->current_wp_id) { + + if (verbose) warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u (again) from %u staying in state MAVLINK_WPM_STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid); + + } else if (wpr.seq == wpm->current_wp_id + 1) { + + if (verbose) warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u from %u staying in state MAVLINK_WPM_STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid); - if (mission.current_index == wpr.seq) { - wp.current = true; - } else { - wp.current = false; - } - - map_mission_item_to_mavlink_mission_item(&mission_item, &wp); - mavlink_wpm_send_waypoint(wpm->current_partner_sysid, wpm->current_partner_compid, &wp); } else { - mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_ERROR); + mavlink_missionlib_send_gcs_string("REJ. WP CMD: Req. WP was unexpected"); + if (verbose) warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because the requested waypoint ID (%u) was not the expected (%u or %u).", wpr.seq, wpm->current_wp_id, wpm->current_wp_id + 1); + break; } } else { - // if (verbose) - { - if (!(wpm->current_state == MAVLINK_WPM_STATE_SENDLIST || wpm->current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS)) { -#ifdef MAVLINK_WPM_NO_PRINTF - mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy"); -#else - if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because i'm doing something else already (state=%i).\n", wpm->current_state); - -#endif - break; - - } else if (wpm->current_state == MAVLINK_WPM_STATE_SENDLIST) { - if (wpr.seq != 0) { -#ifdef MAVLINK_WPM_NO_PRINTF - mavlink_missionlib_send_gcs_string("REJ. WP CMD: First id != 0"); -#else - - if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because the first requested waypoint ID (%u) was not 0.\n", wpr.seq); - -#endif - } - - } else if (wpm->current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS) { - if (wpr.seq != wpm->current_wp_id && wpr.seq != wpm->current_wp_id + 1) { -#ifdef MAVLINK_WPM_NO_PRINTF - mavlink_missionlib_send_gcs_string("REJ. WP CMD: Req. WP was unexpected"); -#else - - if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because the requested waypoint ID (%u) was not the expected (%u or %u).\n", wpr.seq, wpm->current_wp_id, wpm->current_wp_id + 1); - -#endif - - } else if (wpr.seq >= wpm->size) { -#ifdef MAVLINK_WPM_NO_PRINTF - mavlink_missionlib_send_gcs_string("REJ. WP CMD: Req. WP not in list"); -#else - - if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because the requested waypoint ID (%u) was out of bounds.\n", wpr.seq); - -#endif - } - - } else { -#ifdef MAVLINK_WPM_NO_PRINTF - mavlink_missionlib_send_gcs_string("REJ. WP CMD: ?"); -#else - - if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST - FIXME: missed error description\n"); - -#endif - } - } + mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy"); + if (verbose) warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because i'm doing something else already (state=%i).", wpm->current_state); + break; } + wpm->current_wp_id = wpr.seq; + wpm->current_state = MAVLINK_WPM_STATE_SENDLIST_SENDWPS; + + if (wpr.seq < wpm->size) { + + mavlink_wpm_send_waypoint(wpm->current_partner_sysid, wpm->current_partner_compid,wpm->current_wp_id); + + } else { + mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_ERROR); + if (verbose) warnx("ERROR: Waypoint %u out of bounds", wpr.seq); + } + + } else { //we we're target but already communicating with someone else if ((wpr.target_system == mavlink_system.sysid /*&& wpr.target_component == mavlink_wpm_comp_id*/) && !(msg->sysid == wpm->current_partner_sysid && msg->compid == wpm->current_partner_compid)) { -#ifdef MAVLINK_WPM_NO_PRINTF + mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy"); -#else - - if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST from ID %u because i'm already talking to ID %u.\n", msg->sysid, wpm->current_partner_sysid); - -#endif + if (verbose) warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST from ID %u because i'm already talking to ID %u.", msg->sysid, wpm->current_partner_sysid); } else { -#ifdef MAVLINK_WPM_NO_PRINTF + mavlink_missionlib_send_gcs_string("REJ. WP CMD: target id mismatch"); -#else - - if (MAVLINK_WPM_VERBOSE) printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH\n"); - -#endif + if (verbose) warnx("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH"); } - } - break; } - case MAVLINK_MSG_ID_MISSION_COUNT: { + case MAVLINK_MSG_ID_MISSION_COUNT: { mavlink_mission_count_t wpc; mavlink_msg_mission_count_decode(msg, &wpc); if (wpc.target_system == mavlink_system.sysid/* && wpc.target_component == mavlink_wpm_comp_id*/) { wpm->timestamp_lastaction = now; - if (wpm->current_state == MAVLINK_WPM_STATE_IDLE || (wpm->current_state == MAVLINK_WPM_STATE_GETLIST && wpm->current_wp_id == 0)) { -// printf("wpc count in: %d\n",wpc.count); -// printf("Comp id: %d\n",msg->compid); -// printf("Current partner sysid: %d\n",wpm->current_partner_sysid); + if (wpm->current_state == MAVLINK_WPM_STATE_IDLE) { - if (wpc.count > 0) { - if (wpm->current_state == MAVLINK_WPM_STATE_IDLE) { -#ifdef MAVLINK_WPM_NO_PRINTF - mavlink_missionlib_send_gcs_string("WP CMD OK: state -> GETLIST"); -#else - - if (MAVLINK_WPM_VERBOSE) printf("Got MAVLINK_MSG_ID_MISSION_ITEM_COUNT (%u) from %u changing state to MAVLINK_WPM_STATE_GETLIST\n", wpc.count, msg->sysid); - -#endif - } - - if (wpm->current_state == MAVLINK_WPM_STATE_GETLIST) { -#ifdef MAVLINK_WPM_NO_PRINTF - mavlink_missionlib_send_gcs_string("WP CMD OK AGAIN"); -#else - - if (MAVLINK_WPM_VERBOSE) printf("Got MAVLINK_MSG_ID_MISSION_ITEM_COUNT (%u) again from %u\n", wpc.count, msg->sysid); - -#endif - } - - wpm->current_state = MAVLINK_WPM_STATE_GETLIST; - wpm->current_wp_id = 0; - wpm->current_partner_sysid = msg->sysid; - wpm->current_partner_compid = msg->compid; - wpm->current_count = wpc.count; - - if (wpc.count > NUM_MISSIONS_SUPPORTED) { - warnx("Too many waypoints: %d, supported: %d", wpc.count, NUM_MISSIONS_SUPPORTED); - } - -#ifdef MAVLINK_WPM_NO_PRINTF - mavlink_missionlib_send_gcs_string("CLR RCV BUF: READY"); -#else - - if (MAVLINK_WPM_VERBOSE) printf("clearing receive buffer and readying for receiving waypoints\n"); - -#endif - wpm->rcv_size = 0; - //while(waypoints_receive_buffer->size() > 0) -// { -// delete waypoints_receive_buffer->back(); -// waypoints_receive_buffer->pop_back(); -// } - - mavlink_wpm_send_waypoint_request(wpm->current_partner_sysid, wpm->current_partner_compid, wpm->current_wp_id); - - } else if (wpc.count == 0) { -#ifdef MAVLINK_WPM_NO_PRINTF - mavlink_missionlib_send_gcs_string("COUNT 0"); -#else - - if (MAVLINK_WPM_VERBOSE) printf("got waypoint count of 0, clearing waypoint list and staying in state MAVLINK_WPM_STATE_IDLE\n"); - -#endif - wpm->rcv_size = 0; - //while(waypoints_receive_buffer->size() > 0) -// { -// delete waypoints->back(); -// waypoints->pop_back(); -// } - // wpm->current_active_wp_id = -1; - // wpm->yaw_reached = false; - // wpm->pos_reached = false; + if (wpc.count > NUM_MISSIONS_SUPPORTED) { + if (verbose) warnx("Too many waypoints: %d, supported: %d", wpc.count, NUM_MISSIONS_SUPPORTED); + mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_NO_SPACE); break; - - } else { -#ifdef MAVLINK_WPM_NO_PRINTF - mavlink_missionlib_send_gcs_string("IGN WP CMD"); -#else - - if (MAVLINK_WPM_VERBOSE) printf("Ignoring MAVLINK_MSG_ID_MISSION_ITEM_COUNT from %u with count of %u\n", msg->sysid, wpc.count); - -#endif } + if (wpc.count == 0) { + mavlink_missionlib_send_gcs_string("COUNT 0"); + if (verbose) warnx("got waypoint count of 0, clearing waypoint list and staying in state MAVLINK_WPM_STATE_IDLE"); + break; + } + + if (verbose) warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_COUNT (%u) from %u changing state to MAVLINK_WPM_STATE_GETLIST", wpc.count, msg->sysid); + + wpm->current_state = MAVLINK_WPM_STATE_GETLIST; + wpm->current_wp_id = 0; + wpm->current_partner_sysid = msg->sysid; + wpm->current_partner_compid = msg->compid; + wpm->current_count = wpc.count; + + mavlink_wpm_send_waypoint_request(wpm->current_partner_sysid, wpm->current_partner_compid, wpm->current_wp_id); + + } else if (wpm->current_state == MAVLINK_WPM_STATE_GETLIST) { + + if (wpm->current_wp_id == 0) { + mavlink_missionlib_send_gcs_string("WP CMD OK AGAIN"); + if (verbose) warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_COUNT (%u) again from %u", wpc.count, msg->sysid); + } else { + mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy"); + if (verbose) warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_COUNT because i'm already receiving waypoint %u.", wpm->current_wp_id); + } } else { - if (!(wpm->current_state == MAVLINK_WPM_STATE_IDLE || wpm->current_state == MAVLINK_WPM_STATE_GETLIST)) { -#ifdef MAVLINK_WPM_NO_PRINTF - mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy"); -#else - - if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_COUNT because i'm doing something else already (state=%i).\n", wpm->current_state); - -#endif - - } else if (wpm->current_state == MAVLINK_WPM_STATE_GETLIST && wpm->current_wp_id != 0) { -#ifdef MAVLINK_WPM_NO_PRINTF - mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy"); -#else - - if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_COUNT because i'm already receiving waypoint %u.\n", wpm->current_wp_id); - -#endif - - } else { -#ifdef MAVLINK_WPM_NO_PRINTF - mavlink_missionlib_send_gcs_string("REJ. WP CMD: ?"); -#else - - if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_COUNT - FIXME: missed error description\n"); - -#endif - } + mavlink_missionlib_send_gcs_string("IGN MISSION_COUNT CMD: Busy"); + if (verbose) warnx("IGN MISSION_COUNT CMD: Busy"); } - } else { -#ifdef MAVLINK_WPM_NO_PRINTF - mavlink_missionlib_send_gcs_string("REJ. WP CMD: target id mismatch"); -#else - if (MAVLINK_WPM_VERBOSE) printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH\n"); - -#endif + mavlink_missionlib_send_gcs_string("REJ. WP COUNT CMD: target id mismatch"); + if (verbose) warnx("IGNORED WAYPOINT COUNT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH"); } - } break; - case MAVLINK_MSG_ID_MISSION_ITEM: { + case MAVLINK_MSG_ID_MISSION_ITEM: { mavlink_mission_item_t wp; mavlink_msg_mission_item_decode(msg, &wp); - // mavlink_missionlib_send_gcs_string("GOT WP"); -// printf("sysid=%d, current_partner_sysid=%d\n", msg->sysid, wpm->current_partner_sysid); -// printf("compid=%d, current_partner_compid=%d\n", msg->compid, wpm->current_partner_compid); - -// if((msg->sysid == wpm->current_partner_sysid && msg->compid == wpm->current_partner_compid) && (wp.target_system == mavlink_system.sysid /*&& wp.target_component == mavlink_wpm_comp_id*/)) if (wp.target_system == mavlink_system.sysid && wp.target_component == mavlink_wpm_comp_id) { wpm->timestamp_lastaction = now; -// printf("wpm->current_state=%u, wp.seq = %d, wpm->current_wp_id=%d\n", wpm->current_state, wp.seq, wpm->current_wp_id); + /* + * ensure that we are in the correct state and that the first waypoint has id 0 + * and the following waypoints have the correct ids + */ -// wpm->current_state = MAVLINK_WPM_STATE_GETLIST;//removeme debug XXX TODO + if (wpm->current_state == MAVLINK_WPM_STATE_GETLIST) { - //ensure that we are in the correct state and that the first waypoint has id 0 and the following waypoints have the correct ids - if ((wpm->current_state == MAVLINK_WPM_STATE_GETLIST && wp.seq == 0) || - (wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS && wp.seq == wpm->current_wp_id && - wp.seq < wpm->current_count)) { - //mavlink_missionlib_send_gcs_string("DEBUG 2"); + if (wp.seq != 0) { + mavlink_missionlib_send_gcs_string("Ignored MISSION_ITEM WP not 0"); + warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM because the first waypoint ID (%u) was not 0.", wp.seq); + break; + } + } else if (wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS) { -// if (verbose && wpm->current_state == MAVLINK_WPM_STATE_GETLIST) // printf("Got MAVLINK_MSG_ID_MISSION_ITEM %u from %u changing state to MAVLINK_WPM_STATE_GETLIST_GETWPS\n", wp.seq, msg->sysid); -// if (verbose && wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS && wp.seq == wpm->current_wp_id) // printf("Got MAVLINK_MSG_ID_MISSION_ITEM %u from %u\n", wp.seq, msg->sysid); -// if (verbose && wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS && wp.seq-1 == wpm->current_wp_id) // printf("Got MAVLINK_MSG_ID_MISSION_ITEM %u (again) from %u\n", wp.seq, msg->sysid); -// - wpm->current_state = MAVLINK_WPM_STATE_GETLIST_GETWPS; - // mavlink_mission_item_t *newwp = &(wpm->rcv_waypoints[wp.seq]); - // memcpy(newwp, &wp, sizeof(mavlink_mission_item_t)); - - struct mission_item_s mission_item; - - int ret = map_mavlink_mission_item_to_mission_item(&wp, &mission_item); - - if (ret != OK) { - mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, ret); - wpm->current_state = MAVLINK_WPM_STATE_IDLE; + if (wp.seq >= wpm->current_count) { + mavlink_missionlib_send_gcs_string("Ignored MISSION_ITEM WP out of bounds"); + warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM because the waypoint ID (%u) was out of bounds.", wp.seq); break; } - size_t len = sizeof(struct mission_item_s); - - dm_item_t dm_next; - - if (wpm->current_dataman_id == 0) { - dm_next = DM_KEY_WAYPOINTS_OFFBOARD_1; - mission.dataman_id = 1; - } else { - dm_next = DM_KEY_WAYPOINTS_OFFBOARD_0; - mission.dataman_id = 0; - } - - if (dm_write(dm_next, wp.seq, DM_PERSIST_IN_FLIGHT_RESET, &mission_item, len) != len) { - mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_ERROR); - wpm->current_state = MAVLINK_WPM_STATE_IDLE; - break; - } - - if (wp.current) { - mission.current_index = wp.seq; - } - - wpm->current_wp_id = wp.seq + 1; - - // if (verbose) // printf ("Added new waypoint to list. X= %f\t Y= %f\t Z= %f\t Yaw= %f\n", newwp->x, newwp->y, newwp->z, newwp->param4); -// printf ("Added new waypoint to list. X= %f\t Y= %f\t Z= %f\t Yaw= %f\n", newwp->x, newwp->y, newwp->z, newwp->param4); - -// printf ("wpm->current_wp_id =%d, wpm->current_count=%d\n\n", wpm->current_wp_id, wpm->current_count); - if (wpm->current_wp_id == wpm->current_count && wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS) { - // mavlink_missionlib_send_gcs_string("GOT ALL WPS"); - // if (verbose) // printf("Got all %u waypoints, changing state to MAVLINK_WPM_STATE_IDLE\n", wpm->current_count); - - mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, 0); - - // if (wpm->current_active_wp_id > wpm->rcv_size - 1) { - // wpm->current_active_wp_id = wpm->rcv_size - 1; - // } - - // bool copy_error = false; - - // // switch the waypoints list - // // FIXME CHECK!!! - // uint32_t i; - - // for (i = 0; i < wpm->current_count; ++i) { - // wpm->waypoints[i] = wpm->rcv_waypoints[i]; - // if (map_mavlink_mission_item_to_mission_item(&wpm->rcv_waypoints[i], &mission.items[i]) != OK) { - // copy_error = true; - // } - - // } - // TODO: update count? - - - mission.count = wpm->current_count; - - publish_mission(); - - wpm->current_dataman_id = mission.dataman_id; - wpm->size = wpm->current_count; - - //get the new current waypoint - - // for (i = 0; i < wpm->size; i++) { - // if (wpm->waypoints[i].current == 1) { - // wpm->current_active_wp_id = i; - // //// if (verbose) // printf("New current waypoint %u\n", current_active_wp_id); - // // wpm->yaw_reached = false; - // // wpm->pos_reached = false; - // mavlink_wpm_send_waypoint_current(wpm->current_active_wp_id); - // // mavlink_wpm_send_setpoint(wpm->current_active_wp_id); - // // wpm->timestamp_firstinside_orbit = 0; - // break; - // } - // } - - // if (i == wpm->size) { - // wpm->current_active_wp_id = -1; - // wpm->yaw_reached = false; - // wpm->pos_reached = false; - // wpm->timestamp_firstinside_orbit = 0; - // } - - wpm->current_state = MAVLINK_WPM_STATE_IDLE; - - } else { + if (wp.seq != wpm->current_wp_id) { + warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM because the waypoint ID (%u) was not the expected %u.", wp.seq, wpm->current_wp_id); mavlink_wpm_send_waypoint_request(wpm->current_partner_sysid, wpm->current_partner_compid, wpm->current_wp_id); + break; } + } + + wpm->current_state = MAVLINK_WPM_STATE_GETLIST_GETWPS; + + struct mission_item_s mission_item; + + int ret = map_mavlink_mission_item_to_mission_item(&wp, &mission_item); + + if (ret != OK) { + mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, ret); + wpm->current_state = MAVLINK_WPM_STATE_IDLE; + break; + } + + ssize_t len = sizeof(struct mission_item_s); + + dm_item_t dm_next; + + if (wpm->current_dataman_id == 0) { + dm_next = DM_KEY_WAYPOINTS_OFFBOARD_1; + mission.dataman_id = 1; + } else { + dm_next = DM_KEY_WAYPOINTS_OFFBOARD_0; + mission.dataman_id = 0; + } + + if (dm_write(dm_next, wp.seq, DM_PERSIST_IN_FLIGHT_RESET, &mission_item, len) != len) { + mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_ERROR); + wpm->current_state = MAVLINK_WPM_STATE_IDLE; + break; + } + + if (wp.current) { + mission.current_index = wp.seq; + } + + wpm->current_wp_id = wp.seq + 1; + + if (wpm->current_wp_id == wpm->current_count && wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS) { + + if (verbose) warnx("Got all %u waypoints, changing state to MAVLINK_WPM_STATE_IDLE", wpm->current_count); + + mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_ACCEPTED); + + mission.count = wpm->current_count; + + publish_mission(); + + wpm->current_dataman_id = mission.dataman_id; + wpm->size = wpm->current_count; + + wpm->current_state = MAVLINK_WPM_STATE_IDLE; } else { - - mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_INVALID_SEQUENCE); - - // if (wpm->current_state == MAVLINK_WPM_STATE_IDLE) { - // //we're done receiving waypoints, answer with ack. - // mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, 0); - // printf("Received MAVLINK_MSG_ID_MISSION_ITEM while state=MAVLINK_WPM_STATE_IDLE, answered with WAYPOINT_ACK.\n"); - // } - -// // if (verbose) -// { -// if (!(wpm->current_state == MAVLINK_WPM_STATE_GETLIST || wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS)) { -// // printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM %u because i'm doing something else already (state=%i).\n", wp.seq, wpm->current_state); -// break; - -// } else if (wpm->current_state == MAVLINK_WPM_STATE_GETLIST) { -// if (!(wp.seq == 0)) { -// // printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM because the first waypoint ID (%u) was not 0.\n", wp.seq); -// } else { -// // printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM %u - FIXME: missed error description\n", wp.seq); -// } -// } else if (wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS) { -// if (!(wp.seq == wpm->current_wp_id)) { -// // printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM because the waypoint ID (%u) was not the expected %u.\n", wp.seq, wpm->current_wp_id); -// mavlink_wpm_send_waypoint_request(wpm->current_partner_sysid, wpm->current_partner_compid, wpm->current_wp_id); - -// } else if (!(wp.seq < wpm->current_count)) { -// // printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM because the waypoint ID (%u) was out of bounds.\n", wp.seq); -// } else { -// // printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM %u - FIXME: missed error description\n", wp.seq); -// } -// } else { -// // printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM %u - FIXME: missed error description\n", wp.seq); -// } -// } + mavlink_wpm_send_waypoint_request(wpm->current_partner_sysid, wpm->current_partner_compid, wpm->current_wp_id); } + } else { - //we we're target but already communicating with someone else - if ((wp.target_system == mavlink_system.sysid /*&& wp.target_component == mavlink_wpm_comp_id*/) && !(msg->sysid == wpm->current_partner_sysid && msg->compid == wpm->current_partner_compid) && wpm->current_state != MAVLINK_WPM_STATE_IDLE) { - // if (verbose) // printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM %u from ID %u because i'm already talking to ID %u.\n", wp.seq, msg->sysid, wpm->current_partner_sysid); - } else if (wp.target_system == mavlink_system.sysid /* && wp.target_component == mavlink_wpm_comp_id*/) { - // if (verbose) // printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM %u from ID %u because i have no idea what to do with it\n", wp.seq, msg->sysid); - } + mavlink_missionlib_send_gcs_string("REJ. WP CMD: target id mismatch"); + if (verbose) warnx("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH"); } break; } - case MAVLINK_MSG_ID_MISSION_CLEAR_ALL: { + case MAVLINK_MSG_ID_MISSION_CLEAR_ALL: { mavlink_mission_clear_all_t wpca; mavlink_msg_mission_clear_all_decode(msg, &wpca); - if (wpca.target_system == mavlink_system.sysid /*&& wpca.target_component == mavlink_wpm_comp_id */ && wpm->current_state == MAVLINK_WPM_STATE_IDLE) { - wpm->timestamp_lastaction = now; + if (wpca.target_system == mavlink_system.sysid /*&& wpca.target_component == mavlink_wpm_comp_id */) { - // if (verbose) // printf("Got MAVLINK_MSG_ID_MISSION_ITEM_CLEAR_LIST from %u deleting all waypoints\n", msg->sysid); - // Delete all waypoints - wpm->size = 0; - // wpm->current_active_wp_id = -1; - // wpm->yaw_reached = false; - // wpm->pos_reached = false; + if (wpm->current_state == MAVLINK_WPM_STATE_IDLE) { + wpm->timestamp_lastaction = now; - /* prepare mission topic */ - mission.dataman_id = -1; - mission.count = 0; - mission.current_index = -1; + wpm->size = 0; - if (dm_clear(DM_KEY_WAYPOINTS_OFFBOARD_0) == OK && dm_clear(DM_KEY_WAYPOINTS_OFFBOARD_1) == OK) { - mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_ACCEPTED); + /* prepare mission topic */ + mission.dataman_id = -1; + mission.count = 0; + mission.current_index = -1; + publish_mission(); + + if (dm_clear(DM_KEY_WAYPOINTS_OFFBOARD_0) == OK && dm_clear(DM_KEY_WAYPOINTS_OFFBOARD_1) == OK) { + mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_ACCEPTED); + } else { + mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_ERROR); + } + + } else { - mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_ERROR); + mavlink_missionlib_send_gcs_string("IGN WP CLEAR CMD: Busy"); + if (verbose) warnx("IGN WP CLEAR CMD: Busy"); } - publish_mission(); - } else if (wpca.target_system == mavlink_system.sysid /*&& wpca.target_component == mavlink_wpm_comp_id */ && wpm->current_state != MAVLINK_WPM_STATE_IDLE) { - // if (verbose) // printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_CLEAR_LIST from %u because i'm doing something else already (state=%i).\n", msg->sysid, wpm->current_state); - warnx("not cleared"); + + mavlink_missionlib_send_gcs_string("REJ. WP CLERR CMD: target id mismatch"); + if (verbose) warnx("IGNORED WAYPOINT CLEAR COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH"); } break; } default: { - // if (debug) // printf("Waypoint: received message of unknown type"); + /* other messages might should get caught by mavlink and others */ break; } } - - // check_waypoints_reached(now, global_pos, local_pos); } diff --git a/src/modules/mavlink/waypoints.h b/src/modules/mavlink/waypoints.h index 801bc0bcf1..f8b58c7d92 100644 --- a/src/modules/mavlink/waypoints.h +++ b/src/modules/mavlink/waypoints.h @@ -46,19 +46,10 @@ or in the same folder as this source file */ #include - -// #ifndef MAVLINK_SEND_UART_BYTES -// #define MAVLINK_SEND_UART_BYTES(chan, buffer, len) mavlink_send_uart_bytes(chan, buffer, len) -// #endif -//extern mavlink_system_t mavlink_system; #include "mavlink_bridge_header.h" #include -#include -#include -#include #include -// FIXME XXX - TO BE MOVED TO XML enum MAVLINK_WPM_STATES { MAVLINK_WPM_STATE_IDLE = 0, MAVLINK_WPM_STATE_SENDLIST, @@ -79,44 +70,24 @@ enum MAVLINK_WPM_CODES { }; -/* WAYPOINT MANAGER - MISSION LIB */ - -#define MAVLINK_WPM_MAX_WP_COUNT 15 -// #define MAVLINK_WPM_CONFIG_IN_FLIGHT_UPDATE ///< Enable double buffer and in-flight updates -#ifndef MAVLINK_WPM_TEXT_FEEDBACK -#define MAVLINK_WPM_TEXT_FEEDBACK 0 ///< Report back status information as text -#endif +#define MAVLINK_WPM_MAX_WP_COUNT 255 #define MAVLINK_WPM_PROTOCOL_TIMEOUT_DEFAULT 5000000 ///< Protocol communication timeout in useconds #define MAVLINK_WPM_SETPOINT_DELAY_DEFAULT 1000000 ///< When to send a new setpoint #define MAVLINK_WPM_PROTOCOL_DELAY_DEFAULT 40000 struct mavlink_wpm_storage { - mavlink_mission_item_t waypoints[MAVLINK_WPM_MAX_WP_COUNT]; ///< Currently active waypoints -// #ifdef MAVLINK_WPM_CONFIG_IN_FLIGHT_UPDATE - // mavlink_mission_item_t rcv_waypoints[MAVLINK_WPM_MAX_WP_COUNT]; ///< Receive buffer for next waypoints -// #endif uint16_t size; uint16_t max_size; - uint16_t rcv_size; enum MAVLINK_WPM_STATES current_state; int16_t current_wp_id; ///< Waypoint in current transmission - // int16_t current_active_wp_id; ///< Waypoint the system is currently heading towards uint16_t current_count; uint8_t current_partner_sysid; uint8_t current_partner_compid; uint64_t timestamp_lastaction; - // uint64_t timestamp_last_send_setpoint; - // uint64_t timestamp_firstinside_orbit; - // uint64_t timestamp_lastoutside_orbit; + uint64_t timestamp_last_send_setpoint; uint32_t timeout; int current_dataman_id; - // uint32_t delay_setpoint; - // float accept_range_yaw; - // float accept_range_distance; - // bool yaw_reached; - // bool pos_reached; - // bool idle; }; typedef struct mavlink_wpm_storage mavlink_wpm_storage; @@ -126,13 +97,16 @@ int map_mission_item_to_mavlink_mission_item(const struct mission_item_s *missio void mavlink_wpm_init(mavlink_wpm_storage *state); -int mavlink_waypoint_eventloop(uint64_t now, const struct vehicle_global_position_s *global_position, - struct vehicle_local_position_s *local_pos, struct navigation_capabilities_s *nav_cap); -void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehicle_global_position_s *global_pos , - struct vehicle_local_position_s *local_pos); +void mavlink_waypoint_eventloop(uint64_t now); +void mavlink_wpm_message_handler(const mavlink_message_t *msg); extern void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1, float param2, float param3, float param4, float param5_lat_x, float param6_lon_y, float param7_alt_z, uint8_t frame, uint16_t command); +static uint8_t missionlib_msg_buf[MAVLINK_MAX_PACKET_LEN]; + +void mavlink_missionlib_send_message(mavlink_message_t *msg); +int mavlink_missionlib_send_gcs_string(const char *string); + #endif /* WAYPOINTS_H_ */ diff --git a/src/modules/uORB/topics/mission.h b/src/modules/uORB/topics/mission.h index dcdb234fa4..9b4250115d 100644 --- a/src/modules/uORB/topics/mission.h +++ b/src/modules/uORB/topics/mission.h @@ -91,7 +91,6 @@ struct mission_item_s float time_inside; /**< time that the MAV should stay inside the radius before advancing in seconds */ float pitch_min; /**< minimal pitch angle for fixed wing takeoff waypoints */ bool autocontinue; /**< true if next waypoint should follow after this one */ - int index; /**< index matching the mavlink waypoint */ enum ORIGIN origin; /**< where the waypoint has been generated */ }; From ea9efd75dd688c42a16667d199be08bdb1aacddf Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sun, 29 Dec 2013 14:50:49 +0100 Subject: [PATCH 253/447] Navigator: Small error message fix --- src/modules/navigator/navigator_main.cpp | 7 ++----- 1 file changed, 2 insertions(+), 5 deletions(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 37c2a0389f..0a0ee25414 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -977,9 +977,8 @@ Navigator::start_mission() mavlink_log_info(_mavlink_fd, "[navigator] heading to offboard WP %d", index); } } else { - /* since a mission is not started without WPs available, this is not supposed to happen */ + /* since a mission is started without knowledge if there are more mission items available, this can fail */ _mission_item_triplet.current_valid = false; - warnx("ERROR: current WP can't be set"); } ret = _mission.get_next_mission_item(&_mission_item_triplet.next); @@ -1308,11 +1307,9 @@ bool Navigator::cmp_mission_item_equivalent(const struct mission_item_s a, const fabsf(a.nav_cmd - b.nav_cmd) < FLT_EPSILON && fabsf(a.radius - b.radius) < FLT_EPSILON && fabsf(a.time_inside - b.time_inside) < FLT_EPSILON && - fabsf(a.autocontinue - b.autocontinue) < FLT_EPSILON && - fabsf(a.index - b.index) < FLT_EPSILON) { + fabsf(a.autocontinue - b.autocontinue) < FLT_EPSILON) { return true; } else { - warnx("a.index %d, b.index %d", a.index, b.index); return false; } } From 70cc5413b43eaebc20ca5bda87dc8f66147b7501 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 29 Dec 2013 16:17:09 +0100 Subject: [PATCH 254/447] fw att controller: update parameter descriptions and default values --- .../fw_att_control/fw_att_control_params.c | 125 ++++++++++++------ 1 file changed, 87 insertions(+), 38 deletions(-) diff --git a/src/modules/fw_att_control/fw_att_control_params.c b/src/modules/fw_att_control/fw_att_control_params.c index 0b37f48bad..e129568bbe 100644 --- a/src/modules/fw_att_control/fw_att_control_params.c +++ b/src/modules/fw_att_control/fw_att_control_params.c @@ -38,68 +38,67 @@ f * Copyright (c) 2013 PX4 Development Team. All rights reserved. * Parameters defined by the fixed-wing attitude control task * * @author Lorenz Meier + * @author Thomas Gubler */ #include #include -//XXX resolve unclear naming of paramters: FW_P_P --> FW_PR_P /* * Controller parameters, accessible via MAVLink * */ -//xxx: update descriptions // @DisplayName Attitude Time Constant -// @Description This defines the latency between a step input and the achieved setpoint. Half a second is a good start value and fits for most average systems. Smaller systems may require smaller values, but as this will wear out servos faster, the value should only be decreased as needed. +// @Description This defines the latency between a step input and the achieved setpoint (inverse to a P gain). Half a second is a good start value and fits for most average systems. Smaller systems may require smaller values, but as this will wear out servos faster, the value should only be decreased as needed. // @Range 0.4 to 1.0 seconds, in tens of seconds PARAM_DEFINE_FLOAT(FW_ATT_TC, 0.5f); -// @DisplayName Proportional gain. -// @Description This defines how much the elevator input will be commanded dependend on the current pitch error. +// @DisplayName Pitch rate proportional gain. +// @Description This defines how much the elevator input will be commanded depending on the current body angular rate error. // @Range 10 to 200, 1 increments -PARAM_DEFINE_FLOAT(FW_PR_P, 0.3f); +PARAM_DEFINE_FLOAT(FW_PR_P, 0.05f); // @DisplayName Damping gain. // @Description This gain damps the airframe pitch rate. In particular relevant for flying wings. // @Range 0.0 to 10.0, 0.1 increments PARAM_DEFINE_FLOAT(FW_PR_D, 0.0f); //xxx: remove -// @DisplayName Integrator gain. +// @DisplayName Pitch rate integrator gain. // @Description This gain defines how much control response will result out of a steady state error. It trims any constant error. // @Range 0 to 50.0 -PARAM_DEFINE_FLOAT(FW_PR_I, 0.05f); +PARAM_DEFINE_FLOAT(FW_PR_I, 0.0f); // @DisplayName Maximum positive / up pitch rate. // @Description This limits the maximum pitch up angular rate the controller will output (in degrees per second). Setting a value of zero disables the limit. // @Range 0 to 90.0 degrees per seconds, in 1 increments -PARAM_DEFINE_FLOAT(FW_P_RMAX_POS, 60.0f); +PARAM_DEFINE_FLOAT(FW_P_RMAX_POS, 0.0f); // @DisplayName Maximum negative / down pitch rate. // @Description This limits the maximum pitch down up angular rate the controller will output (in degrees per second). Setting a value of zero disables the limit. // @Range 0 to 90.0 degrees per seconds, in 1 increments -PARAM_DEFINE_FLOAT(FW_P_RMAX_NEG, 60.0f); +PARAM_DEFINE_FLOAT(FW_P_RMAX_NEG, 0.0f); -// @DisplayName Pitch Integrator Anti-Windup -// @Description This limits the range in degrees the integrator can wind up to. -// @Range 0.0 to 45.0 -// @Increment 1.0 +// @DisplayName Pitch rate integrator limit +// @Description The portion of the integrator part in the control surface deflection is limited to this value +// @Range 0.0 to 1 +// @Increment 0.1 PARAM_DEFINE_FLOAT(FW_PR_IMAX, 0.2f); -// @DisplayName Roll feedforward gain. +// @DisplayName Roll to Pitch feedforward gain. // @Description This compensates during turns and ensures the nose stays level. // @Range 0.5 2.0 // @Increment 0.05 // @User User -PARAM_DEFINE_FLOAT(FW_P_ROLLFF, 1.0f); +PARAM_DEFINE_FLOAT(FW_P_ROLLFF, 0.0f); -// @DisplayName Proportional Gain. -// @Description This gain controls the roll angle to roll actuator output. +// @DisplayName Roll rate proportional Gain. +// @Description This defines how much the aileron input will be commanded depending on the current body angular rate error. // @Range 10.0 200.0 // @Increment 10.0 // @User User -PARAM_DEFINE_FLOAT(FW_RR_P, 0.5f); +PARAM_DEFINE_FLOAT(FW_RR_P, 0.05f); // @DisplayName Damping Gain // @Description Controls the roll rate to roll actuator output. It helps to reduce motions in turbulence. @@ -108,37 +107,87 @@ PARAM_DEFINE_FLOAT(FW_RR_P, 0.5f); // @User User PARAM_DEFINE_FLOAT(FW_RR_D, 0.0f); //xxx: remove -// @DisplayName Integrator Gain -// @Description This gain controls the contribution of the integral to roll actuator outputs. It trims out steady state errors. +// @DisplayName Roll rate integrator Gain +// @Description This gain defines how much control response will result out of a steady state error. It trims any constant error. // @Range 0.0 100.0 // @Increment 5.0 // @User User -PARAM_DEFINE_FLOAT(FW_RR_I, 0.05f); +PARAM_DEFINE_FLOAT(FW_RR_I, 0.0f); // @DisplayName Roll Integrator Anti-Windup -// @Description This limits the range in degrees the integrator can wind up to. -// @Range 0.0 to 45.0 -// @Increment 1.0 +// @Description The portion of the integrator part in the control surface deflection is limited to this value. +// @Range 0.0 to 1.0 +// @Increment 0.1 PARAM_DEFINE_FLOAT(FW_RR_IMAX, 0.2f); // @DisplayName Maximum Roll Rate // @Description This limits the maximum roll rate the controller will output (in degrees per second). Setting a value of zero disables the limit. // @Range 0 to 90.0 degrees per seconds // @Increment 1.0 -PARAM_DEFINE_FLOAT(FW_R_RMAX, 60); +PARAM_DEFINE_FLOAT(FW_R_RMAX, 0); +// @DisplayName Yaw rate proportional gain. +// @Description This defines how much the rudder input will be commanded depending on the current body angular rate error. +// @Range 10 to 200, 1 increments +PARAM_DEFINE_FLOAT(FW_YR_P, 0.05); -PARAM_DEFINE_FLOAT(FW_YR_P, 0.5); -PARAM_DEFINE_FLOAT(FW_YR_I, 0.05); +// @DisplayName Yaw rate integrator gain. +// @Description This gain defines how much control response will result out of a steady state error. It trims any constant error. +// @Range 0 to 50.0 +PARAM_DEFINE_FLOAT(FW_YR_I, 0.0f); + +// @DisplayName Yaw rate integrator limit +// @Description The portion of the integrator part in the control surface deflection is limited to this value +// @Range 0.0 to 1 +// @Increment 0.1 PARAM_DEFINE_FLOAT(FW_YR_IMAX, 0.2f); -PARAM_DEFINE_FLOAT(FW_YR_D, 0); //xxx: remove -PARAM_DEFINE_FLOAT(FW_Y_ROLLFF, 0); -PARAM_DEFINE_FLOAT(FW_AIRSPD_MIN, 9.0f); -PARAM_DEFINE_FLOAT(FW_AIRSPD_TRIM, 12.0f); -PARAM_DEFINE_FLOAT(FW_AIRSPD_MAX, 18.0f); -PARAM_DEFINE_FLOAT(FW_Y_RMAX, 60); -PARAM_DEFINE_FLOAT(FW_YCO_VMIN, 1.0f); -PARAM_DEFINE_FLOAT(FW_RR_FF, 0.0f); -PARAM_DEFINE_FLOAT(FW_PR_FF, 0.0f); -PARAM_DEFINE_FLOAT(FW_YR_FF, 0.0f); +PARAM_DEFINE_FLOAT(FW_YR_D, 0); //xxx: remove +PARAM_DEFINE_FLOAT(FW_Y_ROLLFF, 0); //xxx: remove + +// @DisplayName Maximum Yaw Rate +// @Description This limits the maximum yaw rate the controller will output (in degrees per second). Setting a value of zero disables the limit. +// @Range 0 to 90.0 degrees per seconds +// @Increment 1.0 +PARAM_DEFINE_FLOAT(FW_Y_RMAX, 0); + +// @DisplayName Roll rate feed forward +// @Description Direct feed forward from rate setpoint to control surface output +// @Range 0 to 10 +// @Increment 0.1 +PARAM_DEFINE_FLOAT(FW_RR_FF, 0.3f); + +// @DisplayName Pitch rate feed forward +// @Description Direct feed forward from rate setpoint to control surface output +// @Range 0 to 10 +// @Increment 0.1 +PARAM_DEFINE_FLOAT(FW_PR_FF, 0.4f); + +// @DisplayName Yaw rate feed forward +// @Description Direct feed forward from rate setpoint to control surface output +// @Range 0 to 10 +// @Increment 0.1 +PARAM_DEFINE_FLOAT(FW_YR_FF, 0.3f); + +// @DisplayName Minimal speed for yaw coordination +// @Description For airspeeds above this value the yaw rate is calculated for a coordinated turn. Set to a very high value to disable. +// @Range 0 to 90.0 degrees per seconds +// @Increment 1.0 +PARAM_DEFINE_FLOAT(FW_YCO_VMIN, 1000.0f); + +/* Airspeed parameters: the following parameters about airspeed are used by the attitude and the positon controller */ + +// @DisplayName Minimum Airspeed +// @Description If the airspeed falls below this value the TECS controller will try to increase airspeed more aggressively +// @Range 0.0 to 30 +PARAM_DEFINE_FLOAT(FW_AIRSPD_MIN, 13.0f); + +// @DisplayName Trim Airspeed +// @Description The TECS controller tries to fly at this airspeed +// @Range 0.0 to 30 +PARAM_DEFINE_FLOAT(FW_AIRSPD_TRIM, 20.0f); + +// @DisplayName Maximum Airspeed +// @Description If the airspeed is above this value the TECS controller will try to decrease airspeed more aggressively +// @Range 0.0 to 30 +PARAM_DEFINE_FLOAT(FW_AIRSPD_MAX, 50.0f); From 6a9423b428c78e4c2ea295544df50f5b1f0cd49d Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 29 Dec 2013 16:35:52 +0100 Subject: [PATCH 255/447] fw att ctrl: remove renamed parameters --- src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp | 1 - src/lib/ecl/attitude_fw/ecl_pitch_controller.h | 4 ---- src/lib/ecl/attitude_fw/ecl_roll_controller.cpp | 1 - src/lib/ecl/attitude_fw/ecl_roll_controller.h | 5 ----- src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp | 1 - src/lib/ecl/attitude_fw/ecl_yaw_controller.h | 5 ----- src/modules/fw_att_control/fw_att_control_main.cpp | 9 --------- src/modules/fw_att_control/fw_att_control_params.c | 13 ------------- 8 files changed, 39 deletions(-) diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp index d7dbbebd42..882fac02b1 100644 --- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp @@ -52,7 +52,6 @@ ECL_PitchController::ECL_PitchController() : _tc(0.1f), _k_p(0.0f), _k_i(0.0f), - _k_d(0.0f), _k_ff(0.0f), _integrator_max(0.0f), _max_rate_pos(0.0f), diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.h b/src/lib/ecl/attitude_fw/ecl_pitch_controller.h index 2ca0490fdc..30a82a86a1 100644 --- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.h +++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.h @@ -77,9 +77,6 @@ public: void set_k_i(float k_i) { _k_i = k_i; } - void set_k_d(float k_d) { - _k_d = k_d; - } void set_k_ff(float k_ff) { _k_ff = k_ff; @@ -119,7 +116,6 @@ private: float _tc; float _k_p; float _k_i; - float _k_d; float _k_ff; float _integrator_max; float _max_rate_pos; diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp index bd6c9da712..9e7d35f680 100644 --- a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp @@ -52,7 +52,6 @@ ECL_RollController::ECL_RollController() : _tc(0.1f), _k_p(0.0f), _k_i(0.0f), - _k_d(0.0f), _k_ff(0.0f), _integrator_max(0.0f), _max_rate(0.0f), diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.h b/src/lib/ecl/attitude_fw/ecl_roll_controller.h index efc7b89441..92c64b95f5 100644 --- a/src/lib/ecl/attitude_fw/ecl_roll_controller.h +++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.h @@ -80,10 +80,6 @@ public: _k_i = k_i; } - void set_k_d(float k_d) { - _k_d = k_d; - } - void set_k_ff(float k_ff) { _k_ff = k_ff; } @@ -113,7 +109,6 @@ private: float _tc; float _k_p; float _k_i; - float _k_d; float _k_ff; float _integrator_max; float _max_rate; diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp index 7c366aaf2d..15e2b0f71b 100644 --- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp @@ -50,7 +50,6 @@ ECL_YawController::ECL_YawController() : _last_run(0), _k_p(0.0f), _k_i(0.0f), - _k_d(0.0f), _k_ff(0.0f), _integrator_max(0.0f), _max_rate(0.0f), diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.h b/src/lib/ecl/attitude_fw/ecl_yaw_controller.h index f15645fcf6..f61698c364 100644 --- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.h +++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.h @@ -75,10 +75,6 @@ public: _k_i = k_i; } - void set_k_d(float k_d) { - _k_d = k_d; - } - void set_k_ff(float k_ff) { _k_ff = k_ff; } @@ -116,7 +112,6 @@ private: uint64_t _last_run; float _k_p; float _k_i; - float _k_d; float _k_ff; float _integrator_max; float _max_rate; diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index 26777f737c..9d8b420acb 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -310,7 +310,6 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : _parameter_handles.tconst = param_find("FW_ATT_TC"); _parameter_handles.p_p = param_find("FW_PR_P"); - _parameter_handles.p_d = param_find("FW_PR_D"); _parameter_handles.p_i = param_find("FW_PR_I"); _parameter_handles.p_ff = param_find("FW_PR_FF"); _parameter_handles.p_rmax_pos = param_find("FW_P_RMAX_POS"); @@ -319,7 +318,6 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : _parameter_handles.p_roll_feedforward = param_find("FW_P_ROLLFF"); _parameter_handles.r_p = param_find("FW_RR_P"); - _parameter_handles.r_d = param_find("FW_RR_D"); _parameter_handles.r_i = param_find("FW_RR_I"); _parameter_handles.r_ff = param_find("FW_RR_FF"); _parameter_handles.r_integrator_max = param_find("FW_RR_IMAX"); @@ -327,7 +325,6 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : _parameter_handles.y_p = param_find("FW_YR_P"); _parameter_handles.y_i = param_find("FW_YR_I"); - _parameter_handles.y_d = param_find("FW_YR_D"); _parameter_handles.y_ff = param_find("FW_YR_FF"); _parameter_handles.y_roll_feedforward = param_find("FW_Y_ROLLFF"); _parameter_handles.y_integrator_max = param_find("FW_YR_IMAX"); @@ -374,7 +371,6 @@ FixedwingAttitudeControl::parameters_update() param_get(_parameter_handles.tconst, &(_parameters.tconst)); param_get(_parameter_handles.p_p, &(_parameters.p_p)); - param_get(_parameter_handles.p_d, &(_parameters.p_d)); param_get(_parameter_handles.p_i, &(_parameters.p_i)); param_get(_parameter_handles.p_ff, &(_parameters.p_ff)); param_get(_parameter_handles.p_rmax_pos, &(_parameters.p_rmax_pos)); @@ -383,7 +379,6 @@ FixedwingAttitudeControl::parameters_update() param_get(_parameter_handles.p_roll_feedforward, &(_parameters.p_roll_feedforward)); param_get(_parameter_handles.r_p, &(_parameters.r_p)); - param_get(_parameter_handles.r_d, &(_parameters.r_d)); param_get(_parameter_handles.r_i, &(_parameters.r_i)); param_get(_parameter_handles.r_ff, &(_parameters.r_ff)); @@ -392,7 +387,6 @@ FixedwingAttitudeControl::parameters_update() param_get(_parameter_handles.y_p, &(_parameters.y_p)); param_get(_parameter_handles.y_i, &(_parameters.y_i)); - param_get(_parameter_handles.y_d, &(_parameters.y_d)); param_get(_parameter_handles.y_ff, &(_parameters.y_ff)); param_get(_parameter_handles.y_roll_feedforward, &(_parameters.y_roll_feedforward)); param_get(_parameter_handles.y_integrator_max, &(_parameters.y_integrator_max)); @@ -407,7 +401,6 @@ FixedwingAttitudeControl::parameters_update() _pitch_ctrl.set_time_constant(_parameters.tconst); _pitch_ctrl.set_k_p(_parameters.p_p); _pitch_ctrl.set_k_i(_parameters.p_i); - _pitch_ctrl.set_k_d(_parameters.p_d); _pitch_ctrl.set_k_ff(_parameters.p_ff); _pitch_ctrl.set_integrator_max(_parameters.p_integrator_max); _pitch_ctrl.set_max_rate_pos(math::radians(_parameters.p_rmax_pos)); @@ -418,7 +411,6 @@ FixedwingAttitudeControl::parameters_update() _roll_ctrl.set_time_constant(_parameters.tconst); _roll_ctrl.set_k_p(_parameters.r_p); _roll_ctrl.set_k_i(_parameters.r_i); - _roll_ctrl.set_k_d(_parameters.r_d); _roll_ctrl.set_k_ff(_parameters.r_ff); _roll_ctrl.set_integrator_max(_parameters.r_integrator_max); _roll_ctrl.set_max_rate(math::radians(_parameters.r_rmax)); @@ -426,7 +418,6 @@ FixedwingAttitudeControl::parameters_update() /* yaw control parameters */ _yaw_ctrl.set_k_p(_parameters.y_p); _yaw_ctrl.set_k_i(_parameters.y_i); - _yaw_ctrl.set_k_d(_parameters.y_d); _yaw_ctrl.set_k_ff(_parameters.y_ff); _yaw_ctrl.set_k_roll_ff(_parameters.y_roll_feedforward); _yaw_ctrl.set_integrator_max(_parameters.y_integrator_max); diff --git a/src/modules/fw_att_control/fw_att_control_params.c b/src/modules/fw_att_control/fw_att_control_params.c index e129568bbe..16227b62f4 100644 --- a/src/modules/fw_att_control/fw_att_control_params.c +++ b/src/modules/fw_att_control/fw_att_control_params.c @@ -60,11 +60,6 @@ PARAM_DEFINE_FLOAT(FW_ATT_TC, 0.5f); // @Range 10 to 200, 1 increments PARAM_DEFINE_FLOAT(FW_PR_P, 0.05f); -// @DisplayName Damping gain. -// @Description This gain damps the airframe pitch rate. In particular relevant for flying wings. -// @Range 0.0 to 10.0, 0.1 increments -PARAM_DEFINE_FLOAT(FW_PR_D, 0.0f); //xxx: remove - // @DisplayName Pitch rate integrator gain. // @Description This gain defines how much control response will result out of a steady state error. It trims any constant error. // @Range 0 to 50.0 @@ -100,13 +95,6 @@ PARAM_DEFINE_FLOAT(FW_P_ROLLFF, 0.0f); // @User User PARAM_DEFINE_FLOAT(FW_RR_P, 0.05f); -// @DisplayName Damping Gain -// @Description Controls the roll rate to roll actuator output. It helps to reduce motions in turbulence. -// @Range 0.0 10.0 -// @Increment 1.0 -// @User User -PARAM_DEFINE_FLOAT(FW_RR_D, 0.0f); //xxx: remove - // @DisplayName Roll rate integrator Gain // @Description This gain defines how much control response will result out of a steady state error. It trims any constant error. // @Range 0.0 100.0 @@ -142,7 +130,6 @@ PARAM_DEFINE_FLOAT(FW_YR_I, 0.0f); // @Increment 0.1 PARAM_DEFINE_FLOAT(FW_YR_IMAX, 0.2f); -PARAM_DEFINE_FLOAT(FW_YR_D, 0); //xxx: remove PARAM_DEFINE_FLOAT(FW_Y_ROLLFF, 0); //xxx: remove // @DisplayName Maximum Yaw Rate From 95bdc1a9bd364ce95abe06b097579cc8a9162e33 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 29 Dec 2013 16:42:31 +0100 Subject: [PATCH 256/447] fw att ctrl: removed unused parameter --- src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp | 1 + src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp | 1 - src/lib/ecl/attitude_fw/ecl_yaw_controller.h | 4 ---- src/modules/fw_att_control/fw_att_control_main.cpp | 3 --- src/modules/fw_att_control/fw_att_control_params.c | 4 +--- 5 files changed, 2 insertions(+), 11 deletions(-) diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp index 882fac02b1..b66d1dba53 100644 --- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp @@ -90,6 +90,7 @@ float ECL_PitchController::control_attitude(float pitch_setpoint, float roll, fl } /* calculate the offset in the rate resulting from rolling */ + //xxx needs explanation and conversion to body angular rates or should be removed float turn_offset = fabsf((CONSTANTS_ONE_G / airspeed) * tanf(roll) * sinf(roll)) * _roll_ff; if (inverted) diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp index 15e2b0f71b..5e22007277 100644 --- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp @@ -53,7 +53,6 @@ ECL_YawController::ECL_YawController() : _k_ff(0.0f), _integrator_max(0.0f), _max_rate(0.0f), - _roll_ff(0.0f), _last_output(0.0f), _integrator(0.0f), _rate_error(0.0f), diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.h b/src/lib/ecl/attitude_fw/ecl_yaw_controller.h index f61698c364..03f3202d03 100644 --- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.h +++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.h @@ -87,10 +87,6 @@ public: _max_rate = max_rate; } - void set_k_roll_ff(float roll_ff) { - _roll_ff = roll_ff; - } - void set_coordinated_min_speed(float coordinated_min_speed) { _coordinated_min_speed = coordinated_min_speed; } diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index 9d8b420acb..df9d325b35 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -326,7 +326,6 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : _parameter_handles.y_p = param_find("FW_YR_P"); _parameter_handles.y_i = param_find("FW_YR_I"); _parameter_handles.y_ff = param_find("FW_YR_FF"); - _parameter_handles.y_roll_feedforward = param_find("FW_Y_ROLLFF"); _parameter_handles.y_integrator_max = param_find("FW_YR_IMAX"); _parameter_handles.y_rmax = param_find("FW_Y_RMAX"); @@ -388,7 +387,6 @@ FixedwingAttitudeControl::parameters_update() param_get(_parameter_handles.y_p, &(_parameters.y_p)); param_get(_parameter_handles.y_i, &(_parameters.y_i)); param_get(_parameter_handles.y_ff, &(_parameters.y_ff)); - param_get(_parameter_handles.y_roll_feedforward, &(_parameters.y_roll_feedforward)); param_get(_parameter_handles.y_integrator_max, &(_parameters.y_integrator_max)); param_get(_parameter_handles.y_coordinated_min_speed, &(_parameters.y_coordinated_min_speed)); param_get(_parameter_handles.y_rmax, &(_parameters.y_rmax)); @@ -419,7 +417,6 @@ FixedwingAttitudeControl::parameters_update() _yaw_ctrl.set_k_p(_parameters.y_p); _yaw_ctrl.set_k_i(_parameters.y_i); _yaw_ctrl.set_k_ff(_parameters.y_ff); - _yaw_ctrl.set_k_roll_ff(_parameters.y_roll_feedforward); _yaw_ctrl.set_integrator_max(_parameters.y_integrator_max); _yaw_ctrl.set_coordinated_min_speed(_parameters.y_coordinated_min_speed); _yaw_ctrl.set_max_rate(math::radians(_parameters.y_rmax)); diff --git a/src/modules/fw_att_control/fw_att_control_params.c b/src/modules/fw_att_control/fw_att_control_params.c index 16227b62f4..1c615094c7 100644 --- a/src/modules/fw_att_control/fw_att_control_params.c +++ b/src/modules/fw_att_control/fw_att_control_params.c @@ -86,7 +86,7 @@ PARAM_DEFINE_FLOAT(FW_PR_IMAX, 0.2f); // @Range 0.5 2.0 // @Increment 0.05 // @User User -PARAM_DEFINE_FLOAT(FW_P_ROLLFF, 0.0f); +PARAM_DEFINE_FLOAT(FW_P_ROLLFF, 0.0f); //xxx: set to 0 as default, see comment in ECL_PitchController::control_attitude (float turn_offset = ...) // @DisplayName Roll rate proportional Gain. // @Description This defines how much the aileron input will be commanded depending on the current body angular rate error. @@ -130,8 +130,6 @@ PARAM_DEFINE_FLOAT(FW_YR_I, 0.0f); // @Increment 0.1 PARAM_DEFINE_FLOAT(FW_YR_IMAX, 0.2f); -PARAM_DEFINE_FLOAT(FW_Y_ROLLFF, 0); //xxx: remove - // @DisplayName Maximum Yaw Rate // @Description This limits the maximum yaw rate the controller will output (in degrees per second). Setting a value of zero disables the limit. // @Range 0 to 90.0 degrees per seconds From 52960e06c601cacab90a196803e479dce539cd2b Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 29 Dec 2013 17:15:38 +0100 Subject: [PATCH 257/447] add fw autoland documentation --- Documentation/fw_landing.png | Bin 0 -> 17371 bytes src/modules/fw_pos_control_l1/landingslope.h | 16 ++++++++-------- 2 files changed, 8 insertions(+), 8 deletions(-) create mode 100644 Documentation/fw_landing.png diff --git a/Documentation/fw_landing.png b/Documentation/fw_landing.png new file mode 100644 index 0000000000000000000000000000000000000000..c1165f16a6e6501ce200f611803d3582070189c5 GIT binary patch literal 17371 zcmd43Rd5_l(5@+2wwPrxGc&U-)`&;U7Be$j7Be$5Gcz+YGcz-7%=e$Odv12G_o5@Z zBc{50YO*Td%zSb}5(L=@%PruF@uN-8WZ;3Yy58 z{?o|*5@f#(5Ps_y0ZP<#YT}c!sY-3aj*ZQfLrlw*r7Dy?Xq}0vYWgwEWt0Yh-)lq~ zkDi8Jj8daTm_`g^-yF0Zm`x__v(i&qj?^dbj^lx_#Qw-HsVOxO$U@M1p0V)$$U+h< z$U@M>Bfp@DzyEhl^xx?py>l>~z_c{np(sL5XDlITJzy%kg_wkdL~B)w;mG#$%^p>O z^eeO3ELq+V(G9L(Wk#=Km}@i3f_|r%}<+ght}?+TZR=f>Fq2O3&nqx_x~-v9601 z&FSq|2O#(E8$NEN(5}IMgMhGeax&`v3%=grv9!DcP5d{OTq<|&WMA)FR4lnv=k>#c z)5xFUXkrmDG5hmnW=kt8RyyKy9%t$xQZy2uu#gaDcN;7>!wVENw7cFV&ZP|#`9@3i zk1r@hJc*%5{5kEa0VWfgRFCOAJ%iPTn-PZx7a{1IQXwHBmiDS`vla$ytu|)_O-)U( z#Y*k8!XgE8sH3luHNo)1GyenI9Po)$U)iL%7+7MF%fo|%gu|fQ{gF6Ut)*gw;m*#^ z^UaPl%_eI-1B1T(kvJF_n4RSsgJVY?8r912r!AkThzOa~3n1seajmWhB;g>1eMxqV zx5->DQYeG;P-M4E$8{!rZ0)k~%A6Sww2ts1x#O8Q%6s_qk-#-wAfp?W#TKDlKy5%c zRt~b8oV6UJ$yDY+hRf#8Y=M-Th6an{vAn>?y`n*o(ND%WczbP#+cB#5Rf}?5)=UGx zD7&F^_=G}kX2(THR5XVxTvcd2wYeg>BBQay^)8<**$keXIQKowSjyPnp{tMREldZP z|1`e3d&~By*mEJeEW7@kSNkJ#Z{Hq_adC5lAA2mc7bk5nFb*q@5DoZfx}n_-z^rn! z=M6=^&U@RCV1Bt9f(ESQi-uwN`};FlE;E&vmrF=UOm|_Rp$P{e;!|lh(veFg<{Jz} zYIV3Pf3+qhVG{bhGe_WZ7D~jBh=_^Z9%cFL?(GSe01Jv0@*vApDp&^14vP6w;K6k2 zsJd_U_PAJgxnKsSfFEfZ_pgrpQ3MB5<%}XajSfe$eEj?{u&^W?9Esn4z#i)=0+|1F zgMGf9F@g?QbejHZy}nt?F2fO2@An&V0zTJYCBTB~o0}Y$tIhYjDZz5JntwUMLH38^ zDd_?gcnKzPzhT8Sc_%$%SrtcGNjFzWI__AQLiPl>3G2S z0f&)qI+Oc;F#gV=1LxEf?R~qThL{ zZ?nV0Y!#A5rM&p->tHO2+x41gadGi=sMct#*={dT4`R&dR)z(P>W|J}8;_`i*=7T! zVlRTL23a=z`pB`Qf@C5Qow(UF)4_1;bY7Rp>T0ITjkcl7jkYW^D%p*@VqgJ)&0;Y; zIT`DNl!GHNJ3AXsFep8~D)x6Mfc%_`%lXII>(r6-%a;OuHvT8tv92ctS@HgnyzjmGPFYZ!xt_@VLk_IJ5=yyM+MsNCI?K`x}_3(Z*?iGEj;YzhGRMY zOw_UP(m`Yp5RwvrKZBhso~;(Jr`JTD7XrI?lA=_q3~PwgB9)XQ-HsA0Ox4HBreg0L zffYWN!asZ_j-9o!c*VqLa#y@fiS6+vAq$0Le{;Zu!0Ju&_z;M!#Z(hV%ZaO)bc0pb zkJljkGx1157Lsm7YNT$opLYafd51IGG*IjaMkKo*`^?*W47WN3cKsHECJz4%fMlfN z;Yh8~9ICz0q6&EQjL^1J?R*7__XUcueP3!NgeE_Z`U^Mkt{jWE($DKojCkb{Aeq9MAb{TJM?dKm7gew|&(WfuDga z)Q%+yS=Bwe9;<*LZf8F%Nwe4r=y}R$R(D5l&_*{H?7mnVjFbm$slvh~F#~B4LPA2b z@a`r6GHk5B6$$i$T@S2dn>xJ%p;_`4sYt3>?;&}lp0_algE+iG#J5F%|AM{VpWFf7 zuJ7;JmYYSqy}hNAnRXA?NQO3i0qA<0~iemvu(sD!06U^}j$Vl|gQKLfF}X z9vq|Loq2d`77GtQKD!$9TS6V(aLi<^8W;#6X-{WRulu zmF@ieoJyq}i`{(w&s?F5bP7vA0U2>`)c>L}^U0hs3(P_n=E;rncGF&oW8onNubPWs)gkNqC)SGRI!R`(x3|4B5 zq_YI@X=!OUJ?|#Yzw+9ljs8hfpYtnt#E)rVF4!wbH8_r$E!39)+0CgQ&Z+?9l(@V+ z#I+2ZK9`^GcXG-#`l3!3tLz2wz$8Xp6--7QC2(-?Qn@Svvqh`L@)Y-vhoyy+xgupC zFcu&oA>Z}&*#_EHfPj*XHkY#A0JyR|@hF;t_|*oBq}sYVMF91~{s@|K36PxKrS2P9jw|94QRcRS4ttu(n z!f<|?~j z+y4zn`EkYW8s(C{wc~d|$g~TT>C|F_xMSVG=GO>nXjq(H&kZ2^p|`4YFSRaTs!}n^ zx#cPR>lYXb3QE28Mru{pCr{r)*5wTv9^Ux=NF2A%C*R1(2%r0X@M?pFV4b$3BO5x6 z(y*g$0U)7w{Co@NxiHYLH-?i=e`u2`lhYQaC*je=XhzY^Q&5g@ z&lntVVvgS56}+>v(?2lK{^i!lkq4j9^ZIuxF84>`DBB-bEq>CeAv-uYSXx>JhlaWzhVeh#JwLnd z?(I=)x3U=<8-Is_s&hJ9bc~v-))O9|m`GtYK@15ADc9}F6b?c(;+Dn5#bwZLy|<@K znIOZ)3JFO0(RYS)G#{<_es@f$)mbI!%R|fMAhvS`$L}GO#(|7sd@d0%{E^A$)*D3V zEs?@vRNc^Uuvno9T65MLt>799G9B&@_h&_)0oF!F1LrGsCz~C-+9>)jg7{K<0xcMrf9WY1GaO_^U1)Utu zBUFj%w09kNp^jg&dU9#C7c1jG9D`SA{5CDF$wL=PX33-Q`}}w_Ffi~73k&mJ$>4PX zDIxcV(|YrT&^DJV9A=?CR!-KTt-h?_9F+e40eRM93Mo0df?wbRK3{N0lRdmwf3vcj zvw9~pSkQ}*cL%W9vG328q9)Qfi_|%sPL(`8Jwf(Iui0pLI$t6d%HTq+nNKI!@q*Un z{FZ?2Gu~|xgIQ#qnfnVIiwSHpbKw)8<$Efd??f6oH8gSU!eEUL(RN|jfW z#YIC+tzhwahM6#T7Y?4LGN?z;tROk9*QcAt*Or%U_XZJ2#pBW@s$mcpYa=sod^Pq^{pk9DR4nYS z&}6d4yFY=b>M-vA8z31Q+cS1^dz&klB><1bL^KqcVqYO7Bm@luGsBq0=Vm{+bbTN` z&JjL5EOBstUhZ>wd|XTz$H&8?!Og=XP$$Cx&ATKi4N!9EJ1k|H8!L|KAzj!Qa+zz- zOfOfG2nJfMQYq*B1L}Ge%zwOz!Kx&ay{vcOF)}i;`+UCR@f(XR(bQQapOuF2IGy(A ziiUxy)tjPZ@Vao=ixl>(kEavcz!U46ey_Qy0Zq@;kk8^5?DR>tIx93{U<7Uhb5yC4 zZwOigsvq);Xs$GSGD98(gs!eG4!e2(N}WlGS`7&t2Hl_c=Nrql#)#8}vO0$&*}*76 zn{i|r*%T6^=bE{waxWA5I>gD`litGgW;>Dzp3BQ>`vMM@(}y$p4xVs47}ZKFYExo( zqP!C-VIiUFx;lf``;+C>RhB>9V7q&J*xdGH=rl^%LIJShIP9U%&#qt?3|hfOMJB0U zX(=fu;ZrYmEwdVhDhEs-EkeJ{gnD154^WLsm2d z-XU@d^3p3rucA?D#yl}goEzx{h0rq91RPeel34PD+dIY6@f18nrDXs0_m(kBp3&%N zWFtMjoWUrb5pq;3TvyY zqs>Y5v<{6A;VU_l=RS+v(w~!4f3vH5dfu>h0Y{U97`y!8FuQwxm&u;aiUv+$Fv=Fq zVejAM0_DWbtgS!C9pjCSjiodBu%)D=U@_>1YmLT|x$H;^q>>69PiCc4*+Q~>KDoz^ zmJ6gXPZ!F9T3Wa}+#hzAYYf0FSLRsf~+g(+4RbJNeQaaH8;~OMY7# zg_$tp7*G91qgW`N$_ z+Lz4Fvp?@26ci+0w%mn4y~b=PN)Kr>?$fy~p=fA#Fp7p`&z5dl*2)OX;e_8%R0MJ} z@>C4yIR)WGQn&|WNt84+G!v}{V@Xii5-}uv?)MU3s}E;Ob$H@0w+Ah;r8hFv35Rah z6eC1gGphIq{1b8gl%OS$YY--(GWB`iR6RY&&*KQ)`K5Cfpwv>Eh?fr)J1m34ZXUnm z7=Mz$=IQ12a$Yyp>~x0FWWBK)Cnu02mn8t!+}etYg3@!dHxv>UMoL6PG|`xnptagy zAub~F-D0VdoS8XhrOu>Fo`so067of-` ztGC2Zv82#o&g4jyE|}Hl3m1bZB{c?LlqX$2 zR;pSx*&l*-^Y}P8GlL>0H^sJA)YKJ!R3MdPyilqfPAHf;TOj4I)B7Wp%`A4p#D=J9 z-O}oNNgn}=VQ_7Iy%Mj>C7ZrTAw4s-f|t*;hw|fCt@-j*pVIhV@{zT}MJkZcr(Ee| z7SQpK2z+}y_?6vTEPZj-+t(*duhl%gtcNA~-=<@FOYUbjSC}(*Qmot6StHD>4zlS| z-rzZ5xwz=`UlZA>bbO6W8>M+a4@S-Wc0@&gk0q`mV@BZgoG!3H#xg~=^WYoFGQSvB z+iVG>XJk-oHCNXNGrJYQChfK%Zq>}VkUAhu6pYBpKzCteo^XhUnnsKtP4>K9^{MLq zUMRhOShBs+LHxn+-OYt(G+qDR_f$vh6ueI0^_Y}^&$YW;y)GP2c{8>=_J#VLQegE3 zWh1@y#_Ue6hQTnSzzT`LU%jrL8{Pe3)}2DREO7!n%#spfAtjQJ+5aJIwatYQ0|R3` znc2^#^Cb|4R1AwZpRY&SWU!kDf_u*=Fn^!S9CE?%_wzsLvPj^371CIuS)VB;Eo20) zg`%VN76aNPbc|+-;|P&}Kqe|VfXQg|=cdOMKiFsj?a)e{3AIjpb4=d2CZagupii8j zF%Rp%)gKB&jk1YNUhi0m3=04_K##}O@2gGuVtEcoI(S(CWZ|9>0FojD{_$c>JWIgq z-cN+z_4;qE(U|?yS9?+-YPKgPy^1qI}o3|fC~b_ci#p7uxLT3l~(6(X&m zM`%)h?7%&5O@7@tPc*%h516P|@_eI5IbSa!4}gIBv;C{0W2XdHT$F))q}y@7%{+H{ zT0WK2rqZX==}d)>k8hUj+h6rMyzO&?m`S1-JGk>hzY z?_ziz&i4V23KclxnFuj48o0t$I`UNn@Aq-=Zp8k~$(HO2Kp<*?RMO$~PM=Z<@W*uF zFrVi$6E274pXF*jH8r(r#}lBy*So`DBz{;{7NLNEfaO*v{$HoFMZ3F0nb^4h!UnuiS)=n^B=@+@BD~`>tS3) zF8_=!h+M2T^eru^3hs+I!$CuD?+%2WMEc{Pq6&e2bps4;US3w0ORZE6EA#5TE@}3d zfS$f%{FkC;lZ`8x4j;}TeS+y78GL4O=8!H~09br1( zWNDdQT}>}wtyWFXpl?_>tXAVs&fqW^3_ssr`~w4V_FWlS;Gaqqi{S0YRgG#1t8Nd* z9PUk5OG8A%Fqn@d-wlLe&ea%*|H6gqP{yrzz zpb&vu;9Dv1N1D*U~skB!HC}0(ZSE_a{0af zwBT~1ZLY~iHx!*Vu&$0(9N4jvmzQ_4+CXbGmRJBtp!p67**VWV7k3obQ=(X8>*4~H zZN5;7$8GOFnn1gEbTkx+&#Ttq&TegOZMjk#Fq%N?@OZvrB8GiFlLvJ)m38#AErU-B<2cGM=ukE{D}>ewHBxHFbz`naWI=D&zJ_ zj96c9udS`^_TfYtoqDbRRp(n|AObEbF78;y7o)d;%_2Jz5>h0gpschsJio_dzGjp4 zt6RKo|J=}WvmI$?XXkpGOO$K|53}Vm6Ig6q+{tnc>E-36h`4y2!4P8S%OP%~^~P)O zkn|EQLWBvKI*i9gtCRUmu4t)lm-h9)@j>5?8BYe>-lR_r7cpwauwjh4B z)v3(h)^@t<@Zg{zE6BCy-;YvY0l=(L#%6PGe$B96)_ies5oDgPcc&X|F0E%5s|`{s zD=Q~iYiny|_`J@?O?NjpGhG-cC^@j`H2Lws({5jgss4`vOx^r9{G^$N(Q}-`l~`Zd zqP@imO?U!6uC++Jqe*-nkBbJ&H6NWFmC1-{?;Q9NPXEX;-rKFM&#?>fCS9|v!^7#4 zxJ8JQ?cc5!>{g|d#uN241(b5|Xl<^{;NajVvIZ~r-pHE9>zC@8yEQ%nbLRiz~c$#?aF=HwDWc=FUZZG@`n{I;6F7>Rx zo%Buu;Qeq@S*GMmtym9vw9}{8^Q~A7WiZ^g&IAFuoe9RK>w|-sm{_aBJ;CX0ai-2> z3VvzA)IN1S^e1(1Ul7v6l;V^c1`f_>lCJkisd8Df#}flHGxJYI9X?ImNLrP0iG$IE zkdTlW!ON|#DVwej!u^YhIH6&3Y4RM9B7@NTNv$m2#~(OnCQ1ouh1Xvp9+;-K(gR*7y8wU_4(fe> z(J<3HSF1qL*q7n=cr+Y}#Gj%^=hH?toyq;tV7`#6)nXq{uZi(jAx}&p`xm&SrRDKb z6=8?R(_++86dAC^4OKI^=+yr#E)H@%HYMAN0Ia`786W>BmV+U(p^hcmQ+uR=6^%{T_-xkALD{N7-gLs>& z-lu19$V_1W^_}06?$o33k@;;4D?Vfug+_<_gToeB04#d0cAGOJ%H>!br_pC@>uIre zgR`_5cL0m%=>Bj`UtdFhwI}y{MVT_`!IGHC%X$i;AEh6#05Ah`tw_c3By4O-f`2q+ zV>pdI9T_hPz=^@>_*>#fbmFj;T;vr=+>5IpFKAnj~tu8^GUJb;Jx_|2R<0!It z=k%@(r^{3s6B)EqPkz_cM&^v9vH0-1?U8+^y}Kn}M`t_#W~;V{&Kv6TRL3QScV1HZaH5r)~v*!8!pED@{K`3G$o9)f5vo!fz7 zbaL|WRo9oSl2W`k33P_+(6Le3J>Y+{05f?AxMnm7w5o^0F{H0G1B!8mM8I_)`>emw zV2SB!LVs1k{LT<3YfAIxcXzj&w(9$)Z7LB;DMhTWM@O=))*64(C_!Ul zVvYkR__@qx$@)ZJ;z3UC`C1QX1^=&4Rg;X>=fq@|=BNO@ZyxkwEQjsyVv6%`)z_oRDmYp+>)+XS0 z7XboMRa8{?JfEXgDl|ZpghqpT!4LWc5t6WYU^!RAKjyW;>CqhYx%MLLj#H|JwWggV zUA+@vaPYJ{$;0W7%*<#ALiU)cc6)Y(0zg8sa+yjb*|)#AoYvbXCn0HRxDRJbMZ!Ud zuOdYKVbdp$&#lk)gJ19kunD<~>l)t);K`V|XO?roSGQgLpe*>Kd;4^idZEcChVgSfpqm0iZwmuGoy~#b^?+tLd zb_Iijh}7CevStesV~M|adNs^?mQ>Sr6@!x=Mms`)6vf`|keTP_8mZWlkh{uM1!~we zvCNu7+u8$ZW}9=>)Tz8}PgDgmJ1vgXDZExy~z4m z{kJ2Ra%!%g#AvCqe~xj5YnbYooYk-a(*D|Q^=%nTl>w&<4c4U_t!5i40j`-z)~piL zDD9^_Li=JWRq8iJWsEBqO=%A%ONuT!+K-jd6>v#UGF<92C5;^9hNxrU+8x;ye*NlZz4$ICweclbB*B8b zLjEX8mv#9P|6>RvS!7{mZg=<37=~I4Du>lS_lNq@dB%sX1q{SOZ*Y|vv3~oSpWPQ`m5dQIG%)-Dp&XU4o0zgdGz38;r(4<0My?6 zAGCQ*k%MEKVT@j#`C1Bd?-*Xs@>0-csunQ41ERF_jwd7?vA1ZFkV5VMpl*(hkimeO!|^Hme0vo}84-5VCQ8>CZw(EREgK2v5{yIcKtJ!YaZRR66I{cFGZauV7GWVBlSGNP0y2nAewsv-UFSiGQVe-&uIddnjYbRE+ zZ&`)p~Y!w)32~v=1%oVTa2?Ii2-l z>UL-Om`6+=jP(r>TAL zkYNX}pPyIMg)j5{D|@P9DcZnmKko>~2S!Rr`*(OW^jCX#aO|2F$!h3d-+wPr=jcq5 zg=94zm#B*?8u5e_%9o=aSQT7iF(0GCQMDXbt?0~cS9UW9N-&6@Jv5Y%gim5HTadh1 zvzYTtq%k^&zAH+)l=+E^JLU(4#AUw^z2*HzuUe&ZaMks7*ml*qcRZ7~f?AI3<||v2 zCpE#4PT-z8N=~ErwzfLHEJ!MM&oc^}A=@_gvKO^8P0VHYEtPW#a;{7ZJCzNjln-v( z7+i*d>@h_}lxZARv#Lk~gM+h`+Pp%-!gc)J_Yxz&l2aX;rkF`wzpGyA>dCHgvsJCH zFB4Y$4w>#^R`~SpDP~^PCv>ZLnrHh(iJOX|A6cr2-ARYOrW`5P}OK! zaK89+snzVcvK({Bs1AVqdYSUMzP=_RBKj*CN1@Q=?e+S2k+x9-6GOY+YzMNY_FHd9 z9(g)B45$r=2 z=#HVxGP~Rq3ojy@%5H&i)$yd$c&7vwe}ZL?Kz`Qrz(Tziz&k+$uWJ z@%azH)mqWCB4UznsT?1i+)5SZ=ga4rl?B(C3MTu4*qoDu2E>c<-m4#=Bq zEc}iO$by6dK4su1(u8Mn2P2tWh7(`l(=#%-olfUze>~QbVF#bj*BVI;M&OE{EmpuF zA`WhDW~Q=Ru-I(!O=j|^9U70HXUFUJtssQ2v!F)75{#R&L$v7we`hKKdPd?jfXlU_ z7A~%P2=vU)&wtCxID{;`<5yfv5(_N=ABS?XvWLerc}m4Z52++qEv`2tCX?woPG^f? z>D&&1jaF;lkg>5-NQG;T9eFm(ZzwA8G?<;Pl-4^P3@r0%!p*q=rIEG2islAIOSN$a zBjyI>RvVL@w26>smo>mVLO-0 z^&I_y(AOjCuH4Rk5UH5&fm^VXvZg9;199655*;Wc^enRLo%AGc$Sj#r!f6Sbf`Nrh&UB^(_q zP=>5lN=WFN)7c_98QFI!DJhk5wclT#AJ{yORG_M`coczHh!pj}paZU(&7DWP!Y6`p zed_OlUG%K3oM9#VT9tx&ueL>Dvn>Hz`&j_IfWUOM-sWk+{iqe><>jS+P>`UDx@?Rg zQSIDHoe2yqY;SMxxBn?#NhC37+d4V1JDc*!x{l9xD8lgkBvTe&*&ac&I-RmwtN(8I zTF>#O2}pw68wqOe^{%Q1S%8~TnX!fgN4q10 z{mbn^iDnbS*-}-J^+xM!#!p(6@Dx@P904G)NFrCf=Af*<#2evk+(&QwzmTszEjPcoEDbkR?^om0nwd2SOVNXm{!KtN9b+-fp_8YcIb zZ#_8_DZ*@YPEsYs>0$+LnMy_PzySG1MkbeCudPC1t=pZkSR|e#5Qqu^g}CpBB;b0u z91HI+aRwYG7qcc9Sy?lIvBZ_24N@6Iq}EjcoK?ifVhIU#kJ4D#Q`zjlMCkBfi-?OW zc6z!o=ytx3BRcZnSD6SyZRF>Z;o;-2bb9fk(D-$9 z81zWn>Z?e{W!0RDFC`_FlwHKhR*dbt+>mdrPyE^5UcbX?&%@n*?+JPEB{HQ9=Tni>Lvf47`QDdQWx=WBwt5^oXe0-}}{gkZ1>1@#?(Q37R zWV;9KKNd$u(An8pt0PsA5*%gM2)&+axZfC!`~H#`QHMWxtck(0rFvjl@4&*?c>54C zPpl^p!S)QJT(f9m4I~$DOEd7FUwfOj%pD{sDUY-~QywW^AJ=SBcwHDU8Fi%RtdVhX zrS=9R>d1*QN;n4KV&Ie;E5~06&R#y&ubypghZ@ppX>bQ-|)xV-5m4f{fn9IvQF$KT`L&t=1Zi-k)!z(>P>wmkf=J zL`#4LwdC2b=rpzmqv#)Zw5pZ)yd}Y82*f(;q6|Q=7RibDa@{KaTFpX)VVBEw@OsUi zxwl(-_1gR%2!OOS$S2_QI?H!@y7BSx<%6q36Hf=8Znmk?a{C9G-*Gj!m?=v~;0ou7 zX^=XJ>9lgjbaZ$Xc6F1IlGb@+FcNWbsckhlcK-$UeBKE%(@8QU@<&F-g@ZyeH=wy# zEBe@4RFQ(GjyhfV>Of)8JY;2}kc1S1zA4Wqv)b;6->F*Vygj{Z(KB$U>!7O%wo&{*8!%Argn(-1hN&#S%e!pZ{X5 zsmN@u(AC}jzhU2h{II*;{_Xws@kCdVrMp$64uC|bRUxCG2%OB|-P_v}E|SftZfcr3 zoEFik_xbWpWi!)1XA%)W!N3qZTdcUbzu&K$5*WL6e!1~_?d=E=5)x|m`4X^NtvH)gM!QJ?HfFR~AupNP0Fu}FJlpAP(Q=~|-Nxohx@lzW zO7TR4CfxY$Z~{bWEb01WoGn$|4H5cOTP!gc1d-qRqEX5sV_*z+`$B*h>vjnQz@n3o zk?p5A-HpB77nty%?J8Stb!J2fekKckK0`=JNo~3vWq7J0(T1DMZ!Lzfw+gn6^rySexWY(D@9UT&wQH954`zs|SrR0C0#n7lI)qzlS#hRL$ z(;h!4^Tl%Y(Vm{)+MS*&!!e=lrt8girz3HcQ*^p5_64&ijx=<1($onrZ)}XC=WU7r zND@sEQBj2g0Q$!VRS*(kq4`4TPkN2`I7&Gd{r+IKI~8a>sw4*OTo91pYU9{N32C<+49Bjw z*%H_Y)CVIZB>ea9AD7*(f4iJw%%%v*nshRg@9scYTzYzVP!N=_uP>;8AA&}iJ9lEb z+0Gq?L7xjGPsvlo5_5E9OQ2PyASNcBdvN5z2NiBkW(%-8A?O&|gOr*Oje1kxxHvS2!*R*F^~J@+kvK|L)9LJVcm-%;`wTuo z4h4OE5D5bL0_=B9L))<=WZVnIijFq}}Qm zg3D<=U2itWHuiM65!TYeO+Y{Z21-z^)S1*-uGCtFX+slBD_2TEUhBu@Uv0D{k%)%M zH(776jXj;O2zR(YP|0V5fr*Cx)awsMQ7TuP$*zGV7SV~Ojr;xQ4^d@hB|H|>-!_*k z=@AHj^hF5*(Z@ox`EmrJLK`fWP%$t9fuY&iMDHITbsy>e$Rthw zlZhjfIy-EI+@H_Q%{^YOA*Gbdl&<5e6N^4F`ycC#g)}4<=`;W$3@=q`&wcq^tTr?^ zH)FkHwQzmG5c@0aw;EpeKoQEfIa_IIYp-;8aI%_AvJHEqp`t>=!|%Vp+jpu-wuthzLQ)=%y&K#~=#@r&EIZru;!$I`aR~NA>^5(?9u!)#eK{<_o0( zEiK&QQ3Nt~tMSx|#VQpzAWZ#iv0@Nf4x0EDAI<=8eqbP=!D5NU^?I9aOfE}cVl;u4 z+v}B0wMu7TZ7t2=Xj0FZ;s+M7|FEpsOJapW9^^uqs*mx@>%-a2!^6PrEUJfxN2y8$ zKus+fh2*DD@H@Hc$A090P?hZ0`s8F}nQ9ddGYO~(thOR`cxWi-Y^h4H?mdjLGuvP& zQhhT@wuseGO$eHJHpYtF&uXpF?sk8~gdm0d-~TNny1qWsK=r}4O4%ZhLlk7>=*w1c zaPZpBa>lc<*B9%L=xF3`-@e^PjUUGYY3A5#748lvUQWxZiZz=Uve}8A&R629tC_qY zZUf*jr%z@RK{`?lUK5&F8fd(gA!f#^*5*u)POGxt?F&(Fvy};orndL@MV>ASWAl4ZbAnKcQD0`U z@ZtUQTN0>Ub}8=fe_oINoZbk^{6D*&bUpaW#6-j%hyaEs_GhA=NM#?Zb{`{)18tCf zvF0TI+su;xuq?N`v9g=bXKOT=OQ&&6EbkbGbae1HSS$^0Y~UjyA%R`4xA;XxAuW`t z7Acph{O1xBKFfT*K9`r5w|94QbraRwTkM2#?|9(ZxwzE6G^}}p zXjLnx+ua;M22)qZT4yo^DSgCa__M1-%dfea>vX9qDlH9HR#q0ItNxz6f9Qs?5Ql_> zfSjPHm{{rav+Kd>X{l_MfYo_*=Gf+@&PJyf??i2l!BDB$Tp@Kqd@7Gq>Fc&1QaFl6 z>*M)KUL*m3X?jM+!x&2_CkcsAOKU6WCg|AMC3tvvNGIl|fti_P6al|lmv=`^-ze&X67NSK0`Boz6C;?{+oRD&tJ5Gft#)Q+ zrkbW^k$R#+dy5lj%D^)_5TkjgaGZWa& z%?%kjxt^7k6^qYTk{)nm`*fj9sa#FrYh8(=Yv#n!)x!e>%y~cWK!I7WH~U6JAb@dj zaI81m^(7MvsEXaBuKJ7k`uaAvwt^^BFBHM1EMX<3pF2A{^HsWAWL3g|cLW?ZFzF*4 zGD^z5!^6Och=|V7v7cX`>PEnkZY6Q1jsLU2B4TQ4n!2PoNXUsk6~@_=ITlSU92_3L zzg%Omyt;b(`F3T}Y;DaQ1cgK}-C&_=JdqlZkf5BpB)!AK=X(9OY0EoJz0O#n-c%v3 z71G$)7?0OE9JCVoJsztqmMYn*(t5YPM3a?RT-|TeKw5ufuqC0Qf*#ChG=8U|X_JJG z4jva5_e0utsZu-7a;0_<{-%W`^huP0jHR*GXzWz58v+V(9&^v(jGK&%4D=4K_b0+f zlNsQ1t%%e&PnR2-gk2@t5Px?E!sa@?JRnY@H7GbU~lY;$kXLDx$@n zJcGxH#aS)I{r(uGrl#gU$5)}*w7a`2R3w`L0+7MNFz9D})a=W`cQ58k&}dc4K@{BZ zu*77RVAU0K!kSi_v&`35gT>OR2hPEugZzIjf9dJzSlssc-Yq3sLE*cc2^O|zi}YUT zi+e*+D~(pTmzS3yL!%b6>hy+5fwQ$jq$d{QP`gPVgh%tUb%;v&G)l zRz5N^(){jlLM;4US1zmX{pC(MOTa5WEG(>4yRFgvZh~{+;cV&Q?5NQJ8UaC>CquTS zwUvgGlk?#4u<)OspUfdoKIT`UOq%o|PCA#JP)l1It*WZ(!R~Hupf?w}-z9j#~k{Ihe zUv7^olNOdg-rqs+4cJxZ8-vMY`iSj@Xc$I83n?e%%gYN$(;ZHvJqfL`+ic>|($Yra zamUKUz!=mpIfDrqDg%M2eo#mp#{#6Jr0R{9*qxo7kSn^UJ!3$z;)&tj0605m=fx=P zgg8eYd|j^xbnRA0x10;-i&fFP!wDH{WR4q8APX`#4K;PhZ$HSL^KgI=W{2 z16TzGg?iJOZ>p-QAfC%F=;h_*X4B(prrFK_4-fCYYjY@yP|nnp>hkh(Je56cEQxWh z-c0dyu|jI2)#+s~N-)zywl5GNPb>oG`T5!Q?oj5#o{^C;l7JszZ_ncG?cLnm3{of{ zj7OzPXJV%>=wzV`)9G}cn1LZuPT=J`5)zV@mewp8KO-aKU=*R;$LoWgqod)~X2)yS zK{B&}t({%Z!a@SK!y%aM{xC{=dwa2J6@k@i{R~q1WBt4z((}s;v*WQm4u@s1pC1G< zG4cJy8YU$r#O8|9&Y?eSpsY*p@JdlQrOa%1lt}5{;f|BNDu7Y^r(PW0= z!|B4@P^SB0{m(MhDir|%0UB;@by0EgGE{Ov@j{vEnSr4FelU;cbM@`b&5Vu1)n*4< zRZ+9UQRL^B*Xz9kE~oW;6{qnyRzW-mM^aN$6A)0+;&4=Iz0rDNGT-WO6bTCN5O6t^ znyfc~0O|()0oa8upD&Z$$GbbluFqGC)T7SMP5^bmVwG+ch@3A`EK;(!uPxPVs^oD# zPxjs@+QV@3Q$h>DQ*N<0`VR)&Xmtt+4c-0xcw29CL6Vb``<3>)6a;fwfnr$rr>4Sz zz$h><2qVz{0@!Q`fZ~D2i#4cjUkIrCI=P3-jWoNxL2!)*iBae{c8kS%IY4eEPAaW{ zK&D<_pxyhk-T1@=x67qg;Sm}R4qz&if9Aw-d3AL*S2WCI(azR(d$vF-9EhlKzk7cMuZyj-bD?k$BG~I`I*-#SGCKN??nr!IEEYp(V`B~Q zt<}|3#Ufc06cpd4CeG{xg3%<#tP}2hkopFjoSclORgKhWFb9_}Q=!kz&9!xMnwa(W zH;XkSvY)r8>L~o`YH4ZdOr>fl>dFrLNgaFHS*hIy3iVf-?ee@sd+!_N6taK$9#yhB z97;M1W|eU% zDe9>Pq3x2Z@cs(a1)!EqP^WY2b(YU3HX~z|bd!{{G>Es8&JxHdRW3VC;xwNRMyFK) z1F^WN99F3(-sR=x_jfX2poVw*gHiBm*BkxV*jSd0PP4f}Q0jMo->_1nL3d5k^?fUv)j3VIg?dbt=)rPH6 zA8$_%`@<*{vgyP9A!y*xh=~8!)C>$J)cyVS@#*yVplvxfO@4dUzM1lW_pV(@d3k-a zXG{P6^)>m$g@q+=Z%qX)SY75T-6f_wEAb5jX!E&s`MV=0Pj-4u)w;MMaB+Il-m0%! ze}8>dwzQONSQ)&WDQfZC`v1Sv?c*Z?%e=no-sjjX87X7~SzttDk@7TjKLhsH z{RQprj@?~W`!IL5dH%T`N#hm4%l*t;)OuW3b+(*n2kiuJ;S`R0wQTc=6COIcx|=nP m5_LcgaSlaQ+}*Md^=G>mc+ccNo5sMvz~JfX=d#Wzp$Py>_*o Date: Sun, 29 Dec 2013 22:38:22 +0400 Subject: [PATCH 258/447] Some mavlink fixes to use vehicle_control_mode, WIP --- src/modules/commander/commander.cpp | 39 ++++++++++++++++++++++++++--- src/modules/mavlink/mavlink.c | 20 ++++++--------- src/modules/mavlink/orb_listener.c | 27 ++++++++++++++++++++ src/modules/mavlink/orb_topics.h | 4 +++ 4 files changed, 75 insertions(+), 15 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 199bfb0daf..98979df3e1 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -193,7 +193,7 @@ void usage(const char *reason); /** * React to commands that are sent e.g. from the mavlink module. */ -void handle_command(struct vehicle_status_s *status, struct vehicle_command_s *cmd, struct actuator_armed_s *armed); +bool handle_command(struct vehicle_status_s *status, struct vehicle_command_s *cmd, struct actuator_armed_s *armed); /** * Mainloop of commander. @@ -345,10 +345,11 @@ void print_status() static orb_advert_t status_pub; -void handle_command(struct vehicle_status_s *status, const struct safety_s *safety, struct vehicle_command_s *cmd, struct actuator_armed_s *armed) +bool handle_command(struct vehicle_status_s *status, const struct safety_s *safety, struct vehicle_command_s *cmd, struct actuator_armed_s *armed) { /* result of the command */ enum VEHICLE_CMD_RESULT result = VEHICLE_CMD_RESULT_UNSUPPORTED; + bool ret = false; /* only handle high-priority commands here */ @@ -375,6 +376,8 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe mavlink_log_info(mavlink_fd, "[cmd] HIL: FAILED resetting armed state"); } } + if (hil_ret == OK) + ret = true; // TODO remove debug code //mavlink_log_critical(mavlink_fd, "#audio: command setmode: %d %d", base_mode, custom_main_mode); @@ -407,6 +410,8 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe arming_res = TRANSITION_NOT_CHANGED; } } + if (arming_res == TRANSITION_CHANGED) + ret = true; /* set main state */ transition_result_t main_res = TRANSITION_DENIED; @@ -447,6 +452,8 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe } } } + if (main_res == TRANSITION_CHANGED) + ret = true; if (arming_res != TRANSITION_DENIED && main_res != TRANSITION_DENIED) { result = VEHICLE_CMD_RESULT_ACCEPTED; @@ -473,6 +480,7 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe if (arming_res == TRANSITION_CHANGED) { mavlink_log_critical(mavlink_fd, "#audio: ARMED by component arm cmd"); result = VEHICLE_CMD_RESULT_ACCEPTED; + ret = true; } else { mavlink_log_critical(mavlink_fd, "#audio: REJECTING component arm cmd"); @@ -482,12 +490,36 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe } break; + case VEHICLE_CMD_OVERRIDE_GOTO: { + // TODO listen vehicle_command topic directly from navigator (?) + unsigned int mav_goto = cmd->param1; + if (mav_goto == 0) { // MAV_GOTO_DO_HOLD + status->set_nav_state = NAV_STATE_LOITER; + status->set_nav_state_timestamp = hrt_absolute_time(); + mavlink_log_critical(mavlink_fd, "#audio: pause mission cmd"); + result = VEHICLE_CMD_RESULT_ACCEPTED; + ret = true; + + } else if (mav_goto == 1) { // MAV_GOTO_DO_CONTINUE + status->set_nav_state = NAV_STATE_MISSION; + status->set_nav_state_timestamp = hrt_absolute_time(); + mavlink_log_critical(mavlink_fd, "#audio: continue mission cmd"); + result = VEHICLE_CMD_RESULT_ACCEPTED; + ret = true; + + } else { + mavlink_log_info(mavlink_fd, "Unsupported OVERRIDE_GOTO: %f %f %f %f %f %f %f %f", cmd->param1, cmd->param2, cmd->param3, cmd->param4, cmd->param5, cmd->param6, cmd->param7); + } + } + break; + /* Flight termination */ case VEHICLE_CMD_DO_SET_SERVO: { //xxx: needs its own mavlink command if (armed->armed && cmd->param3 > 0.5) { //xxx: for safety only for now, param3 is unused by VEHICLE_CMD_DO_SET_SERVO transition_result_t flighttermination_res = flighttermination_state_transition(status, FLIGHTTERMINATION_STATE_ON); result = VEHICLE_CMD_RESULT_ACCEPTED; + ret = true; } else { /* reject parachute depoyment not armed */ @@ -1151,7 +1183,8 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd); /* handle it */ - handle_command(&status, &safety, &cmd, &armed); + if (handle_command(&status, &safety, &cmd, &armed)) + status_changed = true; } /* check which state machines for changes, clear "changed" flag */ diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c index 4c38cf35a1..bc191f30d2 100644 --- a/src/modules/mavlink/mavlink.c +++ b/src/modules/mavlink/mavlink.c @@ -220,22 +220,18 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_base_mode, u } else if (v_status.main_state == MAIN_STATE_AUTO) { *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; - // TODO use control_mode topic - /* - if (v_status.navigation_state == NAVIGATION_STATE_AUTO_READY) { + // TODO review + if (control_mode.nav_state == NAV_STATE_NONE) { custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY; - } else if (v_status.navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) { - custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_TAKEOFF; - } else if (v_status.navigation_state == NAVIGATION_STATE_AUTO_LOITER) { + } else if (control_mode.nav_state == NAV_STATE_LOITER) { custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LOITER; - } else if (v_status.navigation_state == NAVIGATION_STATE_AUTO_MISSION) { + } else if (control_mode.nav_state == NAV_STATE_MISSION_LOITER) { + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LOITER; + } else if (control_mode.nav_state == NAV_STATE_RTL_LOITER) { + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LOITER; + } else if (control_mode.nav_state == NAV_STATE_MISSION) { custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_MISSION; - } else if (v_status.navigation_state == NAVIGATION_STATE_AUTO_RTL) { - custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_RTL; - } else if (v_status.navigation_state == NAVIGATION_STATE_AUTO_LAND) { - custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LAND; } - */ } *mavlink_custom_mode = custom_mode.data; diff --git a/src/modules/mavlink/orb_listener.c b/src/modules/mavlink/orb_listener.c index 28478a8030..e168dded32 100644 --- a/src/modules/mavlink/orb_listener.c +++ b/src/modules/mavlink/orb_listener.c @@ -69,6 +69,7 @@ struct vehicle_global_position_s global_pos; struct vehicle_local_position_s local_pos; struct navigation_capabilities_s nav_cap; struct vehicle_status_s v_status; +struct vehicle_control_mode_s control_mode; struct rc_channels_s rc; struct rc_input_values rc_raw; struct actuator_armed_s armed; @@ -125,6 +126,7 @@ static void l_vehicle_rates_setpoint(const struct listener *l); static void l_home(const struct listener *l); static void l_airspeed(const struct listener *l); static void l_nav_cap(const struct listener *l); +static void l_control_mode(const struct listener *l); static const struct listener listeners[] = { {l_sensor_combined, &mavlink_subs.sensor_sub, 0}, @@ -151,6 +153,7 @@ static const struct listener listeners[] = { {l_home, &mavlink_subs.home_sub, 0}, {l_airspeed, &mavlink_subs.airspeed_sub, 0}, {l_nav_cap, &mavlink_subs.navigation_capabilities_sub, 0}, + {l_control_mode, &mavlink_subs.control_mode_sub, 0}, }; static const unsigned n_listeners = sizeof(listeners) / sizeof(listeners[0]); @@ -678,6 +681,26 @@ l_nav_cap(const struct listener *l) } +void +l_control_mode(const struct listener *l) +{ + orb_copy(ORB_ID(vehicle_control_mode), mavlink_subs.control_mode_sub, &control_mode); + + /* translate the current syste state to mavlink state and mode */ + uint8_t mavlink_state = 0; + uint8_t mavlink_base_mode = 0; + uint32_t mavlink_custom_mode = 0; + get_mavlink_mode_and_state(&mavlink_state, &mavlink_base_mode, &mavlink_custom_mode); + + /* send heartbeat */ + mavlink_msg_heartbeat_send(chan, + mavlink_system.type, + MAV_AUTOPILOT_PX4, + mavlink_base_mode, + mavlink_custom_mode, + mavlink_state); +} + static void * uorb_receive_thread(void *arg) { @@ -753,6 +776,10 @@ uorb_receive_start(void) status_sub = orb_subscribe(ORB_ID(vehicle_status)); orb_set_interval(status_sub, 300); /* max 3.33 Hz updates */ + /* --- CONTROL MODE --- */ + mavlink_subs.control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); + orb_set_interval(mavlink_subs.control_mode_sub, 300); /* max 3.33 Hz updates */ + /* --- RC CHANNELS VALUE --- */ rc_sub = orb_subscribe(ORB_ID(rc_channels)); orb_set_interval(rc_sub, 100); /* 10Hz updates */ diff --git a/src/modules/mavlink/orb_topics.h b/src/modules/mavlink/orb_topics.h index 9000728cbc..4d428406ac 100644 --- a/src/modules/mavlink/orb_topics.h +++ b/src/modules/mavlink/orb_topics.h @@ -95,6 +95,7 @@ struct mavlink_subscriptions { int home_sub; int airspeed_sub; int navigation_capabilities_sub; + int control_mode_sub; }; extern struct mavlink_subscriptions mavlink_subs; @@ -111,6 +112,9 @@ extern struct navigation_capabilities_s nav_cap; /** Vehicle status */ extern struct vehicle_status_s v_status; +/** Vehicle control mode */ +extern struct vehicle_control_mode_s control_mode; + /** RC channels */ extern struct rc_channels_s rc; From 174fd21c6f57f5e12d17d6165777555931c4d562 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 30 Dec 2013 08:55:46 +0400 Subject: [PATCH 259/447] Merge fix --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 5699fa1bb8..944ffe619d 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -833,7 +833,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi if (!land_noreturn_horizontal) {//set target_bearing in first occurrence if (mission_item_triplet.previous_valid) { - target_bearing = get_bearing_to_next_waypoint(prev_wp.getX(), prev_wp.getY(), curr_wp.getX(), curr_wp.getY()); + target_bearing = get_bearing_to_next_waypoint(prev_wp(0), prev_wp(1), curr_wp(0), curr_wp(1)); } else { target_bearing = _att.yaw; } From 3a4a36c736d79a56652b5880a73d44cd8c445b8a Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 30 Dec 2013 09:00:23 +0400 Subject: [PATCH 260/447] mc_att_control_vector fix, multirotor_attitude_control and multirotor_pos_control removed --- .../mc_att_control_vector_main.cpp | 10 - src/modules/multirotor_att_control/module.mk | 42 -- .../multirotor_att_control_main.c | 465 --------------- .../multirotor_attitude_control.c | 254 -------- .../multirotor_attitude_control.h | 65 -- .../multirotor_rate_control.c | 196 ------- .../multirotor_rate_control.h | 64 -- .../multirotor_pos_control.c | 553 ------------------ 8 files changed, 1649 deletions(-) delete mode 100755 src/modules/multirotor_att_control/module.mk delete mode 100644 src/modules/multirotor_att_control/multirotor_att_control_main.c delete mode 100644 src/modules/multirotor_att_control/multirotor_attitude_control.c delete mode 100644 src/modules/multirotor_att_control/multirotor_attitude_control.h delete mode 100644 src/modules/multirotor_att_control/multirotor_rate_control.c delete mode 100644 src/modules/multirotor_att_control/multirotor_rate_control.h delete mode 100644 src/modules/multirotor_pos_control/multirotor_pos_control.c diff --git a/src/modules/mc_att_control_vector/mc_att_control_vector_main.cpp b/src/modules/mc_att_control_vector/mc_att_control_vector_main.cpp index ef6a46810e..5f436fb71c 100644 --- a/src/modules/mc_att_control_vector/mc_att_control_vector_main.cpp +++ b/src/modules/mc_att_control_vector/mc_att_control_vector_main.cpp @@ -523,16 +523,6 @@ MulticopterAttitudeControl::task_main() } } else { - if (!_control_mode.flag_control_auto_enabled) { - /* no control, try to stay on place */ - if (!_control_mode.flag_control_velocity_enabled) { - /* no velocity control, reset attitude setpoint */ - _att_sp.roll_body = 0.0f; - _att_sp.pitch_body = 0.0f; - publish_att_sp = true; - } - } - /* reset yaw setpoint after non-manual control */ reset_yaw_sp = true; } diff --git a/src/modules/multirotor_att_control/module.mk b/src/modules/multirotor_att_control/module.mk deleted file mode 100755 index 7569e1c7ee..0000000000 --- a/src/modules/multirotor_att_control/module.mk +++ /dev/null @@ -1,42 +0,0 @@ -############################################################################ -# -# Copyright (c) 2012, 2013 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. -# -############################################################################ - -# -# Build multirotor attitude controller -# - -MODULE_COMMAND = multirotor_att_control - -SRCS = multirotor_att_control_main.c \ - multirotor_attitude_control.c \ - multirotor_rate_control.c diff --git a/src/modules/multirotor_att_control/multirotor_att_control_main.c b/src/modules/multirotor_att_control/multirotor_att_control_main.c deleted file mode 100644 index 111e9197f6..0000000000 --- a/src/modules/multirotor_att_control/multirotor_att_control_main.c +++ /dev/null @@ -1,465 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: Lorenz Meier - * Anton Babushkin - * - * 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 multirotor_att_control_main.c - * - * Implementation of multirotor attitude control main loop. - * - * @author Lorenz Meier - * @author Anton Babushkin - */ - -#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 -#include -#include - -#include "multirotor_attitude_control.h" -#include "multirotor_rate_control.h" - -__EXPORT int multirotor_att_control_main(int argc, char *argv[]); - -static bool thread_should_exit; -static int mc_task; -static bool motor_test_mode = false; -static const float min_takeoff_throttle = 0.3f; -static const float yaw_deadzone = 0.01f; - -static int -mc_thread_main(int argc, char *argv[]) -{ - /* declare and safely initialize all structs */ - struct vehicle_attitude_s att; - memset(&att, 0, sizeof(att)); - struct vehicle_attitude_setpoint_s att_sp; - memset(&att_sp, 0, sizeof(att_sp)); - struct offboard_control_setpoint_s offboard_sp; - memset(&offboard_sp, 0, sizeof(offboard_sp)); - struct vehicle_control_mode_s control_mode; - memset(&control_mode, 0, sizeof(control_mode)); - struct manual_control_setpoint_s manual; - memset(&manual, 0, sizeof(manual)); - struct sensor_combined_s sensor; - memset(&sensor, 0, sizeof(sensor)); - struct vehicle_rates_setpoint_s rates_sp; - memset(&rates_sp, 0, sizeof(rates_sp)); - struct vehicle_status_s status; - memset(&status, 0, sizeof(status)); - struct actuator_controls_s actuators; - memset(&actuators, 0, sizeof(actuators)); - - /* subscribe */ - int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude)); - int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update)); - int vehicle_attitude_setpoint_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); - int offboard_control_setpoint_sub = orb_subscribe(ORB_ID(offboard_control_setpoint)); - int vehicle_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); - int manual_control_setpoint_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); - int sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined)); - int vehicle_rates_setpoint_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint)); - int vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status)); - - /* publish actuator controls */ - for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++) { - actuators.control[i] = 0.0f; - } - - orb_advert_t actuator_pub = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &actuators); - orb_advert_t att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp); - orb_advert_t rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &rates_sp); - - /* register the perf counter */ - perf_counter_t mc_loop_perf = perf_alloc(PC_ELAPSED, "multirotor_att_control_runtime"); - perf_counter_t mc_interval_perf = perf_alloc(PC_INTERVAL, "multirotor_att_control_interval"); - perf_counter_t mc_err_perf = perf_alloc(PC_COUNT, "multirotor_att_control_err"); - - warnx("starting"); - - /* store last control mode to detect mode switches */ - bool control_yaw_position = true; - bool reset_yaw_sp = true; - - struct pollfd fds[1] = { - { .fd = vehicle_attitude_sub, .events = POLLIN }, - }; - - while (!thread_should_exit) { - - /* wait for a sensor update, check for exit condition every 500 ms */ - int ret = poll(fds, 1, 500); - - if (ret < 0) { - /* poll error, count it in perf */ - perf_count(mc_err_perf); - - } else if (ret > 0) { - /* only run controller if attitude changed */ - perf_begin(mc_loop_perf); - - /* attitude */ - orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &att); - - bool updated; - - /* parameters */ - orb_check(parameter_update_sub, &updated); - - if (updated) { - struct parameter_update_s update; - orb_copy(ORB_ID(parameter_update), parameter_update_sub, &update); - /* update parameters */ - } - - /* control mode */ - orb_check(vehicle_control_mode_sub, &updated); - - if (updated) { - orb_copy(ORB_ID(vehicle_control_mode), vehicle_control_mode_sub, &control_mode); - } - - /* manual control setpoint */ - orb_check(manual_control_setpoint_sub, &updated); - - if (updated) { - orb_copy(ORB_ID(manual_control_setpoint), manual_control_setpoint_sub, &manual); - } - - /* attitude setpoint */ - orb_check(vehicle_attitude_setpoint_sub, &updated); - - if (updated) { - orb_copy(ORB_ID(vehicle_attitude_setpoint), vehicle_attitude_setpoint_sub, &att_sp); - } - - /* offboard control setpoint */ - orb_check(offboard_control_setpoint_sub, &updated); - - if (updated) { - orb_copy(ORB_ID(offboard_control_setpoint), offboard_control_setpoint_sub, &offboard_sp); - } - - /* vehicle status */ - orb_check(vehicle_status_sub, &updated); - - if (updated) { - orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &status); - } - - /* sensors */ - orb_check(sensor_combined_sub, &updated); - - if (updated) { - orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor); - } - - /* set flag to safe value */ - control_yaw_position = true; - - /* reset yaw setpoint if not armed */ - if (!control_mode.flag_armed) { - reset_yaw_sp = true; - } - - /* define which input is the dominating control input */ - if (control_mode.flag_control_offboard_enabled) { - /* offboard inputs */ - if (offboard_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_RATES) { - rates_sp.roll = offboard_sp.p1; - rates_sp.pitch = offboard_sp.p2; - rates_sp.yaw = offboard_sp.p3; - rates_sp.thrust = offboard_sp.p4; - rates_sp.timestamp = hrt_absolute_time(); - orb_publish(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, &rates_sp); - - } else if (offboard_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE) { - att_sp.roll_body = offboard_sp.p1; - att_sp.pitch_body = offboard_sp.p2; - att_sp.yaw_body = offboard_sp.p3; - att_sp.thrust = offboard_sp.p4; - att_sp.timestamp = hrt_absolute_time(); - /* publish the result to the vehicle actuators */ - orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); - } - - /* reset yaw setpoint after offboard control */ - reset_yaw_sp = true; - - } else if (control_mode.flag_control_manual_enabled) { - /* manual input */ - if (control_mode.flag_control_attitude_enabled) { - /* control attitude, update attitude setpoint depending on mode */ - if (att_sp.thrust < 0.1f) { - /* no thrust, don't try to control yaw */ - rates_sp.yaw = 0.0f; - control_yaw_position = false; - - if (status.condition_landed) { - /* reset yaw setpoint if on ground */ - reset_yaw_sp = true; - } - - } else { - /* only move yaw setpoint if manual input is != 0 */ - if (manual.yaw < -yaw_deadzone || yaw_deadzone < manual.yaw) { - /* control yaw rate */ - control_yaw_position = false; - rates_sp.yaw = manual.yaw; - reset_yaw_sp = true; // has no effect on control, just for beautiful log - - } else { - control_yaw_position = true; - } - } - - if (!control_mode.flag_control_velocity_enabled) { - /* update attitude setpoint if not in position control mode */ - att_sp.roll_body = manual.roll; - att_sp.pitch_body = manual.pitch; - - if (!control_mode.flag_control_climb_rate_enabled) { - /* pass throttle directly if not in altitude control mode */ - att_sp.thrust = manual.throttle; - } - } - - /* reset yaw setpint to current position if needed */ - if (reset_yaw_sp) { - att_sp.yaw_body = att.yaw; - reset_yaw_sp = false; - } - - if (motor_test_mode) { - printf("testmode"); - att_sp.roll_body = 0.0f; - att_sp.pitch_body = 0.0f; - att_sp.yaw_body = 0.0f; - att_sp.thrust = 0.1f; - } - - att_sp.timestamp = hrt_absolute_time(); - - /* publish the attitude setpoint */ - orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); - - } else { - /* manual rate inputs (ACRO), from RC control or joystick */ - if (control_mode.flag_control_rates_enabled) { - rates_sp.roll = manual.roll; - rates_sp.pitch = manual.pitch; - rates_sp.yaw = manual.yaw; - rates_sp.thrust = manual.throttle; - rates_sp.timestamp = hrt_absolute_time(); - } - - /* reset yaw setpoint after ACRO */ - reset_yaw_sp = true; - } - - } else { - if (!control_mode.flag_control_attitude_enabled) { - /* no control, try to stay on place */ - if (!control_mode.flag_control_velocity_enabled) { - /* no velocity control, reset attitude setpoint */ - att_sp.roll_body = 0.0f; - att_sp.pitch_body = 0.0f; - att_sp.timestamp = hrt_absolute_time(); - orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); - } - } - - /* reset yaw setpoint after non-manual control */ - reset_yaw_sp = true; - } - - /* check if we should we reset integrals */ - bool reset_integral = !control_mode.flag_armed || att_sp.thrust < 0.1f; // TODO use landed status instead of throttle - - /* run attitude controller if needed */ - if (control_mode.flag_control_attitude_enabled) { - multirotor_control_attitude(&att_sp, &att, &rates_sp, control_yaw_position, reset_integral); - orb_publish(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, &rates_sp); - } - - /* measure in what intervals the controller runs */ - perf_count(mc_interval_perf); - - /* run rates controller if needed */ - if (control_mode.flag_control_rates_enabled) { - /* get current rate setpoint */ - bool rates_sp_updated = false; - orb_check(vehicle_rates_setpoint_sub, &rates_sp_updated); - - if (rates_sp_updated) { - orb_copy(ORB_ID(vehicle_rates_setpoint), vehicle_rates_setpoint_sub, &rates_sp); - } - - /* apply controller */ - float rates[3]; - rates[0] = att.rollspeed; - rates[1] = att.pitchspeed; - rates[2] = att.yawspeed; - multirotor_control_rates(&rates_sp, rates, &actuators, reset_integral); - - } else { - /* rates controller disabled, set actuators to zero for safety */ - actuators.control[0] = 0.0f; - actuators.control[1] = 0.0f; - actuators.control[2] = 0.0f; - actuators.control[3] = 0.0f; - } - - /* fill in manual control values */ - actuators.control[4] = manual.flaps; - actuators.control[5] = manual.aux1; - actuators.control[6] = manual.aux2; - actuators.control[7] = manual.aux3; - - actuators.timestamp = hrt_absolute_time(); - orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); - - perf_end(mc_loop_perf); - } - } - - warnx("stopping, disarming motors"); - - /* kill all outputs */ - for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++) - actuators.control[i] = 0.0f; - - orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); - - close(vehicle_attitude_sub); - close(vehicle_control_mode_sub); - close(manual_control_setpoint_sub); - close(actuator_pub); - close(att_sp_pub); - - perf_print_counter(mc_loop_perf); - perf_free(mc_loop_perf); - - fflush(stdout); - exit(0); -} - -static void -usage(const char *reason) -{ - if (reason) - fprintf(stderr, "%s\n", reason); - - fprintf(stderr, "usage: multirotor_att_control [-m ] [-t] {start|status|stop}\n"); - fprintf(stderr, " is 'rates' or 'attitude'\n"); - fprintf(stderr, " -t enables motor test mode with 10%% thrust\n"); - exit(1); -} - -int multirotor_att_control_main(int argc, char *argv[]) -{ - int ch; - unsigned int optioncount = 0; - - while ((ch = getopt(argc, argv, "tm:")) != EOF) { - switch (ch) { - case 't': - motor_test_mode = true; - optioncount += 1; - break; - - case ':': - usage("missing parameter"); - break; - - default: - fprintf(stderr, "option: -%c\n", ch); - usage("unrecognized option"); - break; - } - } - - argc -= optioncount; - //argv += optioncount; - - if (argc < 1) - usage("missing command"); - - if (!strcmp(argv[1 + optioncount], "start")) { - - thread_should_exit = false; - mc_task = task_spawn_cmd("multirotor_att_control", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 15, - 2048, - mc_thread_main, - NULL); - exit(0); - } - - if (!strcmp(argv[1 + optioncount], "stop")) { - thread_should_exit = true; - exit(0); - } - - usage("unrecognized command"); - exit(1); -} diff --git a/src/modules/multirotor_att_control/multirotor_attitude_control.c b/src/modules/multirotor_att_control/multirotor_attitude_control.c deleted file mode 100644 index b7df0433af..0000000000 --- a/src/modules/multirotor_att_control/multirotor_attitude_control.c +++ /dev/null @@ -1,254 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. - * Author: Thomas Gubler - * Julian Oes - * Laurens Mackay - * Tobias Naegeli - * Martin Rutschmann - * Lorenz Meier - * - * 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 multirotor_attitude_control.c - * - * Implementation of attitude controller for multirotors. - * - * @author Thomas Gubler - * @author Julian Oes - * @author Laurens Mackay - * @author Tobias Naegeli - * @author Martin Rutschmann - * @author Lorenz Meier - */ - -#include "multirotor_attitude_control.h" -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -PARAM_DEFINE_FLOAT(MC_YAWPOS_P, 2.0f); -PARAM_DEFINE_FLOAT(MC_YAWPOS_I, 0.15f); -PARAM_DEFINE_FLOAT(MC_YAWPOS_D, 0.0f); -//PARAM_DEFINE_FLOAT(MC_YAWPOS_AWU, 1.0f); -//PARAM_DEFINE_FLOAT(MC_YAWPOS_LIM, 3.0f); - -PARAM_DEFINE_FLOAT(MC_ATT_P, 6.8f); -PARAM_DEFINE_FLOAT(MC_ATT_I, 0.0f); -PARAM_DEFINE_FLOAT(MC_ATT_D, 0.0f); -//PARAM_DEFINE_FLOAT(MC_ATT_AWU, 0.05f); -//PARAM_DEFINE_FLOAT(MC_ATT_LIM, 0.4f); - -//PARAM_DEFINE_FLOAT(MC_ATT_XOFF, 0.0f); -//PARAM_DEFINE_FLOAT(MC_ATT_YOFF, 0.0f); - -struct mc_att_control_params { - float yaw_p; - float yaw_i; - float yaw_d; - //float yaw_awu; - //float yaw_lim; - - float att_p; - float att_i; - float att_d; - //float att_awu; - //float att_lim; - - //float att_xoff; - //float att_yoff; -}; - -struct mc_att_control_param_handles { - param_t yaw_p; - param_t yaw_i; - param_t yaw_d; - //param_t yaw_awu; - //param_t yaw_lim; - - param_t att_p; - param_t att_i; - param_t att_d; - //param_t att_awu; - //param_t att_lim; - - //param_t att_xoff; - //param_t att_yoff; -}; - -/** - * Initialize all parameter handles and values - * - */ -static int parameters_init(struct mc_att_control_param_handles *h); - -/** - * Update all parameters - * - */ -static int parameters_update(const struct mc_att_control_param_handles *h, struct mc_att_control_params *p); - - -static int parameters_init(struct mc_att_control_param_handles *h) -{ - /* PID parameters */ - h->yaw_p = param_find("MC_YAWPOS_P"); - h->yaw_i = param_find("MC_YAWPOS_I"); - h->yaw_d = param_find("MC_YAWPOS_D"); - //h->yaw_awu = param_find("MC_YAWPOS_AWU"); - //h->yaw_lim = param_find("MC_YAWPOS_LIM"); - - h->att_p = param_find("MC_ATT_P"); - h->att_i = param_find("MC_ATT_I"); - h->att_d = param_find("MC_ATT_D"); - //h->att_awu = param_find("MC_ATT_AWU"); - //h->att_lim = param_find("MC_ATT_LIM"); - - //h->att_xoff = param_find("MC_ATT_XOFF"); - //h->att_yoff = param_find("MC_ATT_YOFF"); - - return OK; -} - -static int parameters_update(const struct mc_att_control_param_handles *h, struct mc_att_control_params *p) -{ - param_get(h->yaw_p, &(p->yaw_p)); - param_get(h->yaw_i, &(p->yaw_i)); - param_get(h->yaw_d, &(p->yaw_d)); - //param_get(h->yaw_awu, &(p->yaw_awu)); - //param_get(h->yaw_lim, &(p->yaw_lim)); - - param_get(h->att_p, &(p->att_p)); - param_get(h->att_i, &(p->att_i)); - param_get(h->att_d, &(p->att_d)); - //param_get(h->att_awu, &(p->att_awu)); - //param_get(h->att_lim, &(p->att_lim)); - - //param_get(h->att_xoff, &(p->att_xoff)); - //param_get(h->att_yoff, &(p->att_yoff)); - - return OK; -} - -void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, - const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, bool control_yaw_position, bool reset_integral) -{ - static uint64_t last_run = 0; - static uint64_t last_input = 0; - float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f; - last_run = hrt_absolute_time(); - - if (last_input != att_sp->timestamp) { - last_input = att_sp->timestamp; - } - - static int motor_skip_counter = 0; - - static PID_t pitch_controller; - static PID_t roll_controller; - - static struct mc_att_control_params p; - static struct mc_att_control_param_handles h; - - static bool initialized = false; - - static float yaw_error; - - /* initialize the pid controllers when the function is called for the first time */ - if (initialized == false) { - parameters_init(&h); - parameters_update(&h, &p); - - pid_init(&pitch_controller, PID_MODE_DERIVATIV_SET, 0.0f); - pid_init(&roll_controller, PID_MODE_DERIVATIV_SET, 0.0f); - - initialized = true; - } - - /* load new parameters with lower rate */ - if (motor_skip_counter % 500 == 0) { - /* update parameters from storage */ - parameters_update(&h, &p); - - /* apply parameters */ - pid_set_parameters(&pitch_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f); - pid_set_parameters(&roll_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f); - } - - /* reset integrals if needed */ - if (reset_integral) { - pid_reset_integral(&pitch_controller); - pid_reset_integral(&roll_controller); - //TODO pid_reset_integral(&yaw_controller); - } - - /* calculate current control outputs */ - - /* control pitch (forward) output */ - rates_sp->pitch = pid_calculate(&pitch_controller, att_sp->pitch_body , - att->pitch, att->pitchspeed, deltaT); - - /* control roll (left/right) output */ - rates_sp->roll = pid_calculate(&roll_controller, att_sp->roll_body , - att->roll, att->rollspeed, deltaT); - - if (control_yaw_position) { - /* control yaw rate */ - // TODO use pid lib - - /* positive error: rotate to right, negative error, rotate to left (NED frame) */ - // yaw_error = _wrap_pi(att_sp->yaw_body - att->yaw); - - yaw_error = att_sp->yaw_body - att->yaw; - - if (yaw_error > M_PI_F) { - yaw_error -= M_TWOPI_F; - - } else if (yaw_error < -M_PI_F) { - yaw_error += M_TWOPI_F; - } - - rates_sp->yaw = p.yaw_p * (yaw_error) - (p.yaw_d * att->yawspeed); - } - - rates_sp->thrust = att_sp->thrust; - //need to update the timestamp now that we've touched rates_sp - rates_sp->timestamp = hrt_absolute_time(); - - motor_skip_counter++; -} diff --git a/src/modules/multirotor_att_control/multirotor_attitude_control.h b/src/modules/multirotor_att_control/multirotor_attitude_control.h deleted file mode 100644 index 431a435f7d..0000000000 --- a/src/modules/multirotor_att_control/multirotor_attitude_control.h +++ /dev/null @@ -1,65 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: Thomas Gubler - * Julian Oes - * Laurens Mackay - * Tobias Naegeli - * Martin Rutschmann - * Lorenz Meier - * - * 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 multirotor_attitude_control.h - * - * Definition of attitude controller for multirotors. - * - * @author Thomas Gubler - * @author Julian Oes - * @author Laurens Mackay - * @author Tobias Naegeli - * @author Martin Rutschmann - * @author Lorenz Meier - */ - -#ifndef MULTIROTOR_ATTITUDE_CONTROL_H_ -#define MULTIROTOR_ATTITUDE_CONTROL_H_ - -#include -#include -#include -#include -#include - -void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, - const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, bool control_yaw_position, bool reset_integral); - -#endif /* MULTIROTOR_ATTITUDE_CONTROL_H_ */ diff --git a/src/modules/multirotor_att_control/multirotor_rate_control.c b/src/modules/multirotor_att_control/multirotor_rate_control.c deleted file mode 100644 index eb41bf93e0..0000000000 --- a/src/modules/multirotor_att_control/multirotor_rate_control.c +++ /dev/null @@ -1,196 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012-2013 PX4 Development Team. All rights reserved. - * Author: Tobias Naegeli - * Lorenz Meier - * Anton Babushkin - * Julian Oes - * - * 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 multirotor_rate_control.c - * - * Implementation of rate controller for multirotors. - * - * @author Tobias Naegeli - * @author Lorenz Meier - * @author Anton Babushkin - * @author Julian Oes - */ - -#include "multirotor_rate_control.h" -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -PARAM_DEFINE_FLOAT(MC_YAWRATE_P, 0.3f); -PARAM_DEFINE_FLOAT(MC_YAWRATE_D, 0.005f); -PARAM_DEFINE_FLOAT(MC_YAWRATE_I, 0.2f); - -PARAM_DEFINE_FLOAT(MC_ATTRATE_P, 0.09f); -PARAM_DEFINE_FLOAT(MC_ATTRATE_D, 0.002f); -PARAM_DEFINE_FLOAT(MC_ATTRATE_I, 0.0f); - -struct mc_rate_control_params { - - float yawrate_p; - float yawrate_d; - float yawrate_i; - - float attrate_p; - float attrate_d; - float attrate_i; - - float rate_lim; -}; - -struct mc_rate_control_param_handles { - - param_t yawrate_p; - param_t yawrate_i; - param_t yawrate_d; - - param_t attrate_p; - param_t attrate_i; - param_t attrate_d; -}; - -/** - * Initialize all parameter handles and values - * - */ -static int parameters_init(struct mc_rate_control_param_handles *h); - -/** - * Update all parameters - * - */ -static int parameters_update(const struct mc_rate_control_param_handles *h, struct mc_rate_control_params *p); - - -static int parameters_init(struct mc_rate_control_param_handles *h) -{ - /* PID parameters */ - h->yawrate_p = param_find("MC_YAWRATE_P"); - h->yawrate_i = param_find("MC_YAWRATE_I"); - h->yawrate_d = param_find("MC_YAWRATE_D"); - - h->attrate_p = param_find("MC_ATTRATE_P"); - h->attrate_i = param_find("MC_ATTRATE_I"); - h->attrate_d = param_find("MC_ATTRATE_D"); - - return OK; -} - -static int parameters_update(const struct mc_rate_control_param_handles *h, struct mc_rate_control_params *p) -{ - param_get(h->yawrate_p, &(p->yawrate_p)); - param_get(h->yawrate_i, &(p->yawrate_i)); - param_get(h->yawrate_d, &(p->yawrate_d)); - - param_get(h->attrate_p, &(p->attrate_p)); - param_get(h->attrate_i, &(p->attrate_i)); - param_get(h->attrate_d, &(p->attrate_d)); - - return OK; -} - -void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, - const float rates[], struct actuator_controls_s *actuators, bool reset_integral) -{ - static uint64_t last_run = 0; - const float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f; - static uint64_t last_input = 0; - - if (last_input != rate_sp->timestamp) { - last_input = rate_sp->timestamp; - } - - last_run = hrt_absolute_time(); - - static int motor_skip_counter = 0; - - static PID_t pitch_rate_controller; - static PID_t roll_rate_controller; - static PID_t yaw_rate_controller; - - static struct mc_rate_control_params p; - static struct mc_rate_control_param_handles h; - - static bool initialized = false; - - /* initialize the pid controllers when the function is called for the first time */ - if (initialized == false) { - parameters_init(&h); - parameters_update(&h, &p); - initialized = true; - - pid_init(&pitch_rate_controller, PID_MODE_DERIVATIV_CALC_NO_SP, 0.003f); - pid_init(&roll_rate_controller, PID_MODE_DERIVATIV_CALC_NO_SP, 0.003f); - pid_init(&yaw_rate_controller, PID_MODE_DERIVATIV_CALC_NO_SP, 0.003f); - } - - /* load new parameters with lower rate */ - if (motor_skip_counter % 2500 == 0) { - /* update parameters from storage */ - parameters_update(&h, &p); - pid_set_parameters(&pitch_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f); - pid_set_parameters(&roll_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f); - pid_set_parameters(&yaw_rate_controller, p.yawrate_p, p.yawrate_i, p.yawrate_d, 1.0f, 1.0f); - } - - /* reset integrals if needed */ - if (reset_integral) { - pid_reset_integral(&pitch_rate_controller); - pid_reset_integral(&roll_rate_controller); - pid_reset_integral(&yaw_rate_controller); - } - - /* run pitch, roll and yaw controllers */ - float pitch_control = pid_calculate(&pitch_rate_controller, rate_sp->pitch, rates[1], 0.0f, deltaT); - float roll_control = pid_calculate(&roll_rate_controller, rate_sp->roll, rates[0], 0.0f, deltaT); - float yaw_control = pid_calculate(&yaw_rate_controller, rate_sp->yaw, rates[2], 0.0f, deltaT); - - actuators->control[0] = roll_control; - actuators->control[1] = pitch_control; - actuators->control[2] = yaw_control; - actuators->control[3] = rate_sp->thrust; - - motor_skip_counter++; -} diff --git a/src/modules/multirotor_att_control/multirotor_rate_control.h b/src/modules/multirotor_att_control/multirotor_rate_control.h deleted file mode 100644 index ca7794c59b..0000000000 --- a/src/modules/multirotor_att_control/multirotor_rate_control.h +++ /dev/null @@ -1,64 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012-2013 PX4 Development Team. All rights reserved. - * Author: Thomas Gubler - * Julian Oes - * Laurens Mackay - * Tobias Naegeli - * Martin Rutschmann - * Lorenz Meier - * - * 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 multirotor_attitude_control.h - * - * Definition of rate controller for multirotors. - * - * @author Thomas Gubler - * @author Julian Oes - * @author Laurens Mackay - * @author Tobias Naegeli - * @author Martin Rutschmann - * @author Lorenz Meier - */ - -#ifndef MULTIROTOR_RATE_CONTROL_H_ -#define MULTIROTOR_RATE_CONTROL_H_ - -#include -#include -#include -#include - -void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, - const float rates[], struct actuator_controls_s *actuators, bool reset_integral); - -#endif /* MULTIROTOR_RATE_CONTROL_H_ */ diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c deleted file mode 100644 index 2ca6504208..0000000000 --- a/src/modules/multirotor_pos_control/multirotor_pos_control.c +++ /dev/null @@ -1,553 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. - * Author: Anton Babushkin - * - * 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 multirotor_pos_control.c - * - * Multirotor position controller - */ - -#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 - -#include "multirotor_pos_control_params.h" -#include "thrust_pid.h" - - -static bool thread_should_exit = false; /**< Deamon exit flag */ -static bool thread_running = false; /**< Deamon status flag */ -static int deamon_task; /**< Handle of deamon task / thread */ - -__EXPORT int multirotor_pos_control_main(int argc, char *argv[]); - -/** - * Mainloop of position controller. - */ -static int multirotor_pos_control_thread_main(int argc, char *argv[]); - -/** - * Print the correct usage. - */ -static void usage(const char *reason); - -static float scale_control(float ctl, float end, float dz); - -static float norm(float x, float y); - -static void usage(const char *reason) -{ - if (reason) - fprintf(stderr, "%s\n", reason); - - fprintf(stderr, "usage: multirotor_pos_control {start|stop|status}\n\n"); - exit(1); -} - -/** - * The deamon app only briefly exists to start - * the background job. The stack size assigned in the - * Makefile does only apply to this management task. - * - * The actual stack size should be set in the call - * to task_spawn(). - */ -int multirotor_pos_control_main(int argc, char *argv[]) -{ - if (argc < 1) - usage("missing command"); - - if (!strcmp(argv[1], "start")) { - - if (thread_running) { - warnx("already running"); - /* this is not an error */ - exit(0); - } - - warnx("start"); - thread_should_exit = false; - deamon_task = task_spawn_cmd("multirotor_pos_control", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 60, - 4096, - multirotor_pos_control_thread_main, - (argv) ? (const char **)&argv[2] : (const char **)NULL); - exit(0); - } - - if (!strcmp(argv[1], "stop")) { - warnx("stop"); - thread_should_exit = true; - exit(0); - } - - if (!strcmp(argv[1], "status")) { - if (thread_running) { - warnx("app is running"); - - } else { - warnx("app not started"); - } - - exit(0); - } - - usage("unrecognized command"); - exit(1); -} - -static float scale_control(float ctl, float end, float dz) -{ - if (ctl > dz) { - return (ctl - dz) / (end - dz); - - } else if (ctl < -dz) { - return (ctl + dz) / (end - dz); - - } else { - return 0.0f; - } -} - -static float norm(float x, float y) -{ - return sqrtf(x * x + y * y); -} - -static int multirotor_pos_control_thread_main(int argc, char *argv[]) -{ - /* welcome user */ - warnx("started"); - static int mavlink_fd; - mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); - mavlink_log_info(mavlink_fd, "[mpc] started"); - - /* structures */ - struct vehicle_control_mode_s control_mode; - memset(&control_mode, 0, sizeof(control_mode)); - struct vehicle_attitude_s att; - memset(&att, 0, sizeof(att)); - struct vehicle_attitude_setpoint_s att_sp; - memset(&att_sp, 0, sizeof(att_sp)); - struct manual_control_setpoint_s manual; - memset(&manual, 0, sizeof(manual)); - struct vehicle_local_position_s local_pos; - memset(&local_pos, 0, sizeof(local_pos)); - struct mission_item_triplet_s triplet; - memset(&triplet, 0, sizeof(triplet)); - struct vehicle_global_velocity_setpoint_s global_vel_sp; - memset(&global_vel_sp, 0, sizeof(global_vel_sp)); - struct vehicle_local_position_setpoint_s local_pos_sp; - memset(&local_pos_sp, 0, sizeof(local_pos_sp)); - - /* subscribe to attitude, motor setpoints and system state */ - int param_sub = orb_subscribe(ORB_ID(parameter_update)); - int control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); - int att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); - int att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); - int manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); - int mission_triplet_sub = orb_subscribe(ORB_ID(mission_item_triplet)); - - /* publish setpoint */ - orb_advert_t local_pos_sp_pub = orb_advertise(ORB_ID(vehicle_local_position_setpoint), &local_pos_sp); - orb_advert_t global_vel_sp_pub = orb_advertise(ORB_ID(vehicle_global_velocity_setpoint), &global_vel_sp); - orb_advert_t att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp); - - bool reset_mission_sp = false; - bool global_pos_sp_valid = false; - bool reset_man_sp_z = true; - bool reset_man_sp_xy = true; - bool reset_int_z = true; - bool reset_int_z_manual = false; - bool reset_int_xy = true; - bool was_armed = false; - bool reset_auto_sp_xy = true; - bool reset_auto_sp_z = true; - bool reset_takeoff_sp = true; - - hrt_abstime t_prev = 0; - const float alt_ctl_dz = 0.2f; - const float pos_ctl_dz = 0.05f; - - float ref_alt = 0.0f; - hrt_abstime ref_alt_t = 0; - uint64_t local_ref_timestamp = 0; - - PID_t xy_pos_pids[2]; - PID_t xy_vel_pids[2]; - PID_t z_pos_pid; - thrust_pid_t z_vel_pid; - - thread_running = true; - - struct multirotor_position_control_params params; - struct multirotor_position_control_param_handles params_h; - parameters_init(¶ms_h); - parameters_update(¶ms_h, ¶ms); - - - for (int i = 0; i < 2; i++) { - pid_init(&(xy_pos_pids[i]), params.xy_p, 0.0f, params.xy_d, 1.0f, 0.0f, PID_MODE_DERIVATIV_SET, 0.02f); - pid_init(&(xy_vel_pids[i]), params.xy_vel_p, params.xy_vel_i, params.xy_vel_d, 1.0f, params.tilt_max, PID_MODE_DERIVATIV_CALC_NO_SP, 0.02f); - } - - pid_init(&z_pos_pid, params.z_p, 0.0f, params.z_d, 1.0f, params.z_vel_max, PID_MODE_DERIVATIV_SET, 0.02f); - thrust_pid_init(&z_vel_pid, params.z_vel_p, params.z_vel_i, params.z_vel_d, -params.thr_max, -params.thr_min, PID_MODE_DERIVATIV_CALC_NO_SP, 0.02f); - - while (!thread_should_exit) { - - bool param_updated; - orb_check(param_sub, ¶m_updated); - - if (param_updated) { - /* clear updated flag */ - struct parameter_update_s ps; - orb_copy(ORB_ID(parameter_update), param_sub, &ps); - /* update params */ - parameters_update(¶ms_h, ¶ms); - - for (int i = 0; i < 2; i++) { - pid_set_parameters(&(xy_pos_pids[i]), params.xy_p, 0.0f, params.xy_d, 1.0f, 0.0f); - /* use integral_limit_out = tilt_max / 2 */ - float i_limit; - - if (params.xy_vel_i > 0.0f) { - i_limit = params.tilt_max / params.xy_vel_i / 2.0f; - - } else { - i_limit = 0.0f; // not used - } - - pid_set_parameters(&(xy_vel_pids[i]), params.xy_vel_p, params.xy_vel_i, params.xy_vel_d, i_limit, params.tilt_max); - } - - pid_set_parameters(&z_pos_pid, params.z_p, 0.0f, params.z_d, 1.0f, params.z_vel_max); - thrust_pid_set_parameters(&z_vel_pid, params.z_vel_p, params.z_vel_i, params.z_vel_d, -params.thr_max, -params.thr_min); - } - - bool updated; - - orb_check(control_mode_sub, &updated); - - if (updated) { - orb_copy(ORB_ID(vehicle_control_mode), control_mode_sub, &control_mode); - } - - orb_check(mission_triplet_sub, &updated); - - if (updated) { - orb_copy(ORB_ID(mission_item_triplet), mission_triplet_sub, &triplet); - global_pos_sp_valid = triplet.current_valid; - reset_mission_sp = true; - } - - hrt_abstime t = hrt_absolute_time(); - float dt; - - if (t_prev != 0) { - dt = (t - t_prev) * 0.000001f; - - } else { - dt = 0.0f; - } - - if (control_mode.flag_armed && !was_armed) { - /* reset setpoints and integrals on arming */ - reset_man_sp_z = true; - reset_man_sp_xy = true; - reset_auto_sp_z = true; - reset_auto_sp_xy = true; - reset_takeoff_sp = true; - reset_int_z = true; - reset_int_xy = true; - } - - was_armed = control_mode.flag_armed; - - t_prev = t; - - if (control_mode.flag_control_altitude_enabled || control_mode.flag_control_velocity_enabled || control_mode.flag_control_position_enabled) { - orb_copy(ORB_ID(manual_control_setpoint), manual_sub, &manual); - orb_copy(ORB_ID(vehicle_attitude), att_sub, &att); - orb_copy(ORB_ID(vehicle_attitude_setpoint), att_sp_sub, &att_sp); - - float z_sp_offs_max = params.z_vel_max / params.z_p * 2.0f; - float xy_sp_offs_max = params.xy_vel_max / params.xy_p * 2.0f; - float sp_move_rate[3] = { 0.0f, 0.0f, 0.0f }; - - if (control_mode.flag_control_manual_enabled) { - /* manual control */ - /* check for reference point updates and correct setpoint */ - if (local_pos.ref_timestamp != ref_alt_t) { - if (ref_alt_t != 0) { - /* home alt changed, don't follow large ground level changes in manual flight */ - local_pos_sp.z += local_pos.ref_alt - ref_alt; - } - - ref_alt_t = local_pos.ref_timestamp; - ref_alt = local_pos.ref_alt; - // TODO also correct XY setpoint - } - - /* reset setpoints to current position if needed */ - if (control_mode.flag_control_altitude_enabled) { - if (reset_man_sp_z) { - reset_man_sp_z = false; - local_pos_sp.z = local_pos.z; - mavlink_log_info(mavlink_fd, "[mpc] reset alt sp: %.2f", (double) - local_pos_sp.z); - } - - /* move altitude setpoint with throttle stick */ - float z_sp_ctl = scale_control(manual.throttle - 0.5f, 0.5f, alt_ctl_dz); - - if (z_sp_ctl != 0.0f) { - sp_move_rate[2] = -z_sp_ctl * params.z_vel_max; - local_pos_sp.z += sp_move_rate[2] * dt; - - if (local_pos_sp.z > local_pos.z + z_sp_offs_max) { - local_pos_sp.z = local_pos.z + z_sp_offs_max; - - } else if (local_pos_sp.z < local_pos.z - z_sp_offs_max) { - local_pos_sp.z = local_pos.z - z_sp_offs_max; - } - } - } - - if (control_mode.flag_control_position_enabled) { - if (reset_man_sp_xy) { - reset_man_sp_xy = false; - local_pos_sp.x = local_pos.x; - local_pos_sp.y = local_pos.y; - pid_reset_integral(&xy_vel_pids[0]); - pid_reset_integral(&xy_vel_pids[1]); - mavlink_log_info(mavlink_fd, "[mpc] reset pos sp: %.2f, %.2f", (double)local_pos_sp.x, (double)local_pos_sp.y); - } - - /* move position setpoint with roll/pitch stick */ - float pos_pitch_sp_ctl = scale_control(-manual.pitch / params.rc_scale_pitch, 1.0f, pos_ctl_dz); - float pos_roll_sp_ctl = scale_control(manual.roll / params.rc_scale_roll, 1.0f, pos_ctl_dz); - - if (pos_pitch_sp_ctl != 0.0f || pos_roll_sp_ctl != 0.0f) { - /* calculate direction and increment of control in NED frame */ - float xy_sp_ctl_dir = att.yaw + atan2f(pos_roll_sp_ctl, pos_pitch_sp_ctl); - float xy_sp_ctl_speed = norm(pos_pitch_sp_ctl, pos_roll_sp_ctl) * params.xy_vel_max; - sp_move_rate[0] = cosf(xy_sp_ctl_dir) * xy_sp_ctl_speed; - sp_move_rate[1] = sinf(xy_sp_ctl_dir) * xy_sp_ctl_speed; - local_pos_sp.x += sp_move_rate[0] * dt; - local_pos_sp.y += sp_move_rate[1] * dt; - /* limit maximum setpoint from position offset and preserve direction - * fail safe, should not happen in normal operation */ - float pos_vec_x = local_pos_sp.x - local_pos.x; - float pos_vec_y = local_pos_sp.y - local_pos.y; - float pos_vec_norm = norm(pos_vec_x, pos_vec_y) / xy_sp_offs_max; - - if (pos_vec_norm > 1.0f) { - local_pos_sp.x = local_pos.x + pos_vec_x / pos_vec_norm; - local_pos_sp.y = local_pos.y + pos_vec_y / pos_vec_norm; - } - } - } - - /* copy yaw setpoint to vehicle_local_position_setpoint topic */ - local_pos_sp.yaw = att_sp.yaw_body; - - /* local position setpoint is valid and can be used for auto loiter after position controlled mode */ - reset_auto_sp_xy = !control_mode.flag_control_position_enabled; - reset_auto_sp_z = !control_mode.flag_control_altitude_enabled; - reset_takeoff_sp = true; - - /* force reprojection of global setpoint after manual mode */ - reset_mission_sp = true; - } - /* AUTO not implemented */ - - /* publish local position setpoint */ - orb_publish(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_pub, &local_pos_sp); - - /* run position & altitude controllers, calculate velocity setpoint */ - if (control_mode.flag_control_altitude_enabled) { - global_vel_sp.vz = pid_calculate(&z_pos_pid, local_pos_sp.z, local_pos.z, local_pos.vz - sp_move_rate[2], dt) + sp_move_rate[2]; - - } else { - reset_man_sp_z = true; - global_vel_sp.vz = 0.0f; - } - - if (control_mode.flag_control_position_enabled) { - /* calculate velocity set point in NED frame */ - global_vel_sp.vx = pid_calculate(&xy_pos_pids[0], local_pos_sp.x, local_pos.x, local_pos.vx - sp_move_rate[0], dt) + sp_move_rate[0]; - global_vel_sp.vy = pid_calculate(&xy_pos_pids[1], local_pos_sp.y, local_pos.y, local_pos.vy - sp_move_rate[1], dt) + sp_move_rate[1]; - - /* limit horizontal speed */ - float xy_vel_sp_norm = norm(global_vel_sp.vx, global_vel_sp.vy) / params.xy_vel_max; - - if (xy_vel_sp_norm > 1.0f) { - global_vel_sp.vx /= xy_vel_sp_norm; - global_vel_sp.vy /= xy_vel_sp_norm; - } - - } else { - reset_man_sp_xy = true; - global_vel_sp.vx = 0.0f; - global_vel_sp.vy = 0.0f; - } - - /* publish new velocity setpoint */ - orb_publish(ORB_ID(vehicle_global_velocity_setpoint), global_vel_sp_pub, &global_vel_sp); - // TODO subscribe to velocity setpoint if altitude/position control disabled - - if (control_mode.flag_control_climb_rate_enabled || control_mode.flag_control_velocity_enabled) { - /* run velocity controllers, calculate thrust vector with attitude-thrust compensation */ - float thrust_sp[3] = { 0.0f, 0.0f, 0.0f }; - - if (control_mode.flag_control_climb_rate_enabled) { - if (reset_int_z) { - reset_int_z = false; - float i = params.thr_min; - - if (reset_int_z_manual) { - i = manual.throttle; - - if (i < params.thr_min) { - i = params.thr_min; - - } else if (i > params.thr_max) { - i = params.thr_max; - } - } - - thrust_pid_set_integral(&z_vel_pid, -i); - mavlink_log_info(mavlink_fd, "[mpc] reset hovering thrust: %.2f", (double)i); - } - - thrust_sp[2] = thrust_pid_calculate(&z_vel_pid, global_vel_sp.vz, local_pos.vz, dt, att.R[2][2]); - att_sp.thrust = -thrust_sp[2]; - - } else { - /* reset thrust integral when altitude control enabled */ - reset_int_z = true; - } - - if (control_mode.flag_control_velocity_enabled) { - /* calculate thrust set point in NED frame */ - if (reset_int_xy) { - reset_int_xy = false; - pid_reset_integral(&xy_vel_pids[0]); - pid_reset_integral(&xy_vel_pids[1]); - mavlink_log_info(mavlink_fd, "[mpc] reset pos integral"); - } - - thrust_sp[0] = pid_calculate(&xy_vel_pids[0], global_vel_sp.vx, local_pos.vx, 0.0f, dt); - thrust_sp[1] = pid_calculate(&xy_vel_pids[1], global_vel_sp.vy, local_pos.vy, 0.0f, dt); - - /* thrust_vector now contains desired acceleration (but not in m/s^2) in NED frame */ - /* limit horizontal part of thrust */ - float thrust_xy_dir = atan2f(thrust_sp[1], thrust_sp[0]); - /* assuming that vertical component of thrust is g, - * horizontal component = g * tan(alpha) */ - float tilt = atanf(norm(thrust_sp[0], thrust_sp[1])); - - if (tilt > params.tilt_max) { - tilt = params.tilt_max; - } - - /* convert direction to body frame */ - thrust_xy_dir -= att.yaw; - /* calculate roll and pitch */ - att_sp.roll_body = sinf(thrust_xy_dir) * tilt; - att_sp.pitch_body = -cosf(thrust_xy_dir) * tilt / cosf(att_sp.roll_body); - - } else { - reset_int_xy = true; - } - - att_sp.timestamp = hrt_absolute_time(); - - /* publish new attitude setpoint */ - orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); - } - - } else { - /* position controller disabled, reset setpoints */ - reset_man_sp_z = true; - reset_man_sp_xy = true; - reset_int_z = true; - reset_int_xy = true; - reset_mission_sp = true; - reset_auto_sp_xy = true; - reset_auto_sp_z = true; - } - - /* reset altitude controller integral (hovering throttle) to manual throttle after manual throttle control */ - reset_int_z_manual = control_mode.flag_armed && control_mode.flag_control_manual_enabled && !control_mode.flag_control_climb_rate_enabled; - - /* run at approximately 50 Hz */ - usleep(20000); - } - - warnx("stopped"); - mavlink_log_info(mavlink_fd, "[mpc] stopped"); - - thread_running = false; - - fflush(stdout); - return 0; -} - From 33385a783cec5045a910e4890fa8c8f4b2fc7641 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 30 Dec 2013 14:04:24 +0400 Subject: [PATCH 261/447] Added NONE = not mapped state for mission and return switches --- src/modules/commander/commander.cpp | 8 ++++---- src/modules/sensors/sensors.cpp | 24 ++++++++++++------------ src/modules/uORB/topics/vehicle_status.h | 2 ++ 3 files changed, 18 insertions(+), 16 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 98979df3e1..03d3c02d15 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1392,7 +1392,7 @@ check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_sta current_status->mode_switch = MODE_SWITCH_ASSISTED; } - /* land switch */ + /* return switch */ if (!isfinite(sp_man->return_switch)) { current_status->return_switch = RETURN_SWITCH_NONE; @@ -1400,7 +1400,7 @@ check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_sta current_status->return_switch = RETURN_SWITCH_RETURN; } else { - current_status->return_switch = RETURN_SWITCH_NONE; + current_status->return_switch = RETURN_SWITCH_NORMAL; } /* assisted switch */ @@ -1416,10 +1416,10 @@ check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_sta /* mission switch */ if (!isfinite(sp_man->mission_switch)) { - current_status->mission_switch = MISSION_SWITCH_MISSION; + current_status->mission_switch = MISSION_SWITCH_NONE; } else if (sp_man->mission_switch > STICK_ON_OFF_LIMIT) { - current_status->mission_switch = MISSION_SWITCH_NONE; + current_status->mission_switch = MISSION_SWITCH_LOITER; } else { current_status->mission_switch = MISSION_SWITCH_MISSION; diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index d9f037c271..9baf1a6aff 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -1372,18 +1372,6 @@ Sensors::rc_poll() manual_control.yaw *= _parameters.rc_scale_yaw; } - /* mode switch input */ - manual_control.mode_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MODE]].scaled); - - /* land switch input */ - manual_control.return_switch = limit_minus_one_to_one(_rc.chan[_rc.function[RETURN]].scaled); - - /* assisted switch input */ - manual_control.assisted_switch = limit_minus_one_to_one(_rc.chan[_rc.function[ASSISTED]].scaled); - - /* mission switch input */ - manual_control.mission_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MISSION]].scaled); - /* flaps */ if (_rc.function[FLAPS] >= 0) { @@ -1394,14 +1382,26 @@ Sensors::rc_poll() } } + /* mode switch input */ if (_rc.function[MODE] >= 0) { manual_control.mode_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MODE]].scaled); } + /* assisted switch input */ + if (_rc.function[ASSISTED] >= 0) { + manual_control.assisted_switch = limit_minus_one_to_one(_rc.chan[_rc.function[ASSISTED]].scaled); + } + + /* mission switch input */ if (_rc.function[MISSION] >= 0) { manual_control.mission_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MISSION]].scaled); } + /* return switch input */ + if (_rc.function[RETURN] >= 0) { + manual_control.return_switch = limit_minus_one_to_one(_rc.chan[_rc.function[RETURN]].scaled); + } + // if (_rc.function[OFFBOARD_MODE] >= 0) { // manual_control.auto_offboard_input_switch = limit_minus_one_to_one(_rc.chan[_rc.function[OFFBOARD_MODE]].scaled); // } diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index ae3a24a1b7..1a9dec5f51 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -99,11 +99,13 @@ typedef enum { typedef enum { RETURN_SWITCH_NONE = 0, + RETURN_SWITCH_NORMAL, RETURN_SWITCH_RETURN } return_switch_pos_t; typedef enum { MISSION_SWITCH_NONE = 0, + MISSION_SWITCH_LOITER, MISSION_SWITCH_MISSION } mission_switch_pos_t; From c35c0a90d32e6a8003a136516b33056c9021ff32 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 30 Dec 2013 14:04:55 +0400 Subject: [PATCH 262/447] navigator: use switches to select nav state --- src/modules/navigator/navigator_main.cpp | 106 ++++++++++++++++------- 1 file changed, 73 insertions(+), 33 deletions(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 0a0ee25414..4176cee1b3 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -526,6 +526,8 @@ Navigator::task_main() /* rate limit position updates to 50 Hz */ orb_set_interval(_global_pos_sub, 20); + unsigned prevState = 0; + /* wakeup source(s) */ struct pollfd fds[7]; @@ -545,7 +547,6 @@ Navigator::task_main() fds[6].fd = _vstatus_sub; fds[6].events = POLLIN; - while (!_task_should_exit) { /* wait for up to 100ms for data */ @@ -571,41 +572,75 @@ Navigator::task_main() /* Evaluate state machine from commander and set the navigator mode accordingly */ if (_vstatus.main_state == MAIN_STATE_AUTO) { - if (_vstatus.set_nav_state_timestamp != _set_nav_state_timestamp) { - /* commander requested new navigation mode, try to set it */ - _set_nav_state_timestamp = _vstatus.set_nav_state_timestamp; - - switch (_vstatus.set_nav_state) { - case NAV_STATE_INIT: - case NAV_STATE_NONE: - /* nothing to do */ - break; - - case NAV_STATE_LOITER: - dispatch(EVENT_LOITER_REQUESTED); - break; - - case NAV_STATE_MISSION: - dispatch(EVENT_MISSION_REQUESTED); - break; - - case NAV_STATE_RTL: + bool stick_mode = false; + if (!_vstatus.rc_signal_lost) { + /* RC signal available, use control switches to set mode */ + /* RETURN switch, overrides MISSION switch */ + if (_vstatus.return_switch == RETURN_SWITCH_RETURN) { dispatch(EVENT_RTL_REQUESTED); - break; - - default: - warnx("ERROR: Requested navigation state not supported"); - break; - } - - } else { - /* try mission, if none is available fallback to loiter instead */ - if (_mission.current_mission_available()) { - dispatch(EVENT_MISSION_REQUESTED); + stick_mode = true; } else { - dispatch(EVENT_LOITER_REQUESTED); + /* MISSION switch */ + if (!stick_mode) { + if (_vstatus.mission_switch == MISSION_SWITCH_LOITER) { + dispatch(EVENT_LOITER_REQUESTED); + stick_mode = true; + } else if (_vstatus.mission_switch == MISSION_SWITCH_MISSION) { + /* switch to mission only if available */ + if (_mission.current_mission_available()) { + dispatch(EVENT_MISSION_REQUESTED); + } else { + dispatch(EVENT_LOITER_REQUESTED); + } + stick_mode = true; + } + } + if (!stick_mode && _vstatus.return_switch == RETURN_SWITCH_NORMAL && myState == NAV_STATE_RTL) { + /* RETURN switch is in normal mode, no MISSION switch mapped, interrupt if in RTL state */ + dispatch(EVENT_LOITER_REQUESTED); + stick_mode = true; + } + } + } + + if (!stick_mode) { + if (_vstatus.set_nav_state_timestamp != _set_nav_state_timestamp) { + /* commander requested new navigation mode, try to set it */ + _set_nav_state_timestamp = _vstatus.set_nav_state_timestamp; + + switch (_vstatus.set_nav_state) { + case NAV_STATE_INIT: + case NAV_STATE_NONE: + /* nothing to do */ + break; + + case NAV_STATE_LOITER: + dispatch(EVENT_LOITER_REQUESTED); + break; + + case NAV_STATE_MISSION: + dispatch(EVENT_MISSION_REQUESTED); + break; + + case NAV_STATE_RTL: + dispatch(EVENT_RTL_REQUESTED); + break; + + default: + warnx("ERROR: Requested navigation state not supported"); + break; + } + + } else { + /* on first switch to AUTO try mission, if none is available fallback to loiter instead */ + if (myState == NAV_STATE_INIT || myState == NAV_STATE_NONE) { + if (_mission.current_mission_available()) { + dispatch(EVENT_MISSION_REQUESTED); + } else { + dispatch(EVENT_LOITER_REQUESTED); + } + } } - break; } } else { @@ -673,6 +708,11 @@ Navigator::task_main() } } + if (myState != prevState) { + mavlink_log_info(_mavlink_fd, "[navigator] nav state %d -> %d", prevState, myState); + prevState = myState; + } + publish_control_mode(); perf_end(_loop_perf); From 35e62a13f8b34447ed9abddd9fa349d392674a95 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 30 Dec 2013 14:05:38 +0400 Subject: [PATCH 263/447] Use new controllers in logging build --- makefiles/config_px4fmu-v2_logging.mk | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/makefiles/config_px4fmu-v2_logging.mk b/makefiles/config_px4fmu-v2_logging.mk index 4e3cae5eec..0986b3feed 100644 --- a/makefiles/config_px4fmu-v2_logging.mk +++ b/makefiles/config_px4fmu-v2_logging.mk @@ -83,8 +83,8 @@ MODULES += examples/flow_position_estimator #MODULES += modules/segway # XXX Needs GCC 4.7 fix MODULES += modules/fw_pos_control_l1 MODULES += modules/fw_att_control -MODULES += modules/multirotor_att_control -MODULES += modules/multirotor_pos_control +MODULES += modules/mc_att_control_vector +MODULES += modules/mc_pos_control # # Logging From 9cc1fc1cb59255c1390f058f03918b0d105c2d26 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 30 Dec 2013 19:08:09 +0100 Subject: [PATCH 264/447] fixed launchdetection logic, catapult tested in HIL --- .../launchdetection/CatapultLaunchMethod.cpp | 5 ++ .../fw_pos_control_l1_main.cpp | 67 ++++++++++++------- 2 files changed, 48 insertions(+), 24 deletions(-) diff --git a/src/lib/launchdetection/CatapultLaunchMethod.cpp b/src/lib/launchdetection/CatapultLaunchMethod.cpp index 0a95b46f60..d5c759b17b 100644 --- a/src/lib/launchdetection/CatapultLaunchMethod.cpp +++ b/src/lib/launchdetection/CatapultLaunchMethod.cpp @@ -39,6 +39,7 @@ */ #include "CatapultLaunchMethod.h" +#include CatapultLaunchMethod::CatapultLaunchMethod() : last_timestamp(0), @@ -61,11 +62,15 @@ void CatapultLaunchMethod::update(float accel_x) if (accel_x > threshold_accel.get()) { integrator += accel_x * dt; +// warnx("*** integrator %.3f, threshold_accel %.3f, threshold_time %.3f, accel_x %.3f, dt %.3f", +// (double)integrator, (double)threshold_accel.get(), (double)threshold_time.get(), (double)accel_x, (double)dt); if (integrator > threshold_accel.get() * threshold_time.get()) { launchDetected = true; } } else { +// warnx("integrator %.3f, threshold_accel %.3f, threshold_time %.3f, accel_x %.3f, dt %.3f", +// (double)integrator, (double)threshold_accel.get(), (double)threshold_time.get(), (double)accel_x, (double)dt); /* reset integrator */ integrator = 0.0f; launchDetected = false; diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 26f6768ccf..5056bcdc13 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -76,6 +76,7 @@ #include #include #include +#include #include #include #include @@ -132,7 +133,7 @@ private: int _control_mode_sub; /**< vehicle status subscription */ int _params_sub; /**< notification of parameter updates */ int _manual_control_sub; /**< notification of manual control updates */ - int _accel_sub; /**< body frame accelerations */ + int _sensor_combined_sub; /**< for body frame accelerations */ orb_advert_t _attitude_sp_pub; /**< attitude setpoint */ orb_advert_t _nav_capabilities_pub; /**< navigation capabilities publication */ @@ -145,7 +146,7 @@ private: struct vehicle_control_mode_s _control_mode; /**< vehicle status */ struct vehicle_global_position_s _global_pos; /**< global vehicle position */ struct mission_item_triplet_s _mission_item_triplet; /**< triplet of mission items */ - struct accel_report _accel; /**< body frame accelerations */ + struct sensor_combined_s _sensor_combined; /**< for body frame accelerations */ perf_counter_t _loop_perf; /**< loop performance counter */ @@ -171,6 +172,10 @@ private: bool land_motor_lim; bool land_onslope; + /* takeoff/launch states */ + bool launch_detected; + bool launch_detection_message_sent; + /* Landingslope object */ Landingslope landingslope; @@ -311,7 +316,7 @@ private: /** * Check for accel updates. */ - void vehicle_accel_poll(); + void vehicle_sensor_combined_poll(); /** * Check for set triplet updates. @@ -389,7 +394,9 @@ FixedwingPositionControl::FixedwingPositionControl() : land_onslope(false), flare_curve_alt_last(0.0f), _mavlink_fd(-1), - launchDetector() + launchDetector(), + launch_detected(false), + launch_detection_message_sent(false) { /* safely initialize structs */ vehicle_attitude_s _att = {0}; @@ -400,7 +407,7 @@ FixedwingPositionControl::FixedwingPositionControl() : vehicle_control_mode_s _control_mode = {0}; vehicle_global_position_s _global_pos = {0}; mission_item_triplet_s _mission_item_triplet = {0}; - accel_report _accel = {0}; + sensor_combined_s _sensor_combined = {0}; @@ -631,14 +638,14 @@ FixedwingPositionControl::vehicle_attitude_poll() } void -FixedwingPositionControl::vehicle_accel_poll() +FixedwingPositionControl::vehicle_sensor_combined_poll() { /* check if there is a new position */ - bool accel_updated; - orb_check(_accel_sub, &accel_updated); + bool sensors_updated; + orb_check(_sensor_combined_sub, &sensors_updated); - if (accel_updated) { - orb_copy(ORB_ID(sensor_accel), _accel_sub, &_accel); + if (sensors_updated) { + orb_copy(ORB_ID(sensor_combined), _sensor_combined_sub, &_sensor_combined); } } @@ -756,7 +763,7 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio float baro_altitude = _global_pos.alt; /* filter speed and altitude for controller */ - math::Vector3 accel_body(_accel.x, _accel.y, _accel.z); + math::Vector3 accel_body(_sensor_combined.accelerometer_m_s2[0], _sensor_combined.accelerometer_m_s2[1], _sensor_combined.accelerometer_m_s2[2]); math::Vector3 accel_earth = _R_nb.transpose() * accel_body; _tecs.update_50hz(baro_altitude, _airspeed.indicated_airspeed_m_s, _R_nb, accel_body, accel_earth); @@ -973,24 +980,30 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio } else if (mission_item_triplet.current.nav_cmd == NAV_CMD_TAKEOFF) { /* Perform launch detection */ - bool do_fly_takeoff = false; - warnx("Launch detection running"); - if (launchDetector.launchDetectionEnabled()) { - launchDetector.update(_accel.x); - if (launchDetector.getLaunchDetected()) { - do_fly_takeoff = true; - warnx("Launch detected. Taking off!"); +// warnx("Launch detection running"); + if(!launch_detected) { //do not do further checks once a launch was detected + if (launchDetector.launchDetectionEnabled()) { +// warnx("Launch detection enabled"); + if(!launch_detection_message_sent) { + mavlink_log_info(_mavlink_fd, "#audio: Launchdetection running"); + launch_detection_message_sent = true; + } + launchDetector.update(_sensor_combined.accelerometer_m_s2[0]); + if (launchDetector.getLaunchDetected()) { + launch_detected = true; + warnx("Launch detected. Taking off!"); + } + } else { + /* no takeoff detection --> fly */ + launch_detected = true; } - } else { - /* no takeoff detection --> fly */ - do_fly_takeoff = true; } _l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed); _att_sp.roll_body = _l1_control.nav_roll(); _att_sp.yaw_body = _l1_control.nav_bearing(); - if (do_fly_takeoff) { + if (launch_detected) { /* apply minimum pitch and limit roll if target altitude is not within 10 meters */ if (altitude_error > 15.0f) { @@ -1037,6 +1050,12 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio land_onslope = false; } + /* reset takeoff/launch state */ + if (mission_item_triplet.current.nav_cmd != NAV_CMD_TAKEOFF) { + launch_detected = false; + launch_detection_message_sent = false; + } + if (was_circle_mode && !_l1_control.circle_mode()) { /* just kicked out of loiter, reset roll integrals */ _att_sp.roll_reset_integral = true; @@ -1151,7 +1170,7 @@ FixedwingPositionControl::task_main() _global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); _mission_item_triplet_sub = orb_subscribe(ORB_ID(mission_item_triplet)); _att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); - _accel_sub = orb_subscribe(ORB_ID(sensor_accel)); + _sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined)); _control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); _airspeed_sub = orb_subscribe(ORB_ID(airspeed)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); @@ -1233,7 +1252,7 @@ FixedwingPositionControl::task_main() vehicle_attitude_poll(); vehicle_setpoint_poll(); - vehicle_accel_poll(); + vehicle_sensor_combined_poll(); vehicle_airspeed_poll(); // vehicle_baro_poll(); From 080ecf56caac571f905f657875cec9fa8e7e17d0 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 30 Dec 2013 22:21:53 +0100 Subject: [PATCH 265/447] launchdetection: add mavlink text output --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 5056bcdc13..455fe36742 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -991,7 +991,7 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio launchDetector.update(_sensor_combined.accelerometer_m_s2[0]); if (launchDetector.getLaunchDetected()) { launch_detected = true; - warnx("Launch detected. Taking off!"); + mavlink_log_info(_mavlink_fd, "#audio: Takeoff"); } } else { /* no takeoff detection --> fly */ From d35a1699078b6cb8a41d58bdbbd09164e5c0adf2 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 31 Dec 2013 12:46:35 +0400 Subject: [PATCH 266/447] navigator: NAV_STATE_INIT removed, minor fixes --- src/modules/commander/commander.cpp | 2 +- src/modules/navigator/navigator_main.cpp | 103 +++++++++--------- src/modules/uORB/topics/home_position.h | 2 +- .../uORB/topics/vehicle_control_mode.h | 3 +- 4 files changed, 52 insertions(+), 58 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 03d3c02d15..1e53181217 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -605,7 +605,7 @@ int commander_thread_main(int argc, char *argv[]) memset(&armed, 0, sizeof(armed)); status.main_state = MAIN_STATE_MANUAL; - status.set_nav_state = NAV_STATE_INIT; + status.set_nav_state = NAV_STATE_NONE; status.set_nav_state_timestamp = 0; status.arming_state = ARMING_STATE_INIT; status.hil_state = HIL_STATE_OFF; diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 4176cee1b3..4288873735 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -375,7 +375,8 @@ Navigator::Navigator() : memset(&_mission_result, 0, sizeof(struct mission_result_s)); /* Initialize state machine */ - myState = NAV_STATE_INIT; + myState = NAV_STATE_NONE; + start_none(); } Navigator::~Navigator() @@ -526,7 +527,10 @@ Navigator::task_main() /* rate limit position updates to 50 Hz */ orb_set_interval(_global_pos_sub, 20); - unsigned prevState = 0; + unsigned prevState = NAV_STATE_NONE; + bool pub_control_mode = true; + hrt_abstime mavlink_open_time = 0; + const hrt_abstime mavlink_open_period = 500000000; /* wakeup source(s) */ struct pollfd fds[7]; @@ -565,10 +569,16 @@ Navigator::task_main() perf_begin(_loop_perf); - /* only update vehicle status if it changed */ + if (_mavlink_fd < 0 && hrt_absolute_time() > mavlink_open_time + mavlink_open_period) { + /* try to open the mavlink log device every once in a while */ + mavlink_open_time = hrt_abstime(); + _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); + } + + /* vehicle status updated */ if (fds[6].revents & POLLIN) { - vehicle_status_update(); + pub_control_mode = true; /* Evaluate state machine from commander and set the navigator mode accordingly */ if (_vstatus.main_state == MAIN_STATE_AUTO) { @@ -609,7 +619,6 @@ Navigator::task_main() _set_nav_state_timestamp = _vstatus.set_nav_state_timestamp; switch (_vstatus.set_nav_state) { - case NAV_STATE_INIT: case NAV_STATE_NONE: /* nothing to do */ break; @@ -632,8 +641,8 @@ Navigator::task_main() } } else { - /* on first switch to AUTO try mission, if none is available fallback to loiter instead */ - if (myState == NAV_STATE_INIT || myState == NAV_STATE_NONE) { + /* on first switch to AUTO try mission by default, if none is available fallback to loiter */ + if (myState == NAV_STATE_NONE) { if (_mission.current_mission_available()) { dispatch(EVENT_MISSION_REQUESTED); } else { @@ -647,52 +656,46 @@ Navigator::task_main() /* not in AUTO */ dispatch(EVENT_NONE_REQUESTED); } - - /* XXX Hack to get mavlink output going, try opening the fd with 5Hz */ - if (_mavlink_fd < 0) { - /* try to open the mavlink log device every once in a while */ - _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); - } } - /* only update parameters if it changed */ + /* parameters updated */ if (fds[0].revents & POLLIN) { parameters_update(); /* note that these new parameters won't be in effect until a mission triplet is published again */ } - /* only update craft capabilities if they have changed */ + /* navigation capabilities updated */ if (fds[3].revents & POLLIN) { navigation_capabilities_update(); } + /* offboard mission updated */ if (fds[4].revents & POLLIN) { offboard_mission_update(_vstatus.is_rotary_wing); // XXX check if mission really changed dispatch(EVENT_MISSION_CHANGED); } + /* onboard mission updated */ if (fds[5].revents & POLLIN) { onboard_mission_update(); // XXX check if mission really changed dispatch(EVENT_MISSION_CHANGED); } + /* home position updated */ if (fds[2].revents & POLLIN) { home_position_update(); // XXX check if home position really changed dispatch(EVENT_HOME_POSITION_CHANGED); } - /* only run controller if position changed */ + /* global position updated */ if (fds[1].revents & POLLIN) { global_position_update(); /* only check if waypoint has been reached in Mission or RTL mode */ - if (mission_item_reached()) { - - if (_vstatus.main_state == MAIN_STATE_AUTO && - (myState == NAV_STATE_MISSION)) { - + if (myState == NAV_STATE_MISSION || myState == NAV_STATE_RTL) { + if (mission_item_reached()) { /* advance by one mission item */ _mission.move_to_next(); @@ -702,18 +705,22 @@ Navigator::task_main() } else { dispatch(EVENT_MISSION_FINISHED); } - } else { - dispatch(EVENT_MISSION_FINISHED); } } } + + /* notify user about state changes */ if (myState != prevState) { mavlink_log_info(_mavlink_fd, "[navigator] nav state %d -> %d", prevState, myState); prevState = myState; + pub_control_mode = true; } - publish_control_mode(); + /* publish control mode if updated */ + if (pub_control_mode) { + publish_control_mode(); + } perf_end(_loop_perf); } @@ -767,9 +774,6 @@ Navigator::status() } switch (myState) { - case NAV_STATE_INIT: - warnx("State: Init"); - break; case NAV_STATE_NONE: warnx("State: None"); break; @@ -885,16 +889,6 @@ Navigator::fence_point(int argc, char *argv[]) StateTable::Tran const Navigator::myTable[NAV_STATE_MAX][MAX_EVENT] = { - { - /* STATE_INIT */ - /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE}, - /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER}, - /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, - /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, - /* EVENT_MISSION_FINISHED */ {NO_ACTION, NAV_STATE_INIT}, - /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_INIT}, - /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_INIT}, - }, { /* STATE_NONE */ /* EVENT_NONE_REQUESTED */ {NO_ACTION, NAV_STATE_NONE}, @@ -976,19 +970,21 @@ Navigator::start_loiter() _mission_item_triplet.current.lat = (double)_global_pos.lat / 1e7d; _mission_item_triplet.current.lon = (double)_global_pos.lon / 1e7d; - _mission_item_triplet.current.yaw = 0.0f; // TODO use current yaw sp here or set to undefined? + _mission_item_triplet.current.yaw = NAN; // NAN means to use current yaw get_loiter_item(&_mission_item_triplet.current); - float global_min_alt = _parameters.min_altitude + _home_pos.altitude; + // TODO use relative altitude to allow flying without global reference (?) + _mission_item_triplet.current.altitude_is_relative = false; + float min_alt_amsl = _parameters.min_altitude + _home_pos.altitude; /* Use current altitude if above min altitude set by parameter */ - if (_global_pos.alt < global_min_alt) { - _mission_item_triplet.current.altitude = global_min_alt; - mavlink_log_info(_mavlink_fd, "[navigator] loiter %.1fm higher", (double)(global_min_alt - _global_pos.alt)); + if (_global_pos.alt < min_alt_amsl) { + _mission_item_triplet.current.altitude = min_alt_amsl; + mavlink_log_info(_mavlink_fd, "[navigator] loiter %.1fm higher", (double)(min_alt_amsl - _global_pos.alt)); } else { _mission_item_triplet.current.altitude = _global_pos.alt; - mavlink_log_info(_mavlink_fd, "[navigator] loiter here"); + mavlink_log_info(_mavlink_fd, "[navigator] loiter at current altitude"); } publish_mission_item_triplet(); @@ -998,7 +994,7 @@ Navigator::start_loiter() void Navigator::start_mission() { - /* leave previous mission item as isas is */ + /* leave previous mission item as is as is */ int ret; bool onboard; @@ -1108,7 +1104,7 @@ Navigator::start_rtl() _mission_item_triplet.current.lat = _home_pos.lat; _mission_item_triplet.current.lon = _home_pos.lon; _mission_item_triplet.current.altitude = _home_pos.altitude + _parameters.min_altitude; - _mission_item_triplet.current.yaw = 0.0f; + _mission_item_triplet.current.yaw = 0.0f; // TODO use heading to waypoint? _mission_item_triplet.current.nav_cmd = NAV_CMD_RETURN_TO_LAUNCH; _mission_item_triplet.current.loiter_direction = 1; _mission_item_triplet.current.loiter_radius = _parameters.loiter_radius; // TODO: get rid of magic number @@ -1212,8 +1208,7 @@ Navigator::mission_item_reached() } /* check if required yaw reached */ - float yaw_sp = _wrap_pi(_mission_item_triplet.current.yaw); - float yaw_err = _wrap_pi(yaw_sp - _global_pos.yaw); + float yaw_err = _wrap_pi(_mission_item_triplet.current.yaw - _global_pos.yaw); if (fabsf(yaw_err) < 0.05f) { /* XXX get rid of magic number */ _waypoint_yaw_reached = true; @@ -1223,7 +1218,7 @@ Navigator::mission_item_reached() if (_waypoint_position_reached /* && _waypoint_yaw_reached */) { /* XXX what about yaw? */ if (_time_first_inside_orbit == 0) { - /* XXX announcment? */ + /* XXX announcement? */ _time_first_inside_orbit = now; } @@ -1337,17 +1332,17 @@ Navigator::publish_control_mode() } bool Navigator::cmp_mission_item_equivalent(const struct mission_item_s a, const struct mission_item_s b) { - if (fabsf(a.altitude_is_relative - b.altitude_is_relative) < FLT_EPSILON && - fabsf(a.lat - b.lat) < FLT_EPSILON && - fabsf(a.lon - b.lon) < FLT_EPSILON && + if (a.altitude_is_relative == b.altitude_is_relative && + fabs(a.lat - b.lat) < FLT_EPSILON && + fabs(a.lon - b.lon) < FLT_EPSILON && fabsf(a.altitude - b.altitude) < FLT_EPSILON && fabsf(a.yaw - b.yaw) < FLT_EPSILON && fabsf(a.loiter_radius - b.loiter_radius) < FLT_EPSILON && - fabsf(a.loiter_direction - b.loiter_direction) < FLT_EPSILON && - fabsf(a.nav_cmd - b.nav_cmd) < FLT_EPSILON && + a.loiter_direction == b.loiter_direction && + a.nav_cmd == b.nav_cmd && fabsf(a.radius - b.radius) < FLT_EPSILON && fabsf(a.time_inside - b.time_inside) < FLT_EPSILON && - fabsf(a.autocontinue - b.autocontinue) < FLT_EPSILON) { + a.autocontinue == b.autocontinue) { return true; } else { return false; diff --git a/src/modules/uORB/topics/home_position.h b/src/modules/uORB/topics/home_position.h index 1cf37e29c0..3e2fee84e9 100644 --- a/src/modules/uORB/topics/home_position.h +++ b/src/modules/uORB/topics/home_position.h @@ -59,7 +59,7 @@ struct home_position_s { uint64_t timestamp; /**< Timestamp (microseconds since system boot) */ - bool altitude_is_relative; + //bool altitude_is_relative; // TODO what means home relative altitude? we need clear definition of reference altitude then double lat; /**< Latitude in degrees */ double lon; /**< Longitude in degrees */ float altitude; /**< Altitude in meters */ diff --git a/src/modules/uORB/topics/vehicle_control_mode.h b/src/modules/uORB/topics/vehicle_control_mode.h index 5ce968f675..5efb3f5bc7 100644 --- a/src/modules/uORB/topics/vehicle_control_mode.h +++ b/src/modules/uORB/topics/vehicle_control_mode.h @@ -62,8 +62,7 @@ */ typedef enum { - NAV_STATE_INIT = 0, - NAV_STATE_NONE, + NAV_STATE_NONE = 0, NAV_STATE_LOITER, NAV_STATE_MISSION, NAV_STATE_MISSION_LOITER, From 2e6cd186155d20f53fc45ab2f014d7ebb339c66c Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 31 Dec 2013 13:10:35 +0400 Subject: [PATCH 267/447] navigator: mavlink reopening fixed --- src/modules/navigator/navigator_main.cpp | 13 ++++++++----- 1 file changed, 8 insertions(+), 5 deletions(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 4288873735..12f761b05a 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -497,11 +497,14 @@ Navigator::task_main_trampoline(int argc, char *argv[]) void Navigator::task_main() { - /* inform about start */ warnx("Initializing.."); fflush(stdout); + _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); + + mavlink_log_info(_mavlink_fd, "[navigator] started"); + _fence_valid = load_fence(GEOFENCE_MAX_VERTICES); /* @@ -530,7 +533,7 @@ Navigator::task_main() unsigned prevState = NAV_STATE_NONE; bool pub_control_mode = true; hrt_abstime mavlink_open_time = 0; - const hrt_abstime mavlink_open_period = 500000000; + const hrt_abstime mavlink_open_interval = 500000; /* wakeup source(s) */ struct pollfd fds[7]; @@ -569,9 +572,9 @@ Navigator::task_main() perf_begin(_loop_perf); - if (_mavlink_fd < 0 && hrt_absolute_time() > mavlink_open_time + mavlink_open_period) { - /* try to open the mavlink log device every once in a while */ - mavlink_open_time = hrt_abstime(); + if (_mavlink_fd < 0 && hrt_absolute_time() > mavlink_open_time) { + /* try to reopen the mavlink log device with specified interval */ + mavlink_open_time = hrt_abstime() + mavlink_open_interval; _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); } From f6ee354c10026488c937568033a640d72a07cd14 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 31 Dec 2013 15:44:52 +0400 Subject: [PATCH 268/447] navigator: mode switching fixes --- src/modules/navigator/navigator_main.cpp | 31 ++++++++++++------------ 1 file changed, 16 insertions(+), 15 deletions(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 12f761b05a..34dcf89045 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -594,19 +594,17 @@ Navigator::task_main() stick_mode = true; } else { /* MISSION switch */ - if (!stick_mode) { - if (_vstatus.mission_switch == MISSION_SWITCH_LOITER) { + if (_vstatus.mission_switch == MISSION_SWITCH_LOITER) { + dispatch(EVENT_LOITER_REQUESTED); + stick_mode = true; + } else if (_vstatus.mission_switch == MISSION_SWITCH_MISSION) { + /* switch to mission only if available */ + if (_mission.current_mission_available()) { + dispatch(EVENT_MISSION_REQUESTED); + } else { dispatch(EVENT_LOITER_REQUESTED); - stick_mode = true; - } else if (_vstatus.mission_switch == MISSION_SWITCH_MISSION) { - /* switch to mission only if available */ - if (_mission.current_mission_available()) { - dispatch(EVENT_MISSION_REQUESTED); - } else { - dispatch(EVENT_LOITER_REQUESTED); - } - stick_mode = true; } + stick_mode = true; } if (!stick_mode && _vstatus.return_switch == RETURN_SWITCH_NORMAL && myState == NAV_STATE_RTL) { /* RETURN switch is in normal mode, no MISSION switch mapped, interrupt if in RTL state */ @@ -631,7 +629,11 @@ Navigator::task_main() break; case NAV_STATE_MISSION: - dispatch(EVENT_MISSION_REQUESTED); + if (_mission.current_mission_available()) { + dispatch(EVENT_MISSION_REQUESTED); + } else { + dispatch(EVENT_LOITER_REQUESTED); + } break; case NAV_STATE_RTL: @@ -997,8 +999,7 @@ Navigator::start_loiter() void Navigator::start_mission() { - /* leave previous mission item as is as is */ - + /* leave previous mission item as is */ int ret; bool onboard; unsigned index; @@ -1110,7 +1111,7 @@ Navigator::start_rtl() _mission_item_triplet.current.yaw = 0.0f; // TODO use heading to waypoint? _mission_item_triplet.current.nav_cmd = NAV_CMD_RETURN_TO_LAUNCH; _mission_item_triplet.current.loiter_direction = 1; - _mission_item_triplet.current.loiter_radius = _parameters.loiter_radius; // TODO: get rid of magic number + _mission_item_triplet.current.loiter_radius = _parameters.loiter_radius; _mission_item_triplet.current.radius = 50.0f; // TODO: get rid of magic number _mission_item_triplet.current.autocontinue = false; _mission_item_triplet.current_valid = true; From 7f9a7ffe82372d311a7947284871d794a8934493 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 31 Dec 2013 19:30:41 +0400 Subject: [PATCH 269/447] navigator: MISSION_LOITER and RTL_LOITER modes removed --- src/modules/mavlink/mavlink.c | 7 +- src/modules/navigator/navigator_main.cpp | 150 ++++++++---------- .../uORB/topics/vehicle_control_mode.h | 2 - 3 files changed, 70 insertions(+), 89 deletions(-) diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c index bc191f30d2..d4e4c027b4 100644 --- a/src/modules/mavlink/mavlink.c +++ b/src/modules/mavlink/mavlink.c @@ -220,17 +220,14 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_base_mode, u } else if (v_status.main_state == MAIN_STATE_AUTO) { *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; - // TODO review if (control_mode.nav_state == NAV_STATE_NONE) { custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY; } else if (control_mode.nav_state == NAV_STATE_LOITER) { custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LOITER; - } else if (control_mode.nav_state == NAV_STATE_MISSION_LOITER) { - custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LOITER; - } else if (control_mode.nav_state == NAV_STATE_RTL_LOITER) { - custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LOITER; } else if (control_mode.nav_state == NAV_STATE_MISSION) { custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_MISSION; + } else if (control_mode.nav_state == NAV_STATE_RTL) { + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_RTL; } } *mavlink_custom_mode = custom_mode.data; diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 34dcf89045..3380c232bd 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -165,6 +165,8 @@ private: class Mission _mission; + bool _reset_loiter_pos; /**< if true then loiter position should be set to current position */ + bool _rtl_done; bool _waypoint_position_reached; bool _waypoint_yaw_reached; uint64_t _time_first_inside_orbit; @@ -260,9 +262,9 @@ private: void start_none(); void start_loiter(); void start_mission(); - void start_mission_loiter(); + void finish_mission(); void start_rtl(); - void start_rtl_loiter(); + void finish_rtl(); /** * Guards offboard mission @@ -354,6 +356,8 @@ Navigator::Navigator() : _fence_valid(false), _inside_fence(true), _mission(), + _reset_loiter_pos(true), + _rtl_done(false), _waypoint_position_reached(false), _waypoint_yaw_reached(false), _time_first_inside_orbit(0), @@ -590,7 +594,7 @@ Navigator::task_main() /* RC signal available, use control switches to set mode */ /* RETURN switch, overrides MISSION switch */ if (_vstatus.return_switch == RETURN_SWITCH_RETURN) { - dispatch(EVENT_RTL_REQUESTED); + dispatch(_rtl_done ? EVENT_LOITER_REQUESTED : EVENT_RTL_REQUESTED); stick_mode = true; } else { /* MISSION switch */ @@ -637,7 +641,7 @@ Navigator::task_main() break; case NAV_STATE_RTL: - dispatch(EVENT_RTL_REQUESTED); + dispatch(_rtl_done ? EVENT_LOITER_REQUESTED : EVENT_RTL_REQUESTED); break; default: @@ -698,16 +702,21 @@ Navigator::task_main() /* global position updated */ if (fds[1].revents & POLLIN) { global_position_update(); - /* only check if waypoint has been reached in Mission or RTL mode */ + /* only check if waypoint has been reached in MISSION */ if (myState == NAV_STATE_MISSION || myState == NAV_STATE_RTL) { if (mission_item_reached()) { - /* advance by one mission item */ - _mission.move_to_next(); + if (myState == NAV_STATE_MISSION) { + /* advance by one mission item */ + _mission.move_to_next(); - /* if no more mission items available send this to state machine and start loiter at the last WP */ - if (_mission.current_mission_available()) { - advance_mission(); + /* if no more mission items available send this to state machine and start loiter at the last WP */ + if (_mission.current_mission_available()) { + advance_mission(); + } else { + dispatch(EVENT_MISSION_FINISHED); + } } else { + /* RTL finished */ dispatch(EVENT_MISSION_FINISHED); } } @@ -788,15 +797,9 @@ Navigator::status() case NAV_STATE_MISSION: warnx("State: Mission"); break; - case NAV_STATE_MISSION_LOITER: - warnx("State: Loiter after Mission"); - break; case NAV_STATE_RTL: warnx("State: RTL"); break; - case NAV_STATE_RTL_LOITER: - warnx("State: Loiter after RTL"); - break; default: warnx("State: Unknown"); break; @@ -920,39 +923,19 @@ StateTable::Tran const Navigator::myTable[NAV_STATE_MAX][MAX_EVENT] = { /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER}, /* EVENT_MISSION_REQUESTED */ {NO_ACTION, NAV_STATE_MISSION}, /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, - /* EVENT_MISSION_FINISHED */ {ACTION(&Navigator::start_mission_loiter), NAV_STATE_MISSION_LOITER}, + /* EVENT_MISSION_FINISHED */ {ACTION(&Navigator::finish_mission), NAV_STATE_LOITER}, /* EVENT_MISSION_CHANGED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_MISSION}, }, - { - /* STATE_MISSION_LOITER */ - /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE}, - /* EVENT_LOITER_REQUESTED */ {NO_ACTION, NAV_STATE_MISSION_LOITER}, - /* EVENT_MISSION_REQUESTED */ {NO_ACTION, NAV_STATE_MISSION_LOITER}, - /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, - /* EVENT_MISSION_FINISHED */ {NO_ACTION, NAV_STATE_MISSION_LOITER}, - /* EVENT_MISSION_CHANGED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, - /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_MISSION_LOITER}, - }, { /* STATE_RTL */ /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE}, /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER}, /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, /* EVENT_RTL_REQUESTED */ {NO_ACTION, NAV_STATE_RTL}, - /* EVENT_MISSION_FINISHED */ {ACTION(&Navigator::start_rtl_loiter), NAV_STATE_RTL_LOITER}, + /* EVENT_MISSION_FINISHED */ {ACTION(&Navigator::finish_rtl), NAV_STATE_LOITER}, /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_RTL}, - /* EVENT_HOME_POSITION_CHANGED */ {ACTION(&Navigator::start_loiter), NAV_STATE_RTL}, - }, - { - /* STATE_RTL_LOITER */ - /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE}, - /* EVENT_LOITER_REQUESTED */ {NO_ACTION, NAV_STATE_RTL_LOITER}, - /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, - /* EVENT_RTL_REQUESTED */ {NO_ACTION, NAV_STATE_RTL_LOITER}, - /* EVENT_MISSION_FINISHED */ {NO_ACTION, NAV_STATE_RTL_LOITER}, - /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_RTL_LOITER}, - /* EVENT_HOME_POSITION_CHANGED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER}, + /* EVENT_HOME_POSITION_CHANGED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, }, }; @@ -963,35 +946,42 @@ Navigator::start_none() _mission_item_triplet.current_valid = false; _mission_item_triplet.next_valid = false; + _reset_loiter_pos = true; + _rtl_done = false; + publish_mission_item_triplet(); } void Navigator::start_loiter() { + /* set loiter position if needed */ + if (_reset_loiter_pos || !_mission_item_triplet.current_valid) { + _reset_loiter_pos = false; + + _mission_item_triplet.current.lat = (double)_global_pos.lat / 1e7d; + _mission_item_triplet.current.lon = (double)_global_pos.lon / 1e7d; + _mission_item_triplet.current.yaw = NAN; // NAN means to use current yaw + + _mission_item_triplet.current.altitude_is_relative = false; + float min_alt_amsl = _parameters.min_altitude + _home_pos.altitude; + + /* Use current altitude if above min altitude set by parameter */ + if (_global_pos.alt < min_alt_amsl) { + _mission_item_triplet.current.altitude = min_alt_amsl; + mavlink_log_info(_mavlink_fd, "[navigator] loiter %.1fm higher", (double)(min_alt_amsl - _global_pos.alt)); + } else { + _mission_item_triplet.current.altitude = _global_pos.alt; + mavlink_log_info(_mavlink_fd, "[navigator] loiter at current altitude"); + } + } + _mission_item_triplet.previous_valid = false; _mission_item_triplet.current_valid = true; _mission_item_triplet.next_valid = false; - _mission_item_triplet.current.lat = (double)_global_pos.lat / 1e7d; - _mission_item_triplet.current.lon = (double)_global_pos.lon / 1e7d; - _mission_item_triplet.current.yaw = NAN; // NAN means to use current yaw - get_loiter_item(&_mission_item_triplet.current); - // TODO use relative altitude to allow flying without global reference (?) - _mission_item_triplet.current.altitude_is_relative = false; - float min_alt_amsl = _parameters.min_altitude + _home_pos.altitude; - - /* Use current altitude if above min altitude set by parameter */ - if (_global_pos.alt < min_alt_amsl) { - _mission_item_triplet.current.altitude = min_alt_amsl; - mavlink_log_info(_mavlink_fd, "[navigator] loiter %.1fm higher", (double)(min_alt_amsl - _global_pos.alt)); - } else { - _mission_item_triplet.current.altitude = _global_pos.alt; - mavlink_log_info(_mavlink_fd, "[navigator] loiter at current altitude"); - } - publish_mission_item_triplet(); } @@ -999,7 +989,10 @@ Navigator::start_loiter() void Navigator::start_mission() { - /* leave previous mission item as is */ + // TODO set prev item to current position? + _reset_loiter_pos = true; + _rtl_done = false; + int ret; bool onboard; unsigned index; @@ -1012,9 +1005,9 @@ Navigator::start_mission() _mission_item_triplet.current_valid = true; if (onboard) { - mavlink_log_info(_mavlink_fd, "[navigator] heading to onboard WP %d", index); + mavlink_log_info(_mavlink_fd, "[navigator] mission started, onboard WP %d", index); } else { - mavlink_log_info(_mavlink_fd, "[navigator] heading to offboard WP %d", index); + mavlink_log_info(_mavlink_fd, "[navigator] mission started, offboard WP %d", index); } } else { /* since a mission is started without knowledge if there are more mission items available, this can fail */ @@ -1044,6 +1037,8 @@ Navigator::advance_mission() memcpy(&_mission_item_triplet.previous, &_mission_item_triplet.current, sizeof(mission_item_s)); _mission_item_triplet.previous_valid = _mission_item_triplet.current_valid; + _reset_loiter_pos = true; + int ret; bool onboard; unsigned index; @@ -1082,23 +1077,21 @@ Navigator::advance_mission() } void -Navigator::start_mission_loiter() +Navigator::finish_mission() { - /* make sure the current WP is valid */ - if (!_mission_item_triplet.current_valid) { - warnx("ERROR: cannot switch to offboard mission loiter"); - } + /* loiter at last waypoint */ + _reset_loiter_pos = false; - get_loiter_item(&_mission_item_triplet.current); + mavlink_log_info(_mavlink_fd, "[navigator] mission completed"); - publish_mission_item_triplet(); - - mavlink_log_info(_mavlink_fd, "[navigator] loiter at last WP"); + start_loiter(); } void Navigator::start_rtl() { + _reset_loiter_pos = true; + _rtl_done = false; /* discard all mission item and insert RTL item */ _mission_item_triplet.previous_valid = false; @@ -1118,26 +1111,19 @@ Navigator::start_rtl() publish_mission_item_triplet(); - mavlink_log_info(_mavlink_fd, "[navigator] return to launch"); + mavlink_log_info(_mavlink_fd, "[navigator] RTL started"); } - void -Navigator::start_rtl_loiter() +Navigator::finish_rtl() { - _mission_item_triplet.previous_valid = false; - _mission_item_triplet.current_valid = true; - _mission_item_triplet.next_valid = false; + /* loiter at home position */ + _reset_loiter_pos = false; + _rtl_done = true; - _mission_item_triplet.current.lat = _home_pos.lat; - _mission_item_triplet.current.lon = _home_pos.lon; - _mission_item_triplet.current.altitude = _home_pos.altitude + _parameters.min_altitude; - - get_loiter_item(&_mission_item_triplet.current); + mavlink_log_info(_mavlink_fd, "[navigator] RTL completed"); - publish_mission_item_triplet(); - - mavlink_log_info(_mavlink_fd, "[navigator] loiter after RTL"); + start_loiter(); } bool diff --git a/src/modules/uORB/topics/vehicle_control_mode.h b/src/modules/uORB/topics/vehicle_control_mode.h index 5efb3f5bc7..9d5dad9f98 100644 --- a/src/modules/uORB/topics/vehicle_control_mode.h +++ b/src/modules/uORB/topics/vehicle_control_mode.h @@ -65,9 +65,7 @@ typedef enum { NAV_STATE_NONE = 0, NAV_STATE_LOITER, NAV_STATE_MISSION, - NAV_STATE_MISSION_LOITER, NAV_STATE_RTL, - NAV_STATE_RTL_LOITER, NAV_STATE_MAX } nav_state_t; From 7eee6ce1b2df2400e7e6a9b58d16011c68cbdd8c Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 1 Jan 2014 13:30:26 +0400 Subject: [PATCH 270/447] mc_pos_control: takeoff / landing implemented --- .../mc_pos_control/mc_pos_control_main.cpp | 59 ++++++++++--------- .../mc_pos_control/mc_pos_control_params.c | 1 + 2 files changed, 32 insertions(+), 28 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 e2a5a5def2..df74d15841 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -147,6 +147,7 @@ private: param_t xy_vel_max; param_t xy_ff; param_t tilt_max; + param_t land_speed; param_t rc_scale_pitch; param_t rc_scale_roll; @@ -159,6 +160,7 @@ private: float thr_min; float thr_max; float tilt_max; + float land_speed; float rc_scale_pitch; float rc_scale_roll; @@ -280,6 +282,7 @@ MulticopterPositionControl::MulticopterPositionControl() : _params_handles.xy_vel_max = param_find("MPC_XY_VEL_MAX"); _params_handles.xy_ff = param_find("MPC_XY_FF"); _params_handles.tilt_max = param_find("MPC_TILT_MAX"); + _params_handles.land_speed = param_find("MPC_LAND_SPEED"); _params_handles.rc_scale_pitch = param_find("RC_SCALE_PITCH"); _params_handles.rc_scale_roll = param_find("RC_SCALE_ROLL"); _params_handles.rc_scale_yaw = param_find("RC_SCALE_YAW"); @@ -328,6 +331,7 @@ MulticopterPositionControl::parameters_update(bool force) param_get(_params_handles.thr_min, &_params.thr_min); param_get(_params_handles.thr_max, &_params.thr_max); param_get(_params_handles.tilt_max, &_params.tilt_max); + param_get(_params_handles.land_speed, &_params.land_speed); param_get(_params_handles.rc_scale_pitch, &_params.rc_scale_pitch); param_get(_params_handles.rc_scale_roll, &_params.rc_scale_roll); param_get(_params_handles.rc_scale_yaw, &_params.rc_scale_yaw); @@ -449,14 +453,12 @@ MulticopterPositionControl::task_main() /* get an initial update for all sensor and status data */ poll_subscriptions(); - bool reset_man_sp_z = true; - bool reset_man_sp_xy = true; + bool reset_sp_z = true; + bool reset_sp_xy = true; bool reset_int_z = true; bool reset_int_z_manual = false; bool reset_int_xy = true; bool was_armed = false; - bool reset_auto_sp_xy = true; - bool reset_auto_sp_z = true; bool reset_takeoff_sp = true; hrt_abstime t_prev = 0; @@ -508,10 +510,8 @@ MulticopterPositionControl::task_main() if (_control_mode.flag_armed && !was_armed) { /* reset setpoints and integrals on arming */ - reset_man_sp_z = true; - reset_man_sp_xy = true; - reset_auto_sp_z = true; - reset_auto_sp_xy = true; + reset_sp_z = true; + reset_sp_xy = true; reset_takeoff_sp = true; reset_int_z = true; reset_int_xy = true; @@ -558,8 +558,8 @@ MulticopterPositionControl::task_main() /* manual control */ if (_control_mode.flag_control_altitude_enabled) { /* reset setpoint Z to current altitude if needed */ - if (reset_man_sp_z) { - reset_man_sp_z = false; + if (reset_sp_z) { + reset_sp_z = false; _pos_sp(2) = _pos(2); mavlink_log_info(mavlink_fd, "[mpc] reset alt sp: %.2f", (double) - _pos_sp(2)); } @@ -570,8 +570,8 @@ MulticopterPositionControl::task_main() if (_control_mode.flag_control_position_enabled) { /* reset setpoint XY to current position if needed */ - if (reset_man_sp_xy) { - reset_man_sp_xy = false; + if (reset_sp_xy) { + reset_sp_xy = false; _pos_sp(0) = _pos(0); _pos_sp(1) = _pos(1); mavlink_log_info(mavlink_fd, "[mpc] reset pos sp: %.2f, %.2f", (double)_pos_sp(0), (double)_pos_sp(1)); @@ -604,9 +604,6 @@ MulticopterPositionControl::task_main() _pos_sp = _pos + pos_sp_offs.emult(_params.vel_max); } - /* local position setpoint is valid and can be used for auto loiter after position controlled mode */ - reset_auto_sp_xy = !_control_mode.flag_control_position_enabled; - reset_auto_sp_z = !_control_mode.flag_control_altitude_enabled; reset_takeoff_sp = true; } else { /* AUTO */ @@ -617,19 +614,22 @@ MulticopterPositionControl::task_main() // TODO home altitude can be != ref_alt, check home_position topic _pos_sp(2) = -(item.altitude_is_relative ? item.altitude : item.altitude - ref_alt); - /* in case of interrupted mission don't go to waypoint but stop */ - reset_auto_sp_xy = true; - reset_auto_sp_z = true; + if (_mission_items.current.nav_cmd == NAV_CMD_TAKEOFF) { + /* add gap for takeoff setpoint */ + _pos_sp(2) -= 0.5f * _params.vel_max(2) / _params.pos_p(2); + } + + /* in case of interrupted mission don't go to waypoint but stay at current position */ + reset_sp_xy = true; + reset_sp_z = true; } else { /* no waypoint, loiter, reset position setpoint if needed */ - if (reset_auto_sp_xy) { - reset_auto_sp_xy = false; + if (reset_sp_xy) { _pos_sp(0) = _pos(0); _pos_sp(1) = _pos(1); } - if (reset_auto_sp_z) { - reset_auto_sp_z = false; + if (reset_sp_z) { _pos_sp(2) = _pos(2); } } @@ -654,17 +654,22 @@ MulticopterPositionControl::task_main() _vel_sp = pos_err.emult(_params.pos_p) + sp_move_rate.emult(_params.vel_ff); if (!_control_mode.flag_control_altitude_enabled) { - reset_man_sp_z = true; + reset_sp_z = true; _vel_sp(2) = 0.0f; } if (!_control_mode.flag_control_position_enabled) { - reset_man_sp_xy = true; + reset_sp_xy = true; _vel_sp(0) = 0.0f; _vel_sp(1) = 0.0f; } if (!_control_mode.flag_control_manual_enabled) { + /* use constant descend rate when landing, ignore altitude setpoint */ + if (_mission_items.current_valid && _mission_items.current.nav_cmd == NAV_CMD_LAND) { + _vel_sp(2) = _params.land_speed; + } + /* limit 3D speed only in AUTO mode */ float vel_sp_norm = _vel_sp.edivide(_params.vel_max).length(); @@ -898,12 +903,10 @@ MulticopterPositionControl::task_main() } } else { /* position controller disabled, reset setpoints */ - reset_man_sp_z = true; - reset_man_sp_xy = true; + reset_sp_z = true; + reset_sp_xy = true; reset_int_z = true; reset_int_xy = true; - reset_auto_sp_xy = true; - reset_auto_sp_z = true; } /* reset altitude controller integral (hovering throttle) to manual throttle after manual throttle control */ 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 92fa94dcd1..67aa248721 100644 --- a/src/modules/mc_pos_control/mc_pos_control_params.c +++ b/src/modules/mc_pos_control/mc_pos_control_params.c @@ -23,3 +23,4 @@ PARAM_DEFINE_FLOAT(MPC_XY_VEL_D, 0.01f); PARAM_DEFINE_FLOAT(MPC_XY_VEL_MAX, 5.0f); PARAM_DEFINE_FLOAT(MPC_XY_FF, 0.5f); PARAM_DEFINE_FLOAT(MPC_TILT_MAX, 1.0f); +PARAM_DEFINE_FLOAT(MPC_LAND_SPEED, 1.0f); From 3bc94f1fe6edba3a48bcb799f97c51168a16bf76 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 1 Jan 2014 13:31:27 +0400 Subject: [PATCH 271/447] navigator: takeoff fix --- src/modules/navigator/navigator_main.cpp | 16 +++++++++++++--- 1 file changed, 13 insertions(+), 3 deletions(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 3380c232bd..982ebefccb 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -1173,8 +1173,12 @@ Navigator::mission_item_reached() float dist_z = -1.0f; // if (coordinate_frame == (int)MAV_FRAME_GLOBAL) { + + // current relative or AMSL altitude depending on _mission_item_triplet.current.altitude_is_relative + float current_alt = _mission_item_triplet.current.altitude_is_relative ? _global_pos.relative_alt : _global_pos.alt; + dist = get_distance_to_point_global_wgs84(_mission_item_triplet.current.lat, _mission_item_triplet.current.lon, _mission_item_triplet.current.altitude, - (double)_global_pos.lat / 1e7d, (double)_global_pos.lon / 1e7d, _global_pos.alt, + (double)_global_pos.lat / 1e7d, (double)_global_pos.lon / 1e7d, current_alt, &dist_xy, &dist_z); // warnx("1 lat: %2.2f, lon: %2.2f, alt: %2.2f", _mission_item_triplet.current.lat, _mission_item_triplet.current.lon, _mission_item_triplet.current.altitude); @@ -1193,8 +1197,14 @@ Navigator::mission_item_reached() // // XXX TODO // } - if (dist >= 0.0f && dist_xy <= orbit && dist_z >= 0.0f && dist_z <= vertical_switch_distance) { - _waypoint_position_reached = true; + if (_mission_item_triplet.current.nav_cmd == NAV_CMD_TAKEOFF) { + /* require only altitude for takeoff */ + if (current_alt > _mission_item_triplet.current.altitude) + _waypoint_position_reached = true; + } else { + if (dist >= 0.0f && dist_xy <= orbit && dist_z >= 0.0f && dist_z <= vertical_switch_distance) { + _waypoint_position_reached = true; + } } /* check if required yaw reached */ From 40d03666fdc037b578400b8e04705ca0f5a1a092 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 1 Jan 2014 13:40:32 +0400 Subject: [PATCH 272/447] mc_pos_control: yaw in AUTO fixed --- 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 df74d15841..c864db7ed6 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -619,6 +619,8 @@ MulticopterPositionControl::task_main() _pos_sp(2) -= 0.5f * _params.vel_max(2) / _params.pos_p(2); } + _att_sp.yaw_body = _mission_items.current.yaw; + /* in case of interrupted mission don't go to waypoint but stay at current position */ reset_sp_xy = true; reset_sp_z = true; From 8c1c7bca18e6d4c996852ef042a50ee34af6cc33 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 1 Jan 2014 14:02:24 +0400 Subject: [PATCH 273/447] mc_pos_control: takeoff fixes, ignore position and yaw, takeoff vertically --- .../mc_pos_control/mc_pos_control_main.cpp | 32 ++++++++++++------- 1 file changed, 21 insertions(+), 11 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 c864db7ed6..3c05cec075 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -609,29 +609,39 @@ MulticopterPositionControl::task_main() /* AUTO */ if (_mission_items.current_valid) { struct mission_item_s item = _mission_items.current; - map_projection_project(item.lat, item.lon, &_pos_sp(0), &_pos_sp(1)); - // TODO home altitude can be != ref_alt, check home_position topic - _pos_sp(2) = -(item.altitude_is_relative ? item.altitude : item.altitude - ref_alt); + if (item.nav_cmd == NAV_CMD_TAKEOFF) { + /* use current position setpoint or current position */ + if (reset_sp_xy) { + reset_sp_xy = false; + _pos_sp(0) = _pos(0); + _pos_sp(1) = _pos(1); + } - if (_mission_items.current.nav_cmd == NAV_CMD_TAKEOFF) { /* add gap for takeoff setpoint */ _pos_sp(2) -= 0.5f * _params.vel_max(2) / _params.pos_p(2); + reset_sp_z = true; + } else { + map_projection_project(item.lat, item.lon, &_pos_sp(0), &_pos_sp(1)); + + // TODO home altitude can be != ref_alt, check home_position topic + _pos_sp(2) = -(item.altitude_is_relative ? item.altitude : item.altitude - ref_alt); + + _att_sp.yaw_body = _mission_items.current.yaw; + + /* in case of interrupted mission don't go to waypoint but stay at current position */ + reset_sp_xy = true; + reset_sp_z = true; } - - _att_sp.yaw_body = _mission_items.current.yaw; - - /* in case of interrupted mission don't go to waypoint but stay at current position */ - reset_sp_xy = true; - reset_sp_z = true; - } else { /* no waypoint, loiter, reset position setpoint if needed */ if (reset_sp_xy) { + reset_sp_xy = false; _pos_sp(0) = _pos(0); _pos_sp(1) = _pos(1); } if (reset_sp_z) { + reset_sp_z = false; _pos_sp(2) = _pos(2); } } From 6827e45aee2f1a6e756c56a91b46152eb2ea5d08 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 1 Jan 2014 14:03:42 +0400 Subject: [PATCH 274/447] mc_pos_control, mc_att_control_vector: code style fixed --- .../mc_att_control_vector_main.cpp | 1 + .../mc_pos_control/mc_pos_control_main.cpp | 28 +++++++++++++++++-- 2 files changed, 26 insertions(+), 3 deletions(-) diff --git a/src/modules/mc_att_control_vector/mc_att_control_vector_main.cpp b/src/modules/mc_att_control_vector/mc_att_control_vector_main.cpp index 5f436fb71c..f13c04e45b 100644 --- a/src/modules/mc_att_control_vector/mc_att_control_vector_main.cpp +++ b/src/modules/mc_att_control_vector/mc_att_control_vector_main.cpp @@ -530,6 +530,7 @@ MulticopterAttitudeControl::task_main() if (_att_sp.R_valid) { /* rotation matrix in _att_sp is valid, use it */ R_sp.set(&_att_sp.R_body[0][0]); + } else { /* rotation matrix in _att_sp is not valid, use euler angles instead */ R_sp.from_euler(_att_sp.roll_body, _att_sp.pitch_body, _att_sp.yaw_body); 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 3c05cec075..a416228db3 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -322,6 +322,7 @@ MulticopterPositionControl::parameters_update(bool force) struct parameter_update_s param_upd; orb_check(_params_sub, &updated); + if (updated) orb_copy(ORB_ID(parameter_update), _params_sub, ¶m_upd); @@ -380,26 +381,32 @@ MulticopterPositionControl::poll_subscriptions() bool updated; orb_check(_att_sub, &updated); + if (updated) orb_copy(ORB_ID(vehicle_attitude), _att_sub, &_att); orb_check(_att_sp_sub, &updated); + if (updated) orb_copy(ORB_ID(vehicle_attitude_setpoint), _att_sp_sub, &_att_sp); orb_check(_control_mode_sub, &updated); + if (updated) orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode); orb_check(_manual_sub, &updated); + if (updated) orb_copy(ORB_ID(manual_control_setpoint), _manual_sub, &_manual); orb_check(_arming_sub, &updated); + if (updated) orb_copy(ORB_ID(actuator_armed), _arming_sub, &_arming); orb_check(_mission_items_sub, &updated); + if (updated) orb_copy(ORB_ID(mission_item_triplet), _mission_items_sub, &_mission_items); } @@ -516,12 +523,13 @@ MulticopterPositionControl::task_main() reset_int_z = true; reset_int_xy = true; } + was_armed = _control_mode.flag_armed; if (_control_mode.flag_control_altitude_enabled || - _control_mode.flag_control_position_enabled || - _control_mode.flag_control_climb_rate_enabled || - _control_mode.flag_control_velocity_enabled) { + _control_mode.flag_control_position_enabled || + _control_mode.flag_control_climb_rate_enabled || + _control_mode.flag_control_velocity_enabled) { _pos(0) = _local_pos.x; _pos(1) = _local_pos.y; @@ -548,6 +556,7 @@ MulticopterPositionControl::task_main() _pos_sp(1) += ref_change_y; _pos_sp(2) += _local_pos.ref_alt - ref_alt; } + ref_timestamp = _local_pos.ref_timestamp; ref_lat = _local_pos.ref_lat; ref_lon = _local_pos.ref_lon; @@ -584,6 +593,7 @@ MulticopterPositionControl::task_main() /* limit setpoint move rate */ float sp_move_norm = sp_move_rate.length(); + if (sp_move_norm > 1.0f) { sp_move_rate /= sp_move_norm; } @@ -599,12 +609,14 @@ MulticopterPositionControl::task_main() /* check if position setpoint is too far from actual position */ math::Vector<3> pos_sp_offs = (_pos_sp - _pos).edivide(_params.vel_max); float pos_sp_offs_norm = pos_sp_offs.length(); + if (pos_sp_offs_norm > 1.0f) { pos_sp_offs /= pos_sp_offs_norm; _pos_sp = _pos + pos_sp_offs.emult(_params.vel_max); } reset_takeoff_sp = true; + } else { /* AUTO */ if (_mission_items.current_valid) { @@ -621,6 +633,7 @@ MulticopterPositionControl::task_main() /* add gap for takeoff setpoint */ _pos_sp(2) -= 0.5f * _params.vel_max(2) / _params.pos_p(2); reset_sp_z = true; + } else { map_projection_project(item.lat, item.lon, &_pos_sp(0), &_pos_sp(1)); @@ -633,6 +646,7 @@ MulticopterPositionControl::task_main() reset_sp_xy = true; reset_sp_z = true; } + } else { /* no waypoint, loiter, reset position setpoint if needed */ if (reset_sp_xy) { @@ -640,6 +654,7 @@ MulticopterPositionControl::task_main() _pos_sp(0) = _pos(0); _pos_sp(1) = _pos(1); } + if (reset_sp_z) { reset_sp_z = false; _pos_sp(2) = _pos(2); @@ -723,6 +738,7 @@ MulticopterPositionControl::task_main() thrust_int(2) = -i; mavlink_log_info(mavlink_fd, "[mpc] reset hovering thrust: %.2f", (double)i); } + } else { reset_int_z = true; } @@ -734,6 +750,7 @@ MulticopterPositionControl::task_main() thrust_int(1) = 0.0f; mavlink_log_info(mavlink_fd, "[mpc] reset xy vel integral"); } + } else { reset_int_xy = true; } @@ -752,6 +769,7 @@ MulticopterPositionControl::task_main() thrust_sp(0) = 0.0f; thrust_sp(1) = 0.0f; } + if (!_control_mode.flag_control_climb_rate_enabled) { thrust_sp(2) = 0.0f; } @@ -762,6 +780,7 @@ MulticopterPositionControl::task_main() /* limit min lift */ float thr_min = _params.thr_min; + if (!_control_mode.flag_control_velocity_enabled && thr_min < 0.0f) { /* don't allow downside thrust direction in manual attitude mode */ thr_min = 0.0f; @@ -856,7 +875,9 @@ MulticopterPositionControl::task_main() if (body_z(2) < 0.0f) { body_x = -body_x; } + body_x.normalize(); + } else { /* desired thrust is in XY plane, set X downside to construct correct matrix, * but yaw component will not be used actually */ @@ -913,6 +934,7 @@ MulticopterPositionControl::task_main() } else { reset_int_z = true; } + } else { /* position controller disabled, reset setpoints */ reset_sp_z = true; From 5dda4dc993d0e3a8bda3214aa1d5d1c1ea6cb577 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 1 Jan 2014 18:19:02 +0400 Subject: [PATCH 275/447] Merge branch 'master' into navigator_new_vector fix --- makefiles/config_px4fmu-v2_logging.mk | 158 -------------------------- 1 file changed, 158 deletions(-) delete mode 100644 makefiles/config_px4fmu-v2_logging.mk diff --git a/makefiles/config_px4fmu-v2_logging.mk b/makefiles/config_px4fmu-v2_logging.mk deleted file mode 100644 index 0986b3feed..0000000000 --- a/makefiles/config_px4fmu-v2_logging.mk +++ /dev/null @@ -1,158 +0,0 @@ -# -# Makefile for the px4fmu_default configuration -# - -# -# Use the configuration's ROMFS, copy the px4iov2 firmware into -# the ROMFS if it's available -# -ROMFS_ROOT = $(PX4_BASE)/ROMFS/px4fmu_logging -ROMFS_OPTIONAL_FILES = $(PX4_BASE)/Images/px4io-v2_default.bin - -# -# Board support modules -# -MODULES += drivers/device -MODULES += drivers/stm32 -MODULES += drivers/stm32/adc -MODULES += drivers/stm32/tone_alarm -MODULES += drivers/led -MODULES += drivers/px4fmu -MODULES += drivers/px4io -MODULES += drivers/boards/px4fmu-v2 -MODULES += drivers/rgbled -MODULES += drivers/mpu6000 -MODULES += drivers/lsm303d -MODULES += drivers/l3gd20 -MODULES += drivers/hmc5883 -MODULES += drivers/ms5611 -MODULES += drivers/mb12xx -MODULES += drivers/gps -MODULES += drivers/hil -MODULES += drivers/hott/hott_telemetry -MODULES += drivers/hott/hott_sensors -MODULES += drivers/blinkm -MODULES += drivers/roboclaw -MODULES += drivers/airspeed -MODULES += drivers/ets_airspeed -MODULES += drivers/meas_airspeed -MODULES += modules/sensors - -# Needs to be burned to the ground and re-written; for now, -# just don't build it. -#MODULES += drivers/mkblctrl - -# -# System commands -# -MODULES += systemcmds/ramtron -MODULES += systemcmds/bl_update -MODULES += systemcmds/boardinfo -MODULES += systemcmds/mixer -MODULES += systemcmds/param -MODULES += systemcmds/perf -MODULES += systemcmds/preflight_check -MODULES += systemcmds/pwm -MODULES += systemcmds/esc_calib -MODULES += systemcmds/reboot -MODULES += systemcmds/top -MODULES += systemcmds/tests -MODULES += systemcmds/config -MODULES += systemcmds/nshterm - -# -# General system control -# -MODULES += modules/commander -MODULES += modules/navigator -MODULES += modules/mavlink -MODULES += modules/mavlink_onboard - -# -# Estimation modules (EKF/ SO3 / other filters) -# -MODULES += modules/attitude_estimator_ekf -MODULES += modules/attitude_estimator_so3 -MODULES += modules/att_pos_estimator_ekf -MODULES += modules/position_estimator_inav -MODULES += examples/flow_position_estimator - -# -# Vehicle Control -# -#MODULES += modules/segway # XXX Needs GCC 4.7 fix -MODULES += modules/fw_pos_control_l1 -MODULES += modules/fw_att_control -MODULES += modules/mc_att_control_vector -MODULES += modules/mc_pos_control - -# -# Logging -# -MODULES += modules/sdlog2 - -# -# Unit tests -# -#MODULES += modules/unit_test -#MODULES += modules/commander/commander_tests - -# -# Library modules -# -MODULES += modules/systemlib -MODULES += modules/systemlib/mixer -MODULES += modules/controllib -MODULES += modules/uORB - -# -# Libraries -# -LIBRARIES += lib/mathlib/CMSIS -MODULES += lib/mathlib -MODULES += lib/mathlib/math/filter -MODULES += lib/ecl -MODULES += lib/external_lgpl -MODULES += lib/geo -MODULES += lib/conversion -MODULES += modules/dataman - -# -# Demo apps -# -#MODULES += examples/math_demo -# Tutorial code from -# https://pixhawk.ethz.ch/px4/dev/hello_sky -MODULES += examples/px4_simple_app - -# Tutorial code from -# https://pixhawk.ethz.ch/px4/dev/daemon -#MODULES += examples/px4_daemon_app - -# Tutorial code from -# https://pixhawk.ethz.ch/px4/dev/debug_values -#MODULES += examples/px4_mavlink_debug - -# Tutorial code from -# https://pixhawk.ethz.ch/px4/dev/example_fixedwing_control -#MODULES += examples/fixedwing_control - -# Hardware test -#MODULES += examples/hwtest - -# -# Transitional support - add commands from the NuttX export archive. -# -# In general, these should move to modules over time. -# -# Each entry here is ... but we use a helper macro -# to make the table a bit more readable. -# -define _B - $(strip $1).$(or $(strip $2),SCHED_PRIORITY_DEFAULT).$(or $(strip $3),CONFIG_PTHREAD_STACK_DEFAULT).$(strip $4) -endef - -# command priority stack entrypoint -BUILTIN_COMMANDS := \ - $(call _B, sercon, , 2048, sercon_main ) \ - $(call _B, serdis, , 2048, serdis_main ) From ec8bc6c0208a5eb9ad3decf7bec196c5bf113dd4 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 1 Jan 2014 16:33:50 +0100 Subject: [PATCH 276/447] fw pos ctrl: remove a wrong transpose --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 455fe36742..bbb205b2f8 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -764,7 +764,7 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio /* filter speed and altitude for controller */ math::Vector3 accel_body(_sensor_combined.accelerometer_m_s2[0], _sensor_combined.accelerometer_m_s2[1], _sensor_combined.accelerometer_m_s2[2]); - math::Vector3 accel_earth = _R_nb.transpose() * accel_body; + math::Vector3 accel_earth = _R_nb * accel_body; _tecs.update_50hz(baro_altitude, _airspeed.indicated_airspeed_m_s, _R_nb, accel_body, accel_earth); float altitude_error = _mission_item_triplet.current.altitude - _global_pos.alt; From 352a1ef095d111f06e951d39dd221c1f345ddce0 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 1 Jan 2014 23:20:36 +0400 Subject: [PATCH 277/447] mc_pos_control: minor fixes --- .../mc_pos_control/mc_pos_control_main.cpp | 18 ++++++------------ 1 file changed, 6 insertions(+), 12 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 a416228db3..3ee98bd410 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -130,8 +130,6 @@ private: struct vehicle_global_velocity_setpoint_s _global_vel_sp; /**< vehicle global velocity setpoint */ struct { - param_t takeoff_alt; - param_t takeoff_gap; param_t thr_min; param_t thr_max; param_t z_p; @@ -155,8 +153,6 @@ private: } _params_handles; /**< handles for interesting parameters */ struct { - float takeoff_alt; - float takeoff_gap; float thr_min; float thr_max; float tilt_max; @@ -265,8 +261,6 @@ MulticopterPositionControl::MulticopterPositionControl() : _vel_sp.zero(); _vel_prev.zero(); - _params_handles.takeoff_alt = param_find("NAV_TAKEOFF_ALT"); - _params_handles.takeoff_gap = param_find("NAV_TAKEOFF_GAP"); _params_handles.thr_min = param_find("MPC_THR_MIN"); _params_handles.thr_max = param_find("MPC_THR_MAX"); _params_handles.z_p = param_find("MPC_Z_P"); @@ -327,8 +321,6 @@ MulticopterPositionControl::parameters_update(bool force) orb_copy(ORB_ID(parameter_update), _params_sub, ¶m_upd); if (updated || force) { - param_get(_params_handles.takeoff_alt, &_params.takeoff_alt); - param_get(_params_handles.takeoff_gap, &_params.takeoff_gap); param_get(_params_handles.thr_min, &_params.thr_min); param_get(_params_handles.thr_max, &_params.thr_max); param_get(_params_handles.tilt_max, &_params.tilt_max); @@ -622,6 +614,9 @@ MulticopterPositionControl::task_main() if (_mission_items.current_valid) { struct mission_item_s item = _mission_items.current; + // TODO home altitude can be != ref_alt, check home_position topic + _pos_sp(2) = -(item.altitude_is_relative ? item.altitude : item.altitude - ref_alt); + if (item.nav_cmd == NAV_CMD_TAKEOFF) { /* use current position setpoint or current position */ if (reset_sp_xy) { @@ -637,10 +632,9 @@ MulticopterPositionControl::task_main() } else { map_projection_project(item.lat, item.lon, &_pos_sp(0), &_pos_sp(1)); - // TODO home altitude can be != ref_alt, check home_position topic - _pos_sp(2) = -(item.altitude_is_relative ? item.altitude : item.altitude - ref_alt); - - _att_sp.yaw_body = _mission_items.current.yaw; + if (isfinite(_mission_items.current.yaw)) { + _att_sp.yaw_body = _mission_items.current.yaw; + } /* in case of interrupted mission don't go to waypoint but stay at current position */ reset_sp_xy = true; From 3c72c9bf7fce882a1d9138eb83e5d5745e79c271 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 1 Jan 2014 23:21:29 +0400 Subject: [PATCH 278/447] navigator: force takeoff if first mission item is not takeoff and vehicle landed --- src/modules/commander/commander_params.c | 2 - src/modules/navigator/navigator_main.cpp | 156 +++++++++++++++++------ src/modules/navigator/navigator_params.c | 4 +- src/modules/uORB/topics/mission.h | 2 +- 4 files changed, 124 insertions(+), 40 deletions(-) diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index 40d0386d5e..691d3efcb0 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -45,8 +45,6 @@ #include #include -PARAM_DEFINE_FLOAT(NAV_TAKEOFF_ALT, 5.0f); -PARAM_DEFINE_FLOAT(NAV_TAKEOFF_GAP, 3.0f); PARAM_DEFINE_FLOAT(TRIM_ROLL, 0.0f); PARAM_DEFINE_FLOAT(TRIM_PITCH, 0.0f); PARAM_DEFINE_FLOAT(TRIM_YAW, 0.0f); diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 982ebefccb..757eb46908 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -170,6 +170,7 @@ private: bool _waypoint_position_reached; bool _waypoint_yaw_reached; uint64_t _time_first_inside_orbit; + bool _force_takeoff; MissionFeasibilityChecker missionFeasiblityChecker; @@ -179,13 +180,16 @@ private: float min_altitude; float loiter_radius; int onboard_mission_enabled; + float land_alt; + float rtl_alt; } _parameters; /**< local copies of parameters */ struct { param_t min_altitude; param_t loiter_radius; param_t onboard_mission_enabled; - + param_t land_alt; + param_t rtl_alt; } _parameter_handles; /**< handles for parameters */ enum Event { @@ -204,6 +208,14 @@ private: */ static StateTable::Tran const myTable[NAV_STATE_MAX][MAX_EVENT]; + enum RTLState { + RTL_STATE_CLIMB, + RTL_STATE_RETURN, + RTL_STATE_LAND + }; + + enum RTLState _rtl_state; + /** * Update our local parameter cache. */ @@ -289,7 +301,12 @@ private: /** * Helper function to get a loiter item */ - void get_loiter_item(mission_item_s *new_loiter_position); + void get_loiter_item(mission_item_s *item); + + /** + * Helper function to get a takeoff item + */ + void get_takeoff_item(mission_item_s *item); /** * Publish a new mission item triplet for position controller @@ -361,13 +378,16 @@ Navigator::Navigator() : _waypoint_position_reached(false), _waypoint_yaw_reached(false), _time_first_inside_orbit(0), - _set_nav_state_timestamp(0) + _set_nav_state_timestamp(0), + _force_takeoff(false) { memset(&_fence, 0, sizeof(_fence)); _parameter_handles.min_altitude = param_find("NAV_MIN_ALT"); _parameter_handles.loiter_radius = param_find("NAV_LOITER_RAD"); _parameter_handles.onboard_mission_enabled = param_find("NAV_ONB_MIS_EN"); + _parameter_handles.land_alt = param_find("NAV_LAND_ALT"); + _parameter_handles.rtl_alt = param_find("NAV_RTL_ALT"); _mission_item_triplet.previous_valid = false; _mission_item_triplet.current_valid = false; @@ -418,6 +438,8 @@ Navigator::parameters_update() param_get(_parameter_handles.min_altitude, &(_parameters.min_altitude)); param_get(_parameter_handles.loiter_radius, &(_parameters.loiter_radius)); param_get(_parameter_handles.onboard_mission_enabled, &(_parameters.onboard_mission_enabled)); + param_get(_parameter_handles.land_alt, &(_parameters.land_alt)); + param_get(_parameter_handles.rtl_alt, &(_parameters.rtl_alt)); _mission.set_onboard_mission_allowed((bool)_parameter_handles.onboard_mission_enabled); } @@ -706,8 +728,14 @@ Navigator::task_main() if (myState == NAV_STATE_MISSION || myState == NAV_STATE_RTL) { if (mission_item_reached()) { if (myState == NAV_STATE_MISSION) { - /* advance by one mission item */ - _mission.move_to_next(); + if (_force_takeoff) { + /* forced takeoff completed */ + _force_takeoff = false; + mavlink_log_info(_mavlink_fd, "[navigator] forced takeoff completed"); + } else { + /* advance by one mission item */ + _mission.move_to_next(); + } /* if no more mission items available send this to state machine and start loiter at the last WP */ if (_mission.current_mission_available()) { @@ -948,6 +976,7 @@ Navigator::start_none() _reset_loiter_pos = true; _rtl_done = false; + _force_takeoff = false; publish_mission_item_triplet(); } @@ -955,6 +984,8 @@ Navigator::start_none() void Navigator::start_loiter() { + _force_takeoff = false; + /* set loiter position if needed */ if (_reset_loiter_pos || !_mission_item_triplet.current_valid) { _reset_loiter_pos = false; @@ -985,51 +1016,77 @@ Navigator::start_loiter() publish_mission_item_triplet(); } - void Navigator::start_mission() { - // TODO set prev item to current position? - _reset_loiter_pos = true; - _rtl_done = false; - int ret; bool onboard; unsigned index; + // TODO set prev item to current position? + _reset_loiter_pos = true; + _rtl_done = false; + _force_takeoff = false; + + _mission_item_triplet.previous_valid = false; + ret = _mission.get_current_mission_item(&_mission_item_triplet.current, &onboard, &index); if (ret == OK) { - - add_home_pos_to_rtl(&_mission_item_triplet.current); _mission_item_triplet.current_valid = true; + add_home_pos_to_rtl(&_mission_item_triplet.current); - if (onboard) { - mavlink_log_info(_mavlink_fd, "[navigator] mission started, onboard WP %d", index); + if (_mission_item_triplet.current.nav_cmd != NAV_CMD_TAKEOFF && _vstatus.condition_landed) { + /* if landed, takeoff first, if this not defined in mission */ + _force_takeoff = true; + + /* move current mission item to next */ + memcpy(&_mission_item_triplet.next, &_mission_item_triplet.current, sizeof(mission_item_s)); + _mission_item_triplet.next_valid = true; + + /* fill takeoff item */ + get_takeoff_item(&_mission_item_triplet.current); + if (_vstatus.is_rotary_wing) { + /* for VTOL use current position */ + _mission_item_triplet.current.lat = (double)_global_pos.lat / 1e7d; + _mission_item_triplet.current.lon = (double)_global_pos.lon / 1e7d; + } else { + /* for fixed wing use first waypoint position */ + // TODO what if first mission item has no lat/lon? + _mission_item_triplet.current.lat = _mission_item_triplet.next.lat; + _mission_item_triplet.current.lon = _mission_item_triplet.next.lon; + } + _mission_item_triplet.current.altitude = _global_pos.alt + _parameters.land_alt; + _mission_item_triplet.current.altitude_is_relative = false; + mavlink_log_info(_mavlink_fd, "[navigator] mission started, force takeoff to %.1fm", _mission_item_triplet.current.altitude); } else { - mavlink_log_info(_mavlink_fd, "[navigator] mission started, offboard WP %d", index); + /* normal mission start: mission starts with takeoff item or vehicle is already in air */ + ret = _mission.get_next_mission_item(&_mission_item_triplet.next); + + if (ret == OK) { + add_home_pos_to_rtl(&_mission_item_triplet.next); + _mission_item_triplet.next_valid = true; + } else { + /* this will fail for the last WP */ + _mission_item_triplet.next_valid = false; + } + + if (onboard) { + mavlink_log_info(_mavlink_fd, "[navigator] mission started, onboard WP %d", index); + } else { + mavlink_log_info(_mavlink_fd, "[navigator] mission started, offboard WP %d", index); + } } } else { - /* since a mission is started without knowledge if there are more mission items available, this can fail */ + /* mission is started without knowledge if there are more mission, indicates bug in navigator */ _mission_item_triplet.current_valid = false; - } - - ret = _mission.get_next_mission_item(&_mission_item_triplet.next); - - if (ret == OK) { - - add_home_pos_to_rtl(&_mission_item_triplet.next); - _mission_item_triplet.next_valid = true; - } else { - /* this will fail for the last WP */ _mission_item_triplet.next_valid = false; + mavlink_log_critical(_mavlink_fd, "[navigator] error: MISSION mode without mission defined"); } publish_mission_item_triplet(); } - - void Navigator::advance_mission() { @@ -1174,7 +1231,7 @@ Navigator::mission_item_reached() // if (coordinate_frame == (int)MAV_FRAME_GLOBAL) { - // current relative or AMSL altitude depending on _mission_item_triplet.current.altitude_is_relative + /* current relative or AMSL altitude depending on mission item altitude_is_relative flag */ float current_alt = _mission_item_triplet.current.altitude_is_relative ? _global_pos.relative_alt : _global_pos.alt; dist = get_distance_to_point_global_wgs84(_mission_item_triplet.current.lat, _mission_item_triplet.current.lon, _mission_item_triplet.current.altitude, @@ -1199,7 +1256,7 @@ Navigator::mission_item_reached() if (_mission_item_triplet.current.nav_cmd == NAV_CMD_TAKEOFF) { /* require only altitude for takeoff */ - if (current_alt > _mission_item_triplet.current.altitude) + if (current_alt >= _mission_item_triplet.current.altitude) _waypoint_position_reached = true; } else { if (dist >= 0.0f && dist_xy <= orbit && dist_z >= 0.0f && dist_z <= vertical_switch_distance) { @@ -1237,13 +1294,40 @@ Navigator::mission_item_reached() } void -Navigator::get_loiter_item(struct mission_item_s *new_loiter_position) +Navigator::get_loiter_item(struct mission_item_s *item) { - new_loiter_position->nav_cmd = NAV_CMD_LOITER_UNLIMITED; - new_loiter_position->loiter_direction = 1; - new_loiter_position->loiter_radius = _parameters.loiter_radius; // TODO: get rid of magic number - new_loiter_position->radius = 50.0f; // TODO: get rid of magic number - new_loiter_position->autocontinue = false; + //item->altitude_is_relative + //item->lat + //item->lon + //item->altitude + item->yaw = NAN; + item->loiter_radius = _parameters.loiter_radius; // TODO: get rid of magic number + item->loiter_direction = 1; + item->nav_cmd = NAV_CMD_LOITER_UNLIMITED; + item->radius = 50.0f; // TODO: get rid of magic number + //item->time_inside + //item->pitch_min + item->autocontinue = false; + item->origin = ORIGIN_ONBOARD; + +} + +void +Navigator::get_takeoff_item(mission_item_s *item) +{ + //item->altitude_is_relative + //item->lat + //item->lon + //item->altitude + item->yaw = NAN; + item->loiter_radius = _parameters.loiter_radius; // TODO: get rid of magic number + item->loiter_direction = 1; + item->nav_cmd = NAV_CMD_TAKEOFF; + item->radius = 50.0f; // TODO: get rid of magic number + item->time_inside = 0.0f; + item->pitch_min = 0.3; // TODO: get rid of magic number + item->autocontinue = true; + item->origin = ORIGIN_ONBOARD; } void diff --git a/src/modules/navigator/navigator_params.c b/src/modules/navigator/navigator_params.c index b9d887379b..63601ce6dd 100644 --- a/src/modules/navigator/navigator_params.c +++ b/src/modules/navigator/navigator_params.c @@ -53,4 +53,6 @@ PARAM_DEFINE_FLOAT(NAV_MIN_ALT, 50.0f); PARAM_DEFINE_FLOAT(NAV_LOITER_RAD, 100.0f); -PARAM_DEFINE_INT32(NAV_ONB_MIS_EN, 0); \ No newline at end of file +PARAM_DEFINE_INT32(NAV_ONB_MIS_EN, 0); +PARAM_DEFINE_FLOAT(NAV_LAND_ALT, 10.0f); // default TAKEOFF altitude, slow descend from this altitude when landing in RTL mode +PARAM_DEFINE_FLOAT(NAV_RTL_ALT, 10.0f); // min altitude for going home in RTL mode diff --git a/src/modules/uORB/topics/mission.h b/src/modules/uORB/topics/mission.h index 9b4250115d..95bceb8bb0 100644 --- a/src/modules/uORB/topics/mission.h +++ b/src/modules/uORB/topics/mission.h @@ -83,7 +83,7 @@ struct mission_item_s double lat; /**< latitude in degrees */ double lon; /**< longitude in degrees */ float altitude; /**< altitude in meters */ - float yaw; /**< in radians NED -PI..+PI */ + float yaw; /**< in radians NED -PI..+PI, NAN means don't change yaw */ float loiter_radius; /**< loiter radius in meters, 0 for a VTOL to hover */ int8_t loiter_direction; /**< 1: positive / clockwise, -1, negative. */ enum NAV_CMD nav_cmd; /**< navigation command */ From 4191ae33c264459f0a85d9c03b8cb4893c6ee33e Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 1 Jan 2014 21:54:33 +0100 Subject: [PATCH 279/447] navigator/mission feasibility: prepare for pre-mission fence checking --- .../navigator/mission_feasibility_checker.cpp | 21 ++++++++++--------- .../navigator/mission_feasibility_checker.h | 10 ++++----- src/modules/navigator/navigator_main.cpp | 2 +- 3 files changed, 17 insertions(+), 16 deletions(-) diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp index 25b2636bb8..cc079dee19 100644 --- a/src/modules/navigator/mission_feasibility_checker.cpp +++ b/src/modules/navigator/mission_feasibility_checker.cpp @@ -48,6 +48,7 @@ #include #include #include +#include /* oddly, ERROR is not defined for c++ */ #ifdef ERROR @@ -61,7 +62,7 @@ MissionFeasibilityChecker::MissionFeasibilityChecker() : _mavlink_fd(-1), _capab } -bool MissionFeasibilityChecker::checkMissionFeasible(bool isRotarywing, dm_item_t dm_current, size_t nItems) +bool MissionFeasibilityChecker::checkMissionFeasible(bool isRotarywing, dm_item_t dm_current, size_t nMissionItems, const struct fence_s &fence) { /* Init if not done yet */ init(); @@ -74,39 +75,39 @@ bool MissionFeasibilityChecker::checkMissionFeasible(bool isRotarywing, dm_item_ if (isRotarywing) - return checkMissionFeasibleRotarywing(dm_current, nItems); + return checkMissionFeasibleRotarywing(dm_current, nMissionItems, fence); else - return checkMissionFeasibleFixedwing(dm_current, nItems); + return checkMissionFeasibleFixedwing(dm_current, nMissionItems, fence); } -bool MissionFeasibilityChecker::checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nItems) +bool MissionFeasibilityChecker::checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nMissionItems, const struct fence_s &fence) { - return checkGeofence(dm_current, nItems); + return checkGeofence(dm_current, nMissionItems, fence); } -bool MissionFeasibilityChecker::checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nItems) +bool MissionFeasibilityChecker::checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nMissionItems, const struct fence_s &fence) { /* Update fixed wing navigation capabilites */ updateNavigationCapabilities(); // warnx("_nav_caps.landing_slope_angle_rad %.4f, _nav_caps.landing_horizontal_slope_displacement %.4f", _nav_caps.landing_slope_angle_rad, _nav_caps.landing_horizontal_slope_displacement); - return (checkFixedWingLanding(dm_current, nItems) && checkGeofence(dm_current, nItems)); + return (checkFixedWingLanding(dm_current, nMissionItems) && checkGeofence(dm_current, nMissionItems, fence)); } -bool MissionFeasibilityChecker::checkGeofence(dm_item_t dm_current, size_t nItems) +bool MissionFeasibilityChecker::checkGeofence(dm_item_t dm_current, size_t nMissionItems, const struct fence_s &fence) { //xxx: check geofence return true; } -bool MissionFeasibilityChecker::checkFixedWingLanding(dm_item_t dm_current, size_t nItems) +bool MissionFeasibilityChecker::checkFixedWingLanding(dm_item_t dm_current, size_t nMissionItems) { /* Go through all mission items and search for a landing waypoint * if landing waypoint is found: the previous waypoint is checked to be at a feasible distance and altitude given the landing slope */ - for (size_t i = 0; i < nItems; i++) { + for (size_t i = 0; i < nMissionItems; i++) { static struct mission_item_s missionitem; const ssize_t len = sizeof(struct mission_item_s); if (dm_read(dm_current, i, &missionitem, len) != len) { diff --git a/src/modules/navigator/mission_feasibility_checker.h b/src/modules/navigator/mission_feasibility_checker.h index 7d1cc2f8a9..ef235ead44 100644 --- a/src/modules/navigator/mission_feasibility_checker.h +++ b/src/modules/navigator/mission_feasibility_checker.h @@ -57,15 +57,15 @@ private: void init(); /* Checks for all airframes */ - bool checkGeofence(dm_item_t dm_current, size_t nItems); + bool checkGeofence(dm_item_t dm_current, size_t nMissionItems, const struct fence_s &fence); /* Checks specific to fixedwing airframes */ - bool checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nItems); - bool checkFixedWingLanding(dm_item_t dm_current, size_t nItems); + bool checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nMissionItems, const struct fence_s &fence); + bool checkFixedWingLanding(dm_item_t dm_current, size_t nMissionItems); void updateNavigationCapabilities(); /* Checks specific to rotarywing airframes */ - bool checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nItems); + bool checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nMissionItems, const struct fence_s &fence); public: MissionFeasibilityChecker(); @@ -74,7 +74,7 @@ public: /* * Returns true if mission is feasible and false otherwise */ - bool checkMissionFeasible(bool isRotarywing, dm_item_t dm_current, size_t nItems); + bool checkMissionFeasible(bool isRotarywing, dm_item_t dm_current, size_t nMissionItems, const struct fence_s &fence); }; diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 982ebefccb..c88f237ad0 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -456,7 +456,7 @@ Navigator::offboard_mission_update(bool isrotaryWing) } else { dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1; } - missionFeasiblityChecker.checkMissionFeasible(isrotaryWing, dm_current, (size_t)offboard_mission.count); + missionFeasiblityChecker.checkMissionFeasible(isrotaryWing, dm_current, (size_t)offboard_mission.count, _fence); _mission.set_offboard_dataman_id(offboard_mission.dataman_id); _mission.set_current_offboard_mission_index(offboard_mission.current_index); From 74e2542c07ffc154139ea12d722e5396b083f308 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 2 Jan 2014 14:49:34 +0400 Subject: [PATCH 280/447] navigator: takeoff and mission fixes --- .../fw_pos_control_l1_main.cpp | 2 +- src/modules/mavlink/waypoints.c | 4 +- src/modules/navigator/navigator_main.cpp | 308 +++++++++--------- src/modules/navigator/navigator_params.c | 6 +- src/modules/sdlog2/sdlog2.c | 4 +- src/modules/sdlog2/sdlog2_messages.h | 4 +- src/modules/uORB/topics/mission.h | 2 +- 7 files changed, 166 insertions(+), 164 deletions(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index ee0aca69e3..4f5e6d1a5a 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -1230,7 +1230,7 @@ FixedwingPositionControl::task_main() } /* XXX check if radius makes sense here */ - float turn_distance = _l1_control.switch_distance(_mission_item_triplet.current.radius); + float turn_distance = _l1_control.switch_distance(_mission_item_triplet.current.acceptance_radius); /* lazily publish navigation capabilities */ if (turn_distance != _nav_capabilities.turn_distance && turn_distance > 0) { diff --git a/src/modules/mavlink/waypoints.c b/src/modules/mavlink/waypoints.c index 2ff11e813e..1adfeafde0 100644 --- a/src/modules/mavlink/waypoints.c +++ b/src/modules/mavlink/waypoints.c @@ -143,7 +143,7 @@ int map_mavlink_mission_item_to_mission_item(const mavlink_mission_item_t *mavli mission_item->pitch_min = mavlink_mission_item->param2; break; default: - mission_item->radius = mavlink_mission_item->param2; + mission_item->acceptance_radius = mavlink_mission_item->param2; break; } @@ -173,7 +173,7 @@ int map_mission_item_to_mavlink_mission_item(const struct mission_item_s *missio mavlink_mission_item->param2 = mission_item->pitch_min; break; default: - mavlink_mission_item->param2 = mission_item->radius; + mavlink_mission_item->param2 = mission_item->acceptance_radius; break; } diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 757eb46908..a6f33b5e7c 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -170,7 +170,8 @@ private: bool _waypoint_position_reached; bool _waypoint_yaw_reached; uint64_t _time_first_inside_orbit; - bool _force_takeoff; + bool _need_takeoff; /**< if need to perform vertical takeoff before going to waypoint (only for MISSION mode and VTOL vehicles) */ + bool _do_takeoff; /**< vertical takeoff state, current mission item is generated by navigator (only for MISSION mode and VTOL vehicles) */ MissionFeasibilityChecker missionFeasiblityChecker; @@ -178,16 +179,20 @@ private: struct { float min_altitude; + float acceptance_radius; float loiter_radius; int onboard_mission_enabled; + float takeoff_alt; float land_alt; float rtl_alt; } _parameters; /**< local copies of parameters */ struct { param_t min_altitude; + param_t acceptance_radius; param_t loiter_radius; param_t onboard_mission_enabled; + param_t takeoff_alt; param_t land_alt; param_t rtl_alt; } _parameter_handles; /**< handles for parameters */ @@ -291,7 +296,12 @@ private: /** * Check if current mission item has been reached. */ - bool mission_item_reached(); + bool check_mission_item_reached(); + + /** + * Perform actions when current mission item reached. + */ + void on_mission_item_reached(); /** * Move to next waypoint @@ -379,13 +389,16 @@ Navigator::Navigator() : _waypoint_yaw_reached(false), _time_first_inside_orbit(0), _set_nav_state_timestamp(0), - _force_takeoff(false) + _need_takeoff(true), + _do_takeoff(false) { memset(&_fence, 0, sizeof(_fence)); _parameter_handles.min_altitude = param_find("NAV_MIN_ALT"); + _parameter_handles.acceptance_radius = param_find("NAV_ACCEPT_RAD"); _parameter_handles.loiter_radius = param_find("NAV_LOITER_RAD"); _parameter_handles.onboard_mission_enabled = param_find("NAV_ONB_MIS_EN"); + _parameter_handles.takeoff_alt = param_find("NAV_TAKEOFF_ALT"); _parameter_handles.land_alt = param_find("NAV_LAND_ALT"); _parameter_handles.rtl_alt = param_find("NAV_RTL_ALT"); @@ -436,8 +449,10 @@ Navigator::parameters_update() orb_copy(ORB_ID(parameter_update), _params_sub, &update); param_get(_parameter_handles.min_altitude, &(_parameters.min_altitude)); + param_get(_parameter_handles.acceptance_radius, &(_parameters.acceptance_radius)); param_get(_parameter_handles.loiter_radius, &(_parameters.loiter_radius)); param_get(_parameter_handles.onboard_mission_enabled, &(_parameters.onboard_mission_enabled)); + param_get(_parameter_handles.takeoff_alt, &(_parameters.takeoff_alt)); param_get(_parameter_handles.land_alt, &(_parameters.land_alt)); param_get(_parameter_handles.rtl_alt, &(_parameters.rtl_alt)); @@ -726,27 +741,8 @@ Navigator::task_main() global_position_update(); /* only check if waypoint has been reached in MISSION */ if (myState == NAV_STATE_MISSION || myState == NAV_STATE_RTL) { - if (mission_item_reached()) { - if (myState == NAV_STATE_MISSION) { - if (_force_takeoff) { - /* forced takeoff completed */ - _force_takeoff = false; - mavlink_log_info(_mavlink_fd, "[navigator] forced takeoff completed"); - } else { - /* advance by one mission item */ - _mission.move_to_next(); - } - - /* if no more mission items available send this to state machine and start loiter at the last WP */ - if (_mission.current_mission_available()) { - advance_mission(); - } else { - dispatch(EVENT_MISSION_FINISHED); - } - } else { - /* RTL finished */ - dispatch(EVENT_MISSION_FINISHED); - } + if (check_mission_item_reached()) { + on_mission_item_reached(); } } } @@ -976,7 +972,7 @@ Navigator::start_none() _reset_loiter_pos = true; _rtl_done = false; - _force_takeoff = false; + _do_takeoff = false; publish_mission_item_triplet(); } @@ -984,7 +980,7 @@ Navigator::start_none() void Navigator::start_loiter() { - _force_takeoff = false; + _do_takeoff = false; /* set loiter position if needed */ if (_reset_loiter_pos || !_mission_item_triplet.current_valid) { @@ -997,7 +993,7 @@ Navigator::start_loiter() _mission_item_triplet.current.altitude_is_relative = false; float min_alt_amsl = _parameters.min_altitude + _home_pos.altitude; - /* Use current altitude if above min altitude set by parameter */ + /* use current altitude if above min altitude set by parameter */ if (_global_pos.alt < min_alt_amsl) { _mission_item_triplet.current.altitude = min_alt_amsl; mavlink_log_info(_mavlink_fd, "[navigator] loiter %.1fm higher", (double)(min_alt_amsl - _global_pos.alt)); @@ -1019,72 +1015,10 @@ Navigator::start_loiter() void Navigator::start_mission() { - int ret; - bool onboard; - unsigned index; - - // TODO set prev item to current position? - _reset_loiter_pos = true; _rtl_done = false; - _force_takeoff = false; + _need_takeoff = true; - _mission_item_triplet.previous_valid = false; - - ret = _mission.get_current_mission_item(&_mission_item_triplet.current, &onboard, &index); - - if (ret == OK) { - _mission_item_triplet.current_valid = true; - add_home_pos_to_rtl(&_mission_item_triplet.current); - - if (_mission_item_triplet.current.nav_cmd != NAV_CMD_TAKEOFF && _vstatus.condition_landed) { - /* if landed, takeoff first, if this not defined in mission */ - _force_takeoff = true; - - /* move current mission item to next */ - memcpy(&_mission_item_triplet.next, &_mission_item_triplet.current, sizeof(mission_item_s)); - _mission_item_triplet.next_valid = true; - - /* fill takeoff item */ - get_takeoff_item(&_mission_item_triplet.current); - if (_vstatus.is_rotary_wing) { - /* for VTOL use current position */ - _mission_item_triplet.current.lat = (double)_global_pos.lat / 1e7d; - _mission_item_triplet.current.lon = (double)_global_pos.lon / 1e7d; - } else { - /* for fixed wing use first waypoint position */ - // TODO what if first mission item has no lat/lon? - _mission_item_triplet.current.lat = _mission_item_triplet.next.lat; - _mission_item_triplet.current.lon = _mission_item_triplet.next.lon; - } - _mission_item_triplet.current.altitude = _global_pos.alt + _parameters.land_alt; - _mission_item_triplet.current.altitude_is_relative = false; - mavlink_log_info(_mavlink_fd, "[navigator] mission started, force takeoff to %.1fm", _mission_item_triplet.current.altitude); - } else { - /* normal mission start: mission starts with takeoff item or vehicle is already in air */ - ret = _mission.get_next_mission_item(&_mission_item_triplet.next); - - if (ret == OK) { - add_home_pos_to_rtl(&_mission_item_triplet.next); - _mission_item_triplet.next_valid = true; - } else { - /* this will fail for the last WP */ - _mission_item_triplet.next_valid = false; - } - - if (onboard) { - mavlink_log_info(_mavlink_fd, "[navigator] mission started, onboard WP %d", index); - } else { - mavlink_log_info(_mavlink_fd, "[navigator] mission started, offboard WP %d", index); - } - } - } else { - /* mission is started without knowledge if there are more mission, indicates bug in navigator */ - _mission_item_triplet.current_valid = false; - _mission_item_triplet.next_valid = false; - mavlink_log_critical(_mavlink_fd, "[navigator] error: MISSION mode without mission defined"); - } - - publish_mission_item_triplet(); + advance_mission(); } void @@ -1095,6 +1029,7 @@ Navigator::advance_mission() _mission_item_triplet.previous_valid = _mission_item_triplet.current_valid; _reset_loiter_pos = true; + _do_takeoff = false; int ret; bool onboard; @@ -1103,14 +1038,54 @@ Navigator::advance_mission() ret = _mission.get_current_mission_item(&_mission_item_triplet.current, &onboard, &index); if (ret == OK) { - - add_home_pos_to_rtl(&_mission_item_triplet.current); _mission_item_triplet.current_valid = true; + add_home_pos_to_rtl(&_mission_item_triplet.current); - if (onboard) { - mavlink_log_info(_mavlink_fd, "[navigator] heading to onboard WP %d", index); + if (_vstatus.is_rotary_wing) { + if (_need_takeoff && ( + _mission_item_triplet.current.nav_cmd == NAV_CMD_TAKEOFF || + _mission_item_triplet.current.nav_cmd == NAV_CMD_WAYPOINT || + _mission_item_triplet.current.nav_cmd == NAV_CMD_RETURN_TO_LAUNCH || + _mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || + _mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_TURN_COUNT || + _mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_UNLIMITED + )) { + /* do special TAKEOFF handling for VTOL */ + _need_takeoff = false; + + /* check if we really need takeoff */ + float wp_alt_amsl = _mission_item_triplet.current.altitude; + if (_mission_item_triplet.current.altitude_is_relative) + wp_alt_amsl += _home_pos.altitude; + if (_vstatus.condition_landed || _global_pos.alt - _home_pos.altitude < _parameters.takeoff_alt || + (_mission_item_triplet.next.nav_cmd != NAV_CMD_RETURN_TO_LAUNCH && _global_pos.alt > wp_alt_amsl)) { + /* force TAKEOFF if (landed or near ground) and waypoint altitude is more than current */ + _do_takeoff = true; + + /* move current mission item to next */ + memcpy(&_mission_item_triplet.next, &_mission_item_triplet.current, sizeof(mission_item_s)); + _mission_item_triplet.next_valid = true; + + /* set current item to TAKEOFF */ + get_takeoff_item(&_mission_item_triplet.current); + + _mission_item_triplet.current.lat = (double)_global_pos.lat / 1e7d; + _mission_item_triplet.current.lon = (double)_global_pos.lon / 1e7d; + } + } else if (_mission_item_triplet.current.nav_cmd == NAV_CMD_LAND) { + /* will need takeoff after landing */ + _need_takeoff = true; + } + } + + if (_do_takeoff) { + mavlink_log_info(_mavlink_fd, "[navigator] takeoff to %.1fm", _mission_item_triplet.current.altitude); } else { - mavlink_log_info(_mavlink_fd, "[navigator] heading to offboard WP %d", index); + if (onboard) { + mavlink_log_info(_mavlink_fd, "[navigator] heading to onboard WP %d", index); + } else { + mavlink_log_info(_mavlink_fd, "[navigator] heading to offboard WP %d", index); + } } } else { /* since a mission is not advanced without WPs available, this is not supposed to happen */ @@ -1118,16 +1093,16 @@ Navigator::advance_mission() warnx("ERROR: current WP can't be set"); } - ret = _mission.get_next_mission_item(&_mission_item_triplet.next); + if (!_do_takeoff) { + ret = _mission.get_next_mission_item(&_mission_item_triplet.next); - if (ret == OK) { - - add_home_pos_to_rtl(&_mission_item_triplet.next); - - _mission_item_triplet.next_valid = true; - } else { - /* this will fail for the last WP */ - _mission_item_triplet.next_valid = false; + if (ret == OK) { + add_home_pos_to_rtl(&_mission_item_triplet.next); + _mission_item_triplet.next_valid = true; + } else { + /* this will fail for the last WP */ + _mission_item_triplet.next_valid = false; + } } publish_mission_item_triplet(); @@ -1149,6 +1124,7 @@ Navigator::start_rtl() { _reset_loiter_pos = true; _rtl_done = false; + _do_takeoff = false; /* discard all mission item and insert RTL item */ _mission_item_triplet.previous_valid = false; @@ -1157,12 +1133,13 @@ Navigator::start_rtl() _mission_item_triplet.current.lat = _home_pos.lat; _mission_item_triplet.current.lon = _home_pos.lon; - _mission_item_triplet.current.altitude = _home_pos.altitude + _parameters.min_altitude; + _mission_item_triplet.current.altitude = _home_pos.altitude + _parameters.rtl_alt; + _mission_item_triplet.current.altitude_is_relative = false; _mission_item_triplet.current.yaw = 0.0f; // TODO use heading to waypoint? _mission_item_triplet.current.nav_cmd = NAV_CMD_RETURN_TO_LAUNCH; _mission_item_triplet.current.loiter_direction = 1; _mission_item_triplet.current.loiter_radius = _parameters.loiter_radius; - _mission_item_triplet.current.radius = 50.0f; // TODO: get rid of magic number + _mission_item_triplet.current.acceptance_radius = _parameters.acceptance_radius; _mission_item_triplet.current.autocontinue = false; _mission_item_triplet.current_valid = true; @@ -1184,7 +1161,7 @@ Navigator::finish_rtl() } bool -Navigator::mission_item_reached() +Navigator::check_mission_item_reached() { /* only check if there is actually a mission item to check */ if (!_mission_item_triplet.current_valid) { @@ -1206,22 +1183,15 @@ Navigator::mission_item_reached() } uint64_t now = hrt_absolute_time(); - float orbit; + float acceptance_radius; - if (_mission_item_triplet.current.nav_cmd == NAV_CMD_WAYPOINT && _mission_item_triplet.current.radius > 0.01f) { - - orbit = _mission_item_triplet.current.radius; + if (_mission_item_triplet.current.nav_cmd == NAV_CMD_WAYPOINT && _mission_item_triplet.current.acceptance_radius > 0.01f) { + acceptance_radius = _mission_item_triplet.current.acceptance_radius; } else { - - // XXX set default orbit via param - orbit = 15.0f; + acceptance_radius = _parameters.acceptance_radius; } - /* keep vertical orbit */ - float vertical_switch_distance = orbit; - - // TODO add frame // int coordinate_frame = wpm->waypoints[wpm->current_active_wp_id].frame; @@ -1232,10 +1202,12 @@ Navigator::mission_item_reached() // if (coordinate_frame == (int)MAV_FRAME_GLOBAL) { /* current relative or AMSL altitude depending on mission item altitude_is_relative flag */ - float current_alt = _mission_item_triplet.current.altitude_is_relative ? _global_pos.relative_alt : _global_pos.alt; + float wp_alt_amsl = _mission_item_triplet.current.altitude; + if (_mission_item_triplet.current.altitude_is_relative) + _mission_item_triplet.current.altitude += _home_pos.altitude; - dist = get_distance_to_point_global_wgs84(_mission_item_triplet.current.lat, _mission_item_triplet.current.lon, _mission_item_triplet.current.altitude, - (double)_global_pos.lat / 1e7d, (double)_global_pos.lon / 1e7d, current_alt, + dist = get_distance_to_point_global_wgs84(_mission_item_triplet.current.lat, _mission_item_triplet.current.lon, wp_alt_amsl, + (double)_global_pos.lat / 1e7d, (double)_global_pos.lon / 1e7d, _global_pos.alt, &dist_xy, &dist_z); // warnx("1 lat: %2.2f, lon: %2.2f, alt: %2.2f", _mission_item_triplet.current.lat, _mission_item_triplet.current.lon, _mission_item_triplet.current.altitude); @@ -1254,25 +1226,29 @@ Navigator::mission_item_reached() // // XXX TODO // } - if (_mission_item_triplet.current.nav_cmd == NAV_CMD_TAKEOFF) { - /* require only altitude for takeoff */ - if (current_alt >= _mission_item_triplet.current.altitude) + if (_do_takeoff) { + if (_global_pos.alt > wp_alt_amsl - acceptance_radius) { + /* require only altitude for takeoff */ _waypoint_position_reached = true; + } } else { - if (dist >= 0.0f && dist_xy <= orbit && dist_z >= 0.0f && dist_z <= vertical_switch_distance) { + if (dist >= 0.0f && dist <= acceptance_radius) { _waypoint_position_reached = true; } } - /* check if required yaw reached */ - float yaw_err = _wrap_pi(_mission_item_triplet.current.yaw - _global_pos.yaw); - - if (fabsf(yaw_err) < 0.05f) { /* XXX get rid of magic number */ + if (_vstatus.is_rotary_wing && !_do_takeoff) { + /* check yow only for rotary wing except takeoff */ + float yaw_err = _wrap_pi(_mission_item_triplet.current.yaw - _global_pos.yaw); + if (fabsf(yaw_err) < 0.05f) { /* XXX get rid of magic number */ + _waypoint_yaw_reached = true; + } + } else { _waypoint_yaw_reached = true; } /* check if the current waypoint was reached */ - if (_waypoint_position_reached /* && _waypoint_yaw_reached */) { /* XXX what about yaw? */ + if (_waypoint_position_reached && _waypoint_yaw_reached) { if (_time_first_inside_orbit == 0) { /* XXX announcement? */ @@ -1293,6 +1269,31 @@ Navigator::mission_item_reached() } +void +Navigator::on_mission_item_reached() +{ + if (myState == NAV_STATE_MISSION) { + if (_do_takeoff) { + /* takeoff completed */ + _do_takeoff = false; + mavlink_log_info(_mavlink_fd, "[navigator] takeoff completed"); + } else { + /* advance by one mission item */ + _mission.move_to_next(); + } + + if (_mission.current_mission_available()) { + advance_mission(); + } else { + /* if no more mission items available then finish mission */ + dispatch(EVENT_MISSION_FINISHED); + } + } else { + /* RTL finished */ + dispatch(EVENT_MISSION_FINISHED); + } +} + void Navigator::get_loiter_item(struct mission_item_s *item) { @@ -1300,13 +1301,13 @@ Navigator::get_loiter_item(struct mission_item_s *item) //item->lat //item->lon //item->altitude - item->yaw = NAN; - item->loiter_radius = _parameters.loiter_radius; // TODO: get rid of magic number + //item->yaw + item->loiter_radius = _parameters.loiter_radius; item->loiter_direction = 1; item->nav_cmd = NAV_CMD_LOITER_UNLIMITED; - item->radius = 50.0f; // TODO: get rid of magic number - //item->time_inside - //item->pitch_min + item->acceptance_radius = _parameters.acceptance_radius; + item->time_inside = 0.0f; + item->pitch_min = 0.0f; item->autocontinue = false; item->origin = ORIGIN_ONBOARD; @@ -1320,16 +1321,30 @@ Navigator::get_takeoff_item(mission_item_s *item) //item->lon //item->altitude item->yaw = NAN; - item->loiter_radius = _parameters.loiter_radius; // TODO: get rid of magic number + item->loiter_radius = _parameters.loiter_radius; item->loiter_direction = 1; item->nav_cmd = NAV_CMD_TAKEOFF; - item->radius = 50.0f; // TODO: get rid of magic number + item->acceptance_radius = _parameters.acceptance_radius; item->time_inside = 0.0f; - item->pitch_min = 0.3; // TODO: get rid of magic number + item->pitch_min = 0.0; item->autocontinue = true; item->origin = ORIGIN_ONBOARD; } +void +Navigator::add_home_pos_to_rtl(struct mission_item_s *new_mission_item) +{ + if (new_mission_item->nav_cmd == NAV_CMD_RETURN_TO_LAUNCH) { + /* append the home position to RTL item */ + new_mission_item->lat = _home_pos.lat; + new_mission_item->lon = _home_pos.lon; + new_mission_item->altitude = _home_pos.altitude + _parameters.rtl_alt; + new_mission_item->altitude_is_relative = false; + new_mission_item->loiter_radius = _parameters.loiter_radius; + new_mission_item->acceptance_radius = _parameters.acceptance_radius; + } +} + void Navigator::publish_mission_item_triplet() { @@ -1424,7 +1439,7 @@ bool Navigator::cmp_mission_item_equivalent(const struct mission_item_s a, const fabsf(a.loiter_radius - b.loiter_radius) < FLT_EPSILON && a.loiter_direction == b.loiter_direction && a.nav_cmd == b.nav_cmd && - fabsf(a.radius - b.radius) < FLT_EPSILON && + fabsf(a.acceptance_radius - b.acceptance_radius) < FLT_EPSILON && fabsf(a.time_inside - b.time_inside) < FLT_EPSILON && a.autocontinue == b.autocontinue) { return true; @@ -1485,16 +1500,3 @@ int navigator_main(int argc, char *argv[]) return 0; } - -void -Navigator::add_home_pos_to_rtl(struct mission_item_s *new_mission_item) -{ - if (new_mission_item->nav_cmd == NAV_CMD_RETURN_TO_LAUNCH) { - /* if it is a RTL waypoint, append the home position */ - new_mission_item->lat = _home_pos.lat; - new_mission_item->lon = _home_pos.lon; - new_mission_item->altitude = _home_pos.altitude + _parameters.min_altitude; - new_mission_item->loiter_radius = _parameters.loiter_radius; // TODO: get rid of magic number - new_mission_item->radius = 50.0f; // TODO: get rid of magic number - } -} diff --git a/src/modules/navigator/navigator_params.c b/src/modules/navigator/navigator_params.c index 63601ce6dd..8df47fc3b6 100644 --- a/src/modules/navigator/navigator_params.c +++ b/src/modules/navigator/navigator_params.c @@ -52,7 +52,9 @@ */ PARAM_DEFINE_FLOAT(NAV_MIN_ALT, 50.0f); +PARAM_DEFINE_FLOAT(NAV_ACCEPT_RAD, 10.0f); PARAM_DEFINE_FLOAT(NAV_LOITER_RAD, 100.0f); PARAM_DEFINE_INT32(NAV_ONB_MIS_EN, 0); -PARAM_DEFINE_FLOAT(NAV_LAND_ALT, 10.0f); // default TAKEOFF altitude, slow descend from this altitude when landing in RTL mode -PARAM_DEFINE_FLOAT(NAV_RTL_ALT, 10.0f); // min altitude for going home in RTL mode +PARAM_DEFINE_FLOAT(NAV_TAAKEOFF_ALT, 10.0f); // default TAKEOFF altitude +PARAM_DEFINE_FLOAT(NAV_LAND_ALT, 5.0f); // slow descend from this altitude when landing +PARAM_DEFINE_FLOAT(NAV_RTL_ALT, 30.0f); // min altitude for going home in RTL mode diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 78322aff34..6833ec43f4 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1161,9 +1161,7 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.body.log_GPSP.nav_cmd = buf.triplet.current.nav_cmd; log_msg.body.log_GPSP.loiter_radius = buf.triplet.current.loiter_radius; log_msg.body.log_GPSP.loiter_direction = buf.triplet.current.loiter_direction; - log_msg.body.log_GPSP.loiter_radius = buf.triplet.current.loiter_radius; - log_msg.body.log_GPSP.loiter_direction = buf.triplet.current.loiter_direction; - log_msg.body.log_GPSP.radius = buf.triplet.current.radius; + log_msg.body.log_GPSP.acceptance_radius = buf.triplet.current.acceptance_radius; log_msg.body.log_GPSP.time_inside = buf.triplet.current.time_inside; log_msg.body.log_GPSP.pitch_min = buf.triplet.current.pitch_min; LOGBUFFER_WRITE_AND_COUNT(GPSP); diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index cb1393f1f7..3afaaa2ada 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -217,7 +217,7 @@ struct log_GPSP_s { uint8_t nav_cmd; float loiter_radius; int8_t loiter_direction; - float radius; + float acceptance_radius; float time_inside; float pitch_min; }; @@ -287,7 +287,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(ARSP, "fff", "RollRateSP,PitchRateSP,YawRateSP"), LOG_FORMAT(FLOW, "hhfffBB", "RawX,RawY,CompX,CompY,Dist,Q,SensID"), LOG_FORMAT(GPOS, "LLffff", "Lat,Lon,Alt,VelN,VelE,VelD"), - LOG_FORMAT(GPSP, "BLLffBfbfff", "AltRel,Lat,Lon,Alt,Yaw,NavCmd,LoitRad,LoitDir,Rad,TimeIn,pitMin"), + LOG_FORMAT(GPSP, "BLLffBfbfff", "AltRel,Lat,Lon,Alt,Yaw,NavCmd,LoitR,LoitDir,AccR,TimeIn,PitMin"), LOG_FORMAT(ESC, "HBBBHHHHHHfH", "Counter,NumESC,Conn,N,Ver,Adr,Volt,Amp,RPM,Temp,SetP,SetPRAW"), LOG_FORMAT(GVSP, "fff", "VX,VY,VZ"), /* system-level messages, ID >= 0x80 */ diff --git a/src/modules/uORB/topics/mission.h b/src/modules/uORB/topics/mission.h index 95bceb8bb0..194e2ed0c5 100644 --- a/src/modules/uORB/topics/mission.h +++ b/src/modules/uORB/topics/mission.h @@ -87,7 +87,7 @@ struct mission_item_s float loiter_radius; /**< loiter radius in meters, 0 for a VTOL to hover */ int8_t loiter_direction; /**< 1: positive / clockwise, -1, negative. */ enum NAV_CMD nav_cmd; /**< navigation command */ - float radius; /**< radius in which the mission is accepted as reached in meters */ + float acceptance_radius; /**< default radius in which the mission is accepted as reached in meters */ float time_inside; /**< time that the MAV should stay inside the radius before advancing in seconds */ float pitch_min; /**< minimal pitch angle for fixed wing takeoff waypoints */ bool autocontinue; /**< true if next waypoint should follow after this one */ From fe43a900c744c788a8a0231ec9f0748098fdcb14 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 2 Jan 2014 14:52:49 +0400 Subject: [PATCH 281/447] mc_pos_control: takeoff hack removed --- .../mc_pos_control/mc_pos_control_main.cpp | 32 ++++--------------- 1 file changed, 7 insertions(+), 25 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 3ee98bd410..a50a9e50e5 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -458,7 +458,6 @@ MulticopterPositionControl::task_main() bool reset_int_z_manual = false; bool reset_int_xy = true; bool was_armed = false; - bool reset_takeoff_sp = true; hrt_abstime t_prev = 0; @@ -511,7 +510,6 @@ MulticopterPositionControl::task_main() /* reset setpoints and integrals on arming */ reset_sp_z = true; reset_sp_xy = true; - reset_takeoff_sp = true; reset_int_z = true; reset_int_xy = true; } @@ -607,8 +605,6 @@ MulticopterPositionControl::task_main() _pos_sp = _pos + pos_sp_offs.emult(_params.vel_max); } - reset_takeoff_sp = true; - } else { /* AUTO */ if (_mission_items.current_valid) { @@ -617,30 +613,16 @@ MulticopterPositionControl::task_main() // TODO home altitude can be != ref_alt, check home_position topic _pos_sp(2) = -(item.altitude_is_relative ? item.altitude : item.altitude - ref_alt); - if (item.nav_cmd == NAV_CMD_TAKEOFF) { - /* use current position setpoint or current position */ - if (reset_sp_xy) { - reset_sp_xy = false; - _pos_sp(0) = _pos(0); - _pos_sp(1) = _pos(1); - } + map_projection_project(item.lat, item.lon, &_pos_sp(0), &_pos_sp(1)); - /* add gap for takeoff setpoint */ - _pos_sp(2) -= 0.5f * _params.vel_max(2) / _params.pos_p(2); - reset_sp_z = true; - - } else { - map_projection_project(item.lat, item.lon, &_pos_sp(0), &_pos_sp(1)); - - if (isfinite(_mission_items.current.yaw)) { - _att_sp.yaw_body = _mission_items.current.yaw; - } - - /* in case of interrupted mission don't go to waypoint but stay at current position */ - reset_sp_xy = true; - reset_sp_z = true; + if (isfinite(_mission_items.current.yaw)) { + _att_sp.yaw_body = _mission_items.current.yaw; } + /* in case of interrupted mission don't go to waypoint but stay at current position */ + reset_sp_xy = true; + reset_sp_z = true; + } else { /* no waypoint, loiter, reset position setpoint if needed */ if (reset_sp_xy) { From 911c2bdeeeb624c111c680b228e49d27391af484 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 2 Jan 2014 16:44:31 +0400 Subject: [PATCH 282/447] navigator: takeoff fixes --- src/modules/navigator/navigator_main.cpp | 20 ++++++++++++++------ 1 file changed, 14 insertions(+), 6 deletions(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index a6f33b5e7c..ca807df6f3 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -1053,13 +1053,19 @@ Navigator::advance_mission() /* do special TAKEOFF handling for VTOL */ _need_takeoff = false; - /* check if we really need takeoff */ - float wp_alt_amsl = _mission_item_triplet.current.altitude; + /* calculate desired takeoff altitude AMSL */ + float takeoff_alt_amsl = _mission_item_triplet.current.altitude; if (_mission_item_triplet.current.altitude_is_relative) - wp_alt_amsl += _home_pos.altitude; - if (_vstatus.condition_landed || _global_pos.alt - _home_pos.altitude < _parameters.takeoff_alt || - (_mission_item_triplet.next.nav_cmd != NAV_CMD_RETURN_TO_LAUNCH && _global_pos.alt > wp_alt_amsl)) { - /* force TAKEOFF if (landed or near ground) and waypoint altitude is more than current */ + takeoff_alt_amsl += _home_pos.altitude; + + if (_vstatus.condition_landed) { + /* takeoff to at least NAV_TAKEOFF_ALT from ground if landed */ + takeoff_alt_amsl = fmaxf(takeoff_alt_amsl, _global_pos.alt + _parameters.takeoff_alt); + } + + /* check if we really need takeoff */ + if (_vstatus.condition_landed || _global_pos.alt < takeoff_alt_amsl - _mission_item_triplet.current.acceptance_radius) { + /* force TAKEOFF if landed or waypoint altitude is more than current */ _do_takeoff = true; /* move current mission item to next */ @@ -1071,6 +1077,8 @@ Navigator::advance_mission() _mission_item_triplet.current.lat = (double)_global_pos.lat / 1e7d; _mission_item_triplet.current.lon = (double)_global_pos.lon / 1e7d; + _mission_item_triplet.current.altitude = takeoff_alt_amsl; + _mission_item_triplet.current.altitude_is_relative = false; } } else if (_mission_item_triplet.current.nav_cmd == NAV_CMD_LAND) { /* will need takeoff after landing */ From dca6d97a5288766e3e0da05dc5fdc98108fa7892 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 2 Jan 2014 14:18:02 +0100 Subject: [PATCH 283/447] create geofence class and start moving fence functionality to this class --- src/lib/geo/geo.c | 24 -------- src/lib/geo/geo.h | 11 ---- src/modules/navigator/geofence.cpp | 73 ++++++++++++++++++++++++ src/modules/navigator/geofence.h | 64 +++++++++++++++++++++ src/modules/navigator/module.mk | 3 +- src/modules/navigator/navigator_main.cpp | 2 + 6 files changed, 141 insertions(+), 36 deletions(-) create mode 100644 src/modules/navigator/geofence.cpp create mode 100644 src/modules/navigator/geofence.h diff --git a/src/lib/geo/geo.c b/src/lib/geo/geo.c index f64bfb41a4..08fe2b696e 100644 --- a/src/lib/geo/geo.c +++ b/src/lib/geo/geo.c @@ -503,27 +503,3 @@ __EXPORT float _wrap_360(float bearing) return bearing; } - -__EXPORT bool inside_geofence(const struct vehicle_global_position_s *vehicle, const struct fence_s *fence) -{ - - /* Adaptation of algorithm originally presented as - * PNPOLY - Point Inclusion in Polygon Test - * W. Randolph Franklin (WRF) */ - - unsigned int i, j, vertices = fence->count; - bool c = false; - double lat = vehicle->lat / 1e7d; - double lon = vehicle->lon / 1e7d; - - // skip vertex 0 (return point) - for (i = 0, j = vertices - 1; i < vertices; j = i++) - if (((fence->vertices[i].lon) >= lon != (fence->vertices[j].lon >= lon)) && - (lat <= (fence->vertices[j].lat - fence->vertices[i].lat) * (lon - fence->vertices[i].lon) / - (fence->vertices[j].lon - fence->vertices[i].lon) + fence->vertices[i].lat)) - c = !c; - return c; -} - - - diff --git a/src/lib/geo/geo.h b/src/lib/geo/geo.h index 5f92e14cf3..5f4bce6986 100644 --- a/src/lib/geo/geo.h +++ b/src/lib/geo/geo.h @@ -143,15 +143,4 @@ __EXPORT float _wrap_360(float bearing); __EXPORT float _wrap_pi(float bearing); __EXPORT float _wrap_2pi(float bearing); -/** - * Return whether craft is inside geofence. - * - * Calculate whether point is inside arbitrary polygon - * @param craft pointer craft coordinates - * @param fence pointer to array of coordinates, one per vertex. First and last vertex are assumed connected - * @return true: craft is inside fence, false:craft is outside fence - */ -__EXPORT bool inside_geofence(const struct vehicle_global_position_s *craft, const struct fence_s *fence); - - __END_DECLS diff --git a/src/modules/navigator/geofence.cpp b/src/modules/navigator/geofence.cpp new file mode 100644 index 0000000000..bc5ef44f2a --- /dev/null +++ b/src/modules/navigator/geofence.cpp @@ -0,0 +1,73 @@ +/**************************************************************************** + * + * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * Author: @author Jean Cyr + * @author Thomas Gubler + * + * 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 geofence.cpp + * Provides functions for handling the geofence + */ +#include "geofence.h" + +#include + +Geofence::Geofence() +{ + +} + +Geofence::~Geofence() +{ + +} + + +bool Geofence::inside(const struct vehicle_global_position_s *vehicle) +{ + + /* Adaptation of algorithm originally presented as + * PNPOLY - Point Inclusion in Polygon Test + * W. Randolph Franklin (WRF) */ + + unsigned int i, j, vertices = _fence.count; + bool c = false; + double lat = vehicle->lat / 1e7d; + double lon = vehicle->lon / 1e7d; + + // skip vertex 0 (return point) + for (i = 0, j = vertices - 1; i < vertices; j = i++) + if (((_fence.vertices[i].lon) >= lon != (_fence.vertices[j].lon >= lon)) && + (lat <= (_fence.vertices[j].lat - _fence.vertices[i].lat) * (lon - _fence.vertices[i].lon) / + (_fence.vertices[j].lon - _fence.vertices[i].lon) + _fence.vertices[i].lat)) + c = !c; + return c; +} diff --git a/src/modules/navigator/geofence.h b/src/modules/navigator/geofence.h new file mode 100644 index 0000000000..6d6e6e87c2 --- /dev/null +++ b/src/modules/navigator/geofence.h @@ -0,0 +1,64 @@ +/**************************************************************************** + * + * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * Author: @author Jean Cyr + * @author Thomas Gubler + * + * 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 geofence.h + * Provides functions for handling the geofence + */ + +#ifndef GEOFENCE_H_ +#define GEOFENCE_H_ + +#include + +class Geofence { +private: + struct fence_s _fence; +public: + Geofence(); + ~Geofence(); + + /** + * Return whether craft is inside geofence. + * + * Calculate whether point is inside arbitrary polygon + * @param craft pointer craft coordinates + * @param fence pointer to array of coordinates, one per vertex. First and last vertex are assumed connected + * @return true: craft is inside fence, false:craft is outside fence + */ + bool inside(const struct vehicle_global_position_s *craft); +}; + + +#endif /* GEOFENCE_H_ */ diff --git a/src/modules/navigator/module.mk b/src/modules/navigator/module.mk index 6be4e87a05..b7900e84ed 100644 --- a/src/modules/navigator/module.mk +++ b/src/modules/navigator/module.mk @@ -40,6 +40,7 @@ MODULE_COMMAND = navigator SRCS = navigator_main.cpp \ navigator_params.c \ navigator_mission.cpp \ - mission_feasibility_checker.cpp + mission_feasibility_checker.cpp \ + geofence.cpp INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index c88f237ad0..a9547355ff 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -78,6 +78,7 @@ #include "navigator_mission.h" #include "mission_feasibility_checker.h" +#include "geofence.h" /* oddly, ERROR is not defined for c++ */ @@ -157,6 +158,7 @@ private: perf_counter_t _loop_perf; /**< loop performance counter */ + Geofence geofence; struct fence_s _fence; /**< storage for fence vertices */ bool _fence_valid; /**< flag if fence is valid */ bool _inside_fence; /**< vehicle is inside fence */ From 429a11a21d25e34ca711b2c0debb2ac3e84c45ca Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 2 Jan 2014 15:01:08 +0100 Subject: [PATCH 284/447] navigator/geofence: move more functions to geofence class (WIP) --- src/modules/navigator/geofence.cpp | 97 ++++++++++++- src/modules/navigator/geofence.h | 16 +++ .../navigator/mission_feasibility_checker.cpp | 16 +-- .../navigator/mission_feasibility_checker.h | 9 +- src/modules/navigator/navigator_main.cpp | 127 ++---------------- 5 files changed, 138 insertions(+), 127 deletions(-) diff --git a/src/modules/navigator/geofence.cpp b/src/modules/navigator/geofence.cpp index bc5ef44f2a..4a0528b16f 100644 --- a/src/modules/navigator/geofence.cpp +++ b/src/modules/navigator/geofence.cpp @@ -39,10 +39,14 @@ #include "geofence.h" #include +#include +#include +#include +#include -Geofence::Geofence() +Geofence::Geofence() : _fence_pub(-1) { - + memset(&_fence, 0, sizeof(_fence)); } Geofence::~Geofence() @@ -71,3 +75,92 @@ bool Geofence::inside(const struct vehicle_global_position_s *vehicle) c = !c; return c; } + +bool +Geofence::load(unsigned vertices) +{ + struct fence_s temp_fence; + + unsigned i; + for (i = 0; i < vertices; i++) { + if (dm_read(DM_KEY_FENCE_POINTS, i, temp_fence.vertices + i, sizeof(struct fence_vertex_s)) != sizeof(struct fence_vertex_s)) { + break; + } + } + + temp_fence.count = i; + + if (valid()) + memcpy(&_fence, &temp_fence, sizeof(_fence)); + else + warnx("Invalid fence file, ignored!"); + + return _fence.count != 0; +} + +bool +Geofence::valid() +{ + // NULL fence is valid + if (_fence.count == 0) { + return true; + } + + // Otherwise + if ((_fence.count < 4) || (_fence.count > GEOFENCE_MAX_VERTICES)) { + warnx("Fence must have at least 3 sides and not more than %d", GEOFENCE_MAX_VERTICES - 1); + return false; + } + + return true; +} + +void +Geofence::addPoint(int argc, char *argv[]) +{ + int ix, last; + double lon, lat; + struct fence_vertex_s vertex; + char *end; + + if ((argc == 1) && (strcmp("-clear", argv[0]) == 0)) { + dm_clear(DM_KEY_FENCE_POINTS); + publishFence(0); + return; + } + + if (argc < 3) + errx(1, "Specify: -clear | sequence latitude longitude [-publish]"); + + ix = atoi(argv[0]); + if (ix >= DM_KEY_FENCE_POINTS_MAX) + errx(1, "Sequence must be less than %d", DM_KEY_FENCE_POINTS_MAX); + + lat = strtod(argv[1], &end); + lon = strtod(argv[2], &end); + + last = 0; + if ((argc > 3) && (strcmp(argv[3], "-publish") == 0)) + last = 1; + + vertex.lat = (float)lat; + vertex.lon = (float)lon; + + if (dm_write(DM_KEY_FENCE_POINTS, ix, DM_PERSIST_POWER_ON_RESET, &vertex, sizeof(vertex)) == sizeof(vertex)) { + if (last) + publishFence((unsigned)ix + 1); + return; + } + + errx(1, "can't store fence point"); +} + +void +Geofence::publishFence(unsigned vertices) +{ + if (_fence_pub == -1) + _fence_pub = orb_advertise(ORB_ID(fence), &vertices); + else + orb_publish(ORB_ID(fence), _fence_pub, &vertices); +} + diff --git a/src/modules/navigator/geofence.h b/src/modules/navigator/geofence.h index 6d6e6e87c2..8f3a07b02e 100644 --- a/src/modules/navigator/geofence.h +++ b/src/modules/navigator/geofence.h @@ -45,6 +45,7 @@ class Geofence { private: struct fence_s _fence; + orb_advert_t _fence_pub; /**< publish fence topic */ public: Geofence(); ~Geofence(); @@ -58,6 +59,21 @@ public: * @return true: craft is inside fence, false:craft is outside fence */ bool inside(const struct vehicle_global_position_s *craft); + + + /** + * Load fence parameters. + */ + bool load(unsigned vertices); + + bool valid(); + + /** + * Specify fence vertex position. + */ + void addPoint(int argc, char *argv[]); + + void publishFence(unsigned vertices); }; diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp index cc079dee19..aba2dffff8 100644 --- a/src/modules/navigator/mission_feasibility_checker.cpp +++ b/src/modules/navigator/mission_feasibility_checker.cpp @@ -62,7 +62,7 @@ MissionFeasibilityChecker::MissionFeasibilityChecker() : _mavlink_fd(-1), _capab } -bool MissionFeasibilityChecker::checkMissionFeasible(bool isRotarywing, dm_item_t dm_current, size_t nMissionItems, const struct fence_s &fence) +bool MissionFeasibilityChecker::checkMissionFeasible(bool isRotarywing, dm_item_t dm_current, size_t nMissionItems, Geofence &geofence) { /* Init if not done yet */ init(); @@ -75,27 +75,27 @@ bool MissionFeasibilityChecker::checkMissionFeasible(bool isRotarywing, dm_item_ if (isRotarywing) - return checkMissionFeasibleRotarywing(dm_current, nMissionItems, fence); + return checkMissionFeasibleRotarywing(dm_current, nMissionItems, geofence); else - return checkMissionFeasibleFixedwing(dm_current, nMissionItems, fence); + return checkMissionFeasibleFixedwing(dm_current, nMissionItems, geofence); } -bool MissionFeasibilityChecker::checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nMissionItems, const struct fence_s &fence) +bool MissionFeasibilityChecker::checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence) { - return checkGeofence(dm_current, nMissionItems, fence); + return checkGeofence(dm_current, nMissionItems, geofence); } -bool MissionFeasibilityChecker::checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nMissionItems, const struct fence_s &fence) +bool MissionFeasibilityChecker::checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence) { /* Update fixed wing navigation capabilites */ updateNavigationCapabilities(); // warnx("_nav_caps.landing_slope_angle_rad %.4f, _nav_caps.landing_horizontal_slope_displacement %.4f", _nav_caps.landing_slope_angle_rad, _nav_caps.landing_horizontal_slope_displacement); - return (checkFixedWingLanding(dm_current, nMissionItems) && checkGeofence(dm_current, nMissionItems, fence)); + return (checkFixedWingLanding(dm_current, nMissionItems) && checkGeofence(dm_current, nMissionItems, geofence)); } -bool MissionFeasibilityChecker::checkGeofence(dm_item_t dm_current, size_t nMissionItems, const struct fence_s &fence) +bool MissionFeasibilityChecker::checkGeofence(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence) { //xxx: check geofence return true; diff --git a/src/modules/navigator/mission_feasibility_checker.h b/src/modules/navigator/mission_feasibility_checker.h index ef235ead44..7a0b2a2966 100644 --- a/src/modules/navigator/mission_feasibility_checker.h +++ b/src/modules/navigator/mission_feasibility_checker.h @@ -43,6 +43,7 @@ #include #include #include +#include "geofence.h" class MissionFeasibilityChecker @@ -57,15 +58,15 @@ private: void init(); /* Checks for all airframes */ - bool checkGeofence(dm_item_t dm_current, size_t nMissionItems, const struct fence_s &fence); + bool checkGeofence(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence); /* Checks specific to fixedwing airframes */ - bool checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nMissionItems, const struct fence_s &fence); + bool checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence); bool checkFixedWingLanding(dm_item_t dm_current, size_t nMissionItems); void updateNavigationCapabilities(); /* Checks specific to rotarywing airframes */ - bool checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nMissionItems, const struct fence_s &fence); + bool checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence); public: MissionFeasibilityChecker(); @@ -74,7 +75,7 @@ public: /* * Returns true if mission is feasible and false otherwise */ - bool checkMissionFeasible(bool isRotarywing, dm_item_t dm_current, size_t nMissionItems, const struct fence_s &fence); + bool checkMissionFeasible(bool isRotarywing, dm_item_t dm_current, size_t nMissionItems, Geofence &geofence); }; diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index a9547355ff..354fa733b3 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -120,14 +120,9 @@ public: void status(); /** - * Load fence parameters. + * Add point to geofence */ - bool load_fence(unsigned vertices); - - /** - * Specify fence vertex position. - */ - void fence_point(int argc, char *argv[]); + void add_fence_point(int argc, char *argv[]); private: @@ -145,7 +140,6 @@ private: int _capabilities_sub; /**< notification of vehicle capabilities updates */ orb_advert_t _triplet_pub; /**< publish position setpoint triplet */ - orb_advert_t _fence_pub; /**< publish fence topic */ orb_advert_t _mission_result_pub; /**< publish mission result topic */ orb_advert_t _control_mode_pub; /**< publish vehicle control mode topic */ @@ -158,8 +152,7 @@ private: perf_counter_t _loop_perf; /**< loop performance counter */ - Geofence geofence; - struct fence_s _fence; /**< storage for fence vertices */ + Geofence _geofence; bool _fence_valid; /**< flag if fence is valid */ bool _inside_fence; /**< vehicle is inside fence */ @@ -252,12 +245,8 @@ private: */ void task_main() __attribute__((noreturn)); - void publish_fence(unsigned vertices); - void publish_safepoints(unsigned points); - bool fence_valid(const struct fence_s &fence); - /** * Functions that are triggered when a new state is entered. */ @@ -347,7 +336,6 @@ Navigator::Navigator() : /* publications */ _triplet_pub(-1), - _fence_pub(-1), _mission_result_pub(-1), _control_mode_pub(-1), @@ -365,8 +353,6 @@ Navigator::Navigator() : _time_first_inside_orbit(0), _set_nav_state_timestamp(0) { - memset(&_fence, 0, sizeof(_fence)); - _parameter_handles.min_altitude = param_find("NAV_MIN_ALT"); _parameter_handles.loiter_radius = param_find("NAV_LOITER_RAD"); _parameter_handles.onboard_mission_enabled = param_find("NAV_ONB_MIS_EN"); @@ -458,7 +444,7 @@ Navigator::offboard_mission_update(bool isrotaryWing) } else { dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1; } - missionFeasiblityChecker.checkMissionFeasible(isrotaryWing, dm_current, (size_t)offboard_mission.count, _fence); + missionFeasiblityChecker.checkMissionFeasible(isrotaryWing, dm_current, (size_t)offboard_mission.count, _geofence); _mission.set_offboard_dataman_id(offboard_mission.dataman_id); _mission.set_current_offboard_mission_index(offboard_mission.current_index); @@ -511,7 +497,7 @@ Navigator::task_main() mavlink_log_info(_mavlink_fd, "[navigator] started"); - _fence_valid = load_fence(GEOFENCE_MAX_VERTICES); + _fence_valid = _geofence.load(GEOFENCE_MAX_VERTICES); /* * do subscriptions @@ -782,9 +768,9 @@ Navigator::status() } if (_fence_valid) { warnx("Geofence is valid"); - warnx("Vertex longitude latitude"); - for (unsigned i = 0; i < _fence.count; i++) - warnx("%6u %9.5f %8.5f", i, (double)_fence.vertices[i].lon, (double)_fence.vertices[i].lat); +// warnx("Vertex longitude latitude"); +// for (unsigned i = 0; i < _fence.count; i++) +// warnx("%6u %9.5f %8.5f", i, (double)_fence.vertices[i].lon, (double)_fence.vertices[i].lat); } else { warnx("Geofence not set"); } @@ -808,96 +794,6 @@ Navigator::status() } } -void -Navigator::publish_fence(unsigned vertices) -{ - if (_fence_pub == -1) - _fence_pub = orb_advertise(ORB_ID(fence), &vertices); - else - orb_publish(ORB_ID(fence), _fence_pub, &vertices); -} - -bool -Navigator::fence_valid(const struct fence_s &fence) -{ - // NULL fence is valid - if (fence.count == 0) { - return true; - } - - // Otherwise - if ((fence.count < 4) || (fence.count > GEOFENCE_MAX_VERTICES)) { - warnx("Fence must have at least 3 sides and not more than %d", GEOFENCE_MAX_VERTICES - 1); - return false; - } - - return true; -} - -bool -Navigator::load_fence(unsigned vertices) -{ - struct fence_s temp_fence; - - unsigned i; - for (i = 0; i < vertices; i++) { - if (dm_read(DM_KEY_FENCE_POINTS, i, temp_fence.vertices + i, sizeof(struct fence_vertex_s)) != sizeof(struct fence_vertex_s)) { - break; - } - } - - temp_fence.count = i; - - if (fence_valid(temp_fence)) - memcpy(&_fence, &temp_fence, sizeof(_fence)); - else - warnx("Invalid fence file, ignored!"); - - return _fence.count != 0; -} - -void -Navigator::fence_point(int argc, char *argv[]) -{ - int ix, last; - double lon, lat; - struct fence_vertex_s vertex; - char *end; - - if ((argc == 1) && (strcmp("-clear", argv[0]) == 0)) { - dm_clear(DM_KEY_FENCE_POINTS); - publish_fence(0); - return; - } - - if (argc < 3) - errx(1, "Specify: -clear | sequence latitude longitude [-publish]"); - - ix = atoi(argv[0]); - if (ix >= DM_KEY_FENCE_POINTS_MAX) - errx(1, "Sequence must be less than %d", DM_KEY_FENCE_POINTS_MAX); - - lat = strtod(argv[1], &end); - lon = strtod(argv[2], &end); - - last = 0; - if ((argc > 3) && (strcmp(argv[3], "-publish") == 0)) - last = 1; - - vertex.lat = (float)lat; - vertex.lon = (float)lon; - - if (dm_write(DM_KEY_FENCE_POINTS, ix, DM_PERSIST_POWER_ON_RESET, &vertex, sizeof(vertex)) == sizeof(vertex)) { - if (last) - publish_fence((unsigned)ix + 1); - return; - } - - errx(1, "can't store fence point"); -} - - - StateTable::Tran const Navigator::myTable[NAV_STATE_MAX][MAX_EVENT] = { { /* STATE_NONE */ @@ -1351,6 +1247,11 @@ bool Navigator::cmp_mission_item_equivalent(const struct mission_item_s a, const } } +void Navigator::add_fence_point(int argc, char *argv[]) +{ + _geofence.addPoint(argc, argv); +} + static void usage() { @@ -1395,7 +1296,7 @@ int navigator_main(int argc, char *argv[]) navigator::g_navigator->status(); } else if (!strcmp(argv[1], "fence")) { - navigator::g_navigator->fence_point(argc - 2, argv + 2); + navigator::g_navigator->add_fence_point(argc - 2, argv + 2); } else { usage(); From 220011914c01ef4050ca487059b0d317e6b53fb7 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 2 Jan 2014 21:57:01 +0400 Subject: [PATCH 285/447] navigator: AUTO_READY nav state added, RTL implemented properly --- src/modules/mavlink/mavlink.c | 4 +- src/modules/mavlink/waypoints.c | 4 +- src/modules/navigator/navigator_main.cpp | 364 ++++++++++++------ .../uORB/topics/vehicle_control_mode.h | 1 + 4 files changed, 251 insertions(+), 122 deletions(-) diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c index d4e4c027b4..4d3c9dd2cb 100644 --- a/src/modules/mavlink/mavlink.c +++ b/src/modules/mavlink/mavlink.c @@ -220,7 +220,9 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_base_mode, u } else if (v_status.main_state == MAIN_STATE_AUTO) { *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; - if (control_mode.nav_state == NAV_STATE_NONE) { + if (control_mode.nav_state == NAV_STATE_NONE) { // failsafe, shouldn't happen + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY; + } else if (control_mode.nav_state == NAV_STATE_READY) { custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY; } else if (control_mode.nav_state == NAV_STATE_LOITER) { custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LOITER; diff --git a/src/modules/mavlink/waypoints.c b/src/modules/mavlink/waypoints.c index 1adfeafde0..168666d4e1 100644 --- a/src/modules/mavlink/waypoints.c +++ b/src/modules/mavlink/waypoints.c @@ -152,7 +152,7 @@ int map_mavlink_mission_item_to_mission_item(const mavlink_mission_item_t *mavli mission_item->loiter_direction = (mavlink_mission_item->param3 > 0) ? 1 : -1; /* 1 if positive CW, -1 if negative CCW */ mission_item->nav_cmd = mavlink_mission_item->command; - mission_item->time_inside = mavlink_mission_item->param1 / 1e3f; /* from milliseconds to seconds */ + mission_item->time_inside = mavlink_mission_item->param1; mission_item->autocontinue = mavlink_mission_item->autocontinue; // mission_item->index = mavlink_mission_item->seq; mission_item->origin = ORIGIN_MAVLINK; @@ -184,7 +184,7 @@ int map_mission_item_to_mavlink_mission_item(const struct mission_item_s *missio mavlink_mission_item->param4 = mission_item->yaw*M_RAD_TO_DEG_F; mavlink_mission_item->param3 = mission_item->loiter_radius*(float)mission_item->loiter_direction; mavlink_mission_item->command = mission_item->nav_cmd; - mavlink_mission_item->param1 = mission_item->time_inside * 1e3f; /* from seconds to milliseconds */ + mavlink_mission_item->param1 = mission_item->time_inside; mavlink_mission_item->autocontinue = mission_item->autocontinue; // mavlink_mission_item->seq = mission_item->index; diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index ca807df6f3..61de91dced 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -166,7 +166,6 @@ private: class Mission _mission; bool _reset_loiter_pos; /**< if true then loiter position should be set to current position */ - bool _rtl_done; bool _waypoint_position_reached; bool _waypoint_yaw_reached; uint64_t _time_first_inside_orbit; @@ -199,10 +198,10 @@ private: enum Event { EVENT_NONE_REQUESTED, + EVENT_READY_REQUESTED, EVENT_LOITER_REQUESTED, EVENT_MISSION_REQUESTED, EVENT_RTL_REQUESTED, - EVENT_MISSION_FINISHED, EVENT_MISSION_CHANGED, EVENT_HOME_POSITION_CHANGED, MAX_EVENT @@ -214,8 +213,10 @@ private: static StateTable::Tran const myTable[NAV_STATE_MAX][MAX_EVENT]; enum RTLState { + RTL_STATE_NONE = 0, RTL_STATE_CLIMB, RTL_STATE_RETURN, + RTL_STATE_DESCEND, RTL_STATE_LAND }; @@ -277,9 +278,9 @@ private: * Functions that are triggered when a new state is entered. */ void start_none(); + void start_ready(); void start_loiter(); void start_mission(); - void finish_mission(); void start_rtl(); void finish_rtl(); @@ -308,6 +309,11 @@ private: */ void advance_mission(); + /** + * Switch to next RTL state + */ + void set_rtl_item(); + /** * Helper function to get a loiter item */ @@ -380,11 +386,11 @@ Navigator::Navigator() : _loop_perf(perf_alloc(PC_ELAPSED, "navigator")), /* states */ + _rtl_state(RTL_STATE_NONE), _fence_valid(false), _inside_fence(true), _mission(), _reset_loiter_pos(true), - _rtl_done(false), _waypoint_position_reached(false), _waypoint_yaw_reached(false), _time_first_inside_orbit(0), @@ -625,13 +631,16 @@ Navigator::task_main() pub_control_mode = true; /* Evaluate state machine from commander and set the navigator mode accordingly */ - if (_vstatus.main_state == MAIN_STATE_AUTO) { + if (_vstatus.main_state == MAIN_STATE_AUTO && + (_vstatus.arming_state == ARMING_STATE_ARMED || _vstatus.arming_state == ARMING_STATE_ARMED_ERROR)) { bool stick_mode = false; if (!_vstatus.rc_signal_lost) { /* RC signal available, use control switches to set mode */ /* RETURN switch, overrides MISSION switch */ if (_vstatus.return_switch == RETURN_SWITCH_RETURN) { - dispatch(_rtl_done ? EVENT_LOITER_REQUESTED : EVENT_RTL_REQUESTED); + if (myState != NAV_STATE_READY || _rtl_state != RTL_STATE_LAND) { + dispatch(EVENT_RTL_REQUESTED); + } stick_mode = true; } else { /* MISSION switch */ @@ -678,7 +687,9 @@ Navigator::task_main() break; case NAV_STATE_RTL: - dispatch(_rtl_done ? EVENT_LOITER_REQUESTED : EVENT_RTL_REQUESTED); + if (myState != NAV_STATE_READY || _rtl_state != RTL_STATE_LAND) { + dispatch(EVENT_RTL_REQUESTED); + } break; default: @@ -739,7 +750,7 @@ Navigator::task_main() /* global position updated */ if (fds[1].revents & POLLIN) { global_position_update(); - /* only check if waypoint has been reached in MISSION */ + /* only check if waypoint has been reached in MISSION and RTL modes */ if (myState == NAV_STATE_MISSION || myState == NAV_STATE_RTL) { if (check_mission_item_reached()) { on_mission_item_reached(); @@ -747,7 +758,6 @@ Navigator::task_main() } } - /* notify user about state changes */ if (myState != prevState) { mavlink_log_info(_mavlink_fd, "[navigator] nav state %d -> %d", prevState, myState); @@ -924,40 +934,50 @@ StateTable::Tran const Navigator::myTable[NAV_STATE_MAX][MAX_EVENT] = { { /* STATE_NONE */ /* EVENT_NONE_REQUESTED */ {NO_ACTION, NAV_STATE_NONE}, + /* EVENT_READY_REQUESTED */ {ACTION(&Navigator::start_ready), NAV_STATE_READY}, /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER}, /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, - /* EVENT_MISSION_FINISHED */ {NO_ACTION, NAV_STATE_NONE}, /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_NONE}, /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_NONE}, }, { + /* STATE_READY */ + /* EVENT_NONE_REQUESTED */ {NO_ACTION, NAV_STATE_NONE}, + /* EVENT_READY_REQUESTED */ {NO_ACTION, NAV_STATE_READY}, + /* EVENT_LOITER_REQUESTED */ {NO_ACTION, NAV_STATE_READY}, + /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, + /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, + /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_READY}, + /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_READY}, + }, + { /* STATE_LOITER */ /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE}, + /* EVENT_READY_REQUESTED */ {NO_ACTION, NAV_STATE_LOITER}, /* EVENT_LOITER_REQUESTED */ {NO_ACTION, NAV_STATE_LOITER}, /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, - /* EVENT_MISSION_FINISHED */ {NO_ACTION, NAV_STATE_LOITER}, /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_LOITER}, /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_LOITER}, }, { /* STATE_MISSION */ /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE}, + /* EVENT_READY_REQUESTED */ {ACTION(&Navigator::start_ready), NAV_STATE_READY}, /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER}, /* EVENT_MISSION_REQUESTED */ {NO_ACTION, NAV_STATE_MISSION}, /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, - /* EVENT_MISSION_FINISHED */ {ACTION(&Navigator::finish_mission), NAV_STATE_LOITER}, /* EVENT_MISSION_CHANGED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_MISSION}, }, { /* STATE_RTL */ /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE}, + /* EVENT_READY_REQUESTED */ {ACTION(&Navigator::start_ready), NAV_STATE_READY}, /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER}, /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, /* EVENT_RTL_REQUESTED */ {NO_ACTION, NAV_STATE_RTL}, - /* EVENT_MISSION_FINISHED */ {ACTION(&Navigator::finish_rtl), NAV_STATE_LOITER}, /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_RTL}, /* EVENT_HOME_POSITION_CHANGED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, }, @@ -971,8 +991,26 @@ Navigator::start_none() _mission_item_triplet.next_valid = false; _reset_loiter_pos = true; - _rtl_done = false; _do_takeoff = false; + _rtl_state = RTL_STATE_NONE; + + publish_mission_item_triplet(); +} + +void +Navigator::start_ready() +{ + _mission_item_triplet.previous_valid = false; + _mission_item_triplet.current_valid = false; + _mission_item_triplet.next_valid = false; + + _reset_loiter_pos = true; + _do_takeoff = false; + + // TODO check + if (_rtl_state != RTL_STATE_LAND) { + _rtl_state = RTL_STATE_NONE; + } publish_mission_item_triplet(); } @@ -1015,9 +1053,9 @@ Navigator::start_loiter() void Navigator::start_mission() { - _rtl_done = false; _need_takeoff = true; + mavlink_log_info(_mavlink_fd, "[navigator] mission started"); advance_mission(); } @@ -1041,6 +1079,14 @@ Navigator::advance_mission() _mission_item_triplet.current_valid = true; add_home_pos_to_rtl(&_mission_item_triplet.current); + if (_mission_item_triplet.current.nav_cmd != NAV_CMD_RETURN_TO_LAUNCH && + _mission_item_triplet.current.nav_cmd != NAV_CMD_LOITER_TIME_LIMIT && + _mission_item_triplet.current.nav_cmd != NAV_CMD_LOITER_TURN_COUNT && + _mission_item_triplet.current.nav_cmd != NAV_CMD_LOITER_UNLIMITED) { + /* don't reset RTL state on RTL or LOITER items */ + _rtl_state = RTL_STATE_NONE; + } + if (_vstatus.is_rotary_wing) { if (_need_takeoff && ( _mission_item_triplet.current.nav_cmd == NAV_CMD_TAKEOFF || @@ -1116,56 +1162,128 @@ Navigator::advance_mission() publish_mission_item_triplet(); } -void -Navigator::finish_mission() -{ - /* loiter at last waypoint */ - _reset_loiter_pos = false; - - mavlink_log_info(_mavlink_fd, "[navigator] mission completed"); - - start_loiter(); -} - void Navigator::start_rtl() { _reset_loiter_pos = true; - _rtl_done = false; _do_takeoff = false; - - /* discard all mission item and insert RTL item */ - _mission_item_triplet.previous_valid = false; - _mission_item_triplet.current_valid = true; - _mission_item_triplet.next_valid = false; - - _mission_item_triplet.current.lat = _home_pos.lat; - _mission_item_triplet.current.lon = _home_pos.lon; - _mission_item_triplet.current.altitude = _home_pos.altitude + _parameters.rtl_alt; - _mission_item_triplet.current.altitude_is_relative = false; - _mission_item_triplet.current.yaw = 0.0f; // TODO use heading to waypoint? - _mission_item_triplet.current.nav_cmd = NAV_CMD_RETURN_TO_LAUNCH; - _mission_item_triplet.current.loiter_direction = 1; - _mission_item_triplet.current.loiter_radius = _parameters.loiter_radius; - _mission_item_triplet.current.acceptance_radius = _parameters.acceptance_radius; - _mission_item_triplet.current.autocontinue = false; - _mission_item_triplet.current_valid = true; - - publish_mission_item_triplet(); + if (_rtl_state == RTL_STATE_NONE) + _rtl_state = RTL_STATE_CLIMB; mavlink_log_info(_mavlink_fd, "[navigator] RTL started"); -} + set_rtl_item(); +} void -Navigator::finish_rtl() +Navigator::set_rtl_item() { - /* loiter at home position */ - _reset_loiter_pos = false; - _rtl_done = true; + switch (_rtl_state) { + case RTL_STATE_CLIMB: { + memcpy(&_mission_item_triplet.previous, &_mission_item_triplet.current, sizeof(mission_item_s)); + _mission_item_triplet.previous_valid = _mission_item_triplet.current_valid; - mavlink_log_info(_mavlink_fd, "[navigator] RTL completed"); + _mission_item_triplet.current_valid = true; + _mission_item_triplet.next_valid = false; - start_loiter(); + float climb_alt = _home_pos.altitude + _parameters.rtl_alt; + if (_vstatus.condition_landed) + climb_alt = fmaxf(climb_alt, _global_pos.alt + _parameters.rtl_alt); + + _mission_item_triplet.current.altitude_is_relative = false; + _mission_item_triplet.current.lat = (double)_global_pos.lat / 1e7d; + _mission_item_triplet.current.lon = (double)_global_pos.lon / 1e7d; + _mission_item_triplet.current.altitude = climb_alt; + _mission_item_triplet.current.yaw = NAN; + _mission_item_triplet.current.loiter_radius = _parameters.loiter_radius; + _mission_item_triplet.current.loiter_direction = 1; + _mission_item_triplet.current.nav_cmd = NAV_CMD_TAKEOFF; + _mission_item_triplet.current.acceptance_radius = _parameters.acceptance_radius; + _mission_item_triplet.current.time_inside = 0.0f; + _mission_item_triplet.current.pitch_min = 0.0f; + _mission_item_triplet.current.autocontinue = true; + _mission_item_triplet.current.origin = ORIGIN_ONBOARD; + mavlink_log_info(_mavlink_fd, "[navigator] RTL: climb to %.1fm", climb_alt - _home_pos.altitude); + break; + } + case RTL_STATE_RETURN: { + memcpy(&_mission_item_triplet.previous, &_mission_item_triplet.current, sizeof(mission_item_s)); + _mission_item_triplet.previous_valid = _mission_item_triplet.current_valid; + + _mission_item_triplet.current_valid = true; + _mission_item_triplet.next_valid = false; + + _mission_item_triplet.current.altitude_is_relative = false; + _mission_item_triplet.current.lat = _home_pos.lat; + _mission_item_triplet.current.lon = _home_pos.lon; + // don't change altitude setpoint + _mission_item_triplet.current.yaw = NAN; + _mission_item_triplet.current.loiter_radius = _parameters.loiter_radius; + _mission_item_triplet.current.loiter_direction = 1; + _mission_item_triplet.current.nav_cmd = NAV_CMD_WAYPOINT; + _mission_item_triplet.current.acceptance_radius = _parameters.acceptance_radius; + _mission_item_triplet.current.time_inside = 0.0f; + _mission_item_triplet.current.pitch_min = 0.0f; + _mission_item_triplet.current.autocontinue = true; + _mission_item_triplet.current.origin = ORIGIN_ONBOARD; + mavlink_log_info(_mavlink_fd, "[navigator] RTL: return"); + break; + } + case RTL_STATE_DESCEND: { + memcpy(&_mission_item_triplet.previous, &_mission_item_triplet.current, sizeof(mission_item_s)); + _mission_item_triplet.previous_valid = _mission_item_triplet.current_valid; + + _mission_item_triplet.current_valid = true; + _mission_item_triplet.next_valid = false; + + float descend_alt = _home_pos.altitude + _parameters.land_alt; + + _mission_item_triplet.current.altitude_is_relative = false; + _mission_item_triplet.current.lat = _home_pos.lat; + _mission_item_triplet.current.lon = _home_pos.lon; + _mission_item_triplet.current.altitude = descend_alt; + _mission_item_triplet.current.yaw = NAN; + _mission_item_triplet.current.loiter_radius = _parameters.loiter_radius; + _mission_item_triplet.current.loiter_direction = 1; + _mission_item_triplet.current.nav_cmd = NAV_CMD_WAYPOINT; + _mission_item_triplet.current.acceptance_radius = _parameters.acceptance_radius; + _mission_item_triplet.current.time_inside = 0.0f; + _mission_item_triplet.current.pitch_min = 0.0f; + _mission_item_triplet.current.autocontinue = true; + _mission_item_triplet.current.origin = ORIGIN_ONBOARD; + mavlink_log_info(_mavlink_fd, "[navigator] RTL: descend to %.1fm", descend_alt - _home_pos.altitude); + break; + } + case RTL_STATE_LAND: { + memcpy(&_mission_item_triplet.previous, &_mission_item_triplet.current, sizeof(mission_item_s)); + _mission_item_triplet.previous_valid = _mission_item_triplet.current_valid; + + _mission_item_triplet.current_valid = true; + _mission_item_triplet.next_valid = false; + + _mission_item_triplet.current.altitude_is_relative = false; + _mission_item_triplet.current.lat = _home_pos.lat; + _mission_item_triplet.current.lon = _home_pos.lon; + _mission_item_triplet.current.altitude = _home_pos.altitude; + _mission_item_triplet.current.yaw = NAN; + _mission_item_triplet.current.loiter_radius = _parameters.loiter_radius; + _mission_item_triplet.current.loiter_direction = 1; + _mission_item_triplet.current.nav_cmd = NAV_CMD_LAND; + _mission_item_triplet.current.acceptance_radius = _parameters.acceptance_radius; + _mission_item_triplet.current.time_inside = 0.0f; + _mission_item_triplet.current.pitch_min = 0.0f; + _mission_item_triplet.current.autocontinue = true; + _mission_item_triplet.current.origin = ORIGIN_ONBOARD; + mavlink_log_info(_mavlink_fd, "[navigator] RTL: land"); + break; + } + default: { + mavlink_log_critical(_mavlink_fd, "[navigator] error: unknown RTL state: %d", _rtl_state); + start_loiter(); + break; + } + } + + publish_mission_item_triplet(); } bool @@ -1176,9 +1294,8 @@ Navigator::check_mission_item_reached() return false; } - /* don't try to reach the landing mission, just stay in that mode, XXX maybe add another state for this */ if (_mission_item_triplet.current.nav_cmd == NAV_CMD_LAND) { - return false; + return _vstatus.condition_landed; } /* XXX TODO count turns */ @@ -1191,82 +1308,66 @@ Navigator::check_mission_item_reached() } uint64_t now = hrt_absolute_time(); - float acceptance_radius; - if (_mission_item_triplet.current.nav_cmd == NAV_CMD_WAYPOINT && _mission_item_triplet.current.acceptance_radius > 0.01f) { - acceptance_radius = _mission_item_triplet.current.acceptance_radius; + if (!_waypoint_position_reached) { + float acceptance_radius; - } else { - acceptance_radius = _parameters.acceptance_radius; - } + if (_mission_item_triplet.current.nav_cmd == NAV_CMD_WAYPOINT && _mission_item_triplet.current.acceptance_radius > 0.01f) { + acceptance_radius = _mission_item_triplet.current.acceptance_radius; - // TODO add frame - // int coordinate_frame = wpm->waypoints[wpm->current_active_wp_id].frame; - - float dist = -1.0f; - float dist_xy = -1.0f; - float dist_z = -1.0f; - - // if (coordinate_frame == (int)MAV_FRAME_GLOBAL) { - - /* current relative or AMSL altitude depending on mission item altitude_is_relative flag */ - float wp_alt_amsl = _mission_item_triplet.current.altitude; - if (_mission_item_triplet.current.altitude_is_relative) - _mission_item_triplet.current.altitude += _home_pos.altitude; - - dist = get_distance_to_point_global_wgs84(_mission_item_triplet.current.lat, _mission_item_triplet.current.lon, wp_alt_amsl, - (double)_global_pos.lat / 1e7d, (double)_global_pos.lon / 1e7d, _global_pos.alt, - &dist_xy, &dist_z); - - // warnx("1 lat: %2.2f, lon: %2.2f, alt: %2.2f", _mission_item_triplet.current.lat, _mission_item_triplet.current.lon, _mission_item_triplet.current.altitude); - // warnx("2 lat: %2.2f, lon: %2.2f, alt: %2.2f", (double)_global_pos.lat / 1e7d, (double)_global_pos.lon / 1e7d, _global_pos.alt); - - // warnx("Dist: %4.4f", dist); - - // } else if (coordinate_frame == (int)MAV_FRAME_GLOBAL_RELATIVE_ALT) { - // dist = mavlink_wpm_distance_to_point_global_wgs84(wpm->current_active_wp_id, (float)global_pos->lat * 1e-7f, (float)global_pos->lon * 1e-7f, global_pos->relative_alt, &dist_xy, &dist_z); - - // } else if (coordinate_frame == (int)MAV_FRAME_LOCAL_ENU || coordinate_frame == (int)MAV_FRAME_LOCAL_NED) { - // dist = mavlink_wpm_distance_to_point_local(wpm->current_active_wp_id, local_pos->x, local_pos->y, local_pos->z, &dist_xy, &dist_z); - - // } else if (coordinate_frame == (int)MAV_FRAME_MISSION) { - // /* Check if conditions of mission item are satisfied */ - // // XXX TODO - // } - - if (_do_takeoff) { - if (_global_pos.alt > wp_alt_amsl - acceptance_radius) { - /* require only altitude for takeoff */ - _waypoint_position_reached = true; + } else { + acceptance_radius = _parameters.acceptance_radius; } - } else { - if (dist >= 0.0f && dist <= acceptance_radius) { - _waypoint_position_reached = true; + + float dist = -1.0f; + float dist_xy = -1.0f; + float dist_z = -1.0f; + + /* current relative or AMSL altitude depending on mission item altitude_is_relative flag */ + float wp_alt_amsl = _mission_item_triplet.current.altitude; + if (_mission_item_triplet.current.altitude_is_relative) + _mission_item_triplet.current.altitude += _home_pos.altitude; + + dist = get_distance_to_point_global_wgs84(_mission_item_triplet.current.lat, _mission_item_triplet.current.lon, wp_alt_amsl, + (double)_global_pos.lat / 1e7d, (double)_global_pos.lon / 1e7d, _global_pos.alt, + &dist_xy, &dist_z); + + if (_do_takeoff) { + if (_global_pos.alt > wp_alt_amsl - acceptance_radius) { + /* require only altitude for takeoff */ + _waypoint_position_reached = true; + } + } else { + if (dist >= 0.0f && dist <= acceptance_radius) { + _waypoint_position_reached = true; + } } } - if (_vstatus.is_rotary_wing && !_do_takeoff) { - /* check yow only for rotary wing except takeoff */ - float yaw_err = _wrap_pi(_mission_item_triplet.current.yaw - _global_pos.yaw); - if (fabsf(yaw_err) < 0.05f) { /* XXX get rid of magic number */ + if (!_waypoint_yaw_reached) { + if (_vstatus.is_rotary_wing && !_do_takeoff && isfinite(_mission_item_triplet.current.yaw)) { + /* check yaw if defined only for rotary wing except takeoff */ + float yaw_err = _wrap_pi(_mission_item_triplet.current.yaw - _global_pos.yaw); + if (fabsf(yaw_err) < 0.05f) { /* XXX get rid of magic number */ + _waypoint_yaw_reached = true; + } + } else { _waypoint_yaw_reached = true; } - } else { - _waypoint_yaw_reached = true; } /* check if the current waypoint was reached */ if (_waypoint_position_reached && _waypoint_yaw_reached) { - if (_time_first_inside_orbit == 0) { - /* XXX announcement? */ _time_first_inside_orbit = now; + if (_mission_item_triplet.current.time_inside > 0.01f) { + mavlink_log_info(_mavlink_fd, "[navigator] waypoint reached, wait for %.1fs", _mission_item_triplet.current.time_inside); + } } /* check if the MAV was long enough inside the waypoint orbit */ if ((now - _time_first_inside_orbit >= (uint64_t)_mission_item_triplet.current.time_inside * 1e6) || _mission_item_triplet.current.nav_cmd == NAV_CMD_TAKEOFF) { - _time_first_inside_orbit = 0; _waypoint_yaw_reached = false; _waypoint_position_reached = false; @@ -1294,11 +1395,26 @@ Navigator::on_mission_item_reached() advance_mission(); } else { /* if no more mission items available then finish mission */ - dispatch(EVENT_MISSION_FINISHED); + /* loiter at last waypoint */ + _reset_loiter_pos = false; + mavlink_log_info(_mavlink_fd, "[navigator] mission completed"); + if (_vstatus.condition_landed) { + dispatch(EVENT_READY_REQUESTED); + } else { + dispatch(EVENT_LOITER_REQUESTED); + } } } else { /* RTL finished */ - dispatch(EVENT_MISSION_FINISHED); + if (_rtl_state == RTL_STATE_LAND) { + /* landed at home position */ + mavlink_log_info(_mavlink_fd, "[navigator] RTL completed, landed"); + dispatch(EVENT_READY_REQUESTED); + } else { + /* next RTL step */ + _rtl_state = (RTLState)(_rtl_state + 1); + set_rtl_item(); + } } } @@ -1413,12 +1529,22 @@ Navigator::publish_control_mode() case MAIN_STATE_AUTO: _control_mode.flag_control_manual_enabled = false; - _control_mode.flag_control_rates_enabled = true; - _control_mode.flag_control_attitude_enabled = true; - _control_mode.flag_control_position_enabled = true; - _control_mode.flag_control_velocity_enabled = true; - _control_mode.flag_control_altitude_enabled = true; - _control_mode.flag_control_climb_rate_enabled = true; + if (myState == NAV_STATE_READY) { + /* disable all controllers, armed but idle */ + _control_mode.flag_control_rates_enabled = false; + _control_mode.flag_control_attitude_enabled = false; + _control_mode.flag_control_position_enabled = false; + _control_mode.flag_control_velocity_enabled = false; + _control_mode.flag_control_altitude_enabled = false; + _control_mode.flag_control_climb_rate_enabled = false; + } else { + _control_mode.flag_control_rates_enabled = true; + _control_mode.flag_control_attitude_enabled = true; + _control_mode.flag_control_position_enabled = true; + _control_mode.flag_control_velocity_enabled = true; + _control_mode.flag_control_altitude_enabled = true; + _control_mode.flag_control_climb_rate_enabled = true; + } break; default: diff --git a/src/modules/uORB/topics/vehicle_control_mode.h b/src/modules/uORB/topics/vehicle_control_mode.h index 9d5dad9f98..26dcbd985c 100644 --- a/src/modules/uORB/topics/vehicle_control_mode.h +++ b/src/modules/uORB/topics/vehicle_control_mode.h @@ -63,6 +63,7 @@ typedef enum { NAV_STATE_NONE = 0, + NAV_STATE_READY, NAV_STATE_LOITER, NAV_STATE_MISSION, NAV_STATE_RTL, From dae5c838422a6250e1a7e4920d59cb8976a16a8c Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 2 Jan 2014 22:00:56 +0400 Subject: [PATCH 286/447] mc_att_control_vector: support for disabled rate controller flag to handle AUTO_READY mode --- .../mc_att_control_vector_main.cpp | 20 +++++++++++++------ 1 file changed, 14 insertions(+), 6 deletions(-) diff --git a/src/modules/mc_att_control_vector/mc_att_control_vector_main.cpp b/src/modules/mc_att_control_vector/mc_att_control_vector_main.cpp index f13c04e45b..239bd5570d 100644 --- a/src/modules/mc_att_control_vector/mc_att_control_vector_main.cpp +++ b/src/modules/mc_att_control_vector/mc_att_control_vector_main.cpp @@ -643,11 +643,20 @@ MulticopterAttitudeControl::task_main() } /* publish the attitude controls */ - _actuators.control[0] = (isfinite(control(0))) ? control(0) : 0.0f; - _actuators.control[1] = (isfinite(control(1))) ? control(1) : 0.0f; - _actuators.control[2] = (isfinite(control(2))) ? control(2) : 0.0f; - _actuators.control[3] = (isfinite(_rates_sp.thrust)) ? _rates_sp.thrust : 0.0f; - _actuators.timestamp = hrt_absolute_time(); + if (_control_mode.flag_control_rates_enabled) { + _actuators.control[0] = (isfinite(control(0))) ? control(0) : 0.0f; + _actuators.control[1] = (isfinite(control(1))) ? control(1) : 0.0f; + _actuators.control[2] = (isfinite(control(2))) ? control(2) : 0.0f; + _actuators.control[3] = (isfinite(_rates_sp.thrust)) ? _rates_sp.thrust : 0.0f; + _actuators.timestamp = hrt_absolute_time(); + } else { + /* controller disabled, publish zero attitude controls */ + _actuators.control[0] = 0.0f; + _actuators.control[1] = 0.0f; + _actuators.control[2] = 0.0f; + _actuators.control[3] = 0.0f; + _actuators.timestamp = hrt_absolute_time(); + } if (_actuators_0_pub > 0) { /* publish the attitude setpoint */ @@ -657,7 +666,6 @@ MulticopterAttitudeControl::task_main() /* advertise and publish */ _actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators); } - } perf_end(_loop_perf); From a48264d5d44018412b9443b245e6974d3f54b20d Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 4 Jan 2014 13:37:49 +0100 Subject: [PATCH 287/447] navigator: load geofence from textfile --- src/modules/navigator/geofence.cpp | 101 ++++++++++++++++++++++- src/modules/navigator/geofence.h | 13 ++- src/modules/navigator/navigator_main.cpp | 31 ++++++- 3 files changed, 140 insertions(+), 5 deletions(-) diff --git a/src/modules/navigator/geofence.cpp b/src/modules/navigator/geofence.cpp index 4a0528b16f..199ccb41b0 100644 --- a/src/modules/navigator/geofence.cpp +++ b/src/modules/navigator/geofence.cpp @@ -43,8 +43,21 @@ #include #include #include +#include +#include +#include +#include -Geofence::Geofence() : _fence_pub(-1) + +/* Oddly, ERROR is not defined for C++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; + +Geofence::Geofence() : _fence_pub(-1), + _altitude_min(0), + _altitude_max(0) { memset(&_fence, 0, sizeof(_fence)); } @@ -77,7 +90,7 @@ bool Geofence::inside(const struct vehicle_global_position_s *vehicle) } bool -Geofence::load(unsigned vertices) +Geofence::loadFromDm(unsigned vertices) { struct fence_s temp_fence; @@ -164,3 +177,87 @@ Geofence::publishFence(unsigned vertices) orb_publish(ORB_ID(fence), _fence_pub, &vertices); } +int +Geofence::loadFromFile(const char *filename) +{ + FILE *fp; + char line[120]; + int pointCounter = 0; + bool gotVertical = false; + const char commentChar = '#'; + + /* Make sure no data is left in the datamanager */ + clearDm(); + + /* open the mixer definition file */ + fp = fopen(GEOFENCE_FILENAME, "r"); + if (fp == NULL) { + return ERROR; + } + + /* create geofence points from valid lines and store in DM */ + for (;;) { + + /* get a line, bail on error/EOF */ + if (fgets(line, sizeof(line), fp) == NULL) + break; + + /* Trim leading whitespace */ + size_t textStart = 0; + while((textStart < sizeof(line)/sizeof(char)) && isspace(line[textStart])) textStart++; + + /* if the line starts with #, skip */ + if (line[textStart] == commentChar) + continue; + + if (gotVertical) { + /* Parse the line as a geofence point */ + struct fence_vertex_s vertex; + + if (sscanf(line, "%f %f", &(vertex.lat), &(vertex.lon)) != 2) + return ERROR; + + + if (dm_write(DM_KEY_FENCE_POINTS, pointCounter, DM_PERSIST_POWER_ON_RESET, &vertex, sizeof(vertex)) != sizeof(vertex)) + return ERROR; + + warnx("Geofence: point: %d, lat %.5f: lon: %.5f", pointCounter, (double)vertex.lat, (double)vertex.lon); + + pointCounter++; + } else { + /* Parse the line as the vertical limits */ + if (sscanf(line, "%f %f", &_altitude_min, &_altitude_max) != 2) + return ERROR; + + + warnx("Geofence: alt min: %.4f, alt_max: %.4f", (double)_altitude_min, (double)_altitude_max); + gotVertical = true; + } + + + } + + fclose(fp); + + /* Re-Load imported geofence from DM */ + if(gotVertical && pointCounter > 0) + { + bool fence_valid = loadFromDm(GEOFENCE_MAX_VERTICES); + if (fence_valid) { + warnx("Geofence: imported and loaded successfully"); + return OK; + } else { + warnx("Geofence: datamanager read error"); + return ERROR; + } + } else { + warnx("Geofence: import error"); + } + + return ERROR; +} + +int Geofence::clearDm() +{ + dm_clear(DM_KEY_FENCE_POINTS); +} diff --git a/src/modules/navigator/geofence.h b/src/modules/navigator/geofence.h index 8f3a07b02e..8a1d06e71a 100644 --- a/src/modules/navigator/geofence.h +++ b/src/modules/navigator/geofence.h @@ -42,10 +42,15 @@ #include +#define GEOFENCE_FILENAME "/fs/microsd/etc/geofence.txt" + class Geofence { private: - struct fence_s _fence; + struct fence_s _fence; orb_advert_t _fence_pub; /**< publish fence topic */ + + float _altitude_min; + float _altitude_max; public: Geofence(); ~Geofence(); @@ -64,7 +69,9 @@ public: /** * Load fence parameters. */ - bool load(unsigned vertices); + bool loadFromDm(unsigned vertices); + + int clearDm(); bool valid(); @@ -74,6 +81,8 @@ public: void addPoint(int argc, char *argv[]); void publishFence(unsigned vertices); + + int loadFromFile(const char *filename); }; diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 354fa733b3..a226aac7c1 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -75,6 +75,8 @@ #include #include #include +#include +#include #include "navigator_mission.h" #include "mission_feasibility_checker.h" @@ -124,6 +126,11 @@ public: */ void add_fence_point(int argc, char *argv[]); + /** + * Load fence from file + */ + void load_fence_from_file(const char *filename); + private: bool _task_should_exit; /**< if true, sensor task should exit */ @@ -497,7 +504,22 @@ Navigator::task_main() mavlink_log_info(_mavlink_fd, "[navigator] started"); - _fence_valid = _geofence.load(GEOFENCE_MAX_VERTICES); + _fence_valid = _geofence.loadFromDm(GEOFENCE_MAX_VERTICES); + + /* Try to load the geofence: + * if /fs/microsd/etc/geofence.txt load from this file + * else clear geofence data in datamanager + */ + struct stat buffer; + if( stat (GEOFENCE_FILENAME, &buffer) == 0 ) { + warnx("Try to load geofence.txt"); + _geofence.loadFromFile(GEOFENCE_FILENAME); + } else { + if (_geofence.clearDm() > 0 ) + warnx("Geofence cleared"); + else + warnx("Could not clear geofence"); + } /* * do subscriptions @@ -1252,6 +1274,11 @@ void Navigator::add_fence_point(int argc, char *argv[]) _geofence.addPoint(argc, argv); } +void Navigator::load_fence_from_file(const char *filename) +{ + _geofence.loadFromFile(filename); +} + static void usage() { @@ -1297,6 +1324,8 @@ int navigator_main(int argc, char *argv[]) } else if (!strcmp(argv[1], "fence")) { navigator::g_navigator->add_fence_point(argc - 2, argv + 2); + } else if (!strcmp(argv[1], "fencefile")) { + navigator::g_navigator->load_fence_from_file(GEOFENCE_FILENAME); } else { usage(); From ec60a254d279e826c1d3d8097dcbe4bfa89d1e89 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 4 Jan 2014 14:46:17 +0100 Subject: [PATCH 288/447] navigator: add pre mission geofence check --- src/modules/navigator/geofence.cpp | 9 +++++++-- src/modules/navigator/geofence.h | 1 + .../navigator/mission_feasibility_checker.cpp | 19 ++++++++++++++++++- 3 files changed, 26 insertions(+), 3 deletions(-) diff --git a/src/modules/navigator/geofence.cpp b/src/modules/navigator/geofence.cpp index 199ccb41b0..666d9076e3 100644 --- a/src/modules/navigator/geofence.cpp +++ b/src/modules/navigator/geofence.cpp @@ -70,15 +70,20 @@ Geofence::~Geofence() bool Geofence::inside(const struct vehicle_global_position_s *vehicle) { + double lat = vehicle->lat / 1e7d; + double lon = vehicle->lon / 1e7d; + return inside(lat, lon); +} + +bool Geofence::inside(double lat, double lon) +{ /* Adaptation of algorithm originally presented as * PNPOLY - Point Inclusion in Polygon Test * W. Randolph Franklin (WRF) */ unsigned int i, j, vertices = _fence.count; bool c = false; - double lat = vehicle->lat / 1e7d; - double lon = vehicle->lon / 1e7d; // skip vertex 0 (return point) for (i = 0, j = vertices - 1; i < vertices; j = i++) diff --git a/src/modules/navigator/geofence.h b/src/modules/navigator/geofence.h index 8a1d06e71a..0335f58f61 100644 --- a/src/modules/navigator/geofence.h +++ b/src/modules/navigator/geofence.h @@ -64,6 +64,7 @@ public: * @return true: craft is inside fence, false:craft is outside fence */ bool inside(const struct vehicle_global_position_s *craft); + bool inside(double lat, double lon); /** diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp index aba2dffff8..798e95faa5 100644 --- a/src/modules/navigator/mission_feasibility_checker.cpp +++ b/src/modules/navigator/mission_feasibility_checker.cpp @@ -97,7 +97,24 @@ bool MissionFeasibilityChecker::checkMissionFeasibleFixedwing(dm_item_t dm_curre bool MissionFeasibilityChecker::checkGeofence(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence) { - //xxx: check geofence + /* Check if all mission items are inside the geofence (if we have a valid geofence) */ + if (geofence.valid()) { + for (size_t i = 0; i < nMissionItems; i++) { + static struct mission_item_s missionitem; + const ssize_t len = sizeof(struct mission_item_s); + + if (dm_read(dm_current, i, &missionitem, len) != len) { + /* not supposed to happen unless the datamanager can't access the SD card, etc. */ + return false; + } + + if (!geofence.inside(missionitem.lat, missionitem.lon)) { + mavlink_log_info(_mavlink_fd, "#audio: Geofence violation waypoint %d", i); + return false; + } + } + } + return true; } From 31d1f436adad9dd963797872fa3575319099e940 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 4 Jan 2014 15:17:07 +0100 Subject: [PATCH 289/447] geofence: add simple vertical check --- src/modules/navigator/geofence.cpp | 10 ++++++++-- src/modules/navigator/geofence.h | 2 +- src/modules/navigator/mission_feasibility_checker.cpp | 2 +- 3 files changed, 10 insertions(+), 4 deletions(-) diff --git a/src/modules/navigator/geofence.cpp b/src/modules/navigator/geofence.cpp index 666d9076e3..f76597e607 100644 --- a/src/modules/navigator/geofence.cpp +++ b/src/modules/navigator/geofence.cpp @@ -72,12 +72,18 @@ bool Geofence::inside(const struct vehicle_global_position_s *vehicle) { double lat = vehicle->lat / 1e7d; double lon = vehicle->lon / 1e7d; + float alt = vehicle->alt; - return inside(lat, lon); + return inside(lat, lon, vehicle->alt); } -bool Geofence::inside(double lat, double lon) +bool Geofence::inside(double lat, double lon, float altitude) { + /* Vertical check */ + if (altitude > _altitude_max || altitude < _altitude_min) + return false; + + /*Horizontal check */ /* Adaptation of algorithm originally presented as * PNPOLY - Point Inclusion in Polygon Test * W. Randolph Franklin (WRF) */ diff --git a/src/modules/navigator/geofence.h b/src/modules/navigator/geofence.h index 0335f58f61..d834280ccf 100644 --- a/src/modules/navigator/geofence.h +++ b/src/modules/navigator/geofence.h @@ -64,7 +64,7 @@ public: * @return true: craft is inside fence, false:craft is outside fence */ bool inside(const struct vehicle_global_position_s *craft); - bool inside(double lat, double lon); + bool inside(double lat, double lon, float altitude); /** diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp index 798e95faa5..eaafa217de 100644 --- a/src/modules/navigator/mission_feasibility_checker.cpp +++ b/src/modules/navigator/mission_feasibility_checker.cpp @@ -108,7 +108,7 @@ bool MissionFeasibilityChecker::checkGeofence(dm_item_t dm_current, size_t nMiss return false; } - if (!geofence.inside(missionitem.lat, missionitem.lon)) { + if (!geofence.inside(missionitem.lat, missionitem.lon, missionitem.altitude)) { //xxx: handle relative altitude mavlink_log_info(_mavlink_fd, "#audio: Geofence violation waypoint %d", i); return false; } From 099c2f5a00f9789533888409f478f4157a5f88b6 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 4 Jan 2014 15:50:49 +0100 Subject: [PATCH 290/447] geofence: add DMS coordinate format support for textfile --- src/modules/navigator/geofence.cpp | 22 +++++++++++++++++++--- src/modules/navigator/navigator_main.cpp | 2 +- 2 files changed, 20 insertions(+), 4 deletions(-) diff --git a/src/modules/navigator/geofence.cpp b/src/modules/navigator/geofence.cpp index f76597e607..373429c084 100644 --- a/src/modules/navigator/geofence.cpp +++ b/src/modules/navigator/geofence.cpp @@ -225,12 +225,28 @@ Geofence::loadFromFile(const char *filename) /* Parse the line as a geofence point */ struct fence_vertex_s vertex; - if (sscanf(line, "%f %f", &(vertex.lat), &(vertex.lon)) != 2) - return ERROR; + /* if the line starts with DMS, this means that the coordinate is given as degree minute second instead of decimal degrees */ + if (line[textStart] == 'D' && line[textStart + 1] == 'M' && line[textStart + 2] == 'S') { + /* Handle degree minute second format */ + float lat_d, lat_m, lat_s, lon_d, lon_m, lon_s; + if (sscanf(line, "DMS %f %f %f %f %f %f", &lat_d, &lat_m, &lat_s, &lon_d, &lon_m, &lon_s) != 6) + return ERROR; + +// warnx("Geofence DMS: %.5f %.5f %.5f ; %.5f %.5f %.5f", (double)lat_d, (double)lat_m, (double)lat_s, (double)lon_d, (double)lon_m, (double)lon_s); + + vertex.lat = lat_d + lat_m/60.0f + lat_s/3600.0f; + vertex.lon = lon_d + lon_m/60.0f + lon_s/3600.0f; + + } else { + /* Handle decimal degree format */ + + if (sscanf(line, "%f %f", &(vertex.lat), &(vertex.lon)) != 2) + return ERROR; + } if (dm_write(DM_KEY_FENCE_POINTS, pointCounter, DM_PERSIST_POWER_ON_RESET, &vertex, sizeof(vertex)) != sizeof(vertex)) - return ERROR; + return ERROR; warnx("Geofence: point: %d, lat %.5f: lon: %.5f", pointCounter, (double)vertex.lat, (double)vertex.lon); diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index e52ef16fa5..80e006d147 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -1515,7 +1515,7 @@ void Navigator::load_fence_from_file(const char *filename) static void usage() { - errx(1, "usage: navigator {start|stop|status|fence}"); + errx(1, "usage: navigator {start|stop|status|fence|fencefile}"); } int navigator_main(int argc, char *argv[]) From 70d4ef480ac5461ef54ac72a54bd335007e233cc Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 4 Jan 2014 18:49:31 +0100 Subject: [PATCH 291/447] geofence: do not keep fence in memory --- src/modules/navigator/geofence.cpp | 86 +++++++++++------------- src/modules/navigator/geofence.h | 9 +-- src/modules/navigator/navigator_main.cpp | 2 - 3 files changed, 41 insertions(+), 56 deletions(-) diff --git a/src/modules/navigator/geofence.cpp b/src/modules/navigator/geofence.cpp index 373429c084..1063fd2e2c 100644 --- a/src/modules/navigator/geofence.cpp +++ b/src/modules/navigator/geofence.cpp @@ -57,9 +57,10 @@ static const int ERROR = -1; Geofence::Geofence() : _fence_pub(-1), _altitude_min(0), - _altitude_max(0) + _altitude_max(0), + _verticesCount(0) { - memset(&_fence, 0, sizeof(_fence)); + } Geofence::~Geofence() @@ -79,59 +80,56 @@ bool Geofence::inside(const struct vehicle_global_position_s *vehicle) bool Geofence::inside(double lat, double lon, float altitude) { - /* Vertical check */ - if (altitude > _altitude_max || altitude < _altitude_min) - return false; + if (valid()) { + /* Vertical check */ + if (altitude > _altitude_max || altitude < _altitude_min) + return false; - /*Horizontal check */ - /* Adaptation of algorithm originally presented as - * PNPOLY - Point Inclusion in Polygon Test - * W. Randolph Franklin (WRF) */ + /*Horizontal check */ + /* Adaptation of algorithm originally presented as + * PNPOLY - Point Inclusion in Polygon Test + * W. Randolph Franklin (WRF) */ - unsigned int i, j, vertices = _fence.count; - bool c = false; + bool c = false; - // skip vertex 0 (return point) - for (i = 0, j = vertices - 1; i < vertices; j = i++) - if (((_fence.vertices[i].lon) >= lon != (_fence.vertices[j].lon >= lon)) && - (lat <= (_fence.vertices[j].lat - _fence.vertices[i].lat) * (lon - _fence.vertices[i].lon) / - (_fence.vertices[j].lon - _fence.vertices[i].lon) + _fence.vertices[i].lat)) - c = !c; - return c; -} + struct fence_vertex_s temp_vertex_i; + struct fence_vertex_s temp_vertex_j; -bool -Geofence::loadFromDm(unsigned vertices) -{ - struct fence_s temp_fence; + /* Red until fence is finished */ + for (int i = 0, j = _verticesCount - 1; i < _verticesCount; j = i++) { + if (dm_read(DM_KEY_FENCE_POINTS, i, &temp_vertex_i, sizeof(struct fence_vertex_s)) != sizeof(struct fence_vertex_s)) { + break; + } + if (dm_read(DM_KEY_FENCE_POINTS, j, &temp_vertex_j, sizeof(struct fence_vertex_s)) != sizeof(struct fence_vertex_s)) { + break; + } + + // skip vertex 0 (return point) + if (((temp_vertex_i.lon) >= lon != (temp_vertex_j.lon >= lon)) && + (lat <= (temp_vertex_j.lat - temp_vertex_i.lat) * (lon - temp_vertex_i.lon) / + (temp_vertex_j.lon - temp_vertex_i.lon) + temp_vertex_i.lat)) { + c = !c; + } - unsigned i; - for (i = 0; i < vertices; i++) { - if (dm_read(DM_KEY_FENCE_POINTS, i, temp_fence.vertices + i, sizeof(struct fence_vertex_s)) != sizeof(struct fence_vertex_s)) { - break; } + + return c; + + } else { + return true; } - - temp_fence.count = i; - - if (valid()) - memcpy(&_fence, &temp_fence, sizeof(_fence)); - else - warnx("Invalid fence file, ignored!"); - - return _fence.count != 0; } bool Geofence::valid() { // NULL fence is valid - if (_fence.count == 0) { + if (_verticesCount == 0) { return true; } // Otherwise - if ((_fence.count < 4) || (_fence.count > GEOFENCE_MAX_VERTICES)) { + if ((_verticesCount < 4) || (_verticesCount > GEOFENCE_MAX_VERTICES)) { warnx("Fence must have at least 3 sides and not more than %d", GEOFENCE_MAX_VERTICES - 1); return false; } @@ -266,17 +264,11 @@ Geofence::loadFromFile(const char *filename) fclose(fp); - /* Re-Load imported geofence from DM */ + /* Check if import was successful */ if(gotVertical && pointCounter > 0) { - bool fence_valid = loadFromDm(GEOFENCE_MAX_VERTICES); - if (fence_valid) { - warnx("Geofence: imported and loaded successfully"); - return OK; - } else { - warnx("Geofence: datamanager read error"); - return ERROR; - } + _verticesCount = pointCounter; + warnx("Geofence: imported successfully"); } else { warnx("Geofence: import error"); } diff --git a/src/modules/navigator/geofence.h b/src/modules/navigator/geofence.h index d834280ccf..9c753a11de 100644 --- a/src/modules/navigator/geofence.h +++ b/src/modules/navigator/geofence.h @@ -46,11 +46,12 @@ class Geofence { private: - struct fence_s _fence; orb_advert_t _fence_pub; /**< publish fence topic */ float _altitude_min; float _altitude_max; + + int _verticesCount; public: Geofence(); ~Geofence(); @@ -66,12 +67,6 @@ public: bool inside(const struct vehicle_global_position_s *craft); bool inside(double lat, double lon, float altitude); - - /** - * Load fence parameters. - */ - bool loadFromDm(unsigned vertices); - int clearDm(); bool valid(); diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 80e006d147..8b5cc45767 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -547,8 +547,6 @@ Navigator::task_main() mavlink_log_info(_mavlink_fd, "[navigator] started"); - _fence_valid = _geofence.loadFromDm(GEOFENCE_MAX_VERTICES); - /* Try to load the geofence: * if /fs/microsd/etc/geofence.txt load from this file * else clear geofence data in datamanager From 819822e1722dc31c0f97b8494c96a6c292b07185 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 4 Jan 2014 22:01:31 +0100 Subject: [PATCH 292/447] navigator/geofence: add isEmpty() function and checks --- src/modules/navigator/geofence.cpp | 62 ++++++++++++++++-------------- src/modules/navigator/geofence.h | 4 +- 2 files changed, 37 insertions(+), 29 deletions(-) diff --git a/src/modules/navigator/geofence.cpp b/src/modules/navigator/geofence.cpp index 1063fd2e2c..a49a71e1b8 100644 --- a/src/modules/navigator/geofence.cpp +++ b/src/modules/navigator/geofence.cpp @@ -81,41 +81,48 @@ bool Geofence::inside(const struct vehicle_global_position_s *vehicle) bool Geofence::inside(double lat, double lon, float altitude) { if (valid()) { - /* Vertical check */ - if (altitude > _altitude_max || altitude < _altitude_min) - return false; - /*Horizontal check */ - /* Adaptation of algorithm originally presented as - * PNPOLY - Point Inclusion in Polygon Test - * W. Randolph Franklin (WRF) */ + if (!isEmpty()) { + /* Vertical check */ + if (altitude > _altitude_max || altitude < _altitude_min) + return false; - bool c = false; + /*Horizontal check */ + /* Adaptation of algorithm originally presented as + * PNPOLY - Point Inclusion in Polygon Test + * W. Randolph Franklin (WRF) */ - struct fence_vertex_s temp_vertex_i; - struct fence_vertex_s temp_vertex_j; + bool c = false; + + struct fence_vertex_s temp_vertex_i; + struct fence_vertex_s temp_vertex_j; + + /* Red until fence is finished */ + for (unsigned i = 0, j = _verticesCount - 1; i < _verticesCount; j = i++) { + if (dm_read(DM_KEY_FENCE_POINTS, i, &temp_vertex_i, sizeof(struct fence_vertex_s)) != sizeof(struct fence_vertex_s)) { + break; + } + if (dm_read(DM_KEY_FENCE_POINTS, j, &temp_vertex_j, sizeof(struct fence_vertex_s)) != sizeof(struct fence_vertex_s)) { + break; + } + + // skip vertex 0 (return point) + if (((temp_vertex_i.lon) >= lon != (temp_vertex_j.lon >= lon)) && + (lat <= (temp_vertex_j.lat - temp_vertex_i.lat) * (lon - temp_vertex_i.lon) / + (temp_vertex_j.lon - temp_vertex_i.lon) + temp_vertex_i.lat)) { + c = !c; + } - /* Red until fence is finished */ - for (int i = 0, j = _verticesCount - 1; i < _verticesCount; j = i++) { - if (dm_read(DM_KEY_FENCE_POINTS, i, &temp_vertex_i, sizeof(struct fence_vertex_s)) != sizeof(struct fence_vertex_s)) { - break; - } - if (dm_read(DM_KEY_FENCE_POINTS, j, &temp_vertex_j, sizeof(struct fence_vertex_s)) != sizeof(struct fence_vertex_s)) { - break; - } - - // skip vertex 0 (return point) - if (((temp_vertex_i.lon) >= lon != (temp_vertex_j.lon >= lon)) && - (lat <= (temp_vertex_j.lat - temp_vertex_i.lat) * (lon - temp_vertex_i.lon) / - (temp_vertex_j.lon - temp_vertex_i.lon) + temp_vertex_i.lat)) { - c = !c; } + return c; + } else { + /* Empty fence --> accept all points */ + return true; } - return c; - } else { + /* Invalid fence --> accept all points */ return true; } } @@ -124,9 +131,8 @@ bool Geofence::valid() { // NULL fence is valid - if (_verticesCount == 0) { + if (isEmpty()) return true; - } // Otherwise if ((_verticesCount < 4) || (_verticesCount > GEOFENCE_MAX_VERTICES)) { diff --git a/src/modules/navigator/geofence.h b/src/modules/navigator/geofence.h index 9c753a11de..781e7a263e 100644 --- a/src/modules/navigator/geofence.h +++ b/src/modules/navigator/geofence.h @@ -51,7 +51,7 @@ private: float _altitude_min; float _altitude_max; - int _verticesCount; + unsigned _verticesCount; public: Geofence(); ~Geofence(); @@ -79,6 +79,8 @@ public: void publishFence(unsigned vertices); int loadFromFile(const char *filename); + + bool isEmpty() {return _verticesCount == 0;} }; From 26af21619b3f2a67c2480872e0f7c14d0572626e Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 5 Jan 2014 10:49:16 +0100 Subject: [PATCH 293/447] navigator/geofence: add parameter to disable geofence --- src/modules/navigator/geofence.cpp | 15 ++++++- src/modules/navigator/geofence.h | 6 +++ src/modules/navigator/geofence_params.c | 55 ++++++++++++++++++++++++ src/modules/navigator/module.mk | 3 +- src/modules/navigator/navigator_main.cpp | 2 + 5 files changed, 78 insertions(+), 3 deletions(-) create mode 100644 src/modules/navigator/geofence_params.c diff --git a/src/modules/navigator/geofence.cpp b/src/modules/navigator/geofence.cpp index a49a71e1b8..9bbaf167a3 100644 --- a/src/modules/navigator/geofence.cpp +++ b/src/modules/navigator/geofence.cpp @@ -58,9 +58,11 @@ static const int ERROR = -1; Geofence::Geofence() : _fence_pub(-1), _altitude_min(0), _altitude_max(0), - _verticesCount(0) + _verticesCount(0), + param_geofence_on(NULL, "GF_ON", false) { - + /* Load initial params */ + updateParams(); } Geofence::~Geofence() @@ -80,6 +82,10 @@ bool Geofence::inside(const struct vehicle_global_position_s *vehicle) bool Geofence::inside(double lat, double lon, float altitude) { + /* Return true if geofence is disabled */ + if (param_geofence_on.get() != 1) + return true; + if (valid()) { if (!isEmpty()) { @@ -286,3 +292,8 @@ int Geofence::clearDm() { dm_clear(DM_KEY_FENCE_POINTS); } + +void Geofence::updateParams() +{ + param_geofence_on.update(); +} diff --git a/src/modules/navigator/geofence.h b/src/modules/navigator/geofence.h index 781e7a263e..5b56ebc7a5 100644 --- a/src/modules/navigator/geofence.h +++ b/src/modules/navigator/geofence.h @@ -41,6 +41,7 @@ #define GEOFENCE_H_ #include +#include #define GEOFENCE_FILENAME "/fs/microsd/etc/geofence.txt" @@ -52,6 +53,9 @@ private: float _altitude_max; unsigned _verticesCount; + + /* Params */ + control::BlockParamInt param_geofence_on; public: Geofence(); ~Geofence(); @@ -81,6 +85,8 @@ public: int loadFromFile(const char *filename); bool isEmpty() {return _verticesCount == 0;} + + void updateParams(); }; diff --git a/src/modules/navigator/geofence_params.c b/src/modules/navigator/geofence_params.c new file mode 100644 index 0000000000..20dd1fe2f0 --- /dev/null +++ b/src/modules/navigator/geofence_params.c @@ -0,0 +1,55 @@ +/**************************************************************************** + * + * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * Author: Lorenz Meier + * + * 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 geofence_params.c + * + * Parameters for geofence + * + * @author Thomas Gubler + */ + +#include + +#include + +/* + * geofence parameters, accessible via MAVLink + * + */ + +// @DisplayName Switch to enable geofence +// @Description if set to 1 geofence is enabled, defaults to 1 because geofence is only enabled when the geofence.txt file is present +// @Range 0 or 1 +PARAM_DEFINE_INT32(GF_ON, 1); diff --git a/src/modules/navigator/module.mk b/src/modules/navigator/module.mk index b7900e84ed..55f8a4caac 100644 --- a/src/modules/navigator/module.mk +++ b/src/modules/navigator/module.mk @@ -41,6 +41,7 @@ SRCS = navigator_main.cpp \ navigator_params.c \ navigator_mission.cpp \ mission_feasibility_checker.cpp \ - geofence.cpp + geofence.cpp \ + geofence_params.c INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 8b5cc45767..b6059a01ef 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -458,6 +458,8 @@ Navigator::parameters_update() param_get(_parameter_handles.rtl_alt, &(_parameters.rtl_alt)); _mission.set_onboard_mission_allowed((bool)_parameter_handles.onboard_mission_enabled); + + _geofence.updateParams(); } void From 95c20ba9f9cc57fffb64f13c7108f4cbb149bf7b Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 5 Jan 2014 11:29:06 +0100 Subject: [PATCH 294/447] add inflight geofence check, issues warning on gcs for now --- src/modules/navigator/navigator_main.cpp | 20 +++++++++++++++++++- 1 file changed, 19 insertions(+), 1 deletion(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index b6059a01ef..bb7520a03f 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -160,6 +160,8 @@ private: perf_counter_t _loop_perf; /**< loop performance counter */ Geofence _geofence; + bool _geofence_violation_warning_sent; + bool _fence_valid; /**< flag if fence is valid */ bool _inside_fence; /**< vehicle is inside fence */ @@ -393,7 +395,8 @@ Navigator::Navigator() : _time_first_inside_orbit(0), _set_nav_state_timestamp(0), _need_takeoff(true), - _do_takeoff(false) + _do_takeoff(false), + _geofence_violation_warning_sent(false) { _parameter_handles.min_altitude = param_find("NAV_MIN_ALT"); _parameter_handles.acceptance_radius = param_find("NAV_ACCEPT_RAD"); @@ -766,6 +769,21 @@ Navigator::task_main() on_mission_item_reached(); } } + + /* Check geofence violation */ + if(!_geofence.inside(&_global_pos)) { + //xxx: publish geofence violation here (or change local flag depending on which app handles the flight termination) + + /* Issue a warning about the geofence violation once */ + if (!_geofence_violation_warning_sent) + { + mavlink_log_critical(_mavlink_fd, "#audio: Geofence violation"); + _geofence_violation_warning_sent = true; + } + } else { + /* Reset the _geofence_violation_warning_sent field */ + _geofence_violation_warning_sent = false; + } } /* notify user about state changes */ From d1e991f1f0183ce6855bf2df15e1fdd311d096d4 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 5 Jan 2014 12:20:25 +0100 Subject: [PATCH 295/447] launchdetection: add minimal throttle, fix parameter update --- src/lib/launchdetection/LaunchDetector.cpp | 8 +++++--- src/lib/launchdetection/LaunchDetector.h | 3 +++ src/lib/launchdetection/launchdetection_params.c | 11 ++++++++--- .../fw_pos_control_l1/fw_pos_control_l1_main.cpp | 2 +- 4 files changed, 17 insertions(+), 7 deletions(-) diff --git a/src/lib/launchdetection/LaunchDetector.cpp b/src/lib/launchdetection/LaunchDetector.cpp index 2545e0a7e2..d894d6a6fb 100644 --- a/src/lib/launchdetection/LaunchDetector.cpp +++ b/src/lib/launchdetection/LaunchDetector.cpp @@ -43,7 +43,8 @@ #include LaunchDetector::LaunchDetector() : - launchdetection_on(NULL, "LAUN_ALL_ON", false) + launchdetection_on(NULL, "LAUN_ALL_ON", false), + throttle_min(NULL, "LAUN_THR_MIN", false) { /* init all detectors */ launchMethods[0] = new CatapultLaunchMethod(); @@ -83,10 +84,11 @@ bool LaunchDetector::getLaunchDetected() void LaunchDetector::updateParams() { warnx(" LaunchDetector::updateParams()"); - //launchdetection_on.update(); + launchdetection_on.update(); + throttle_min.update(); for (uint8_t i = 0; i < sizeof(launchMethods)/sizeof(LaunchMethod); i++) { - //launchMethods[i]->updateParams(); + launchMethods[i]->updateParams(); warnx("updating component %d", i); } diff --git a/src/lib/launchdetection/LaunchDetector.h b/src/lib/launchdetection/LaunchDetector.h index 9818911437..3b8aa0ced0 100644 --- a/src/lib/launchdetection/LaunchDetector.h +++ b/src/lib/launchdetection/LaunchDetector.h @@ -59,11 +59,14 @@ public: void updateParams(); bool launchDetectionEnabled() { return (bool)launchdetection_on.get(); }; + float getMinThrottle() {return throttle_min.get(); } + // virtual bool getLaunchDetected(); protected: private: LaunchMethod* launchMethods[1]; control::BlockParamInt launchdetection_on; + control::BlockParamFloat throttle_min; }; diff --git a/src/lib/launchdetection/launchdetection_params.c b/src/lib/launchdetection/launchdetection_params.c index 536749c88c..e07a2b26da 100644 --- a/src/lib/launchdetection/launchdetection_params.c +++ b/src/lib/launchdetection/launchdetection_params.c @@ -33,11 +33,11 @@ ****************************************************************************/ /** - * @file fw_pos_control_l1_params.c + * @file launchdetection_params.c * - * Parameters defined by the L1 position control task + * Parameters for launchdetection * - * @author Lorenz Meier + * @author Thomas Gubler */ #include @@ -65,3 +65,8 @@ PARAM_DEFINE_FLOAT(LAUN_CAT_A, 30.0f); // @Description LAUN_CAT_A * LAUN_CAT_T serves as threshold to trigger launch detection // @Range > 0, in seconds PARAM_DEFINE_FLOAT(LAUN_CAT_T, 0.05f); + +// @DisplayName Throttle setting while detecting the launch +// @Description The throttle is set to this value while the system is waiting for the takeoff +// @Range 0 to 1 +PARAM_DEFINE_FLOAT(LAUN_THR_MIN, 0.0f); diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 3405185e21..826ee0cdb9 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -1028,7 +1028,7 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio } } else { - throttle_max = 0.0f; + throttle_max = launchDetector.getMinThrottle(); } } From f387c0ccc3fca38a2989b784847743b752a78de6 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 5 Jan 2014 14:19:19 +0100 Subject: [PATCH 296/447] launchdetection: rename pre takeoff throttle param and fix usage --- src/lib/launchdetection/LaunchDetector.cpp | 4 ++-- src/lib/launchdetection/LaunchDetector.h | 4 ++-- src/lib/launchdetection/launchdetection_params.c | 2 +- .../fw_pos_control_l1/fw_pos_control_l1_main.cpp | 14 ++++++++++++-- 4 files changed, 17 insertions(+), 7 deletions(-) diff --git a/src/lib/launchdetection/LaunchDetector.cpp b/src/lib/launchdetection/LaunchDetector.cpp index d894d6a6fb..67b32ad82c 100644 --- a/src/lib/launchdetection/LaunchDetector.cpp +++ b/src/lib/launchdetection/LaunchDetector.cpp @@ -44,7 +44,7 @@ LaunchDetector::LaunchDetector() : launchdetection_on(NULL, "LAUN_ALL_ON", false), - throttle_min(NULL, "LAUN_THR_MIN", false) + throttlePreTakeoff(NULL, "LAUN_THR_PRE", false) { /* init all detectors */ launchMethods[0] = new CatapultLaunchMethod(); @@ -85,7 +85,7 @@ void LaunchDetector::updateParams() { warnx(" LaunchDetector::updateParams()"); launchdetection_on.update(); - throttle_min.update(); + throttlePreTakeoff.update(); for (uint8_t i = 0; i < sizeof(launchMethods)/sizeof(LaunchMethod); i++) { launchMethods[i]->updateParams(); diff --git a/src/lib/launchdetection/LaunchDetector.h b/src/lib/launchdetection/LaunchDetector.h index 3b8aa0ced0..7c2ff826cf 100644 --- a/src/lib/launchdetection/LaunchDetector.h +++ b/src/lib/launchdetection/LaunchDetector.h @@ -59,14 +59,14 @@ public: void updateParams(); bool launchDetectionEnabled() { return (bool)launchdetection_on.get(); }; - float getMinThrottle() {return throttle_min.get(); } + float getThrottlePreTakeoff() {return throttlePreTakeoff.get(); } // virtual bool getLaunchDetected(); protected: private: LaunchMethod* launchMethods[1]; control::BlockParamInt launchdetection_on; - control::BlockParamFloat throttle_min; + control::BlockParamFloat throttlePreTakeoff; }; diff --git a/src/lib/launchdetection/launchdetection_params.c b/src/lib/launchdetection/launchdetection_params.c index e07a2b26da..63a8981aa8 100644 --- a/src/lib/launchdetection/launchdetection_params.c +++ b/src/lib/launchdetection/launchdetection_params.c @@ -69,4 +69,4 @@ PARAM_DEFINE_FLOAT(LAUN_CAT_T, 0.05f); // @DisplayName Throttle setting while detecting the launch // @Description The throttle is set to this value while the system is waiting for the takeoff // @Range 0 to 1 -PARAM_DEFINE_FLOAT(LAUN_THR_MIN, 0.0f); +PARAM_DEFINE_FLOAT(LAUN_THR_PRE, 0.0f); diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 826ee0cdb9..04caf0bbc3 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -174,6 +174,7 @@ private: /* takeoff/launch states */ bool launch_detected; + bool usePreTakeoffThrust; bool launch_detection_message_sent; /* Landingslope object */ @@ -396,6 +397,7 @@ FixedwingPositionControl::FixedwingPositionControl() : _mavlink_fd(-1), launchDetector(), launch_detected(false), + usePreTakeoffThrust(false), launch_detection_message_sent(false) { /* safely initialize structs */ @@ -1004,6 +1006,7 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio _att_sp.yaw_body = _l1_control.nav_bearing(); if (launch_detected) { + usePreTakeoffThrust = false; /* apply minimum pitch and limit roll if target altitude is not within 10 meters */ if (altitude_error > 15.0f) { @@ -1028,7 +1031,7 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio } } else { - throttle_max = launchDetector.getMinThrottle(); + usePreTakeoffThrust = true; } } @@ -1053,6 +1056,7 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio /* reset takeoff/launch state */ if (mission_item_triplet.current.nav_cmd != NAV_CMD_TAKEOFF) { launch_detected = false; + usePreTakeoffThrust = false; launch_detection_message_sent = false; } @@ -1150,8 +1154,14 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio setpoint = false; } + if (usePreTakeoffThrust) { + _att_sp.thrust = launchDetector.getThrottlePreTakeoff(); + } + else { + _att_sp.thrust = math::min(_tecs.get_throttle_demand(), throttle_max); + } _att_sp.pitch_body = _tecs.get_pitch_demand(); - _att_sp.thrust = math::min(_tecs.get_throttle_demand(), throttle_max); + return setpoint; } From 7ee5f127f238d408a7ee5d4bcde5fd759f0d1a1f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 5 Jan 2014 15:20:54 +0100 Subject: [PATCH 297/447] Compile cleanups on the IO firmware --- src/modules/px4iofirmware/registers.c | 2 +- src/modules/px4iofirmware/safety.c | 1 - 2 files changed, 1 insertion(+), 2 deletions(-) diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index 0358725db3..bfc0337f67 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -606,7 +606,7 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) if (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] == UINT8_MAX) { disabled = true; - } else if ((int)(conf[PX4IO_P_RC_CONFIG_ASSIGNMENT]) < 0 || conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] >= PX4IO_RC_MAPPED_CONTROL_CHANNELS) { + } else if (REG_TO_SIGNED(conf[PX4IO_P_RC_CONFIG_ASSIGNMENT]) < 0 || conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] >= PX4IO_RC_MAPPED_CONTROL_CHANNELS) { count++; } diff --git a/src/modules/px4iofirmware/safety.c b/src/modules/px4iofirmware/safety.c index cdb54a80ad..83bd3026eb 100644 --- a/src/modules/px4iofirmware/safety.c +++ b/src/modules/px4iofirmware/safety.c @@ -45,7 +45,6 @@ #include "px4io.h" static struct hrt_call arming_call; -static struct hrt_call heartbeat_call; static struct hrt_call failsafe_call; /* From d114ff1ac96fa3485146ee411cbc1b07a32cb7ce Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 5 Jan 2014 15:21:30 +0100 Subject: [PATCH 298/447] Remove unused variable --- src/modules/systemlib/mixer/mixer.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/src/modules/systemlib/mixer/mixer.cpp b/src/modules/systemlib/mixer/mixer.cpp index cce46bf5fc..20b1f18ed6 100644 --- a/src/modules/systemlib/mixer/mixer.cpp +++ b/src/modules/systemlib/mixer/mixer.cpp @@ -171,7 +171,6 @@ NullMixer * NullMixer::from_text(const char *buf, unsigned &buflen) { NullMixer *nm = nullptr; - const char *end = buf + buflen; /* enforce that the mixer ends with space or a new line */ for (int i = buflen - 1; i >= 0; i--) { From 40196275d0e899eac49731d0c2b1dda96cafb84f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 5 Jan 2014 15:23:54 +0100 Subject: [PATCH 299/447] Compile warning fixes on preflight check --- src/systemcmds/preflight_check/preflight_check.c | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/systemcmds/preflight_check/preflight_check.c b/src/systemcmds/preflight_check/preflight_check.c index 982b03782f..86e4ff545d 100644 --- a/src/systemcmds/preflight_check/preflight_check.c +++ b/src/systemcmds/preflight_check/preflight_check.c @@ -44,6 +44,7 @@ #include #include #include +#include #include #include @@ -87,9 +88,7 @@ int preflight_check_main(int argc, char *argv[]) /* give the system some time to sample the sensors in the background */ usleep(150000); - /* ---- MAG ---- */ - close(fd); fd = open(MAG_DEVICE_PATH, 0); if (fd < 0) { warn("failed to open magnetometer - start with 'hmc5883 start' or 'lsm303d start'"); From 187c2a4bcab6e083389e90ca62417cb4629ff9cb Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 6 Jan 2014 09:17:08 +0100 Subject: [PATCH 300/447] sdlog2: Main & Nav state logging fixed --- src/modules/sdlog2/sdlog2.c | 21 ++++++++++++++++----- 1 file changed, 16 insertions(+), 5 deletions(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 6833ec43f4..3873dc96e9 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -62,6 +62,7 @@ #include #include +#include #include #include #include @@ -684,6 +685,7 @@ int sdlog2_thread_main(int argc, char *argv[]) /* warning! using union here to save memory, elements should be used separately! */ union { struct vehicle_command_s cmd; + struct vehicle_control_mode_s control_mode; struct sensor_combined_s sensor; struct vehicle_attitude_s att; struct vehicle_attitude_setpoint_s att_sp; @@ -709,6 +711,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct { int cmd_sub; int status_sub; + int control_mode_sub; int sensor_sub; int att_sub; int att_sp_sub; @@ -761,7 +764,7 @@ int sdlog2_thread_main(int argc, char *argv[]) /* --- IMPORTANT: DEFINE NUMBER OF ORB STRUCTS TO WAIT FOR HERE --- */ /* number of subscriptions */ - const ssize_t fdsc = 19; + const ssize_t fdsc = 20; /* sanity check variable and index */ ssize_t fdsc_count = 0; /* file descriptors to wait for */ @@ -779,6 +782,12 @@ int sdlog2_thread_main(int argc, char *argv[]) fds[fdsc_count].events = POLLIN; fdsc_count++; + /* --- VEHICLE CONTROL MODE --- */ + subs.control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); + fds[fdsc_count].fd = subs.control_mode_sub; + fds[fdsc_count].events = POLLIN; + fdsc_count++; + /* --- GPS POSITION --- */ subs.gps_pos_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); fds[fdsc_count].fd = subs.gps_pos_sub; @@ -966,11 +975,12 @@ int sdlog2_thread_main(int argc, char *argv[]) /* --- VEHICLE STATUS --- */ if (fds[ifds++].revents & POLLIN) { - // Don't orb_copy, it's already done few lines above + /* don't orb_copy, it's already done few lines above */ + /* copy control mode here to construct STAT message */ + orb_copy(ORB_ID(vehicle_control_mode), subs.control_mode_sub, &buf.control_mode); log_msg.msg_type = LOG_STAT_MSG; - log_msg.body.log_STAT.main_state = (uint8_t) buf_status.main_state; - // TODO use control_mode topic - //log_msg.body.log_STAT.navigation_state = (uint8_t) buf_status.navigation_state; + log_msg.body.log_STAT.main_state = (uint8_t) buf.control_mode.main_state; + log_msg.body.log_STAT.navigation_state = (uint8_t) buf.control_mode.nav_state; log_msg.body.log_STAT.arming_state = (uint8_t) buf_status.arming_state; log_msg.body.log_STAT.battery_voltage = buf_status.battery_voltage; log_msg.body.log_STAT.battery_current = buf_status.battery_current; @@ -979,6 +989,7 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.body.log_STAT.landed = (uint8_t) buf_status.condition_landed; LOGBUFFER_WRITE_AND_COUNT(STAT); } + ifds++; // skip CONTROL MODE, already copied /* --- GPS POSITION --- */ if (fds[ifds++].revents & POLLIN) { From 63815909979d8b01738a18bd694afcf2bc11c3a3 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 6 Jan 2014 14:33:58 +0100 Subject: [PATCH 301/447] attitude_estimator_ekf: acc compensation improvements --- .../attitude_estimator_ekf_main.cpp | 112 +++++++++++------- .../attitude_estimator_ekf_params.c | 6 + .../attitude_estimator_ekf_params.h | 2 + 3 files changed, 80 insertions(+), 40 deletions(-) diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp index 36e82db43b..7cf1000149 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp @@ -59,6 +59,7 @@ #include #include #include +#include #include #include @@ -219,6 +220,8 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds memset(&raw, 0, sizeof(raw)); struct vehicle_gps_position_s gps; memset(&gps, 0, sizeof(gps)); + struct vehicle_global_position_s global_pos; + memset(&global_pos, 0, sizeof(global_pos)); struct vehicle_attitude_s att; memset(&att, 0, sizeof(att)); struct vehicle_control_mode_s control_mode; @@ -226,9 +229,20 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds uint64_t last_data = 0; uint64_t last_measurement = 0; - uint64_t last_gps = 0; + uint64_t last_vel_t = 0; - float vel_prev[3] = { 0.0f, 0.0f, 0.0f }; + /* current velocity */ + math::Vector<3> vel; + vel.zero(); + /* previous velocity */ + math::Vector<3> vel_prev; + vel_prev.zero(); + /* actual acceleration (by GPS velocity) in body frame */ + math::Vector<3> acc; + acc.zero(); + /* rotation matrix */ + math::Matrix<3, 3> R; + R.identity(); /* subscribe to raw data */ int sub_raw = orb_subscribe(ORB_ID(sensor_combined)); @@ -238,6 +252,9 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds /* subscribe to GPS */ int sub_gps = orb_subscribe(ORB_ID(vehicle_gps_position)); + /* subscribe to GPS */ + int sub_global_pos = orb_subscribe(ORB_ID(vehicle_global_position)); + /* subscribe to param changes */ int sub_params = orb_subscribe(ORB_ID(parameter_update)); @@ -276,9 +293,6 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds float gyro_offsets[3] = { 0.0f, 0.0f, 0.0f }; unsigned offset_count = 0; - /* actual acceleration (by GPS velocity) in body frame */ - float acc[3] = { 0.0f, 0.0f, 0.0f }; - /* rotation matrix for magnetic declination */ math::Matrix<3, 3> R_decl; R_decl.identity(); @@ -327,7 +341,18 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds /* get latest measurements */ orb_copy(ORB_ID(sensor_combined), sub_raw, &raw); - orb_copy(ORB_ID(vehicle_gps_position), sub_gps, &gps); + + bool gps_updated; + orb_check(sub_gps, &gps_updated); + if (gps_updated) { + orb_copy(ORB_ID(vehicle_gps_position), sub_gps, &gps); + } + + bool global_pos_updated; + orb_check(sub_global_pos, &global_pos_updated); + if (global_pos_updated) { + orb_copy(ORB_ID(vehicle_global_position), sub_global_pos, &global_pos); + } if (!initialized) { // XXX disabling init for now @@ -374,43 +399,50 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds sensor_last_timestamp[1] = raw.timestamp; } - if (gps.fix_type >= 3 && gps.eph_m < 10.0f && gps.vel_ned_valid && hrt_absolute_time() < gps.timestamp_velocity + 500000) { - if (last_gps != 0 && gps.timestamp_velocity != last_gps) { - float gps_dt = (gps.timestamp_velocity - last_gps) / 1000000.0f; - /* calculate acceleration in NED frame */ - float acc_NED[3]; - acc_NED[0] = (gps.vel_n_m_s - vel_prev[0]) / gps_dt; - acc_NED[1] = (gps.vel_e_m_s - vel_prev[1]) / gps_dt; - acc_NED[2] = (gps.vel_d_m_s - vel_prev[2]) / gps_dt; - - /* project acceleration to body frame */ - for (int i = 0; i < 3; i++) { - acc[i] = 0.0f; - for (int j = 0; j < 3; j++) { - acc[i] += att.R[j][i] * acc_NED[j]; - } - } - - vel_prev[0] = gps.vel_n_m_s; - vel_prev[1] = gps.vel_e_m_s; - vel_prev[2] = gps.vel_d_m_s; + hrt_abstime vel_t = 0; + bool vel_valid = false; + if (ekf_params.acc_comp == 1 && gps.fix_type >= 3 && gps.eph_m < 10.0f && gps.vel_ned_valid && hrt_absolute_time() < gps.timestamp_velocity + 500000) { + vel_valid = true; + if (gps_updated) { + vel_t = gps.timestamp_velocity; + vel(0) = gps.vel_n_m_s; + vel(1) = gps.vel_e_m_s; + vel(2) = gps.vel_d_m_s; } - last_gps = gps.timestamp_velocity; - } else { - acc[0] = 0.0f; - acc[1] = 0.0f; - acc[2] = 0.0f; - - vel_prev[0] = 0.0f; - vel_prev[1] = 0.0f; - vel_prev[2] = 0.0f; - last_gps = 0; + } else if (ekf_params.acc_comp == 2 && global_pos_updated && global_pos.valid && hrt_absolute_time() < global_pos.timestamp + 500000) { + vel_valid = true; + if (global_pos_updated) { + vel_t = global_pos.timestamp; + vel(0) = global_pos.vx; + vel(1) = global_pos.vy; + vel(2) = global_pos.vz; + } } - z_k[3] = raw.accelerometer_m_s2[0] - acc[0]; - z_k[4] = raw.accelerometer_m_s2[1] - acc[1]; - z_k[5] = raw.accelerometer_m_s2[2] - acc[2]; + if (vel_valid) { + /* velocity is valid */ + if (vel_t != 0) { + /* velocity updated */ + if (last_vel_t != 0 && vel_t != last_vel_t) { + float vel_dt = (vel_t - last_vel_t) / 1000000.0f; + /* calculate acceleration in body frame */ + acc = R.transposed() * ((vel - vel_prev) / vel_dt); + } + last_vel_t = vel_t; + vel_prev = vel; + } + + } else { + /* velocity is valid, reset acceleration */ + acc.zero(); + vel_prev.zero(); + last_vel_t = 0; + } + + z_k[3] = raw.accelerometer_m_s2[0] - acc(0); + z_k[4] = raw.accelerometer_m_s2[1] - acc(1); + z_k[5] = raw.accelerometer_m_s2[2] - acc(2); /* update magnetometer measurements */ if (sensor_last_count[2] != raw.magnetometer_counter) { @@ -506,7 +538,7 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds /* magnetic declination */ math::Matrix<3, 3> R_body = (&Rot_matrix[0]); - math::Matrix<3, 3> R = R_decl * R_body; + R = R_decl * R_body; /* copy rotation matrix */ memcpy(&att.R[0][0], &R.data[0][0], sizeof(att.R)); diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c index 999446a47f..44f47b47ca 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c @@ -68,6 +68,8 @@ PARAM_DEFINE_FLOAT(ATT_YAW_OFF3, 0.0f); /* magnetic declination, in degrees */ PARAM_DEFINE_FLOAT(ATT_MAG_DECL, 0.0f); +PARAM_DEFINE_INT32(ATT_ACC_COMP, 0); + int parameters_init(struct attitude_estimator_ekf_param_handles *h) { /* PID parameters */ @@ -88,6 +90,8 @@ int parameters_init(struct attitude_estimator_ekf_param_handles *h) h->mag_decl = param_find("ATT_MAG_DECL"); + h->acc_comp = param_find("ATT_ACC_COMP"); + return OK; } @@ -110,5 +114,7 @@ int parameters_update(const struct attitude_estimator_ekf_param_handles *h, stru param_get(h->mag_decl, &(p->mag_decl)); + param_get(h->acc_comp, &(p->acc_comp)); + return OK; } diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.h b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.h index b4b3ca50d7..74a1416091 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.h +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.h @@ -48,6 +48,7 @@ struct attitude_estimator_ekf_params { float pitch_off; float yaw_off; float mag_decl; + int acc_comp; }; struct attitude_estimator_ekf_param_handles { @@ -55,6 +56,7 @@ struct attitude_estimator_ekf_param_handles { param_t q0, q1, q2, q3, q4; param_t roll_off, pitch_off, yaw_off; param_t mag_decl; + param_t acc_comp; }; /** From 255db83e9346469ca7da02675a54b753cfd1a074 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 8 Jan 2014 15:43:58 +0100 Subject: [PATCH 302/447] mc_pos_control: bug fixed --- .../mc_pos_control/mc_pos_control_main.cpp | 15 +++++++++++++-- 1 file changed, 13 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 a50a9e50e5..28c8861b6d 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -597,12 +597,23 @@ MulticopterPositionControl::task_main() _pos_sp += sp_move_rate * dt; /* check if position setpoint is too far from actual position */ - math::Vector<3> pos_sp_offs = (_pos_sp - _pos).edivide(_params.vel_max); + math::Vector<3> pos_sp_offs; + pos_sp_offs.zero(); + + if (_control_mode.flag_control_position_enabled) { + pos_sp_offs(0) = (_pos_sp(0) - _pos(0)) / _params.sp_offs_max(0); + pos_sp_offs(1) = (_pos_sp(1) - _pos(1)) / _params.sp_offs_max(1); + } + + if (!_control_mode.flag_control_altitude_enabled) { + pos_sp_offs(2) = (_pos_sp(2) - _pos(2)) / _params.sp_offs_max(2); + } + float pos_sp_offs_norm = pos_sp_offs.length(); if (pos_sp_offs_norm > 1.0f) { pos_sp_offs /= pos_sp_offs_norm; - _pos_sp = _pos + pos_sp_offs.emult(_params.vel_max); + _pos_sp = _pos + pos_sp_offs.emult(_params.sp_offs_max); } } else { From ac7787e2a2a2439a5fa47755dda3f775568330dd Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 11 Jan 2014 00:09:10 +0100 Subject: [PATCH 303/447] launchdetection: send warning to qgc every 4s --- .../fw_pos_control_l1/fw_pos_control_l1_main.cpp | 12 +++++------- 1 file changed, 5 insertions(+), 7 deletions(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 04caf0bbc3..c90b0313a6 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -175,7 +175,6 @@ private: /* takeoff/launch states */ bool launch_detected; bool usePreTakeoffThrust; - bool launch_detection_message_sent; /* Landingslope object */ Landingslope landingslope; @@ -397,8 +396,7 @@ FixedwingPositionControl::FixedwingPositionControl() : _mavlink_fd(-1), launchDetector(), launch_detected(false), - usePreTakeoffThrust(false), - launch_detection_message_sent(false) + usePreTakeoffThrust(false) { /* safely initialize structs */ vehicle_attitude_s _att = {0}; @@ -985,10 +983,11 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio // warnx("Launch detection running"); if(!launch_detected) { //do not do further checks once a launch was detected if (launchDetector.launchDetectionEnabled()) { -// warnx("Launch detection enabled"); - if(!launch_detection_message_sent) { + static hrt_abstime last_sent = 0; + if(hrt_absolute_time() - last_sent > 4e6) { +// warnx("Launch detection running"); mavlink_log_info(_mavlink_fd, "#audio: Launchdetection running"); - launch_detection_message_sent = true; + last_sent = hrt_absolute_time(); } launchDetector.update(_sensor_combined.accelerometer_m_s2[0]); if (launchDetector.getLaunchDetected()) { @@ -1057,7 +1056,6 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio if (mission_item_triplet.current.nav_cmd != NAV_CMD_TAKEOFF) { launch_detected = false; usePreTakeoffThrust = false; - launch_detection_message_sent = false; } if (was_circle_mode && !_l1_control.circle_mode()) { From 14c0fae175452da6e28ffc20b265de621a2430ba Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 14 Jan 2014 15:47:21 +0100 Subject: [PATCH 304/447] sdlog2: fixed state logging if navigator not running --- src/modules/sdlog2/sdlog2.c | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 3873dc96e9..7083ce5a2c 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -977,7 +977,9 @@ int sdlog2_thread_main(int argc, char *argv[]) if (fds[ifds++].revents & POLLIN) { /* don't orb_copy, it's already done few lines above */ /* copy control mode here to construct STAT message */ - orb_copy(ORB_ID(vehicle_control_mode), subs.control_mode_sub, &buf.control_mode); + if (fds[ifds].revents & POLLIN) { + orb_copy(ORB_ID(vehicle_control_mode), subs.control_mode_sub, &buf.control_mode); + } log_msg.msg_type = LOG_STAT_MSG; log_msg.body.log_STAT.main_state = (uint8_t) buf.control_mode.main_state; log_msg.body.log_STAT.navigation_state = (uint8_t) buf.control_mode.nav_state; From 36ba4487546a1c9371d3471a04ead07b77679493 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 17 Jan 2014 23:21:16 +0100 Subject: [PATCH 305/447] rcS: start navigator and dataman --- ROMFS/px4fmu_common/init.d/rcS | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 32c7991efb..84e22a2331 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -391,6 +391,16 @@ then fi fi + # + # Start the datamanager + # + dataman start + + # + # Start the navigator + # + navigator start + # # Sensors, Logging, GPS # From 43d751737bdaa04b81255d7be9aa2eab7be778aa Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 18 Jan 2014 00:05:04 +0100 Subject: [PATCH 306/447] rc: use vector control apps for multirotors --- ROMFS/px4fmu_common/init.d/rc.mc_apps | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_apps b/ROMFS/px4fmu_common/init.d/rc.mc_apps index 8b51d57e51..4e0d681475 100644 --- a/ROMFS/px4fmu_common/init.d/rc.mc_apps +++ b/ROMFS/px4fmu_common/init.d/rc.mc_apps @@ -16,9 +16,9 @@ position_estimator_inav start # # Start attitude control # -multirotor_att_control start +mc_att_control_vector start # # Start position control # -multirotor_pos_control start +mc_pos_control start From 5a1b39a17215103b8f8181d1ba5e44257abc5b34 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 18 Jan 2014 00:15:34 +0100 Subject: [PATCH 307/447] RTL on RC failsafe --- src/modules/commander/commander.cpp | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index dbfca38b12..cedcbb3fbf 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1133,6 +1133,14 @@ int commander_thread_main(int argc, char *argv[]) mavlink_log_critical(mavlink_fd, "#audio: CRITICAL: RC SIGNAL LOST"); status.rc_signal_lost = true; status_changed = true; + if (status.main_state != MAIN_STATE_AUTO) { + transition_result_t res = main_state_transition(&status, MAIN_STATE_AUTO); + if (res == TRANSITION_CHANGED) { + mavlink_log_critical(mavlink_fd, "#audio: switching to RTL mode"); + status.set_nav_state = NAV_STATE_RTL; + status.set_nav_state_timestamp = hrt_absolute_time(); + } + } } } } From b175937b5ffab87e8951c620d7673582341f98b4 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 18 Jan 2014 14:25:24 +0100 Subject: [PATCH 308/447] navigator, commander: RTL and RC failsafe fixes --- src/modules/commander/commander.cpp | 2 +- src/modules/navigator/navigator_main.cpp | 38 ++++++++++++++++++------ src/modules/navigator/navigator_params.c | 2 +- 3 files changed, 31 insertions(+), 11 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index cedcbb3fbf..6a668fcd72 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1133,7 +1133,7 @@ int commander_thread_main(int argc, char *argv[]) mavlink_log_critical(mavlink_fd, "#audio: CRITICAL: RC SIGNAL LOST"); status.rc_signal_lost = true; status_changed = true; - if (status.main_state != MAIN_STATE_AUTO) { + if (status.main_state != MAIN_STATE_AUTO && armed.armed) { transition_result_t res = main_state_transition(&status, MAIN_STATE_AUTO); if (res == TRANSITION_CHANGED) { mavlink_log_critical(mavlink_fd, "#audio: switching to RTL mode"); diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index bb7520a03f..4bc05d1070 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -180,6 +180,8 @@ private: uint64_t _set_nav_state_timestamp; /**< timestamp of last handled navigation state request */ + char *nav_states_str[NAV_STATE_MAX]; + struct { float min_altitude; float acceptance_radius; @@ -415,6 +417,12 @@ Navigator::Navigator() : memset(&_mission_result, 0, sizeof(struct mission_result_s)); + nav_states_str[0] = "NONE"; + nav_states_str[1] = "READY"; + nav_states_str[2] = "LOITER"; + nav_states_str[3] = "MISSION"; + nav_states_str[4] = "RTL"; + /* Initialize state machine */ myState = NAV_STATE_NONE; start_none(); @@ -788,7 +796,7 @@ Navigator::task_main() /* notify user about state changes */ if (myState != prevState) { - mavlink_log_info(_mavlink_fd, "[navigator] nav state %d -> %d", prevState, myState); + mavlink_log_info(_mavlink_fd, "[navigator] nav state: %s -> %s", nav_states_str[prevState], nav_states_str[myState]); prevState = myState; pub_control_mode = true; } @@ -881,7 +889,7 @@ StateTable::Tran const Navigator::myTable[NAV_STATE_MAX][MAX_EVENT] = { }, { /* STATE_READY */ - /* EVENT_NONE_REQUESTED */ {NO_ACTION, NAV_STATE_NONE}, + /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE}, /* EVENT_READY_REQUESTED */ {NO_ACTION, NAV_STATE_READY}, /* EVENT_LOITER_REQUESTED */ {NO_ACTION, NAV_STATE_READY}, /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, @@ -917,7 +925,7 @@ StateTable::Tran const Navigator::myTable[NAV_STATE_MAX][MAX_EVENT] = { /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, /* EVENT_RTL_REQUESTED */ {NO_ACTION, NAV_STATE_RTL}, /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_RTL}, - /* EVENT_HOME_POSITION_CHANGED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, + /* EVENT_HOME_POSITION_CHANGED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, // TODO need to reset rtl_state }, }; @@ -945,8 +953,8 @@ Navigator::start_ready() _reset_loiter_pos = true; _do_takeoff = false; - // TODO check if (_rtl_state != RTL_STATE_LAND) { + /* allow RTL if landed not at home */ _rtl_state = RTL_STATE_NONE; } @@ -977,6 +985,11 @@ Navigator::start_loiter() _mission_item_triplet.current.altitude = _global_pos.alt; mavlink_log_info(_mavlink_fd, "[navigator] loiter at current altitude"); } + + if (_rtl_state == RTL_STATE_LAND) { + /* if RTL landing was interrupted, avoid landing from MIN_ALT on next RTL */ + _rtl_state = RTL_STATE_DESCEND; + } } _mission_item_triplet.previous_valid = false; @@ -1103,11 +1116,19 @@ Navigator::advance_mission() void Navigator::start_rtl() { - _reset_loiter_pos = true; _do_takeoff = false; - if (_rtl_state == RTL_STATE_NONE) - _rtl_state = RTL_STATE_CLIMB; - + if (_rtl_state == RTL_STATE_NONE) { + if (_global_pos.alt < _home_pos.altitude + _parameters.rtl_alt) { + _rtl_state = RTL_STATE_CLIMB; + } else { + _rtl_state = RTL_STATE_RETURN; + if (_reset_loiter_pos) { + _mission_item_triplet.current.altitude_is_relative = false; + _mission_item_triplet.current.altitude = _global_pos.alt; + } + } + } + _reset_loiter_pos = true; mavlink_log_info(_mavlink_fd, "[navigator] RTL started"); set_rtl_item(); } @@ -1150,7 +1171,6 @@ Navigator::set_rtl_item() _mission_item_triplet.current_valid = true; _mission_item_triplet.next_valid = false; - _mission_item_triplet.current.altitude_is_relative = false; _mission_item_triplet.current.lat = _home_pos.lat; _mission_item_triplet.current.lon = _home_pos.lon; // don't change altitude setpoint diff --git a/src/modules/navigator/navigator_params.c b/src/modules/navigator/navigator_params.c index 8df47fc3b6..617b2ec310 100644 --- a/src/modules/navigator/navigator_params.c +++ b/src/modules/navigator/navigator_params.c @@ -55,6 +55,6 @@ PARAM_DEFINE_FLOAT(NAV_MIN_ALT, 50.0f); PARAM_DEFINE_FLOAT(NAV_ACCEPT_RAD, 10.0f); PARAM_DEFINE_FLOAT(NAV_LOITER_RAD, 100.0f); PARAM_DEFINE_INT32(NAV_ONB_MIS_EN, 0); -PARAM_DEFINE_FLOAT(NAV_TAAKEOFF_ALT, 10.0f); // default TAKEOFF altitude +PARAM_DEFINE_FLOAT(NAV_TAKEOFF_ALT, 10.0f); // default TAKEOFF altitude PARAM_DEFINE_FLOAT(NAV_LAND_ALT, 5.0f); // slow descend from this altitude when landing PARAM_DEFINE_FLOAT(NAV_RTL_ALT, 30.0f); // min altitude for going home in RTL mode From 5b2f3b0b585bce29910560090fdfaa3d54e26ff3 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 18 Jan 2014 15:18:31 +0100 Subject: [PATCH 309/447] navigator: minor refactoring --- src/modules/navigator/navigator_main.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 4bc05d1070..af2b831dc7 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -309,7 +309,7 @@ private: /** * Move to next waypoint */ - void advance_mission(); + void set_mission_item(); /** * Switch to next RTL state @@ -1007,11 +1007,11 @@ Navigator::start_mission() _need_takeoff = true; mavlink_log_info(_mavlink_fd, "[navigator] mission started"); - advance_mission(); + set_mission_item(); } void -Navigator::advance_mission() +Navigator::set_mission_item() { /* copy current mission to previous item */ memcpy(&_mission_item_triplet.previous, &_mission_item_triplet.current, sizeof(mission_item_s)); @@ -1350,7 +1350,7 @@ Navigator::on_mission_item_reached() } if (_mission.current_mission_available()) { - advance_mission(); + set_mission_item(); } else { /* if no more mission items available then finish mission */ /* loiter at last waypoint */ From 524a502a56e7a5db5d05502ba26e9e7d18872e53 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 18 Jan 2014 16:42:49 +0100 Subject: [PATCH 310/447] autostart fixes --- .../px4fmu_common/init.d/10015_tbs_discovery | 2 - ROMFS/px4fmu_common/init.d/10016_3dr_iris | 30 +++++++-------- ROMFS/px4fmu_common/init.d/1001_rc_quad_x.hil | 30 +++++++-------- ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil | 38 +------------------ ROMFS/px4fmu_common/init.d/4010_dji_f330 | 30 +++++++-------- ROMFS/px4fmu_common/init.d/4011_dji_f450 | 2 - ROMFS/px4fmu_common/init.d/4012_hk_x550 | 2 - 7 files changed, 40 insertions(+), 94 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/10015_tbs_discovery b/ROMFS/px4fmu_common/init.d/10015_tbs_discovery index 63798bb3c0..5740e863ca 100644 --- a/ROMFS/px4fmu_common/init.d/10015_tbs_discovery +++ b/ROMFS/px4fmu_common/init.d/10015_tbs_discovery @@ -12,13 +12,11 @@ then # param set MC_ATT_P 5.0 param set MC_ATT_I 0.0 - param set MC_ATT_D 0.0 param set MC_ATTRATE_P 0.17 param set MC_ATTRATE_I 0.0 param set MC_ATTRATE_D 0.006 param set MC_YAWPOS_P 0.5 param set MC_YAWPOS_I 0.15 - param set MC_YAWPOS_D 0.0 param set MC_YAWRATE_P 0.2 param set MC_YAWRATE_I 0.0 param set MC_YAWRATE_D 0.0 diff --git a/ROMFS/px4fmu_common/init.d/10016_3dr_iris b/ROMFS/px4fmu_common/init.d/10016_3dr_iris index 67c24fab37..62a7942999 100644 --- a/ROMFS/px4fmu_common/init.d/10016_3dr_iris +++ b/ROMFS/px4fmu_common/init.d/10016_3dr_iris @@ -12,32 +12,28 @@ then # param set MC_ATT_P 9.0 param set MC_ATT_I 0.0 - param set MC_ATT_D 0.0 param set MC_ATTRATE_P 0.13 param set MC_ATTRATE_I 0.0 param set MC_ATTRATE_D 0.004 param set MC_YAWPOS_P 0.5 param set MC_YAWPOS_I 0.15 - param set MC_YAWPOS_D 0.0 param set MC_YAWRATE_P 0.2 param set MC_YAWRATE_I 0.0 param set MC_YAWRATE_D 0.0 - param set MPC_TILT_MAX 0.5 - param set MPC_THR_MAX 0.8 - param set MPC_THR_MIN 0.2 - param set MPC_XY_D 0 - param set MPC_XY_P 0.5 - param set MPC_XY_VEL_D 0 - param set MPC_XY_VEL_I 0 - param set MPC_XY_VEL_MAX 3 - param set MPC_XY_VEL_P 0.2 - param set MPC_Z_D 0 - param set MPC_Z_P 1 - param set MPC_Z_VEL_D 0 - param set MPC_Z_VEL_I 0.1 - param set MPC_Z_VEL_MAX 2 - param set MPC_Z_VEL_P 0.20 + param set MPC_TILT_MAX 1.0 + param set MPC_THR_MAX 1.0 + param set MPC_THR_MIN 0.1 + param set MPC_XY_P 1.0 + param set MPC_XY_VEL_D 0.01 + param set MPC_XY_VEL_I 0.02 + param set MPC_XY_VEL_MAX 5 + param set MPC_XY_VEL_P 0.1 + param set MPC_Z_P 1.0 + param set MPC_Z_VEL_D 0.0 + param set MPC_Z_VEL_I 0.02 + param set MPC_Z_VEL_MAX 3 + param set MPC_Z_VEL_P 0.1 param set BAT_V_SCALING 0.00989 param set BAT_C_SCALING 0.0124 diff --git a/ROMFS/px4fmu_common/init.d/1001_rc_quad_x.hil b/ROMFS/px4fmu_common/init.d/1001_rc_quad_x.hil index 8c0797d7cb..9e098093e6 100644 --- a/ROMFS/px4fmu_common/init.d/1001_rc_quad_x.hil +++ b/ROMFS/px4fmu_common/init.d/1001_rc_quad_x.hil @@ -15,29 +15,25 @@ then param set MC_ATTRATE_D 0.004 param set MC_ATT_P 7.0 param set MC_ATT_I 0.0 - param set MC_ATT_D 0.0 param set MC_YAWPOS_P 2.0 param set MC_YAWPOS_I 0.0 - param set MC_YAWPOS_D 0.0 param set MC_YAWRATE_P 0.3 param set MC_YAWRATE_I 0.2 param set MC_YAWRATE_D 0.005 - param set MPC_TILT_MAX 0.5 - param set MPC_THR_MAX 0.8 - param set MPC_THR_MIN 0.2 - param set MPC_XY_D 0 - param set MPC_XY_P 0.5 - param set MPC_XY_VEL_D 0 - param set MPC_XY_VEL_I 0 - param set MPC_XY_VEL_MAX 3 - param set MPC_XY_VEL_P 0.2 - param set MPC_Z_D 0 - param set MPC_Z_P 1 - param set MPC_Z_VEL_D 0 - param set MPC_Z_VEL_I 0.1 - param set MPC_Z_VEL_MAX 2 - param set MPC_Z_VEL_P 0.20 + param set MPC_TILT_MAX 1.0 + param set MPC_THR_MAX 1.0 + param set MPC_THR_MIN 0.1 + param set MPC_XY_P 1.0 + param set MPC_XY_VEL_D 0.01 + param set MPC_XY_VEL_I 0.02 + param set MPC_XY_VEL_MAX 5 + param set MPC_XY_VEL_P 0.1 + param set MPC_Z_P 1.0 + param set MPC_Z_VEL_D 0.0 + param set MPC_Z_VEL_I 0.02 + param set MPC_Z_VEL_MAX 3 + param set MPC_Z_VEL_P 0.1 fi set HIL yes diff --git a/ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil b/ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil index bce3015fc7..297082b798 100644 --- a/ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil +++ b/ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil @@ -5,42 +5,6 @@ # Maintainers: Anton Babushkin # -if [ $DO_AUTOCONFIG == yes ] -then - # - # Default parameters for this platform - # - param set MC_ATTRATE_P 0.12 - param set MC_ATTRATE_I 0.0 - param set MC_ATTRATE_D 0.004 - param set MC_ATT_P 7.0 - param set MC_ATT_I 0.0 - param set MC_ATT_D 0.0 - param set MC_YAWPOS_P 2.0 - param set MC_YAWPOS_I 0.0 - param set MC_YAWPOS_D 0.0 - param set MC_YAWRATE_P 0.3 - param set MC_YAWRATE_I 0.2 - param set MC_YAWRATE_D 0.005 - - param set MPC_TILT_MAX 0.5 - param set MPC_THR_MAX 0.8 - param set MPC_THR_MIN 0.2 - param set MPC_XY_D 0 - param set MPC_XY_P 0.5 - param set MPC_XY_VEL_D 0 - param set MPC_XY_VEL_I 0 - param set MPC_XY_VEL_MAX 3 - param set MPC_XY_VEL_P 0.2 - param set MPC_Z_D 0 - param set MPC_Z_P 1 - param set MPC_Z_VEL_D 0 - param set MPC_Z_VEL_I 0.1 - param set MPC_Z_VEL_MAX 2 - param set MPC_Z_VEL_P 0.20 -fi +sh /etc/ini.d/1001_rc_quad_x.hil -set HIL yes - -set VEHICLE_TYPE mc set MIXER FMU_quad_+ diff --git a/ROMFS/px4fmu_common/init.d/4010_dji_f330 b/ROMFS/px4fmu_common/init.d/4010_dji_f330 index ab1db94d0a..5ce86f593f 100644 --- a/ROMFS/px4fmu_common/init.d/4010_dji_f330 +++ b/ROMFS/px4fmu_common/init.d/4010_dji_f330 @@ -12,32 +12,28 @@ then # param set MC_ATT_P 7.0 param set MC_ATT_I 0.0 - param set MC_ATT_D 0.0 param set MC_ATTRATE_P 0.12 param set MC_ATTRATE_I 0.0 param set MC_ATTRATE_D 0.004 param set MC_YAWPOS_P 2.8 param set MC_YAWPOS_I 0.0 - param set MC_YAWPOS_D 0.0 param set MC_YAWRATE_P 0.2 param set MC_YAWRATE_I 0.05 param set MC_YAWRATE_D 0.0 - param set MPC_TILT_MAX 0.5 - param set MPC_THR_MAX 0.8 - param set MPC_THR_MIN 0.2 - param set MPC_XY_D 0 - param set MPC_XY_P 0.5 - param set MPC_XY_VEL_D 0 - param set MPC_XY_VEL_I 0 - param set MPC_XY_VEL_MAX 3 - param set MPC_XY_VEL_P 0.2 - param set MPC_Z_D 0 - param set MPC_Z_P 1 - param set MPC_Z_VEL_D 0 - param set MPC_Z_VEL_I 0.1 - param set MPC_Z_VEL_MAX 2 - param set MPC_Z_VEL_P 0.20 + param set MPC_TILT_MAX 1.0 + param set MPC_THR_MAX 1.0 + param set MPC_THR_MIN 0.1 + param set MPC_XY_P 1.0 + param set MPC_XY_VEL_D 0.01 + param set MPC_XY_VEL_I 0.02 + param set MPC_XY_VEL_MAX 5 + param set MPC_XY_VEL_P 0.1 + param set MPC_Z_P 1.0 + param set MPC_Z_VEL_D 0.0 + param set MPC_Z_VEL_I 0.02 + param set MPC_Z_VEL_MAX 3 + param set MPC_Z_VEL_P 0.1 fi set VEHICLE_TYPE mc diff --git a/ROMFS/px4fmu_common/init.d/4011_dji_f450 b/ROMFS/px4fmu_common/init.d/4011_dji_f450 index 299771c1d9..1ff5c796a5 100644 --- a/ROMFS/px4fmu_common/init.d/4011_dji_f450 +++ b/ROMFS/px4fmu_common/init.d/4011_dji_f450 @@ -12,13 +12,11 @@ then # param set MC_ATT_P 7.0 param set MC_ATT_I 0.0 - param set MC_ATT_D 0.0 param set MC_ATTRATE_P 0.12 param set MC_ATTRATE_I 0.0 param set MC_ATTRATE_D 0.004 param set MC_YAWPOS_P 2.0 param set MC_YAWPOS_I 0.0 - param set MC_YAWPOS_D 0.0 param set MC_YAWRATE_P 0.3 param set MC_YAWRATE_I 0.2 param set MC_YAWRATE_D 0.005 diff --git a/ROMFS/px4fmu_common/init.d/4012_hk_x550 b/ROMFS/px4fmu_common/init.d/4012_hk_x550 index 7e020cf592..673043c149 100644 --- a/ROMFS/px4fmu_common/init.d/4012_hk_x550 +++ b/ROMFS/px4fmu_common/init.d/4012_hk_x550 @@ -12,13 +12,11 @@ then # param set MC_ATT_P 5.5 param set MC_ATT_I 0 - param set MC_ATT_D 0 param set MC_ATTRATE_P 0.14 param set MC_ATTRATE_I 0 param set MC_ATTRATE_D 0.006 param set MC_YAWPOS_P 0.6 param set MC_YAWPOS_I 0 - param set MC_YAWPOS_D 0 param set MC_YAWRATE_P 0.08 param set MC_YAWRATE_I 0 param set MC_YAWRATE_D 0 From eb752b57d91a8cc824023e0ae7608ba60ca4b459 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 18 Jan 2014 16:59:39 +0100 Subject: [PATCH 311/447] rc: typo fixed --- ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil b/ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil index 297082b798..e958448912 100644 --- a/ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil +++ b/ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil @@ -5,6 +5,6 @@ # Maintainers: Anton Babushkin # -sh /etc/ini.d/1001_rc_quad_x.hil +sh /etc/init.d/1001_rc_quad_x.hil set MIXER FMU_quad_+ From 42f4f459795476c2e695c6a151bd6ccc349658f0 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 19 Jan 2014 10:44:57 +0100 Subject: [PATCH 312/447] mc_att_control_vector renamed to mc_att_control --- ROMFS/px4fmu_common/init.d/rc.mc_apps | 2 +- makefiles/config_px4fmu-v1_default.mk | 2 +- makefiles/config_px4fmu-v2_default.mk | 2 +- .../mc_att_control_main.cpp} | 17 ++---- .../mc_att_control/mc_att_control_params.c | 53 +++++++++++++++++++ .../module.mk | 8 +-- .../mc_att_control_vector_params.c | 20 ------- 7 files changed, 64 insertions(+), 40 deletions(-) rename src/modules/{mc_att_control_vector/mc_att_control_vector_main.cpp => mc_att_control/mc_att_control_main.cpp} (97%) create mode 100644 src/modules/mc_att_control/mc_att_control_params.c rename src/modules/{mc_att_control_vector => mc_att_control}/module.mk (90%) delete mode 100644 src/modules/mc_att_control_vector/mc_att_control_vector_params.c diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_apps b/ROMFS/px4fmu_common/init.d/rc.mc_apps index 4e0d681475..96fe32c8a7 100644 --- a/ROMFS/px4fmu_common/init.d/rc.mc_apps +++ b/ROMFS/px4fmu_common/init.d/rc.mc_apps @@ -16,7 +16,7 @@ position_estimator_inav start # # Start attitude control # -mc_att_control_vector start +mc_att_control start # # Start position control diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk index e8c0ef981d..51be7e1a12 100644 --- a/makefiles/config_px4fmu-v1_default.mk +++ b/makefiles/config_px4fmu-v1_default.mk @@ -82,7 +82,7 @@ MODULES += modules/position_estimator_inav # MODULES += modules/fw_pos_control_l1 MODULES += modules/fw_att_control -MODULES += modules/mc_att_control_vector +MODULES += modules/mc_att_control MODULES += modules/mc_pos_control #MODULES += examples/flow_position_control #MODULES += examples/flow_speed_control diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index 9dd052a4bb..ab05d4e3da 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -85,7 +85,7 @@ MODULES += examples/flow_position_estimator #MODULES += modules/segway # XXX Needs GCC 4.7 fix MODULES += modules/fw_pos_control_l1 MODULES += modules/fw_att_control -MODULES += modules/mc_att_control_vector +MODULES += modules/mc_att_control MODULES += modules/mc_pos_control # diff --git a/src/modules/mc_att_control_vector/mc_att_control_vector_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp similarity index 97% rename from src/modules/mc_att_control_vector/mc_att_control_vector_main.cpp rename to src/modules/mc_att_control/mc_att_control_main.cpp index 239bd5570d..93974c7424 100644 --- a/src/modules/mc_att_control_vector/mc_att_control_vector_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved. * Author: Tobias Naegeli * Lorenz Meier * Anton Babushkin @@ -35,17 +35,8 @@ ****************************************************************************/ /** - * @file mc_att_control_vector_main.c + * @file mc_att_control_main.c * Implementation of a multicopter attitude controller based on desired rotation matrix. - * - * References - * [1] Daniel Mellinger and Vijay Kumar, "Minimum Snap Trajectory Generation and Control for Quadrotors", - * http://www.seas.upenn.edu/~dmel/mellingerICRA11.pdf - * - * @author Tobias Naegeli - * @author Lorenz Meier - * @author Anton Babushkin - * */ #include @@ -82,7 +73,7 @@ * * @ingroup apps */ -extern "C" __EXPORT int mc_att_control_vector_main(int argc, char *argv[]); +extern "C" __EXPORT int mc_att_control_main(int argc, char *argv[]); #define MIN_TAKEOFF_THROTTLE 0.3f #define YAW_DEADZONE 0.01f @@ -698,7 +689,7 @@ MulticopterAttitudeControl::start() return OK; } -int mc_att_control_vector_main(int argc, char *argv[]) +int mc_att_control_main(int argc, char *argv[]) { if (argc < 1) errx(1, "usage: mc_att_control_vector {start|stop|status}"); diff --git a/src/modules/mc_att_control/mc_att_control_params.c b/src/modules/mc_att_control/mc_att_control_params.c new file mode 100644 index 0000000000..299cef6c84 --- /dev/null +++ b/src/modules/mc_att_control/mc_att_control_params.c @@ -0,0 +1,53 @@ +/**************************************************************************** + * + * Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved. + * Author: @author Tobias Naegeli + * @author Lorenz Meier + * @author Anton Babushkin + * + * 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 mc_att_control_params.c + * Parameters for multicopter attitude controller. + */ + +#include + +PARAM_DEFINE_FLOAT(MC_YAW_P, 2.0f); +PARAM_DEFINE_FLOAT(MC_YAW_I, 0.0f); +PARAM_DEFINE_FLOAT(MC_ATT_P, 6.0f); +PARAM_DEFINE_FLOAT(MC_ATT_I, 0.0f); +PARAM_DEFINE_FLOAT(MC_YAWRATE_P, 0.3f); +PARAM_DEFINE_FLOAT(MC_YAWRATE_D, 0.0f); +PARAM_DEFINE_FLOAT(MC_YAWRATE_I, 0.0f); +PARAM_DEFINE_FLOAT(MC_ATTRATE_P, 0.1f); +PARAM_DEFINE_FLOAT(MC_ATTRATE_D, 0.002f); +PARAM_DEFINE_FLOAT(MC_ATTRATE_I, 0.0f); diff --git a/src/modules/mc_att_control_vector/module.mk b/src/modules/mc_att_control/module.mk similarity index 90% rename from src/modules/mc_att_control_vector/module.mk rename to src/modules/mc_att_control/module.mk index de0675df86..64b876f69b 100644 --- a/src/modules/mc_att_control_vector/module.mk +++ b/src/modules/mc_att_control/module.mk @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. +# 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 @@ -35,7 +35,7 @@ # Multirotor attitude controller (vector based, no Euler singularities) # -MODULE_COMMAND = mc_att_control_vector +MODULE_COMMAND = mc_att_control -SRCS = mc_att_control_vector_main.cpp \ - mc_att_control_vector_params.c +SRCS = mc_att_control_main.cpp \ + mc_att_control_params.c diff --git a/src/modules/mc_att_control_vector/mc_att_control_vector_params.c b/src/modules/mc_att_control_vector/mc_att_control_vector_params.c deleted file mode 100644 index 613d1945b6..0000000000 --- a/src/modules/mc_att_control_vector/mc_att_control_vector_params.c +++ /dev/null @@ -1,20 +0,0 @@ -/* - * mc_att_control_vector_params.c - * - * Created on: 26.12.2013 - * Author: ton - */ - -#include - -/* multicopter attitude controller parameters */ -PARAM_DEFINE_FLOAT(MC_YAWPOS_P, 2.0f); -PARAM_DEFINE_FLOAT(MC_YAWPOS_I, 0.0f); -PARAM_DEFINE_FLOAT(MC_ATT_P, 6.0f); -PARAM_DEFINE_FLOAT(MC_ATT_I, 0.0f); -PARAM_DEFINE_FLOAT(MC_YAWRATE_P, 0.3f); -PARAM_DEFINE_FLOAT(MC_YAWRATE_D, 0.0f); -PARAM_DEFINE_FLOAT(MC_YAWRATE_I, 0.0f); -PARAM_DEFINE_FLOAT(MC_ATTRATE_P, 0.1f); -PARAM_DEFINE_FLOAT(MC_ATTRATE_D, 0.002f); -PARAM_DEFINE_FLOAT(MC_ATTRATE_I, 0.0f); From 23cc0684ba3cc5f2c3c18e7677da57c9b263583f Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 19 Jan 2014 10:45:38 +0100 Subject: [PATCH 313/447] rcS: MAV_TYPE bug fixed, use 8 PWM outputs on HIL fake output --- ROMFS/px4fmu_common/init.d/rcS | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index c565275253..45dec5ba66 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -323,7 +323,7 @@ then if [ $OUTPUT_MODE == hil ] then echo "[init] Use HIL as primary output" - if hil mode_pwm + if hil mode_port2_pwm8 then echo "[init] HIL output started" else @@ -474,15 +474,15 @@ then # Use mixer to detect vehicle type if [ $MIXER == FMU_hex_x -o $MIXER == FMU_hex_+ ] then - param set MAV_TYPE 13 + set MAV_TYPE 13 fi if [ $MIXER == FMU_octo_x -o $MIXER == FMU_octo_+ ] then - param set MAV_TYPE 14 + set MAV_TYPE 14 fi if [ $MIXER == FMU_octo_cox ] then - param set MAV_TYPE 14 + set MAV_TYPE 14 fi fi From bda44a35cc99fe95abb30fb301fee1b8bd5aa1c9 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 19 Jan 2014 11:03:31 +0100 Subject: [PATCH 314/447] remove fw_att_control_vector --- .../ecl_fw_att_control_vector.cpp | 165 ---- .../ecl_fw_att_control_vector.h | 115 --- .../fw_att_control_vector_main.cpp | 744 ------------------ .../fw_att_control_vector_params.c | 67 -- src/modules/fw_att_control_vector/module.mk | 42 - 5 files changed, 1133 deletions(-) delete mode 100644 src/modules/fw_att_control_vector/ecl_fw_att_control_vector.cpp delete mode 100644 src/modules/fw_att_control_vector/ecl_fw_att_control_vector.h delete mode 100644 src/modules/fw_att_control_vector/fw_att_control_vector_main.cpp delete mode 100644 src/modules/fw_att_control_vector/fw_att_control_vector_params.c delete mode 100644 src/modules/fw_att_control_vector/module.mk diff --git a/src/modules/fw_att_control_vector/ecl_fw_att_control_vector.cpp b/src/modules/fw_att_control_vector/ecl_fw_att_control_vector.cpp deleted file mode 100644 index 16e60f3e0b..0000000000 --- a/src/modules/fw_att_control_vector/ecl_fw_att_control_vector.cpp +++ /dev/null @@ -1,165 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2013 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 ecl_fw_att_control_vector.cpp - * - * Fixed wing attitude controller - * - * @author Lorenz Meier - * @author Tobias Naegeli - * - */ - -#include -#include -#include "ecl_fw_att_control_vector.h" - -ECL_FWAttControlVector::ECL_FWAttControlVector() : - _integral_error(0.0f, 0.0f), - _integral_max(1000.0f, 1000.0f), - _rates_demanded(0.0f, 0.0f, 0.0f), - _k_p(1.0f, 1.0f, 1.0f), - _k_d(1.0f, 1.0f, 1.0f), - _k_i(1.0f, 1.0f, 1.0f), - _integral_lock(false), - _p_airspeed_min(12.0f), - _p_airspeed_max(24.0f), - _p_tconst(0.1f), - _p_roll_ffd(1.0f), - _airspeed_enabled(false) - { - - } - -/** - * - * @param F_des_in Desired force vector in body frame (NED). Straight flight is (0 0 -1)', - * banking hard right (1 0 -1)' and pitching down (1 0 -1)'. - */ -void ECL_FWAttControlVector::control(float dt, float airspeed, float airspeed_scaling, const math::Dcm &R_nb, float roll, float pitch, float yaw, const math::Vector &F_des_in, - const math::Vector &angular_rates, - math::Vector &moment_des, float &thrust) -{ - if (!isfinite(airspeed) || !airspeed_enabled()) { - // If airspeed is not available or Inf/NaN, use the center - // of min / max - airspeed = 0.5f * (_p_airspeed_min + _p_airspeed_max); - } - - math::Dcm R_bn(R_nb.transpose()); - math::Matrix R_yaw_bn = math::Dcm(math::EulerAngles(0.0f, 0.0f, yaw)).transpose(); - - // Establish actuator signs and lift compensation - float lift_sign = (R_bn(3, 3) >= 0) ? 1.0f : -1.0f; - - float lift_comp = CONSTANTS_ONE_G / math::max(airspeed , float(_p_airspeed_min) * (R_nb(2,3) * R_nb(2,3)) / (R_nb(3,3) * sqrtf((R_nb(2,3) * R_nb(2,3)) + (R_nb(3,3) * R_nb(3,3))))); - //float lift_comp = fabsf((CONSTANTS_ONE_G / math::max(airspeed , float(_p_airspeed_min)) * tanf(roll) * sinf(roll))) * _p_roll_ffd; - - float cy = cosf(yaw); - float sy = sinf(yaw); - - //math::Matrix RYaw = math::Dcm(cy,-sy,0.0f,sy,cy,0.0f,0.0f,0.0f,1.0f); - math::Vector z_b = math::Vector3(R_bn(0,2), R_bn(1,2), R_bn(2,2)); - - math::Vector3 F_des = R_yaw_bn * F_des_in; - - // desired thrust in body frame - // avoid division by zero - // compensates for thrust loss due to roll/pitch - if (F_des(2) >= 0.1f) { - thrust = F_des(2) / R_bn(2, 2); - } else { - F_des(2) = 0.1f; - thrust= F_des(2) / R_bn(2, 2); - } - - math::Vector3 x_B_des; - math::Vector3 y_B_des; - math::Vector3 z_B_des; - - // desired body z axis - z_B_des = (F_des / F_des.norm()); - - // desired direction in world coordinates (yaw angle) - math::Vector3 x_C(cy, sy, 0.0f); - // desired body y axis - y_B_des = z_B_des.cross(x_C) / (z_B_des.cross(x_C)).norm(); - // desired body x axis - x_B_des = y_B_des.cross(z_B_des); - // desired Rotation Matrix - math::Dcm R_des(x_B_des(0), x_B_des(1), x_B_des(2), - y_B_des(0), y_B_des(1), y_B_des(2), - z_B_des(0), z_B_des(1), z_B_des(2)); - - - // Attitude Controller - // P controller - - // error rotation matrix - // operation executed in quaternion space to allow large differences - // XXX switch between operations based on difference, - // benchmark both options - math::Quaternion e_q = math::Quaternion(R_des) - math::Quaternion(R_bn); - // Renormalize - e_q = e_q / e_q.norm(); - math::Matrix e_R = math::Dcm(e_q); - //small angles: math::Matrix e_R = (R_des.transpose() * R_bn - R_bn.transpose() * R_des) * 0.5f; - - // error rotation vector - math::Vector e_R_v(3); - e_R_v(0) = e_R(1,2); - e_R_v(1) = e_R(0,2); - e_R_v(2) = e_R(0,1); - - - // attitude integral error - math::Vector intError = math::Vector3(0.0f, 0.0f, 0.0f); - if (!_integral_lock) { - - if (fabsf(_integral_error(0)) < _integral_max(0)) { - _integral_error(0) = _integral_error(0) + e_R_v(0) * dt; - } - - if (fabsf(_integral_error(1)) < _integral_max(1)) { - _integral_error(1) = _integral_error(1) + e_R_v(1) * dt; - } - - intError(0) = _integral_error(0); - intError(1) = _integral_error(1); - intError(2) = 0.0f; - } - - _rates_demanded = (e_R_v * _k_p(0) + angular_rates * _k_d(0) + intError * _k_i(0)); - moment_des = _rates_demanded; -} diff --git a/src/modules/fw_att_control_vector/ecl_fw_att_control_vector.h b/src/modules/fw_att_control_vector/ecl_fw_att_control_vector.h deleted file mode 100644 index 40e4662a3f..0000000000 --- a/src/modules/fw_att_control_vector/ecl_fw_att_control_vector.h +++ /dev/null @@ -1,115 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2013 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 ecl_fw_att_control_vector.cpp - * - * Fixed wing attitude controller - * - * @author Lorenz Meier - * @author Tobias Naegeli - * - */ - -#include - -class ECL_FWAttControlVector { - -public: - ECL_FWAttControlVector(); - void control(float dt, float airspeed, float airspeed_scaling, const math::Dcm &R_nb, float roll, float pitch, float yaw, const math::Vector &F_des_in, - const math::Vector &angular_rates, - math::Vector &moment_des, float &thrust); - - void set_imax(float integral_max) { - _integral_max(0) = integral_max; - _integral_max(1) = integral_max; - } - - void set_tconst(float tconst) { - _p_tconst = tconst; - } - - void set_k_p(float roll, float pitch, float yaw) { - _k_p(0) = roll; - _k_p(1) = pitch; - _k_p(2) = yaw; - } - - void set_k_d(float roll, float pitch, float yaw) { - _k_d(0) = roll; - _k_d(1) = pitch; - _k_d(2) = yaw; - } - - void set_k_i(float roll, float pitch, float yaw) { - _k_i(0) = roll; - _k_i(1) = pitch; - _k_i(2) = yaw; - } - - void reset_integral() { - _integral_error(0) = 0.0f; - _integral_error(1) = 0.0f; - } - - void lock_integral(bool lock) { - _integral_lock = lock; - } - - bool airspeed_enabled() { - return _airspeed_enabled; - } - - void enable_airspeed(bool airspeed) { - _airspeed_enabled = airspeed; - } - - math::Vector3 get_rates_des() { - return _rates_demanded; - } - -protected: - math::Vector2f _integral_error; - math::Vector2f _integral_max; - math::Vector3 _rates_demanded; - math::Vector3 _k_p; - math::Vector3 _k_d; - math::Vector3 _k_i; - bool _integral_lock; - float _p_airspeed_min; - float _p_airspeed_max; - float _p_tconst; - float _p_roll_ffd; - bool _airspeed_enabled; -}; diff --git a/src/modules/fw_att_control_vector/fw_att_control_vector_main.cpp b/src/modules/fw_att_control_vector/fw_att_control_vector_main.cpp deleted file mode 100644 index c64f82e544..0000000000 --- a/src/modules/fw_att_control_vector/fw_att_control_vector_main.cpp +++ /dev/null @@ -1,744 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2013 PX4 Development Team. All rights reserved. - * Author: Lorenz Meier - * - * 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 fw_att_control_vector_main.c - * Implementation of a generic attitude controller based on classic orthogonal PIDs. - * - * @author Lorenz Meier - * - * Please refer to the library files for the authors and acknowledgements of - * the used control library functions. - */ - -#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 - -#include "ecl_fw_att_control_vector.h" - -/** - * Fixedwing attitude control app start / stop handling function - * - * @ingroup apps - */ -extern "C" __EXPORT int fw_att_control_vector_main(int argc, char *argv[]); - -class FixedwingAttitudeControlVector -{ -public: - /** - * Constructor - */ - FixedwingAttitudeControlVector(); - - /** - * Destructor, also kills the sensors task. - */ - ~FixedwingAttitudeControlVector(); - - /** - * Start the sensors task. - * - * @return OK on success. - */ - int start(); - -private: - - bool _task_should_exit; /**< if true, sensor task should exit */ - int _control_task; /**< task handle for sensor task */ - - int _att_sub; /**< vehicle attitude subscription */ - int _accel_sub; /**< accelerometer subscription */ - int _att_sp_sub; /**< vehicle attitude setpoint */ - int _attitude_sub; /**< raw rc channels data subscription */ - int _airspeed_sub; /**< airspeed subscription */ - int _vstatus_sub; /**< vehicle status subscription */ - int _params_sub; /**< notification of parameter updates */ - int _manual_control_sub; /**< notification of manual control updates */ - int _arming_sub; /**< arming status of outputs */ - - orb_advert_t _rate_sp_pub; /**< rate setpoint publication */ - orb_advert_t _actuators_0_pub; /**< actuator control group 0 setpoint */ - - struct vehicle_attitude_s _att; /**< vehicle attitude */ - struct accel_report _accel; /**< body frame accelerations */ - struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */ - struct manual_control_setpoint_s _manual; /**< r/c channel data */ - struct airspeed_s _airspeed; /**< airspeed */ - struct vehicle_status_s _vstatus; /**< vehicle status */ - struct actuator_controls_s _actuators; /**< actuator control inputs */ - struct actuator_armed_s _arming; /**< actuator arming status */ - - perf_counter_t _loop_perf; /**< loop performance counter */ - - bool _setpoint_valid; /**< flag if the position control setpoint is valid */ - bool _airspeed_valid; /**< flag if the airspeed measurement is valid */ - - struct { - - float tconst; - float p_p; - float p_d; - float p_i; - float p_rmax_up; - float p_rmax_dn; - float p_imax; - float p_rll; - float r_p; - float r_d; - float r_i; - float r_imax; - float r_rmax; - float y_slip; - float y_int; - float y_damp; - float y_rll; - float y_imax; - - float airspeed_min; - float airspeed_trim; - float airspeed_max; - } _parameters; /**< local copies of interesting parameters */ - - struct { - - param_t tconst; - param_t p_p; - param_t p_d; - param_t p_i; - param_t p_rmax_up; - param_t p_rmax_dn; - param_t p_imax; - param_t p_rll; - param_t r_p; - param_t r_d; - param_t r_i; - param_t r_imax; - param_t r_rmax; - param_t y_slip; - param_t y_int; - param_t y_damp; - param_t y_rll; - param_t y_imax; - - param_t airspeed_min; - param_t airspeed_trim; - param_t airspeed_max; - } _parameter_handles; /**< handles for interesting parameters */ - - - ECL_FWAttControlVector _att_control; - - - /** - * Update our local parameter cache. - */ - int parameters_update(); - - /** - * Update control outputs - * - */ - void control_update(); - - /** - * Check for changes in vehicle status. - */ - void vehicle_status_poll(); - - /** - * Check for airspeed updates. - */ - bool vehicle_airspeed_poll(); - - /** - * Check for accel updates. - */ - void vehicle_accel_poll(); - - /** - * Check for set triplet updates. - */ - void vehicle_setpoint_poll(); - - /** - * Check for arming status updates. - */ - void arming_status_poll(); - - /** - * Shim for calling task_main from task_create. - */ - static void task_main_trampoline(int argc, char *argv[]); - - /** - * Main sensor collection task. - */ - void task_main() __attribute__((noreturn)); -}; - -namespace att_control -{ - -/* oddly, ERROR is not defined for c++ */ -#ifdef ERROR -# undef ERROR -#endif -static const int ERROR = -1; - -FixedwingAttitudeControlVector *g_control; -} - -FixedwingAttitudeControlVector::FixedwingAttitudeControlVector() : - - _task_should_exit(false), - _control_task(-1), - -/* subscriptions */ - _att_sub(-1), - _accel_sub(-1), - _airspeed_sub(-1), - _vstatus_sub(-1), - _params_sub(-1), - _manual_control_sub(-1), - _arming_sub(-1), - -/* publications */ - _rate_sp_pub(-1), - _actuators_0_pub(-1), - -/* performance counters */ - _loop_perf(perf_alloc(PC_ELAPSED, "fw att control")), -/* states */ - _setpoint_valid(false), - _airspeed_valid(false) -{ - // _parameter_handles.roll_p = param_find("FW_ROLL_P"); - // _parameter_handles.pitch_p = param_find("FW_PITCH_P"); - _parameter_handles.tconst = param_find("FW_TCONST"); - _parameter_handles.p_p = param_find("FW_P_P"); - _parameter_handles.p_d = param_find("FW_P_D"); - _parameter_handles.p_i = param_find("FW_P_I"); - _parameter_handles.p_rmax_up = param_find("FW_P_RMAX_UP"); - _parameter_handles.p_rmax_dn = param_find("FW_P_RMAX_DN"); - _parameter_handles.p_imax = param_find("FW_P_IMAX"); - _parameter_handles.p_rll = param_find("FW_P_RLL"); - - _parameter_handles.r_p = param_find("FW_R_P"); - _parameter_handles.r_d = param_find("FW_R_D"); - _parameter_handles.r_i = param_find("FW_R_I"); - _parameter_handles.r_imax = param_find("FW_R_IMAX"); - _parameter_handles.r_rmax = param_find("FW_R_RMAX"); - - _parameter_handles.y_slip = param_find("FW_Y_SLIP"); - _parameter_handles.y_int = param_find("FW_Y_INT"); - _parameter_handles.y_damp = param_find("FW_Y_DAMP"); - _parameter_handles.y_rll = param_find("FW_Y_RLL"); - _parameter_handles.y_imax = param_find("FW_Y_IMAX"); - - _parameter_handles.airspeed_min = param_find("FW_AIRSPD_MIN"); - _parameter_handles.airspeed_trim = param_find("FW_AIRSPD_TRIM"); - _parameter_handles.airspeed_max = param_find("FW_AIRSPD_MAX"); - - /* fetch initial parameter values */ - parameters_update(); -} - -FixedwingAttitudeControlVector::~FixedwingAttitudeControlVector() -{ - if (_control_task != -1) { - - /* task wakes up every 100ms or so at the longest */ - _task_should_exit = true; - - /* wait for a second for the task to quit at our request */ - unsigned i = 0; - - do { - /* wait 20ms */ - usleep(20000); - - /* if we have given up, kill it */ - if (++i > 50) { - task_delete(_control_task); - break; - } - } while (_control_task != -1); - } - - att_control::g_control = nullptr; -} - -int -FixedwingAttitudeControlVector::parameters_update() -{ - - param_get(_parameter_handles.tconst, &(_parameters.tconst)); - param_get(_parameter_handles.p_p, &(_parameters.p_p)); - param_get(_parameter_handles.p_d, &(_parameters.p_d)); - param_get(_parameter_handles.p_i, &(_parameters.p_i)); - param_get(_parameter_handles.p_rmax_up, &(_parameters.p_rmax_up)); - param_get(_parameter_handles.p_rmax_dn, &(_parameters.p_rmax_dn)); - param_get(_parameter_handles.p_imax, &(_parameters.p_imax)); - param_get(_parameter_handles.p_rll, &(_parameters.p_rll)); - - param_get(_parameter_handles.r_p, &(_parameters.r_p)); - param_get(_parameter_handles.r_d, &(_parameters.r_d)); - param_get(_parameter_handles.r_i, &(_parameters.r_i)); - param_get(_parameter_handles.r_imax, &(_parameters.r_imax)); - param_get(_parameter_handles.r_rmax, &(_parameters.r_rmax)); - - param_get(_parameter_handles.y_slip, &(_parameters.y_slip)); - param_get(_parameter_handles.y_int, &(_parameters.y_int)); - param_get(_parameter_handles.y_damp, &(_parameters.y_damp)); - param_get(_parameter_handles.y_rll, &(_parameters.y_rll)); - param_get(_parameter_handles.y_imax, &(_parameters.y_imax)); - - param_get(_parameter_handles.airspeed_min, &(_parameters.airspeed_min)); - param_get(_parameter_handles.airspeed_trim, &(_parameters.airspeed_trim)); - param_get(_parameter_handles.airspeed_max, &(_parameters.airspeed_max)); - - /* pitch control parameters */ - _att_control.set_tconst(_parameters.tconst); - _att_control.set_k_p(math::radians(_parameters.r_p), - math::radians(_parameters.p_p), 0.0f); - _att_control.set_k_d(math::radians(_parameters.r_p), - math::radians(_parameters.p_p), 0.0f); - _att_control.set_k_i(math::radians(_parameters.r_i), - math::radians(_parameters.p_i), - math::radians(_parameters.y_int)); - - return OK; -} - -void -FixedwingAttitudeControlVector::vehicle_status_poll() -{ - bool vstatus_updated; - - /* Check HIL state if vehicle status has changed */ - orb_check(_vstatus_sub, &vstatus_updated); - - if (vstatus_updated) { - - orb_copy(ORB_ID(vehicle_status), _vstatus_sub, &_vstatus); - } -} - -bool -FixedwingAttitudeControlVector::vehicle_airspeed_poll() -{ - /* check if there is a new position */ - bool airspeed_updated; - orb_check(_airspeed_sub, &airspeed_updated); - - if (airspeed_updated) { - orb_copy(ORB_ID(airspeed), _airspeed_sub, &_airspeed); - return true; - } - - return false; -} - -void -FixedwingAttitudeControlVector::vehicle_accel_poll() -{ - /* check if there is a new position */ - bool accel_updated; - orb_check(_accel_sub, &accel_updated); - - if (accel_updated) { - orb_copy(ORB_ID(sensor_accel), _accel_sub, &_accel); - } -} - -void -FixedwingAttitudeControlVector::vehicle_setpoint_poll() -{ - /* check if there is a new setpoint */ - bool att_sp_updated; - orb_check(_att_sp_sub, &att_sp_updated); - - if (att_sp_updated) { - orb_copy(ORB_ID(vehicle_attitude_setpoint), _att_sp_sub, &_att_sp); - _setpoint_valid = true; - } -} - -void -FixedwingAttitudeControlVector::arming_status_poll() -{ - /* check if there is a new setpoint */ - bool arming_updated; - orb_check(_arming_sub, &arming_updated); - - if (arming_updated) { - orb_copy(ORB_ID(actuator_armed), _arming_sub, &_arming); - } -} - -void -FixedwingAttitudeControlVector::task_main_trampoline(int argc, char *argv[]) -{ - att_control::g_control->task_main(); -} - -void -FixedwingAttitudeControlVector::task_main() -{ - - /* inform about start */ - warnx("Initializing.."); - fflush(stdout); - - /* - * do subscriptions - */ - _att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); - _att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); - _accel_sub = orb_subscribe(ORB_ID(sensor_accel)); - _airspeed_sub = orb_subscribe(ORB_ID(airspeed)); - _vstatus_sub = orb_subscribe(ORB_ID(vehicle_status)); - _params_sub = orb_subscribe(ORB_ID(parameter_update)); - _manual_control_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); - _arming_sub = orb_subscribe(ORB_ID(actuator_armed)); - - /* rate limit vehicle status updates to 5Hz */ - orb_set_interval(_vstatus_sub, 200); - orb_set_interval(_att_sub, 100); - - parameters_update(); - - /* initialize values of critical structs until first regular update */ - _arming.armed = false; - - /* get an initial update for all sensor and status data */ - (void)vehicle_airspeed_poll(); - vehicle_setpoint_poll(); - vehicle_accel_poll(); - vehicle_status_poll(); - arming_status_poll(); - - /* wakeup source(s) */ - struct pollfd fds[2]; - - /* Setup of loop */ - fds[0].fd = _params_sub; - fds[0].events = POLLIN; - fds[1].fd = _att_sub; - fds[1].events = POLLIN; - - while (!_task_should_exit) { - - /* wait for up to 500ms for data */ - int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100); - - /* timed out - periodic check for _task_should_exit, etc. */ - if (pret == 0) - continue; - - /* this is undesirable but not much we can do - might want to flag unhappy status */ - if (pret < 0) { - warn("poll error %d, %d", pret, errno); - continue; - } - - perf_begin(_loop_perf); - - /* only update parameters if they changed */ - if (fds[0].revents & POLLIN) { - /* read from param to clear updated flag */ - struct parameter_update_s update; - orb_copy(ORB_ID(parameter_update), _params_sub, &update); - - /* update parameters from storage */ - parameters_update(); - } - - /* only run controller if attitude changed */ - if (fds[1].revents & POLLIN) { - - - static uint64_t last_run = 0; - float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f; - last_run = hrt_absolute_time(); - - /* guard against too large deltaT's */ - if (deltaT > 1.0f) - deltaT = 0.01f; - - /* load local copies */ - orb_copy(ORB_ID(vehicle_attitude), _att_sub, &_att); - - _airspeed_valid = vehicle_airspeed_poll(); - - vehicle_setpoint_poll(); - - vehicle_accel_poll(); - - /* check vehicle status for changes to publication state */ - vehicle_status_poll(); - - /* check for arming status changes */ - arming_status_poll(); - - /* lock integrator until armed */ - bool lock_integrator; - if (_arming.armed) { - lock_integrator = false; - } else { - lock_integrator = true; - } - - /* decide if in stabilized or full manual control */ - - if (_vstatus.state_machine == SYSTEM_STATE_MANUAL && !(_vstatus.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS) - ) { - - _actuators.control[0] = _manual.roll; - _actuators.control[1] = _manual.pitch; - _actuators.control[2] = _manual.yaw; - _actuators.control[3] = _manual.throttle; - _actuators.control[4] = _manual.flaps; - - } else if (_vstatus.state_machine == SYSTEM_STATE_AUTO || - (_vstatus.state_machine == SYSTEM_STATE_STABILIZED) || - (_vstatus.state_machine == SYSTEM_STATE_MANUAL && _vstatus.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS)) { - - /* scale from radians to normalized -1 .. 1 range */ - const float actuator_scaling = 1.0f / (M_PI_F / 4.0f); - - /* scale around tuning airspeed */ - - float airspeed; - - /* if airspeed is smaller than min, the sensor is not giving good readings */ - if (!_airspeed_valid || - (_airspeed.indicated_airspeed_m_s < _parameters.airspeed_min) || - !isfinite(_airspeed.indicated_airspeed_m_s)) { - airspeed = _parameters.airspeed_min + (_parameters.airspeed_max - _parameters.airspeed_min) / 2.0f; - } else { - airspeed = _airspeed.indicated_airspeed_m_s; - } - - float airspeed_scaling = _parameters.airspeed_trim / airspeed; - - float roll_sp, pitch_sp; - float throttle_sp = 0.0f; - - if (_vstatus.state_machine == SYSTEM_STATE_AUTO) { - roll_sp = _att_sp.roll_body; - pitch_sp = _att_sp.pitch_body; - throttle_sp = _att_sp.thrust; - } else if ((_vstatus.state_machine == SYSTEM_STATE_STABILIZED) || - (_vstatus.state_machine == SYSTEM_STATE_MANUAL && _vstatus.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS)) { - - /* - * Scale down roll and pitch as the setpoints are radians - * and a typical remote can only do 45 degrees, the mapping is - * -1..+1 to -45..+45 degrees or -0.75..+0.75 radians. - * - * With this mapping the stick angle is a 1:1 representation of - * the commanded attitude. If more than 45 degrees are desired, - * a scaling parameter can be applied to the remote. - */ - roll_sp = _manual.roll * 0.75f; - pitch_sp = _manual.pitch * 0.75f; - throttle_sp = _manual.throttle; - _actuators.control[4] = _manual.flaps; - } - - math::Dcm R_nb(_att.R); - - // Transform body frame forces to - // global frame - const math::Vector3 F_des(pitch_sp, roll_sp, throttle_sp); - const math::Vector3 angular_rates(_att.rollspeed, _att.pitchspeed, _att.yawspeed); - - // Return variables - math::Vector3 moments_des; - float thrust; - - _att_control.control(deltaT, airspeed_scaling, airspeed, R_nb, _att.roll, _att.pitch, _att.yaw, - F_des, angular_rates, moments_des, thrust); - - _actuators.control[0] = (isfinite(moments_des(0))) ? moments_des(0) * actuator_scaling : 0.0f; - _actuators.control[1] = (isfinite(moments_des(1))) ? moments_des(1) * actuator_scaling : 0.0f; - _actuators.control[2] = 0.0f;//(isfinite(moments_des(0))) ? moments_des(0) * actuator_scaling : 0.0f; - - /* throttle passed through */ - _actuators.control[3] = (isfinite(thrust)) ? thrust : 0.0f; - - // warnx("aspd: %s: %6.2f, aspd scaling: %6.2f, controls: %5.2f %5.2f %5.2f %5.2f", (_airspeed_valid) ? "valid" : "unknown", - // airspeed, airspeed_scaling, _actuators.control[0], _actuators.control[1], - // _actuators.control[2], _actuators.control[3]); - - /* - * Lazily publish the rate setpoint (for analysis, the actuators are published below) - * only once available - */ - vehicle_rates_setpoint_s rates_sp; - math::Vector3 rates_des = _att_control.get_rates_des(); - rates_sp.roll = rates_des(0); - rates_sp.pitch = rates_des(1); - rates_sp.yaw = 0.0f; // XXX rates_des(2); - - rates_sp.timestamp = hrt_absolute_time(); - - if (_rate_sp_pub > 0) { - /* publish the attitude setpoint */ - orb_publish(ORB_ID(vehicle_rates_setpoint), _actuators_0_pub, &rates_sp); - - } else { - /* advertise and publish */ - _rate_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &rates_sp); - } - - /* lazily publish the setpoint only once available */ - _actuators.timestamp = hrt_absolute_time(); - - if (_actuators_0_pub > 0) { - /* publish the attitude setpoint */ - orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators); - - } else { - /* advertise and publish */ - _actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators); - } - - } - - } - - perf_end(_loop_perf); - } - - warnx("exiting.\n"); - - _control_task = -1; - _exit(0); -} - -int -FixedwingAttitudeControlVector::start() -{ - ASSERT(_control_task == -1); - - /* start the task */ - _control_task = task_spawn_cmd("fw_att_control", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 5, - 2048, - (main_t)&FixedwingAttitudeControlVector::task_main_trampoline, - nullptr); - - if (_control_task < 0) { - warn("task start failed"); - return -errno; - } - - return OK; -} - -int fw_att_control_vector_main(int argc, char *argv[]) -{ - if (argc < 1) - errx(1, "usage: fw_att_control {start|stop|status}"); - - if (!strcmp(argv[1], "start")) { - - if (att_control::g_control != nullptr) - errx(1, "already running"); - - att_control::g_control = new FixedwingAttitudeControlVector; - - if (att_control::g_control == nullptr) - errx(1, "alloc failed"); - - if (OK != att_control::g_control->start()) { - delete att_control::g_control; - att_control::g_control = nullptr; - err(1, "start failed"); - } - - exit(0); - } - - if (!strcmp(argv[1], "stop")) { - if (att_control::g_control == nullptr) - errx(1, "not running"); - - delete att_control::g_control; - att_control::g_control = nullptr; - exit(0); - } - - if (!strcmp(argv[1], "status")) { - if (att_control::g_control) { - errx(0, "running"); - - } else { - errx(1, "not running"); - } - } - - warnx("unrecognized command"); - return 1; -} diff --git a/src/modules/fw_att_control_vector/fw_att_control_vector_params.c b/src/modules/fw_att_control_vector/fw_att_control_vector_params.c deleted file mode 100644 index 7e434c164b..0000000000 --- a/src/modules/fw_att_control_vector/fw_att_control_vector_params.c +++ /dev/null @@ -1,67 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2013 PX4 Development Team. All rights reserved. - * Author: Lorenz Meier - * - * 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 fw_pos_control_l1_params.c - * - * Parameters defined by the L1 position control task - * - * @author Lorenz Meier - */ - -#include - -#include - -/* - * Controller parameters, accessible via MAVLink - * - */ - -PARAM_DEFINE_FLOAT(FW_TCONST, 0.5f); -PARAM_DEFINE_FLOAT(FW_P_P, 40.0f); -PARAM_DEFINE_FLOAT(FW_P_D, 0.0f); -PARAM_DEFINE_FLOAT(FW_P_I, 0.0f); -PARAM_DEFINE_FLOAT(FW_P_RMAX_UP, 0.0f); -PARAM_DEFINE_FLOAT(FW_P_RMAX_DN, 0.0f); -PARAM_DEFINE_FLOAT(FW_P_IMAX, 15.0f); -PARAM_DEFINE_FLOAT(FW_P_RLL, 1.0f); -PARAM_DEFINE_FLOAT(FW_R_P, 40.0f); -PARAM_DEFINE_FLOAT(FW_R_D, 0.0f); -PARAM_DEFINE_FLOAT(FW_R_I, 0.0f); -PARAM_DEFINE_FLOAT(FW_R_IMAX, 15.0f); -PARAM_DEFINE_FLOAT(FW_R_RMAX, 60); -PARAM_DEFINE_FLOAT(FW_AIRSPD_MIN, 9.0f); -PARAM_DEFINE_FLOAT(FW_AIRSPD_TRIM, 12.0f); -PARAM_DEFINE_FLOAT(FW_AIRSPD_MAX, 18.0f); diff --git a/src/modules/fw_att_control_vector/module.mk b/src/modules/fw_att_control_vector/module.mk deleted file mode 100644 index e78a715954..0000000000 --- a/src/modules/fw_att_control_vector/module.mk +++ /dev/null @@ -1,42 +0,0 @@ -############################################################################ -# -# Copyright (c) 2012, 2013 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. -# -############################################################################ - -# -# Multirotor attitude controller (vector based, no Euler singularities) -# - -MODULE_COMMAND = fw_att_control_vector - -SRCS = fw_att_control_vector_main.cpp \ - ecl_fw_att_control_vector.cpp \ - fw_att_control_vector_params.c From 18b28f0efd0fa308644eb69bc9c90f7378435542 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 19 Jan 2014 13:11:15 +0100 Subject: [PATCH 315/447] Copyright and comments fixes --- .../mc_att_control/mc_att_control_main.cpp | 17 ++++++-- .../mc_pos_control/mc_pos_control_main.cpp | 8 +++- .../mc_pos_control/mc_pos_control_params.c | 41 ++++++++++++++++--- src/modules/mc_pos_control/module.mk | 2 +- src/modules/navigator/navigator_main.cpp | 8 +++- src/modules/navigator/navigator_params.c | 4 +- 6 files changed, 66 insertions(+), 14 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 93974c7424..646dc84e63 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -1,9 +1,9 @@ /**************************************************************************** * * Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved. - * Author: Tobias Naegeli - * Lorenz Meier - * Anton Babushkin + * Author: @author Tobias Naegeli + * @author Lorenz Meier + * @author Anton Babushkin * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -36,7 +36,16 @@ /** * @file mc_att_control_main.c - * Implementation of a multicopter attitude controller based on desired rotation matrix. + * Multicopter attitude controller. + * + * The controller has two loops: P loop for angular error and PD loop for angular rate error. + * Desired rotation calculated keeping in mind that yaw response is normally slower than roll/pitch. + * For small deviations controller rotates copter to have shortest path of thrust vector and independently rotates around yaw, + * so actual rotation axis is not constant. For large deviations controller rotates copter around fixed axis. + * These two approaches fused seamlessly with weight depending on angular error. + * When thrust vector directed near-horizontally (e.g. roll ~= PI/2) yaw setpoint ignored because of singularity. + * Controller doesn't use Euler angles for work, they generated only for more human-friendly control and logging. + * If rotation matrix setpoint is invalid it will be generated from Euler angles for compatibility with old position controllers. */ #include 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 28c8861b6d..eb4d1f75e8 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -1,7 +1,7 @@ /**************************************************************************** * * Copyright (c) 2013 PX4 Development Team. All rights reserved. - * Author: Anton Babushkin + * Author: @author Anton Babushkin * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -34,8 +34,12 @@ /** * @file mc_pos_control_main.cpp + * Multicopter position controller. * - * Multirotor position controller. + * The controller has two loops: P loop for position error and PID loop for velocity error. + * Output of velocity controller is thrust vector that splitted to thrust direction + * (i.e. rotation matrix for multicopter orientation) and thrust module (i.e. multicopter thrust itself). + * Controller doesn't use Euler angles for work, they generated only for more human-friendly control and logging. */ #include 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 67aa248721..a00bc3644f 100644 --- a/src/modules/mc_pos_control/mc_pos_control_params.c +++ b/src/modules/mc_pos_control/mc_pos_control_params.c @@ -1,13 +1,44 @@ -/* - * mc_pos_control_params.c +/**************************************************************************** * - * Created on: 26.12.2013 - * Author: ton + * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * Author: @author Anton Babushkin + * + * 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 mc_pos_control_params.c + * Multicopter position controller parameters. */ #include -/* multicopter position controller parameters */ PARAM_DEFINE_FLOAT(MPC_THR_MIN, 0.0f); PARAM_DEFINE_FLOAT(MPC_THR_MAX, 1.0f); PARAM_DEFINE_FLOAT(MPC_Z_P, 1.0f); diff --git a/src/modules/mc_pos_control/module.mk b/src/modules/mc_pos_control/module.mk index 1d98173fa3..0b566d7bd0 100644 --- a/src/modules/mc_pos_control/module.mk +++ b/src/modules/mc_pos_control/module.mk @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. +# Copyright (c) 2013 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 diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index af2b831dc7..d77d8c6aa6 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -1,9 +1,10 @@ /**************************************************************************** * - * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved. * Author: @author Lorenz Meier * @author Jean Cyr * @author Julian Oes + * @author Anton Babushkin * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -39,6 +40,11 @@ * * Handles missions, geo fencing and failsafe navigation behavior. * Published the mission item triplet for the position controller. + * + * @author Lorenz Meier + * @author Jean Cyr + * @author Julian Oes + * @author Anton Babushkin */ #include diff --git a/src/modules/navigator/navigator_params.c b/src/modules/navigator/navigator_params.c index 617b2ec310..ff819fca40 100644 --- a/src/modules/navigator/navigator_params.c +++ b/src/modules/navigator/navigator_params.c @@ -1,8 +1,9 @@ /**************************************************************************** * * Copyright (c) 2013 PX4 Development Team. All rights reserved. - * Author: @autho Lorenz Meier + * Author: @author Lorenz Meier * @author Julian Oes + * @author Anton Babushkin * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -40,6 +41,7 @@ * * @author Lorenz Meier * @author Julian Oes + * @author Anton Babushkin */ #include From 1f62cede684c50c8af0f3680a670a0e2339fe1ce Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 19 Jan 2014 16:10:57 +0100 Subject: [PATCH 316/447] navigator: for FW: on landing, disable switch to NAV_STATE_READY when landing --- src/modules/navigator/navigator_main.cpp | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index d77d8c6aa6..ca5735509a 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -1259,7 +1259,13 @@ Navigator::check_mission_item_reached() } if (_mission_item_triplet.current.nav_cmd == NAV_CMD_LAND) { - return _vstatus.condition_landed; + if (_vstatus.is_rotary_wing) { + return _vstatus.condition_landed; + } else { + /* For fw there is currently no landing detector: + * make sure control is not stopped when overshooting the landing waypoint */ + return false; + } } /* XXX TODO count turns */ From 8f0cc47372a5f7f00d7940d716a1153230e5f5fc Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 19 Jan 2014 23:26:54 +0100 Subject: [PATCH 317/447] mc_att_control: task name fixed --- src/modules/mc_att_control/mc_att_control_main.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 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 646dc84e63..e8f89923d9 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -683,7 +683,7 @@ MulticopterAttitudeControl::start() ASSERT(_control_task == -1); /* start the task */ - _control_task = task_spawn_cmd("mc_att_control_vector", + _control_task = task_spawn_cmd("mc_att_control", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 2048, @@ -701,7 +701,7 @@ MulticopterAttitudeControl::start() int mc_att_control_main(int argc, char *argv[]) { if (argc < 1) - errx(1, "usage: mc_att_control_vector {start|stop|status}"); + errx(1, "usage: mc_att_control {start|stop|status}"); if (!strcmp(argv[1], "start")) { From d811b0f0dab95949cc03428379555f0f4d1806e2 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 20 Jan 2014 10:17:16 +0100 Subject: [PATCH 318/447] mc_att_control: ATTRATE_I / YAWRATE_I implemented --- .../mc_att_control/mc_att_control_main.cpp | 81 ++++++++++++------- .../mc_att_control/mc_att_control_params.c | 12 +-- 2 files changed, 57 insertions(+), 36 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 e8f89923d9..20c7f88599 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -86,6 +86,7 @@ extern "C" __EXPORT int mc_att_control_main(int argc, char *argv[]); #define MIN_TAKEOFF_THROTTLE 0.3f #define YAW_DEADZONE 0.01f +#define RATES_I_LIMIT 0.5f class MulticopterAttitudeControl { @@ -133,18 +134,21 @@ private: perf_counter_t _loop_perf; /**< loop performance counter */ - math::Matrix<3, 3> _K; /**< diagonal gain matrix for position error */ - math::Matrix<3, 3> _K_rate_p; /**< diagonal gain matrix for angular rate error */ - math::Matrix<3, 3> _K_rate_d; /**< diagonal gain matrix for angular rate derivative */ + math::Vector<3> _K_p; /**< P gain for position error */ + math::Vector<3> _K_rate_p; /**< P gain for angular rate error */ + math::Vector<3> _K_rate_i; /**< I gain for angular rate error */ + math::Vector<3> _K_rate_d; /**< D gain for angular rate error */ math::Vector<3> _rates_prev; /**< angular rates on previous step */ struct { param_t att_p; - param_t att_rate_p; - param_t att_rate_d; param_t yaw_p; + param_t att_rate_p; + param_t att_rate_i; + param_t att_rate_d; param_t yaw_rate_p; + param_t yaw_rate_i; param_t yaw_rate_d; } _parameter_handles; /**< handles for interesting parameters */ @@ -229,17 +233,19 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : memset(&_control_mode, 0, sizeof(_control_mode)); memset(&_arming, 0, sizeof(_arming)); - _K.zero(); + _K_p.zero(); _K_rate_p.zero(); _K_rate_d.zero(); _rates_prev.zero(); _parameter_handles.att_p = param_find("MC_ATT_P"); + _parameter_handles.yaw_p = param_find("MC_YAW_P"); _parameter_handles.att_rate_p = param_find("MC_ATTRATE_P"); + _parameter_handles.att_rate_i = param_find("MC_ATTRATE_I"); _parameter_handles.att_rate_d = param_find("MC_ATTRATE_D"); - _parameter_handles.yaw_p = param_find("MC_YAWPOS_P"); _parameter_handles.yaw_rate_p = param_find("MC_YAWRATE_P"); + _parameter_handles.yaw_rate_i = param_find("MC_YAWRATE_I"); _parameter_handles.yaw_rate_d = param_find("MC_YAWRATE_D"); /* fetch initial parameter values */ @@ -273,31 +279,31 @@ MulticopterAttitudeControl::~MulticopterAttitudeControl() int MulticopterAttitudeControl::parameters_update() { - float att_p; - float att_rate_p; - float att_rate_d; - float yaw_p; - float yaw_rate_p; - float yaw_rate_d; + float v; - param_get(_parameter_handles.att_p, &att_p); - param_get(_parameter_handles.att_rate_p, &att_rate_p); - param_get(_parameter_handles.att_rate_d, &att_rate_d); - param_get(_parameter_handles.yaw_p, &yaw_p); - param_get(_parameter_handles.yaw_rate_p, &yaw_rate_p); - param_get(_parameter_handles.yaw_rate_d, &yaw_rate_d); + param_get(_parameter_handles.att_p, &v); + _K_p(0) = v; + _K_p(1) = v; + param_get(_parameter_handles.yaw_p, &v); + _K_p(2) = v; - _K(0, 0) = att_p; - _K(1, 1) = att_p; - _K(2, 2) = yaw_p; + param_get(_parameter_handles.att_rate_p, &v); + _K_rate_p(0) = v; + _K_rate_p(1) = v; + param_get(_parameter_handles.yaw_rate_p, &v); + _K_rate_p(2) = v; - _K_rate_p(0, 0) = att_rate_p; - _K_rate_p(1, 1) = att_rate_p; - _K_rate_p(2, 2) = yaw_rate_p; + param_get(_parameter_handles.att_rate_i, &v); + _K_rate_i(0) = v; + _K_rate_i(1) = v; + param_get(_parameter_handles.yaw_rate_i, &v); + _K_rate_i(2) = v; - _K_rate_d(0, 0) = att_rate_d; - _K_rate_d(1, 1) = att_rate_d; - _K_rate_d(2, 2) = yaw_rate_d; + param_get(_parameter_handles.att_rate_d, &v); + _K_rate_d(0) = v; + _K_rate_d(1) = v; + param_get(_parameter_handles.yaw_rate_d, &v); + _K_rate_d(2) = v; return OK; } @@ -403,6 +409,10 @@ MulticopterAttitudeControl::task_main() math::Vector<3> rates; rates.zero(); + /* angular rates integral error */ + math::Vector<3> rates_int; + rates_int.zero(); + /* identity matrix */ math::Matrix<3, 3> I; I.identity(); @@ -621,13 +631,24 @@ MulticopterAttitudeControl::task_main() } /* angular rates setpoint*/ - math::Vector<3> rates_sp = _K * e_R; + math::Vector<3> rates_sp = _K_p.emult(e_R); /* feed forward yaw setpoint rate */ rates_sp(2) += yaw_sp_move_rate * yaw_w; - math::Vector<3> control = _K_rate_p * (rates_sp - rates) + _K_rate_d * (_rates_prev - rates) / fmaxf(dt, 0.003f); + math::Vector<3> rates_err = rates_sp - rates; + math::Vector<3> control = _K_rate_p.emult(rates_err) + _K_rate_d.emult(_rates_prev - rates) / fmaxf(dt, 0.003f) + rates_int; _rates_prev = rates; + /* update integral */ + for (int i = 0; i < 3; i++) { + float rate_i = rates_int(i) + _K_rate_i(i) * rates_err(i) * dt; + if (isfinite(rate_i)) { + if (rate_i > -RATES_I_LIMIT && rate_i < RATES_I_LIMIT && control(i) > -RATES_I_LIMIT && control(i) < RATES_I_LIMIT) { + rates_int(i) = rate_i; + } + } + } + /* publish the attitude rates setpoint */ _rates_sp.roll = rates_sp(0); _rates_sp.pitch = rates_sp(1); 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 299cef6c84..a170365eec 100644 --- a/src/modules/mc_att_control/mc_att_control_params.c +++ b/src/modules/mc_att_control/mc_att_control_params.c @@ -41,13 +41,13 @@ #include -PARAM_DEFINE_FLOAT(MC_YAW_P, 2.0f); -PARAM_DEFINE_FLOAT(MC_YAW_I, 0.0f); PARAM_DEFINE_FLOAT(MC_ATT_P, 6.0f); PARAM_DEFINE_FLOAT(MC_ATT_I, 0.0f); -PARAM_DEFINE_FLOAT(MC_YAWRATE_P, 0.3f); -PARAM_DEFINE_FLOAT(MC_YAWRATE_D, 0.0f); -PARAM_DEFINE_FLOAT(MC_YAWRATE_I, 0.0f); +PARAM_DEFINE_FLOAT(MC_YAW_P, 2.0f); +PARAM_DEFINE_FLOAT(MC_YAW_I, 0.0f); PARAM_DEFINE_FLOAT(MC_ATTRATE_P, 0.1f); -PARAM_DEFINE_FLOAT(MC_ATTRATE_D, 0.002f); PARAM_DEFINE_FLOAT(MC_ATTRATE_I, 0.0f); +PARAM_DEFINE_FLOAT(MC_ATTRATE_D, 0.002f); +PARAM_DEFINE_FLOAT(MC_YAWRATE_P, 0.3f); +PARAM_DEFINE_FLOAT(MC_YAWRATE_I, 0.0f); +PARAM_DEFINE_FLOAT(MC_YAWRATE_D, 0.0f); From f4edb448dd77088d12a1d37468014e1f21a97ad7 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 20 Jan 2014 10:26:14 +0100 Subject: [PATCH 319/447] mc_att_control: code style fixed --- src/modules/mc_att_control/mc_att_control_main.cpp | 2 ++ 1 file changed, 2 insertions(+) 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 20c7f88599..44a3b27554 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -642,6 +642,7 @@ MulticopterAttitudeControl::task_main() /* update integral */ for (int i = 0; i < 3; i++) { float rate_i = rates_int(i) + _K_rate_i(i) * rates_err(i) * dt; + if (isfinite(rate_i)) { if (rate_i > -RATES_I_LIMIT && rate_i < RATES_I_LIMIT && control(i) > -RATES_I_LIMIT && control(i) < RATES_I_LIMIT) { rates_int(i) = rate_i; @@ -670,6 +671,7 @@ MulticopterAttitudeControl::task_main() _actuators.control[2] = (isfinite(control(2))) ? control(2) : 0.0f; _actuators.control[3] = (isfinite(_rates_sp.thrust)) ? _rates_sp.thrust : 0.0f; _actuators.timestamp = hrt_absolute_time(); + } else { /* controller disabled, publish zero attitude controls */ _actuators.control[0] = 0.0f; From 9c6cc7a36b3391088510d6301ed6472d487710b2 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 20 Jan 2014 10:26:43 +0100 Subject: [PATCH 320/447] mc_pos_control: AWU fixed --- .../mc_pos_control/mc_pos_control_main.cpp | 18 ++++++++++-------- 1 file changed, 10 insertions(+), 8 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 eb4d1f75e8..c77d25b861 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -828,16 +828,18 @@ MulticopterPositionControl::task_main() } /* update integrals */ - math::Vector<3> m; - m(0) = (_control_mode.flag_control_velocity_enabled && !saturation_xy) ? 1.0f : 0.0f; - m(1) = m(0); - m(2) = (_control_mode.flag_control_climb_rate_enabled && !saturation_z) ? 1.0f : 0.0f; + if (_control_mode.flag_control_velocity_enabled && !saturation_xy) { + thrust_int(0) += vel_err(0) * _params.vel_i(0) * dt; + thrust_int(1) += vel_err(1) * _params.vel_i(1) * dt; + } - thrust_int += vel_err.emult(_params.vel_i) * dt; + if (_control_mode.flag_control_climb_rate_enabled && !saturation_z) { + thrust_int(2) += vel_err(2) * _params.vel_i(2) * dt; - /* protection against flipping on ground when landing */ - if (thrust_int(2) > 0.0f) - thrust_int(2) = 0.0f; + /* protection against flipping on ground when landing */ + if (thrust_int(2) > 0.0f) + thrust_int(2) = 0.0f; + } /* calculate attitude and thrust from thrust vector */ if (_control_mode.flag_control_velocity_enabled) { From 00a799ddadba8656f1b8af617be8db67f2d7209b Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 20 Jan 2014 11:10:50 +0100 Subject: [PATCH 321/447] autostart: default MC_ parameters fixed --- ROMFS/px4fmu_common/init.d/10015_tbs_discovery | 4 ++-- ROMFS/px4fmu_common/init.d/10016_3dr_iris | 4 ++-- ROMFS/px4fmu_common/init.d/1001_rc_quad_x.hil | 8 ++++---- ROMFS/px4fmu_common/init.d/4010_dji_f330 | 4 ++-- ROMFS/px4fmu_common/init.d/4011_dji_f450 | 4 ++-- ROMFS/px4fmu_common/init.d/4012_hk_x550 | 4 ++-- 6 files changed, 14 insertions(+), 14 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/10015_tbs_discovery b/ROMFS/px4fmu_common/init.d/10015_tbs_discovery index 5740e863ca..56c74a3b52 100644 --- a/ROMFS/px4fmu_common/init.d/10015_tbs_discovery +++ b/ROMFS/px4fmu_common/init.d/10015_tbs_discovery @@ -12,11 +12,11 @@ then # param set MC_ATT_P 5.0 param set MC_ATT_I 0.0 + param set MC_YAW_P 0.5 + param set MC_YAW_I 0.15 param set MC_ATTRATE_P 0.17 param set MC_ATTRATE_I 0.0 param set MC_ATTRATE_D 0.006 - param set MC_YAWPOS_P 0.5 - param set MC_YAWPOS_I 0.15 param set MC_YAWRATE_P 0.2 param set MC_YAWRATE_I 0.0 param set MC_YAWRATE_D 0.0 diff --git a/ROMFS/px4fmu_common/init.d/10016_3dr_iris b/ROMFS/px4fmu_common/init.d/10016_3dr_iris index 62a7942999..a3bcb63eb8 100644 --- a/ROMFS/px4fmu_common/init.d/10016_3dr_iris +++ b/ROMFS/px4fmu_common/init.d/10016_3dr_iris @@ -12,11 +12,11 @@ then # param set MC_ATT_P 9.0 param set MC_ATT_I 0.0 + param set MC_YAW_P 0.5 + param set MC_YAW_I 0.15 param set MC_ATTRATE_P 0.13 param set MC_ATTRATE_I 0.0 param set MC_ATTRATE_D 0.004 - param set MC_YAWPOS_P 0.5 - param set MC_YAWPOS_I 0.15 param set MC_YAWRATE_P 0.2 param set MC_YAWRATE_I 0.0 param set MC_YAWRATE_D 0.0 diff --git a/ROMFS/px4fmu_common/init.d/1001_rc_quad_x.hil b/ROMFS/px4fmu_common/init.d/1001_rc_quad_x.hil index 9e098093e6..2d497374a5 100644 --- a/ROMFS/px4fmu_common/init.d/1001_rc_quad_x.hil +++ b/ROMFS/px4fmu_common/init.d/1001_rc_quad_x.hil @@ -10,13 +10,13 @@ then # # Default parameters for this platform # + param set MC_ATT_P 7.0 + param set MC_ATT_I 0.0 + param set MC_YAW_P 2.0 + param set MC_YAW_I 0.0 param set MC_ATTRATE_P 0.12 param set MC_ATTRATE_I 0.0 param set MC_ATTRATE_D 0.004 - param set MC_ATT_P 7.0 - param set MC_ATT_I 0.0 - param set MC_YAWPOS_P 2.0 - param set MC_YAWPOS_I 0.0 param set MC_YAWRATE_P 0.3 param set MC_YAWRATE_I 0.2 param set MC_YAWRATE_D 0.005 diff --git a/ROMFS/px4fmu_common/init.d/4010_dji_f330 b/ROMFS/px4fmu_common/init.d/4010_dji_f330 index 5ce86f593f..e0cf92d97f 100644 --- a/ROMFS/px4fmu_common/init.d/4010_dji_f330 +++ b/ROMFS/px4fmu_common/init.d/4010_dji_f330 @@ -12,11 +12,11 @@ then # param set MC_ATT_P 7.0 param set MC_ATT_I 0.0 + param set MC_YAW_P 2.8 + param set MC_YAW_I 0.0 param set MC_ATTRATE_P 0.12 param set MC_ATTRATE_I 0.0 param set MC_ATTRATE_D 0.004 - param set MC_YAWPOS_P 2.8 - param set MC_YAWPOS_I 0.0 param set MC_YAWRATE_P 0.2 param set MC_YAWRATE_I 0.05 param set MC_YAWRATE_D 0.0 diff --git a/ROMFS/px4fmu_common/init.d/4011_dji_f450 b/ROMFS/px4fmu_common/init.d/4011_dji_f450 index 1ff5c796a5..ced69783db 100644 --- a/ROMFS/px4fmu_common/init.d/4011_dji_f450 +++ b/ROMFS/px4fmu_common/init.d/4011_dji_f450 @@ -12,11 +12,11 @@ then # param set MC_ATT_P 7.0 param set MC_ATT_I 0.0 + param set MC_YAW_P 2.0 + param set MC_YAW_I 0.0 param set MC_ATTRATE_P 0.12 param set MC_ATTRATE_I 0.0 param set MC_ATTRATE_D 0.004 - param set MC_YAWPOS_P 2.0 - param set MC_YAWPOS_I 0.0 param set MC_YAWRATE_P 0.3 param set MC_YAWRATE_I 0.2 param set MC_YAWRATE_D 0.005 diff --git a/ROMFS/px4fmu_common/init.d/4012_hk_x550 b/ROMFS/px4fmu_common/init.d/4012_hk_x550 index 673043c149..e1423e0082 100644 --- a/ROMFS/px4fmu_common/init.d/4012_hk_x550 +++ b/ROMFS/px4fmu_common/init.d/4012_hk_x550 @@ -12,11 +12,11 @@ then # param set MC_ATT_P 5.5 param set MC_ATT_I 0 + param set MC_YAW_P 0.6 + param set MC_YAW_I 0 param set MC_ATTRATE_P 0.14 param set MC_ATTRATE_I 0 param set MC_ATTRATE_D 0.006 - param set MC_YAWPOS_P 0.6 - param set MC_YAWPOS_I 0 param set MC_YAWRATE_P 0.08 param set MC_YAWRATE_I 0 param set MC_YAWRATE_D 0 From 0034c9f0e739e5478cc7005d611817ee08a7ff51 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 20 Jan 2014 11:42:12 +0100 Subject: [PATCH 322/447] mc_att_control: params refactoring --- .../mc_att_control/mc_att_control_main.cpp | 83 ++++++++++--------- 1 file changed, 43 insertions(+), 40 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 44a3b27554..91eabdada3 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -134,11 +134,6 @@ private: perf_counter_t _loop_perf; /**< loop performance counter */ - math::Vector<3> _K_p; /**< P gain for position error */ - math::Vector<3> _K_rate_p; /**< P gain for angular rate error */ - math::Vector<3> _K_rate_i; /**< I gain for angular rate error */ - math::Vector<3> _K_rate_d; /**< D gain for angular rate error */ - math::Vector<3> _rates_prev; /**< angular rates on previous step */ struct { @@ -150,7 +145,14 @@ private: param_t yaw_rate_p; param_t yaw_rate_i; param_t yaw_rate_d; - } _parameter_handles; /**< handles for interesting parameters */ + } _params_handles; /**< handles for interesting parameters */ + + struct { + math::Vector<3> p; /**< P gain for angular error */ + math::Vector<3> rate_p; /**< P gain for angular rate error */ + math::Vector<3> rate_i; /**< I gain for angular rate error */ + math::Vector<3> rate_d; /**< D gain for angular rate error */ + } _params; /** * Update our local parameter cache. @@ -233,20 +235,21 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : memset(&_control_mode, 0, sizeof(_control_mode)); memset(&_arming, 0, sizeof(_arming)); - _K_p.zero(); - _K_rate_p.zero(); - _K_rate_d.zero(); + _params.p.zero(); + _params.rate_p.zero(); + _params.rate_i.zero(); + _params.rate_d.zero(); _rates_prev.zero(); - _parameter_handles.att_p = param_find("MC_ATT_P"); - _parameter_handles.yaw_p = param_find("MC_YAW_P"); - _parameter_handles.att_rate_p = param_find("MC_ATTRATE_P"); - _parameter_handles.att_rate_i = param_find("MC_ATTRATE_I"); - _parameter_handles.att_rate_d = param_find("MC_ATTRATE_D"); - _parameter_handles.yaw_rate_p = param_find("MC_YAWRATE_P"); - _parameter_handles.yaw_rate_i = param_find("MC_YAWRATE_I"); - _parameter_handles.yaw_rate_d = param_find("MC_YAWRATE_D"); + _params_handles.att_p = param_find("MC_ATT_P"); + _params_handles.yaw_p = param_find("MC_YAW_P"); + _params_handles.att_rate_p = param_find("MC_ATTRATE_P"); + _params_handles.att_rate_i = param_find("MC_ATTRATE_I"); + _params_handles.att_rate_d = param_find("MC_ATTRATE_D"); + _params_handles.yaw_rate_p = param_find("MC_YAWRATE_P"); + _params_handles.yaw_rate_i = param_find("MC_YAWRATE_I"); + _params_handles.yaw_rate_d = param_find("MC_YAWRATE_D"); /* fetch initial parameter values */ parameters_update(); @@ -281,29 +284,29 @@ MulticopterAttitudeControl::parameters_update() { float v; - param_get(_parameter_handles.att_p, &v); - _K_p(0) = v; - _K_p(1) = v; - param_get(_parameter_handles.yaw_p, &v); - _K_p(2) = v; + param_get(_params_handles.att_p, &v); + _params.p(0) = v; + _params.p(1) = v; + param_get(_params_handles.yaw_p, &v); + _params.p(2) = v; - param_get(_parameter_handles.att_rate_p, &v); - _K_rate_p(0) = v; - _K_rate_p(1) = v; - param_get(_parameter_handles.yaw_rate_p, &v); - _K_rate_p(2) = v; + param_get(_params_handles.att_rate_p, &v); + _params.rate_p(0) = v; + _params.rate_p(1) = v; + param_get(_params_handles.yaw_rate_p, &v); + _params.rate_p(2) = v; - param_get(_parameter_handles.att_rate_i, &v); - _K_rate_i(0) = v; - _K_rate_i(1) = v; - param_get(_parameter_handles.yaw_rate_i, &v); - _K_rate_i(2) = v; + param_get(_params_handles.att_rate_i, &v); + _params.rate_i(0) = v; + _params.rate_i(1) = v; + param_get(_params_handles.yaw_rate_i, &v); + _params.rate_i(2) = v; - param_get(_parameter_handles.att_rate_d, &v); - _K_rate_d(0) = v; - _K_rate_d(1) = v; - param_get(_parameter_handles.yaw_rate_d, &v); - _K_rate_d(2) = v; + param_get(_params_handles.att_rate_d, &v); + _params.rate_d(0) = v; + _params.rate_d(1) = v; + param_get(_params_handles.yaw_rate_d, &v); + _params.rate_d(2) = v; return OK; } @@ -631,17 +634,17 @@ MulticopterAttitudeControl::task_main() } /* angular rates setpoint*/ - math::Vector<3> rates_sp = _K_p.emult(e_R); + math::Vector<3> rates_sp = _params.p.emult(e_R); /* feed forward yaw setpoint rate */ rates_sp(2) += yaw_sp_move_rate * yaw_w; math::Vector<3> rates_err = rates_sp - rates; - math::Vector<3> control = _K_rate_p.emult(rates_err) + _K_rate_d.emult(_rates_prev - rates) / fmaxf(dt, 0.003f) + rates_int; + math::Vector<3> control = _params.rate_p.emult(rates_err) + _params.rate_d.emult(_rates_prev - rates) / fmaxf(dt, 0.003f) + rates_int; _rates_prev = rates; /* update integral */ for (int i = 0; i < 3; i++) { - float rate_i = rates_int(i) + _K_rate_i(i) * rates_err(i) * dt; + float rate_i = rates_int(i) + _params.rate_i(i) * rates_err(i) * dt; if (isfinite(rate_i)) { if (rate_i > -RATES_I_LIMIT && rate_i < RATES_I_LIMIT && control(i) > -RATES_I_LIMIT && control(i) < RATES_I_LIMIT) { From 08099818003d0e5861dd312ff44674a61c0e6bc3 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 20 Jan 2014 16:46:39 +0100 Subject: [PATCH 323/447] mc_pos_control: limit tilt when landing --- .../mc_pos_control/mc_pos_control_main.cpp | 17 +++++++++++++++-- .../mc_pos_control/mc_pos_control_params.c | 1 + 2 files changed, 16 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 c77d25b861..5b5d9f004c 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -150,6 +150,7 @@ private: param_t xy_ff; param_t tilt_max; param_t land_speed; + param_t land_tilt_max; param_t rc_scale_pitch; param_t rc_scale_roll; @@ -161,6 +162,7 @@ private: float thr_max; float tilt_max; float land_speed; + float land_tilt_max; float rc_scale_pitch; float rc_scale_roll; @@ -281,6 +283,7 @@ MulticopterPositionControl::MulticopterPositionControl() : _params_handles.xy_ff = param_find("MPC_XY_FF"); _params_handles.tilt_max = param_find("MPC_TILT_MAX"); _params_handles.land_speed = param_find("MPC_LAND_SPEED"); + _params_handles.land_tilt_max = param_find("MPC_LAND_TILT_MAX"); _params_handles.rc_scale_pitch = param_find("RC_SCALE_PITCH"); _params_handles.rc_scale_roll = param_find("RC_SCALE_ROLL"); _params_handles.rc_scale_yaw = param_find("RC_SCALE_YAW"); @@ -329,6 +332,7 @@ MulticopterPositionControl::parameters_update(bool force) param_get(_params_handles.thr_max, &_params.thr_max); param_get(_params_handles.tilt_max, &_params.tilt_max); param_get(_params_handles.land_speed, &_params.land_speed); + param_get(_params_handles.land_tilt_max, &_params.land_tilt_max); param_get(_params_handles.rc_scale_pitch, &_params.rc_scale_pitch); param_get(_params_handles.rc_scale_roll, &_params.rc_scale_roll); param_get(_params_handles.rc_scale_yaw, &_params.rc_scale_yaw); @@ -784,10 +788,19 @@ MulticopterPositionControl::task_main() /* limit max tilt */ float tilt = atan2f(math::Vector<2>(thrust_sp(0), thrust_sp(1)).length(), -thrust_sp(2)); + float tilt_max = _params.tilt_max; + if (!_control_mode.flag_control_manual_enabled) { + if (_mission_items.current_valid && _mission_items.current.nav_cmd == NAV_CMD_LAND) { + /* limit max tilt and min lift when landing */ + tilt_max = _params.land_tilt_max; + if (thr_min < 0.0f) + thr_min = 0.0f; + } + } - if (tilt > _params.tilt_max && _params.thr_min >= 0.0f) { + if (tilt > tilt_max && thr_min >= 0.0f) { /* crop horizontal component */ - float k = tanf(_params.tilt_max) / tanf(tilt); + float k = tanf(tilt_max) / tanf(tilt); thrust_sp(0) *= k; thrust_sp(1) *= k; saturation_xy = true; 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 a00bc3644f..8df88617a7 100644 --- a/src/modules/mc_pos_control/mc_pos_control_params.c +++ b/src/modules/mc_pos_control/mc_pos_control_params.c @@ -55,3 +55,4 @@ PARAM_DEFINE_FLOAT(MPC_XY_VEL_MAX, 5.0f); PARAM_DEFINE_FLOAT(MPC_XY_FF, 0.5f); PARAM_DEFINE_FLOAT(MPC_TILT_MAX, 1.0f); PARAM_DEFINE_FLOAT(MPC_LAND_SPEED, 1.0f); +PARAM_DEFINE_FLOAT(MPC_LAND_TILT_MAX, 0.4f); From 7956c8b08e53be876e96ddd434799b96ce5b7661 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 20 Jan 2014 18:35:06 +0100 Subject: [PATCH 324/447] position_estimator_inav: default parameters updated --- .../position_estimator_inav_params.c | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.c b/src/modules/position_estimator_inav/position_estimator_inav_params.c index da4c9482c0..e1bbd75a6e 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.c @@ -41,15 +41,15 @@ #include "position_estimator_inav_params.h" PARAM_DEFINE_FLOAT(INAV_W_Z_BARO, 1.0f); -PARAM_DEFINE_FLOAT(INAV_W_Z_GPS_P, 0.5f); +PARAM_DEFINE_FLOAT(INAV_W_Z_GPS_P, 0.005f); PARAM_DEFINE_FLOAT(INAV_W_Z_ACC, 20.0f); PARAM_DEFINE_FLOAT(INAV_W_Z_SONAR, 3.0f); PARAM_DEFINE_FLOAT(INAV_W_XY_GPS_P, 1.0f); PARAM_DEFINE_FLOAT(INAV_W_XY_GPS_V, 2.0f); -PARAM_DEFINE_FLOAT(INAV_W_XY_ACC, 10.0f); +PARAM_DEFINE_FLOAT(INAV_W_XY_ACC, 20.0f); PARAM_DEFINE_FLOAT(INAV_W_XY_FLOW, 5.0f); PARAM_DEFINE_FLOAT(INAV_W_GPS_FLOW, 0.1f); -PARAM_DEFINE_FLOAT(INAV_W_ACC_BIAS, 0.0f); +PARAM_DEFINE_FLOAT(INAV_W_ACC_BIAS, 0.05f); PARAM_DEFINE_FLOAT(INAV_FLOW_K, 0.0165f); PARAM_DEFINE_FLOAT(INAV_FLOW_Q_MIN, 0.5f); PARAM_DEFINE_FLOAT(INAV_SONAR_FILT, 0.05f); From 2e472cf918074cd2c4addc7d32abc09933d4ee2d Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 20 Jan 2014 23:44:04 +0100 Subject: [PATCH 325/447] attitude_estimator_ekf: acc comp bug fixed, estimated gravity vector logging --- .../attitude_estimator_ekf_main.cpp | 6 ++- .../mc_pos_control/mc_pos_control_main.cpp | 44 ++++++++++--------- src/modules/sdlog2/sdlog2.c | 3 ++ src/modules/sdlog2/sdlog2_messages.h | 5 ++- src/modules/uORB/topics/vehicle_attitude.h | 1 + 5 files changed, 37 insertions(+), 22 deletions(-) diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp index 7cf1000149..6a1bec1531 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp @@ -410,7 +410,7 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds vel(2) = gps.vel_d_m_s; } - } else if (ekf_params.acc_comp == 2 && global_pos_updated && global_pos.valid && hrt_absolute_time() < global_pos.timestamp + 500000) { + } else if (ekf_params.acc_comp == 2 && global_pos.valid && hrt_absolute_time() < global_pos.timestamp + 500000) { vel_valid = true; if (global_pos_updated) { vel_t = global_pos.timestamp; @@ -532,6 +532,10 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds att.pitchacc = x_aposteriori[4]; att.yawacc = x_aposteriori[5]; + att.g_comp[0] = raw.accelerometer_m_s2[0] - acc(0); + att.g_comp[1] = raw.accelerometer_m_s2[1] - acc(1); + att.g_comp[2] = raw.accelerometer_m_s2[2] - acc(2); + /* copy offsets */ memcpy(&att.rate_offsets, &(x_aposteriori[3]), sizeof(att.rate_offsets)); 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 5b5d9f004c..cd1c55192e 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -798,12 +798,29 @@ MulticopterPositionControl::task_main() } } - if (tilt > tilt_max && thr_min >= 0.0f) { - /* crop horizontal component */ - float k = tanf(tilt_max) / tanf(tilt); - thrust_sp(0) *= k; - thrust_sp(1) *= k; - saturation_xy = true; + if (_control_mode.flag_control_velocity_enabled) { + if (tilt > tilt_max && thr_min >= 0.0f) { + /* crop horizontal component */ + float k = tanf(tilt_max) / tanf(tilt); + thrust_sp(0) *= k; + thrust_sp(1) *= k; + saturation_xy = true; + } + } else { + /* thrust compensation for altitude only control mode */ + float att_comp; + + if (_att.R[2][2] > TILT_COS_MAX) { + att_comp = 1.0f / _att.R[2][2]; + } else if (_att.R[2][2] > 0.0f) { + att_comp = ((1.0f / TILT_COS_MAX - 1.0f) / TILT_COS_MAX) * _att.R[2][2] + 1.0f; + saturation_z = true; + } else { + att_comp = 1.0f; + saturation_z = true; + } + + thrust_sp(2) *= att_comp; } /* limit max thrust */ @@ -854,7 +871,7 @@ MulticopterPositionControl::task_main() thrust_int(2) = 0.0f; } - /* calculate attitude and thrust from thrust vector */ + /* calculate attitude setpoint from thrust vector */ if (_control_mode.flag_control_velocity_enabled) { /* desired body_z axis = -normalize(thrust_vector) */ math::Vector<3> body_x; @@ -910,19 +927,6 @@ MulticopterPositionControl::task_main() _att_sp.roll_body = euler(0); _att_sp.pitch_body = euler(1); /* yaw already used to construct rot matrix, but actual rotation matrix can have different yaw near singularity */ - - } else { - /* thrust compensation for altitude only control mode */ - float att_comp; - - if (_att.R[2][2] > TILT_COS_MAX) - att_comp = 1.0f / _att.R[2][2]; - else if (_att.R[2][2] > 0.0f) - att_comp = ((1.0f / TILT_COS_MAX - 1.0f) / TILT_COS_MAX) * _att.R[2][2] + 1.0f; - else - att_comp = 1.0f; - - thrust_abs *= att_comp; } _att_sp.thrust = thrust_abs; diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index d004a416f9..e0bb81e0eb 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1161,6 +1161,9 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.body.log_ATT.roll_rate = buf.att.rollspeed; log_msg.body.log_ATT.pitch_rate = buf.att.pitchspeed; log_msg.body.log_ATT.yaw_rate = buf.att.yawspeed; + log_msg.body.log_ATT.gx = buf.att.g_comp[0]; + log_msg.body.log_ATT.gy = buf.att.g_comp[1]; + log_msg.body.log_ATT.gz = buf.att.g_comp[2]; LOGBUFFER_WRITE_AND_COUNT(ATT); } diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index fd88217359..503dc0afca 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -57,6 +57,9 @@ struct log_ATT_s { float roll_rate; float pitch_rate; float yaw_rate; + float gx; + float gy; + float gz; }; /* --- ATSP - ATTITUDE SET POINT --- */ @@ -289,7 +292,7 @@ struct log_PARM_s { /* construct list of all message formats */ static const struct log_format_s log_formats[] = { /* business-level messages, ID < 0x80 */ - LOG_FORMAT(ATT, "ffffff", "Roll,Pitch,Yaw,RollRate,PitchRate,YawRate"), + LOG_FORMAT(ATT, "fffffffff", "Roll,Pitch,Yaw,RollRate,PitchRate,YawRate,GX,GY,GZ"), LOG_FORMAT(ATSP, "ffff", "RollSP,PitchSP,YawSP,ThrustSP"), LOG_FORMAT(IMU, "fffffffff", "AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ"), LOG_FORMAT(SENS, "ffff", "BaroPres,BaroAlt,BaroTemp,DiffPres"), diff --git a/src/modules/uORB/topics/vehicle_attitude.h b/src/modules/uORB/topics/vehicle_attitude.h index 4380a5ee77..e5a35ff9bc 100755 --- a/src/modules/uORB/topics/vehicle_attitude.h +++ b/src/modules/uORB/topics/vehicle_attitude.h @@ -76,6 +76,7 @@ struct vehicle_attitude_s { float rate_offsets[3]; /**< Offsets of the body angular rates from zero */ float R[3][3]; /**< Rotation matrix body to world, (Tait-Bryan, NED) */ float q[4]; /**< Quaternion (NED) */ + float g_comp[3]; /**< Compensated gravity vector */ bool R_valid; /**< Rotation matrix valid */ bool q_valid; /**< Quaternion valid */ From eb9fd154fef4f01c6eda1bb87ee7ea87c6c04133 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 21 Jan 2014 15:26:51 +0100 Subject: [PATCH 326/447] commander: more robust RC failsafe, but still hotfix, needs to be rewritten --- src/modules/commander/commander.cpp | 15 ++++++++++----- 1 file changed, 10 insertions(+), 5 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 6a668fcd72..47053838cf 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1133,12 +1133,17 @@ int commander_thread_main(int argc, char *argv[]) mavlink_log_critical(mavlink_fd, "#audio: CRITICAL: RC SIGNAL LOST"); status.rc_signal_lost = true; status_changed = true; - if (status.main_state != MAIN_STATE_AUTO && armed.armed) { - transition_result_t res = main_state_transition(&status, MAIN_STATE_AUTO); + } + if (status.main_state != MAIN_STATE_AUTO && armed.armed) { + transition_result_t res = main_state_transition(&status, MAIN_STATE_AUTO); + if (res == TRANSITION_CHANGED) { + mavlink_log_critical(mavlink_fd, "#audio: failsafe, switching to RTL mode"); + status.set_nav_state = NAV_STATE_RTL; + status.set_nav_state_timestamp = hrt_absolute_time(); + } else if (status.main_state != MAIN_STATE_SEATBELT) { + res = main_state_transition(&status, MAIN_STATE_SEATBELT); if (res == TRANSITION_CHANGED) { - mavlink_log_critical(mavlink_fd, "#audio: switching to RTL mode"); - status.set_nav_state = NAV_STATE_RTL; - status.set_nav_state_timestamp = hrt_absolute_time(); + mavlink_log_critical(mavlink_fd, "#audio: failsafe, switching to SEATBELT mode"); } } } From 29759ce0925c7218458523df568a75a6cd073d0d Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 21 Jan 2014 19:00:06 +0100 Subject: [PATCH 327/447] mc_pos_control: MPC_LAND_TILT_MAX param name is too long, replace with MPC_LAND_TILT --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 2 +- src/modules/mc_pos_control/mc_pos_control_params.c | 2 +- 2 files 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 cd1c55192e..e99e4457ba 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -283,7 +283,7 @@ MulticopterPositionControl::MulticopterPositionControl() : _params_handles.xy_ff = param_find("MPC_XY_FF"); _params_handles.tilt_max = param_find("MPC_TILT_MAX"); _params_handles.land_speed = param_find("MPC_LAND_SPEED"); - _params_handles.land_tilt_max = param_find("MPC_LAND_TILT_MAX"); + _params_handles.land_tilt_max = param_find("MPC_LAND_TILT"); _params_handles.rc_scale_pitch = param_find("RC_SCALE_PITCH"); _params_handles.rc_scale_roll = param_find("RC_SCALE_ROLL"); _params_handles.rc_scale_yaw = param_find("RC_SCALE_YAW"); 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 8df88617a7..9eb56545d7 100644 --- a/src/modules/mc_pos_control/mc_pos_control_params.c +++ b/src/modules/mc_pos_control/mc_pos_control_params.c @@ -55,4 +55,4 @@ PARAM_DEFINE_FLOAT(MPC_XY_VEL_MAX, 5.0f); PARAM_DEFINE_FLOAT(MPC_XY_FF, 0.5f); PARAM_DEFINE_FLOAT(MPC_TILT_MAX, 1.0f); PARAM_DEFINE_FLOAT(MPC_LAND_SPEED, 1.0f); -PARAM_DEFINE_FLOAT(MPC_LAND_TILT_MAX, 0.4f); +PARAM_DEFINE_FLOAT(MPC_LAND_TILT, 0.3f); From bdf440e37596e0ff756b48b31591d1e1defe3c64 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 21 Jan 2014 19:02:03 +0100 Subject: [PATCH 328/447] mc_att_control: reset integrals when disarmed --- src/modules/mc_att_control/mc_att_control_main.cpp | 8 ++++++++ 1 file changed, 8 insertions(+) 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 91eabdada3..245ac024b8 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -638,6 +638,14 @@ MulticopterAttitudeControl::task_main() /* feed forward yaw setpoint rate */ rates_sp(2) += yaw_sp_move_rate * yaw_w; + + /* reset integral if disarmed */ + // TODO add LANDED flag here + if (!_arming.armed) { + rates_int.zero(); + } + + /* rate controller */ math::Vector<3> rates_err = rates_sp - rates; math::Vector<3> control = _params.rate_p.emult(rates_err) + _params.rate_d.emult(_rates_prev - rates) / fmaxf(dt, 0.003f) + rates_int; _rates_prev = rates; From fdef07912caf2346e80b19e8e56a030e8d4afb91 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 22 Jan 2014 11:19:00 +0100 Subject: [PATCH 329/447] mc_pos_control: altitude setpoint offset limiting fixed --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 2 +- 1 file changed, 1 insertion(+), 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 e99e4457ba..5ce4500cd0 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -613,7 +613,7 @@ MulticopterPositionControl::task_main() pos_sp_offs(1) = (_pos_sp(1) - _pos(1)) / _params.sp_offs_max(1); } - if (!_control_mode.flag_control_altitude_enabled) { + if (_control_mode.flag_control_altitude_enabled) { pos_sp_offs(2) = (_pos_sp(2) - _pos(2)) / _params.sp_offs_max(2); } From 480d31f7548d2a4dc7ad55dc2de1f9733045bbd3 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 22 Jan 2014 14:58:09 +0100 Subject: [PATCH 330/447] fw: increase invalid airspeed threshold --- src/modules/fw_att_control/fw_att_control_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index c94180d681..e49b3c1405 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -637,7 +637,7 @@ FixedwingAttitudeControl::task_main() /* if airspeed is smaller than min, the sensor is not giving good readings */ if (!_airspeed_valid || - (_airspeed.indicated_airspeed_m_s < 0.1f * _parameters.airspeed_min) || + (_airspeed.indicated_airspeed_m_s < 0.5f * _parameters.airspeed_min) || !isfinite(_airspeed.indicated_airspeed_m_s)) { airspeed = _parameters.airspeed_trim; From 30cf4097c50fed228f14c80e2ea8876104872503 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 22 Jan 2014 15:05:22 +0100 Subject: [PATCH 331/447] fw: remove duplicate feedforward --- src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp | 2 +- src/lib/ecl/attitude_fw/ecl_roll_controller.cpp | 2 +- src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp index b66d1dba53..9584924ccc 100644 --- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp @@ -174,7 +174,7 @@ float ECL_PitchController::control_bodyrate(float roll, float pitch, float integrator_constrained = math::constrain(_integrator * _k_i, -_integrator_max, _integrator_max); /* Apply PI rate controller and store non-limited output */ - _last_output = (_bodyrate_setpoint * _k_ff +_rate_error * _k_p + integrator_constrained + _rate_setpoint * k_ff) * scaler * scaler; //scaler is proportional to 1/airspeed + _last_output = (_bodyrate_setpoint * _k_ff +_rate_error * _k_p + integrator_constrained) * scaler * scaler; //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_roll_controller.cpp b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp index 9e7d35f680..2e86c72dc1 100644 --- a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp @@ -141,7 +141,7 @@ float ECL_RollController::control_bodyrate(float pitch, //warnx("roll: _integrator: %.4f, _integrator_max: %.4f", (double)_integrator, (double)_integrator_max); /* Apply PI rate controller and store non-limited output */ - _last_output = (_bodyrate_setpoint * _k_ff + _rate_error * _k_p + integrator_constrained + _rate_setpoint * k_ff) * scaler * scaler; //scaler is proportional to 1/airspeed + _last_output = (_bodyrate_setpoint * _k_ff + _rate_error * _k_p + integrator_constrained) * scaler * scaler; //scaler is proportional to 1/airspeed return math::constrain(_last_output, -1.0f, 1.0f); } diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp index 5e22007277..255776765b 100644 --- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp @@ -158,7 +158,7 @@ float ECL_YawController::control_bodyrate(float roll, float pitch, float integrator_constrained = math::constrain(_integrator * _k_i, -_integrator_max, _integrator_max); /* Apply PI rate controller and store non-limited output */ - _last_output = (_bodyrate_setpoint * _k_ff + _rate_error * _k_p + integrator_constrained + _rate_setpoint * k_ff) * scaler * scaler; //scaler is proportional to 1/airspeed + _last_output = (_bodyrate_setpoint * _k_ff + _rate_error * _k_p + integrator_constrained) * scaler * 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); From 93e096f63b57bde43679d86b33eb6ffdee8fc5e4 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 22 Jan 2014 13:50:03 +0100 Subject: [PATCH 332/447] position_estimator_inav: minor bug fixed, write debug log on crash --- .../position_estimator_inav_main.c | 20 +++++++++++++++---- 1 file changed, 16 insertions(+), 4 deletions(-) diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index 52d8c9fe3e..02fa6a8f28 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -166,6 +166,19 @@ int position_estimator_inav_main(int argc, char *argv[]) exit(1); } +void write_debug_log(const char *msg, float dt, float x_est[3], float y_est[3], float z_est[3], float corr_acc[3], float corr_gps[3][2], float w_xy_gps_p, float w_xy_gps_v) { + FILE *f = fopen("/fs/microsd/inav.log", "a"); + if (f) { + char *s = malloc(256); + snprintf(s, 256, "%llu %s\n\tdt=%.5f x_est=[%.5f %.5f %.5f] y_est=[%.5f %.5f %.5f] z_est=[%.5f %.5f %.5f]\n", hrt_absolute_time(), msg, dt, x_est[0], x_est[1], x_est[2], y_est[0], y_est[1], y_est[2], z_est[0], z_est[1], z_est[2]); + fputs(f, s); + snprintf(s, 256, "\tacc_corr=[%.5f %.5f %.5f] gps_pos_corr=[%.5f %.5f %.5f] gps_vel_corr=[%.5f %.5f %.5f] w_xy_gps_p=%.5f w_xy_gps_v=%.5f\n", corr_acc[0], corr_acc[1], corr_acc[2], corr_gps[0][0], corr_gps[1][0], corr_gps[2][0], corr_gps[0][1], corr_gps[1][1], corr_gps[2][1], w_xy_gps_p, w_xy_gps_v); + fputs(f, s); + free(s); + } + fclose(f); +} + /**************************************************************************** * main ****************************************************************************/ @@ -715,7 +728,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) inertial_filter_predict(dt, y_est); if (!isfinite(x_est[0]) || !isfinite(y_est[0])) { - warnx("BAD ESTIMATE AFTER PREDICTION %.6f x: %.3f %.3f %.3f y: %.3f %.3f %.3f", dt, x_est[0], x_est[1], x_est[2], y_est[0], y_est[1], y_est[2]); + write_debug_log("BAD ESTIMATE AFTER PREDICTION", dt, x_est, y_est, z_est, corr_acc, corr_gps, w_xy_gps_p, w_xy_gps_v); thread_should_exit = true; } @@ -730,7 +743,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (use_gps_xy) { inertial_filter_correct(corr_gps[0][0], dt, x_est, 0, w_xy_gps_p); - inertial_filter_correct(corr_gps[1][0], dt, y_est, 0, w_xy_gps_v); + inertial_filter_correct(corr_gps[1][0], dt, y_est, 0, w_xy_gps_p); if (gps.vel_ned_valid && t < gps.timestamp_velocity + gps_topic_timeout) { inertial_filter_correct(corr_gps[0][1], dt, x_est, 1, w_xy_gps_v); @@ -739,8 +752,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } if (!isfinite(x_est[0]) || !isfinite(y_est[0])) { - warnx("BAD ESTIMATE AFTER CORRECTION dt: %.6f x: %.3f %.3f %.3f y: %.3f %.3f %.3f", dt, x_est[0], x_est[1], x_est[2], y_est[0], y_est[1], y_est[2]); - warnx("BAD ESTIMATE AFTER CORRECTION acc: %.3f %.3f gps x: %.3f %.3f gps y: %.3f %.3f", corr_acc[0], corr_acc[1], corr_gps[0][0], corr_gps[0][1], corr_gps[1][0], corr_gps[1][1]); + write_debug_log("BAD ESTIMATE AFTER CORRECTION", dt, x_est, y_est, z_est, corr_acc, corr_gps, w_xy_gps_p, w_xy_gps_v); thread_should_exit = true; } } From 6c07a5c2cf83642a192aefccf82f942e6e7c01a5 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 23 Jan 2014 11:49:23 +0100 Subject: [PATCH 333/447] makefile for backside removed --- makefiles/config_px4fmu-v1_backside.mk | 147 ------------------------- 1 file changed, 147 deletions(-) delete mode 100644 makefiles/config_px4fmu-v1_backside.mk diff --git a/makefiles/config_px4fmu-v1_backside.mk b/makefiles/config_px4fmu-v1_backside.mk deleted file mode 100644 index a37aa8f962..0000000000 --- a/makefiles/config_px4fmu-v1_backside.mk +++ /dev/null @@ -1,147 +0,0 @@ -# -# Makefile for the px4fmu_default configuration -# - -# -# Use the configuration's ROMFS. -# -ROMFS_ROOT = $(PX4_BASE)/ROMFS/px4fmu_common -ROMFS_OPTIONAL_FILES = $(PX4_BASE)/Images/px4io-v1_default.bin - -# -# Board support modules -# -MODULES += drivers/device -MODULES += drivers/stm32 -MODULES += drivers/stm32/adc -MODULES += drivers/stm32/tone_alarm -MODULES += drivers/led -MODULES += drivers/px4io -MODULES += drivers/px4fmu -MODULES += drivers/boards/px4fmu-v1 -MODULES += drivers/ardrone_interface -MODULES += drivers/l3gd20 -MODULES += drivers/bma180 -MODULES += drivers/mpu6000 -MODULES += drivers/hmc5883 -MODULES += drivers/ms5611 -MODULES += drivers/gps -MODULES += drivers/hil -MODULES += drivers/blinkm -MODULES += drivers/rgbled -MODULES += drivers/roboclaw -MODULES += drivers/airspeed -MODULES += drivers/ets_airspeed -MODULES += drivers/meas_airspeed -MODULES += modules/sensors - -# -# System commands -# -MODULES += systemcmds/eeprom -MODULES += systemcmds/ramtron -MODULES += systemcmds/bl_update -MODULES += systemcmds/boardinfo -MODULES += systemcmds/i2c -MODULES += systemcmds/mixer -MODULES += systemcmds/param -MODULES += systemcmds/perf -MODULES += systemcmds/preflight_check -MODULES += systemcmds/pwm -MODULES += systemcmds/esc_calib -MODULES += systemcmds/reboot -MODULES += systemcmds/top -MODULES += systemcmds/tests -MODULES += systemcmds/config -MODULES += systemcmds/nshterm -MODULES += systemcmds/hw_ver - -# -# General system control -# -MODULES += modules/commander -MODULES += modules/navigator -MODULES += modules/mavlink -MODULES += modules/mavlink_onboard -MODULES += modules/gpio_led - -# -# Estimation modules (EKF/ SO3 / other filters) -# -MODULES += modules/att_pos_estimator_ekf - -# -# Vehicle Control -# -MODULES += modules/fixedwing_backside - -# -# Logging -# -MODULES += modules/sdlog2 - -# -# Unit tests -# -#MODULES += modules/unit_test -#MODULES += modules/commander/commander_tests - -# -# Library modules -# -MODULES += modules/systemlib -MODULES += modules/systemlib/mixer -MODULES += modules/controllib -MODULES += modules/uORB - -# -# Libraries -# -LIBRARIES += lib/mathlib/CMSIS -MODULES += lib/mathlib -MODULES += lib/mathlib/math/filter -MODULES += lib/ecl -MODULES += lib/external_lgpl -MODULES += lib/geo -MODULES += lib/conversion - -# -# Demo apps -# -#MODULES += examples/math_demo -# Tutorial code from -# https://pixhawk.ethz.ch/px4/dev/hello_sky -#MODULES += examples/px4_simple_app - -# Tutorial code from -# https://pixhawk.ethz.ch/px4/dev/daemon -#MODULES += examples/px4_daemon_app - -# Tutorial code from -# https://pixhawk.ethz.ch/px4/dev/debug_values -#MODULES += examples/px4_mavlink_debug - -# Tutorial code from -# https://pixhawk.ethz.ch/px4/dev/example_fixedwing_control -#MODULES += examples/fixedwing_control - -# Hardware test -#MODULES += examples/hwtest - -# -# Transitional support - add commands from the NuttX export archive. -# -# In general, these should move to modules over time. -# -# Each entry here is ... but we use a helper macro -# to make the table a bit more readable. -# -define _B - $(strip $1).$(or $(strip $2),SCHED_PRIORITY_DEFAULT).$(or $(strip $3),CONFIG_PTHREAD_STACK_DEFAULT).$(strip $4) -endef - -# command priority stack entrypoint -BUILTIN_COMMANDS := \ - $(call _B, sercon, , 2048, sercon_main ) \ - $(call _B, serdis, , 2048, serdis_main ) \ - $(call _B, sysinfo, , 2048, sysinfo_main ) From 6acb8fa66f38d20af57b8c45cc7878257abb24d2 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 23 Jan 2014 12:16:02 +0100 Subject: [PATCH 334/447] Replace mission_item_triplet with position_setpoint_triplet, WIP --- src/modules/controllib/uorb/blocks.cpp | 6 +- src/modules/controllib/uorb/blocks.hpp | 8 +- src/modules/fixedwing_backside/fixedwing.hpp | 2 +- .../fw_pos_control_l1_main.cpp | 104 ++-- src/modules/mavlink/orb_listener.c | 2 +- .../mc_pos_control/mc_pos_control_main.cpp | 31 +- src/modules/navigator/navigator_main.cpp | 451 ++++++++---------- src/modules/sdlog2/sdlog2.c | 2 +- src/modules/uORB/objects_common.cpp | 4 +- ..._triplet.h => position_setpoint_triplet.h} | 38 +- 10 files changed, 304 insertions(+), 344 deletions(-) rename src/modules/uORB/topics/{mission_item_triplet.h => position_setpoint_triplet.h} (77%) diff --git a/src/modules/controllib/uorb/blocks.cpp b/src/modules/controllib/uorb/blocks.cpp index e213ac17fc..e8fecef0d9 100644 --- a/src/modules/controllib/uorb/blocks.cpp +++ b/src/modules/controllib/uorb/blocks.cpp @@ -54,8 +54,8 @@ BlockWaypointGuidance::~BlockWaypointGuidance() {}; void BlockWaypointGuidance::update(vehicle_global_position_s &pos, vehicle_attitude_s &att, - mission_item_s &missionCmd, - mission_item_s &lastMissionCmd) + position_setpoint_s &missionCmd, + position_setpoint_s &lastMissionCmd) { // heading to waypoint @@ -86,7 +86,7 @@ BlockUorbEnabledAutopilot::BlockUorbEnabledAutopilot(SuperBlock *parent, const c _attCmd(&getSubscriptions(), ORB_ID(vehicle_attitude_setpoint), 20), _ratesCmd(&getSubscriptions(), ORB_ID(vehicle_rates_setpoint), 20), _pos(&getSubscriptions() , ORB_ID(vehicle_global_position), 20), - _missionCmd(&getSubscriptions(), ORB_ID(mission_item_triplet), 20), + _missionCmd(&getSubscriptions(), ORB_ID(position_setpoint_triplet), 20), _manual(&getSubscriptions(), ORB_ID(manual_control_setpoint), 20), _status(&getSubscriptions(), ORB_ID(vehicle_status), 20), _param_update(&getSubscriptions(), ORB_ID(parameter_update), 1000), // limit to 1 Hz diff --git a/src/modules/controllib/uorb/blocks.hpp b/src/modules/controllib/uorb/blocks.hpp index 8cc0d77d47..7c80c4b2ba 100644 --- a/src/modules/controllib/uorb/blocks.hpp +++ b/src/modules/controllib/uorb/blocks.hpp @@ -43,7 +43,7 @@ #include #include #include -#include +#include #include #include #include @@ -82,8 +82,8 @@ public: virtual ~BlockWaypointGuidance(); void update(vehicle_global_position_s &pos, vehicle_attitude_s &att, - mission_item_s &missionCmd, - mission_item_s &lastMissionCmd); + position_setpoint_s &missionCmd, + position_setpoint_s &lastMissionCmd); float getPsiCmd() { return _psiCmd; } }; @@ -98,7 +98,7 @@ protected: UOrbSubscription _attCmd; UOrbSubscription _ratesCmd; UOrbSubscription _pos; - UOrbSubscription _missionCmd; + UOrbSubscription _missionCmd; UOrbSubscription _manual; UOrbSubscription _status; UOrbSubscription _param_update; diff --git a/src/modules/fixedwing_backside/fixedwing.hpp b/src/modules/fixedwing_backside/fixedwing.hpp index b4dbc36b09..e1c85c2618 100644 --- a/src/modules/fixedwing_backside/fixedwing.hpp +++ b/src/modules/fixedwing_backside/fixedwing.hpp @@ -264,7 +264,7 @@ private: BlockParamFloat _crMax; struct pollfd _attPoll; - mission_item_triplet_s _lastMissionCmd; + position_setpoint_triplet_s _lastMissionCmd; enum {CH_AIL, CH_ELV, CH_RDR, CH_THR}; uint64_t _timeStamp; public: diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index d8dbf9085a..3889012c9d 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -68,7 +68,7 @@ #include #include #include -#include +#include #include #include #include @@ -126,7 +126,7 @@ private: int _control_task; /**< task handle for sensor task */ int _global_pos_sub; - int _mission_item_triplet_sub; + int _pos_sp_triplet_sub; int _att_sub; /**< vehicle attitude subscription */ int _attitude_sub; /**< raw rc channels data subscription */ int _airspeed_sub; /**< airspeed subscription */ @@ -145,7 +145,7 @@ private: struct airspeed_s _airspeed; /**< airspeed */ struct vehicle_control_mode_s _control_mode; /**< vehicle status */ struct vehicle_global_position_s _global_pos; /**< global vehicle position */ - struct mission_item_triplet_s _mission_item_triplet; /**< triplet of mission items */ + struct position_setpoint_triplet_s _pos_sp_triplet; /**< triplet of mission items */ struct sensor_combined_s _sensor_combined; /**< for body frame accelerations */ perf_counter_t _loop_perf; /**< loop performance counter */ @@ -332,10 +332,10 @@ private: * Control position. */ bool control_position(const math::Vector<2> &global_pos, const math::Vector<2> &ground_speed, - const struct mission_item_triplet_s &_mission_item_triplet); + const struct position_setpoint_triplet_s &_pos_sp_triplet); float calculate_target_airspeed(float airspeed_demand); - void calculate_gndspeed_undershoot(const math::Vector<2> ¤t_position, const math::Vector<2> &ground_speed, const struct mission_item_triplet_s &mission_item_triplet); + void calculate_gndspeed_undershoot(const math::Vector<2> ¤t_position, const math::Vector<2> &ground_speed, const struct position_setpoint_triplet_s &pos_sp_triplet); /** * Shim for calling task_main from task_create. @@ -367,7 +367,7 @@ FixedwingPositionControl::FixedwingPositionControl() : /* subscriptions */ _global_pos_sub(-1), - _mission_item_triplet_sub(-1), + _pos_sp_triplet_sub(-1), _att_sub(-1), _airspeed_sub(-1), _control_mode_sub(-1), @@ -406,7 +406,7 @@ FixedwingPositionControl::FixedwingPositionControl() : airspeed_s _airspeed = {0}; vehicle_control_mode_s _control_mode = {0}; vehicle_global_position_s _global_pos = {0}; - mission_item_triplet_s _mission_item_triplet = {0}; + position_setpoint_triplet_s _pos_sp_triplet = {0}; sensor_combined_s _sensor_combined = {0}; @@ -653,11 +653,11 @@ void FixedwingPositionControl::vehicle_setpoint_poll() { /* check if there is a new setpoint */ - bool mission_item_triplet_updated; - orb_check(_mission_item_triplet_sub, &mission_item_triplet_updated); + bool pos_sp_triplet_updated; + orb_check(_pos_sp_triplet_sub, &pos_sp_triplet_updated); - if (mission_item_triplet_updated) { - orb_copy(ORB_ID(mission_item_triplet), _mission_item_triplet_sub, &_mission_item_triplet); + if (pos_sp_triplet_updated) { + orb_copy(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_sub, &_pos_sp_triplet); _setpoint_valid = true; } } @@ -700,7 +700,7 @@ FixedwingPositionControl::calculate_target_airspeed(float airspeed_demand) } void -FixedwingPositionControl::calculate_gndspeed_undershoot(const math::Vector<2> ¤t_position, const math::Vector<2> &ground_speed, const struct mission_item_triplet_s &mission_item_triplet) +FixedwingPositionControl::calculate_gndspeed_undershoot(const math::Vector<2> ¤t_position, const math::Vector<2> &ground_speed, const struct position_setpoint_triplet_s &pos_sp_triplet) { if (_global_pos_valid) { @@ -713,12 +713,12 @@ FixedwingPositionControl::calculate_gndspeed_undershoot(const math::Vector<2> &c /* The minimum desired ground speed is the minimum airspeed projected on to the ground using the altitude and horizontal difference between the waypoints if available*/ float distance = 0.0f; float delta_altitude = 0.0f; - if (mission_item_triplet.previous_valid) { - distance = get_distance_to_next_waypoint(mission_item_triplet.previous.lat, mission_item_triplet.previous.lon, mission_item_triplet.current.lat, mission_item_triplet.current.lon); - delta_altitude = mission_item_triplet.current.altitude - mission_item_triplet.previous.altitude; + if (pos_sp_triplet.previous.valid) { + distance = get_distance_to_next_waypoint(pos_sp_triplet.previous.lat, pos_sp_triplet.previous.lon, pos_sp_triplet.current.lat, pos_sp_triplet.current.lon); + delta_altitude = pos_sp_triplet.current.alt - pos_sp_triplet.previous.alt; } else { - distance = get_distance_to_next_waypoint(current_position(0), current_position(1), mission_item_triplet.current.lat, mission_item_triplet.current.lon); - delta_altitude = mission_item_triplet.current.altitude - _global_pos.alt; + distance = get_distance_to_next_waypoint(current_position(0), current_position(1), pos_sp_triplet.current.lat, pos_sp_triplet.current.lon); + delta_altitude = pos_sp_triplet.current.alt - _global_pos.alt; } float ground_speed_desired = _parameters.airspeed_min * cosf(atan2f(delta_altitude, distance)); @@ -751,11 +751,11 @@ void FixedwingPositionControl::navigation_capabilities_publish() bool FixedwingPositionControl::control_position(const math::Vector<2> ¤t_position, const math::Vector<2> &ground_speed, - const struct mission_item_triplet_s &mission_item_triplet) + const struct position_setpoint_triplet_s &pos_sp_triplet) { bool setpoint = true; - calculate_gndspeed_undershoot(current_position, ground_speed, mission_item_triplet); + calculate_gndspeed_undershoot(current_position, ground_speed, pos_sp_triplet); float eas2tas = 1.0f; // XXX calculate actual number based on current measurements @@ -767,7 +767,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi math::Vector<3> accel_earth = _R_nb * accel_body; _tecs.update_50hz(baro_altitude, _airspeed.indicated_airspeed_m_s, _R_nb, accel_body, accel_earth); - float altitude_error = _mission_item_triplet.current.altitude - _global_pos.alt; + float altitude_error = _pos_sp_triplet.current.alt - _global_pos.alt; /* no throttle limit as default */ float throttle_max = 1.0f; @@ -785,58 +785,56 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi _tecs.set_speed_weight(_parameters.speed_weight); /* current waypoint (the one currently heading for) */ - math::Vector<2> next_wp(mission_item_triplet.current.lat, mission_item_triplet.current.lon); + math::Vector<2> next_wp(pos_sp_triplet.current.lat, pos_sp_triplet.current.lon); /* current waypoint (the one currently heading for) */ - math::Vector<2> curr_wp(mission_item_triplet.current.lat, mission_item_triplet.current.lon); + math::Vector<2> curr_wp(pos_sp_triplet.current.lat, pos_sp_triplet.current.lon); /* previous waypoint */ math::Vector<2> prev_wp; - if (mission_item_triplet.previous_valid) { - prev_wp(0) = mission_item_triplet.previous.lat; - prev_wp(1) = mission_item_triplet.previous.lon; + if (pos_sp_triplet.previous.valid) { + prev_wp(0) = pos_sp_triplet.previous.lat; + prev_wp(1) = pos_sp_triplet.previous.lon; } else { /* * No valid previous waypoint, go for the current wp. * This is automatically handled by the L1 library. */ - prev_wp(0) = mission_item_triplet.current.lat; - prev_wp(1) = mission_item_triplet.current.lon; + prev_wp(0) = pos_sp_triplet.current.lat; + prev_wp(1) = pos_sp_triplet.current.lon; } - if (mission_item_triplet.current.nav_cmd == NAV_CMD_WAYPOINT || mission_item_triplet.current.nav_cmd == NAV_CMD_RETURN_TO_LAUNCH) { + if (pos_sp_triplet.current.type == SETPOINT_TYPE_NORMAL) { /* waypoint is a plain navigation waypoint */ _l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed); _att_sp.roll_body = _l1_control.nav_roll(); _att_sp.yaw_body = _l1_control.nav_bearing(); - _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _mission_item_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_trim), + _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _pos_sp_triplet.current.alt, calculate_target_airspeed(_parameters.airspeed_trim), _airspeed.indicated_airspeed_m_s, eas2tas, false, math::radians(_parameters.pitch_limit_min), _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max)); - } else if (mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_TURN_COUNT || - mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || - mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_UNLIMITED) { + } else if (pos_sp_triplet.current.type == SETPOINT_TYPE_LOITER) { /* waypoint is a loiter waypoint */ - _l1_control.navigate_loiter(curr_wp, current_position, mission_item_triplet.current.loiter_radius, - mission_item_triplet.current.loiter_direction, ground_speed); + _l1_control.navigate_loiter(curr_wp, current_position, pos_sp_triplet.current.loiter_radius, + pos_sp_triplet.current.loiter_direction, ground_speed); _att_sp.roll_body = _l1_control.nav_roll(); _att_sp.yaw_body = _l1_control.nav_bearing(); - _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _mission_item_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_trim), + _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _pos_sp_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_trim), _airspeed.indicated_airspeed_m_s, eas2tas, false, math::radians(_parameters.pitch_limit_min), _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max)); - } else if (mission_item_triplet.current.nav_cmd == NAV_CMD_LAND) { + } else if (pos_sp_triplet.current.type == SETPOINT_TYPE_LAND) { /* Horizontal landing control */ /* switch to heading hold for the last meters, continue heading hold after */ @@ -847,7 +845,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* heading hold, along the line connecting this and the last waypoint */ if (!land_noreturn_horizontal) {//set target_bearing in first occurrence - if (mission_item_triplet.previous_valid) { + if (pos_sp_triplet.previous_valid) { target_bearing = get_bearing_to_next_waypoint(prev_wp(0), prev_wp(1), curr_wp(0), curr_wp(1)); } else { target_bearing = _att.yaw; @@ -884,18 +882,18 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* apply minimum pitch (flare) and limit roll if close to touch down, altitude error is negative (going down) */ // XXX this could make a great param - float flare_pitch_angle_rad = -math::radians(5.0f);//math::radians(mission_item_triplet.current.param1) + float flare_pitch_angle_rad = -math::radians(5.0f);//math::radians(pos_sp_triplet.current.param1) float throttle_land = _parameters.throttle_min + (_parameters.throttle_max - _parameters.throttle_min) * 0.1f; float airspeed_land = 1.3f * _parameters.airspeed_min; float airspeed_approach = 1.3f * _parameters.airspeed_min; float L_wp_distance = get_distance_to_next_waypoint(prev_wp(0), prev_wp(1), curr_wp(0), curr_wp(1)) * _parameters.land_slope_length; - float L_altitude = landingslope.getLandingSlopeAbsoluteAltitude(L_wp_distance, _mission_item_triplet.current.altitude);//getLandingSlopeAbsoluteAltitude(L_wp_distance, _mission_item_triplet.current.altitude, landing_slope_angle_rad, horizontal_slope_displacement); - float landing_slope_alt_desired = landingslope.getLandingSlopeAbsoluteAltitude(wp_distance, _mission_item_triplet.current.altitude);//getLandingSlopeAbsoluteAltitude(wp_distance, _mission_item_triplet.current.altitude, landing_slope_angle_rad, horizontal_slope_displacement); + float L_altitude = landingslope.getLandingSlopeAbsoluteAltitude(L_wp_distance, _pos_sp_triplet.current.altitude);//getLandingSlopeAbsoluteAltitude(L_wp_distance, _pos_sp_triplet.current.altitude, landing_slope_angle_rad, horizontal_slope_displacement); + float landing_slope_alt_desired = landingslope.getLandingSlopeAbsoluteAltitude(wp_distance, _pos_sp_triplet.current.altitude);//getLandingSlopeAbsoluteAltitude(wp_distance, _pos_sp_triplet.current.altitude, landing_slope_angle_rad, horizontal_slope_displacement); - if ( (_global_pos.alt < _mission_item_triplet.current.altitude + landingslope.flare_relative_alt()) || land_noreturn_vertical) { //checking for land_noreturn to avoid unwanted climb out + if ( (_global_pos.alt < _pos_sp_triplet.current.altitude + landingslope.flare_relative_alt()) || land_noreturn_vertical) { //checking for land_noreturn to avoid unwanted climb out /* land with minimal speed */ @@ -914,12 +912,12 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } - float flare_curve_alt = landingslope.getFlareCurveAltitude(wp_distance, _mission_item_triplet.current.altitude); + float flare_curve_alt = landingslope.getFlareCurveAltitude(wp_distance, _pos_sp_triplet.current.altitude); /* avoid climbout */ if (flare_curve_alt_last < flare_curve_alt && land_noreturn_vertical || land_stayonground) { - flare_curve_alt = mission_item_triplet.current.altitude; + flare_curve_alt = pos_sp_triplet.current.altitude; land_stayonground = true; } @@ -977,7 +975,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max)); } - } else if (mission_item_triplet.current.nav_cmd == NAV_CMD_TAKEOFF) { + } else if (pos_sp_triplet.current.type == SETPOINT_TYPE_TAKEOFF) { /* Perform launch detection */ // warnx("Launch detection running"); @@ -1011,9 +1009,9 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi if (altitude_error > 15.0f) { /* enforce a minimum of 10 degrees pitch up on takeoff, or take parameter */ - _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _mission_item_triplet.current.altitude, calculate_target_airspeed(1.3f * _parameters.airspeed_min), + _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _pos_sp_triplet.current.altitude, calculate_target_airspeed(1.3f * _parameters.airspeed_min), _airspeed.indicated_airspeed_m_s, eas2tas, - true, math::max(math::radians(mission_item_triplet.current.pitch_min), math::radians(10.0f)), + true, math::max(math::radians(pos_sp_triplet.current.pitch_min), math::radians(10.0f)), _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max)); @@ -1022,7 +1020,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } else { - _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _mission_item_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_trim), + _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _pos_sp_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_trim), _airspeed.indicated_airspeed_m_s, eas2tas, false, math::radians(_parameters.pitch_limit_min), _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, @@ -1037,14 +1035,14 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi // warnx("nav bearing: %8.4f bearing err: %8.4f target bearing: %8.4f", (double)_l1_control.nav_bearing(), // (double)_l1_control.bearing_error(), (double)_l1_control.target_bearing()); // warnx("prev wp: %8.4f/%8.4f, next wp: %8.4f/%8.4f prev:%s", (double)prev_wp(0), (double)prev_wp(1), - // (double)next_wp(0), (double)next_wp(1), (mission_item_triplet.previous_valid) ? "valid" : "invalid"); + // (double)next_wp(0), (double)next_wp(1), (pos_sp_triplet.previous_valid) ? "valid" : "invalid"); // XXX at this point we always want no loiter hold if a // mission is active _loiter_hold = false; /* reset land state */ - if (mission_item_triplet.current.nav_cmd != NAV_CMD_LAND) { + if (pos_sp_triplet.current.nav_cmd != NAV_CMD_LAND) { land_noreturn_horizontal = false; land_noreturn_vertical = false; land_stayonground = false; @@ -1053,7 +1051,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } /* reset takeoff/launch state */ - if (mission_item_triplet.current.nav_cmd != NAV_CMD_TAKEOFF) { + if (pos_sp_triplet.current.nav_cmd != NAV_CMD_TAKEOFF) { launch_detected = false; usePreTakeoffThrust = false; } @@ -1176,7 +1174,7 @@ FixedwingPositionControl::task_main() * do subscriptions */ _global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); - _mission_item_triplet_sub = orb_subscribe(ORB_ID(mission_item_triplet)); + _pos_sp_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet)); _att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); _sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined)); _control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); @@ -1271,7 +1269,7 @@ FixedwingPositionControl::task_main() * Attempt to control position, on success (= sensors present and not in manual mode), * publish setpoint. */ - if (control_position(current_position, ground_speed, _mission_item_triplet)) { + if (control_position(current_position, ground_speed, _pos_sp_triplet)) { _att_sp.timestamp = hrt_absolute_time(); /* lazily publish the setpoint only once available */ @@ -1285,7 +1283,7 @@ FixedwingPositionControl::task_main() } /* XXX check if radius makes sense here */ - float turn_distance = _l1_control.switch_distance(_mission_item_triplet.current.acceptance_radius); + float turn_distance = _l1_control.switch_distance(_pos_sp_triplet.current.acceptance_radius); /* lazily publish navigation capabilities */ if (turn_distance != _nav_capabilities.turn_distance && turn_distance > 0) { diff --git a/src/modules/mavlink/orb_listener.c b/src/modules/mavlink/orb_listener.c index 6e177bc4d9..e491911d3e 100644 --- a/src/modules/mavlink/orb_listener.c +++ b/src/modules/mavlink/orb_listener.c @@ -410,7 +410,7 @@ l_local_position(const struct listener *l) void l_global_position_setpoint(const struct listener *l) { - struct mission_item_triplet_s triplet; + struct position_setpoint_triplet_s triplet; orb_copy(ORB_ID(mission_item_triplet), mavlink_subs.triplet_sub, &triplet); uint8_t coordinate_frame = MAV_FRAME_GLOBAL; 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 5ce4500cd0..bc20a4f470 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -117,7 +117,7 @@ private: int _manual_sub; /**< notification of manual control updates */ int _arming_sub; /**< arming status of outputs */ int _local_pos_sub; /**< vehicle local position */ - int _mission_items_sub; /**< mission item triplet */ + int _pos_sp_triplet_sub; /**< mission item triplet */ orb_advert_t _local_pos_sp_pub; /**< local position setpoint publication */ orb_advert_t _att_sp_pub; /**< attitude setpoint publication */ @@ -130,7 +130,7 @@ private: struct actuator_armed_s _arming; /**< actuator arming status */ struct vehicle_local_position_s _local_pos; /**< vehicle local position */ struct vehicle_local_position_setpoint_s _local_pos_sp; /**< vehicle local position */ - struct mission_item_triplet_s _mission_items; /**< vehicle global position setpoint */ + struct position_setpoint_triplet_s _pos_sp_triplet; /**< vehicle global position setpoint triplet */ struct vehicle_global_velocity_setpoint_s _global_vel_sp; /**< vehicle global velocity setpoint */ struct { @@ -236,7 +236,7 @@ MulticopterPositionControl::MulticopterPositionControl() : _manual_sub(-1), _arming_sub(-1), _local_pos_sub(-1), - _mission_items_sub(-1), + _pos_sp_triplet_sub(-1), /* publications */ _local_pos_sp_pub(-1), @@ -250,7 +250,7 @@ MulticopterPositionControl::MulticopterPositionControl() : memset(&_arming, 0, sizeof(_arming)); memset(&_local_pos, 0, sizeof(_local_pos)); memset(&_local_pos_sp, 0, sizeof(_local_pos_sp)); - memset(&_mission_items, 0, sizeof(_mission_items)); + memset(&_pos_sp_triplet, 0, sizeof(_pos_sp_triplet)); memset(&_global_vel_sp, 0, sizeof(_global_vel_sp)); _params.pos_p.zero(); @@ -405,10 +405,10 @@ MulticopterPositionControl::poll_subscriptions() if (updated) orb_copy(ORB_ID(actuator_armed), _arming_sub, &_arming); - orb_check(_mission_items_sub, &updated); + orb_check(_pos_sp_triplet_sub, &updated); if (updated) - orb_copy(ORB_ID(mission_item_triplet), _mission_items_sub, &_mission_items); + orb_copy(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_sub, &_pos_sp_triplet); } float @@ -450,7 +450,7 @@ MulticopterPositionControl::task_main() _manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); _arming_sub = orb_subscribe(ORB_ID(actuator_armed)); _local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position)); - _mission_items_sub = orb_subscribe(ORB_ID(mission_item_triplet)); + _pos_sp_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet)); parameters_update(true); @@ -626,16 +626,15 @@ MulticopterPositionControl::task_main() } else { /* AUTO */ - if (_mission_items.current_valid) { - struct mission_item_s item = _mission_items.current; + if (_pos_sp_triplet.current.valid) { + struct position_setpoint_s current_sp = _pos_sp_triplet.current; - // TODO home altitude can be != ref_alt, check home_position topic - _pos_sp(2) = -(item.altitude_is_relative ? item.altitude : item.altitude - ref_alt); + _pos_sp(2) = -(current_sp.alt - ref_alt); - map_projection_project(item.lat, item.lon, &_pos_sp(0), &_pos_sp(1)); + map_projection_project(current_sp.lat, current_sp.lon, &_pos_sp(0), &_pos_sp(1)); - if (isfinite(_mission_items.current.yaw)) { - _att_sp.yaw_body = _mission_items.current.yaw; + if (isfinite(current_sp.yaw)) { + _att_sp.yaw_body = current_sp.yaw; } /* in case of interrupted mission don't go to waypoint but stay at current position */ @@ -688,7 +687,7 @@ MulticopterPositionControl::task_main() if (!_control_mode.flag_control_manual_enabled) { /* use constant descend rate when landing, ignore altitude setpoint */ - if (_mission_items.current_valid && _mission_items.current.nav_cmd == NAV_CMD_LAND) { + if (_pos_sp_triplet.current_valid && _pos_sp_triplet.current.nav_cmd == NAV_CMD_LAND) { _vel_sp(2) = _params.land_speed; } @@ -790,7 +789,7 @@ MulticopterPositionControl::task_main() float tilt = atan2f(math::Vector<2>(thrust_sp(0), thrust_sp(1)).length(), -thrust_sp(2)); float tilt_max = _params.tilt_max; if (!_control_mode.flag_control_manual_enabled) { - if (_mission_items.current_valid && _mission_items.current.nav_cmd == NAV_CMD_LAND) { + if (_pos_sp_triplet.current_valid && _pos_sp_triplet.current.nav_cmd == NAV_CMD_LAND) { /* limit max tilt and min lift when landing */ tilt_max = _params.land_tilt_max; if (thr_min < 0.0f) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index ca5735509a..dfd07d3158 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -64,7 +64,7 @@ #include #include #include -#include +#include #include #include #include @@ -152,7 +152,7 @@ private: int _onboard_mission_sub; /**< notification of onboard mission updates */ int _capabilities_sub; /**< notification of vehicle capabilities updates */ - orb_advert_t _triplet_pub; /**< publish position setpoint triplet */ + orb_advert_t _pos_sp_triplet_pub; /**< publish position setpoint triplet */ orb_advert_t _mission_result_pub; /**< publish mission result topic */ orb_advert_t _control_mode_pub; /**< publish vehicle control mode topic */ @@ -160,8 +160,10 @@ private: struct vehicle_control_mode_s _control_mode; /**< vehicle control mode */ struct vehicle_global_position_s _global_pos; /**< global vehicle position */ struct home_position_s _home_pos; /**< home position for RTL */ - struct mission_item_triplet_s _mission_item_triplet; /**< triplet of mission items */ + struct position_setpoint_triplet_s _pos_sp_triplet; /**< triplet of position setpoints */ struct mission_result_s _mission_result; /**< mission result for commander/mavlink */ + struct mission_item_s _mission_item; /**< current mission item */ + bool _mission_item_valid; /**< current mission item valid */ perf_counter_t _loop_perf; /**< loop performance counter */ @@ -323,35 +325,24 @@ private: void set_rtl_item(); /** - * Helper function to get a loiter item + * Set position setpoint for mission item */ - void get_loiter_item(mission_item_s *item); + void position_setpoint_from_mission_item(position_setpoint_s *sp, mission_item_s *item); /** * Helper function to get a takeoff item */ - void get_takeoff_item(mission_item_s *item); + void get_takeoff_setpoint(position_setpoint_s *pos_sp); /** * Publish a new mission item triplet for position controller */ - void publish_mission_item_triplet(); + void publish_position_setpoint_triplet(); /** * Publish vehicle_control_mode topic for controllers */ void publish_control_mode(); - - - /** - * Compare two mission items if they are equivalent - * Two mission items can be considered equivalent for the purpose of the navigator even if some fields differ. - * - * @return true if equivalent, false otherwise - */ - bool cmp_mission_item_equivalent(const struct mission_item_s a, const struct mission_item_s b); - - void add_home_pos_to_rtl(struct mission_item_s *new_mission_item); }; namespace navigator @@ -385,7 +376,7 @@ Navigator::Navigator() : _capabilities_sub(-1), /* publications */ - _triplet_pub(-1), + _pos_sp_triplet_pub(-1), _mission_result_pub(-1), _control_mode_pub(-1), @@ -402,6 +393,7 @@ Navigator::Navigator() : _waypoint_yaw_reached(false), _time_first_inside_orbit(0), _set_nav_state_timestamp(0), + _mission_item_valid(false), _need_takeoff(true), _do_takeoff(false), _geofence_violation_warning_sent(false) @@ -414,14 +406,9 @@ Navigator::Navigator() : _parameter_handles.land_alt = param_find("NAV_LAND_ALT"); _parameter_handles.rtl_alt = param_find("NAV_RTL_ALT"); - _mission_item_triplet.previous_valid = false; - _mission_item_triplet.current_valid = false; - _mission_item_triplet.next_valid = false; - memset(&_mission_item_triplet.previous, 0, sizeof(struct mission_item_s)); - memset(&_mission_item_triplet.current, 0, sizeof(struct mission_item_s)); - memset(&_mission_item_triplet.next, 0, sizeof(struct mission_item_s)); - + memset(&_pos_sp_triplet, 0, sizeof(struct position_setpoint_triplet_s)); memset(&_mission_result, 0, sizeof(struct mission_result_s)); + memset(&_mission_item, 0, sizeof(struct mission_item_s)); nav_states_str[0] = "NONE"; nav_states_str[1] = "READY"; @@ -482,7 +469,6 @@ Navigator::parameters_update() void Navigator::global_position_update() { - /* load local copies */ orb_copy(ORB_ID(vehicle_global_position), _global_pos_sub, &_global_pos); } @@ -938,23 +924,25 @@ StateTable::Tran const Navigator::myTable[NAV_STATE_MAX][MAX_EVENT] = { void Navigator::start_none() { - _mission_item_triplet.previous_valid = false; - _mission_item_triplet.current_valid = false; - _mission_item_triplet.next_valid = false; + _pos_sp_triplet.previous.valid = false; + _pos_sp_triplet.current.valid = false; + _pos_sp_triplet.next.valid = false; + _mission_item_valid = false; _reset_loiter_pos = true; _do_takeoff = false; _rtl_state = RTL_STATE_NONE; - publish_mission_item_triplet(); + publish_position_setpoint_triplet(); } void Navigator::start_ready() { - _mission_item_triplet.previous_valid = false; - _mission_item_triplet.current_valid = false; - _mission_item_triplet.next_valid = false; + _pos_sp_triplet.previous.valid = false; + _pos_sp_triplet.current.valid = false; + _pos_sp_triplet.next.valid = false; + _mission_item_valid = false; _reset_loiter_pos = true; _do_takeoff = false; @@ -964,7 +952,7 @@ Navigator::start_ready() _rtl_state = RTL_STATE_NONE; } - publish_mission_item_triplet(); + publish_position_setpoint_triplet(); } void @@ -973,38 +961,38 @@ Navigator::start_loiter() _do_takeoff = false; /* set loiter position if needed */ - if (_reset_loiter_pos || !_mission_item_triplet.current_valid) { + if (_reset_loiter_pos || !_pos_sp_triplet.current.valid) { _reset_loiter_pos = false; - _mission_item_triplet.current.lat = (double)_global_pos.lat / 1e7d; - _mission_item_triplet.current.lon = (double)_global_pos.lon / 1e7d; - _mission_item_triplet.current.yaw = NAN; // NAN means to use current yaw + _pos_sp_triplet.current.lat = (double)_global_pos.lat / 1e7d; + _pos_sp_triplet.current.lon = (double)_global_pos.lon / 1e7d; + _pos_sp_triplet.current.yaw = NAN; // NAN means to use current yaw - _mission_item_triplet.current.altitude_is_relative = false; float min_alt_amsl = _parameters.min_altitude + _home_pos.altitude; /* use current altitude if above min altitude set by parameter */ if (_global_pos.alt < min_alt_amsl) { - _mission_item_triplet.current.altitude = min_alt_amsl; + _pos_sp_triplet.current.altitude = min_alt_amsl; mavlink_log_info(_mavlink_fd, "[navigator] loiter %.1fm higher", (double)(min_alt_amsl - _global_pos.alt)); } else { - _mission_item_triplet.current.altitude = _global_pos.alt; + _pos_sp_triplet.current.altitude = _global_pos.alt; mavlink_log_info(_mavlink_fd, "[navigator] loiter at current altitude"); } + _pos_sp_triplet.current.type = SETPOINT_TYPE_NORMAL; + if (_rtl_state == RTL_STATE_LAND) { /* if RTL landing was interrupted, avoid landing from MIN_ALT on next RTL */ _rtl_state = RTL_STATE_DESCEND; } } - _mission_item_triplet.previous_valid = false; - _mission_item_triplet.current_valid = true; - _mission_item_triplet.next_valid = false; + _pos_sp_triplet.previous.valid = false; + _pos_sp_triplet.current.valid = true; + _pos_sp_triplet.next.valid = false; + _mission_item_valid = false; - get_loiter_item(&_mission_item_triplet.current); - - publish_mission_item_triplet(); + publish_position_setpoint_triplet(); } void @@ -1020,8 +1008,7 @@ void Navigator::set_mission_item() { /* copy current mission to previous item */ - memcpy(&_mission_item_triplet.previous, &_mission_item_triplet.current, sizeof(mission_item_s)); - _mission_item_triplet.previous_valid = _mission_item_triplet.current_valid; + memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s)); _reset_loiter_pos = true; _do_takeoff = false; @@ -1030,36 +1017,34 @@ Navigator::set_mission_item() bool onboard; unsigned index; - ret = _mission.get_current_mission_item(&_mission_item_triplet.current, &onboard, &index); + ret = _mission.get_current_mission_item(&_mission_item, &onboard, &index); if (ret == OK) { - _mission_item_triplet.current_valid = true; - add_home_pos_to_rtl(&_mission_item_triplet.current); + _mission_item_valid = true; + position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item); - if (_mission_item_triplet.current.nav_cmd != NAV_CMD_RETURN_TO_LAUNCH && - _mission_item_triplet.current.nav_cmd != NAV_CMD_LOITER_TIME_LIMIT && - _mission_item_triplet.current.nav_cmd != NAV_CMD_LOITER_TURN_COUNT && - _mission_item_triplet.current.nav_cmd != NAV_CMD_LOITER_UNLIMITED) { + if (_mission_item.nav_cmd != NAV_CMD_RETURN_TO_LAUNCH && + _mission_item.nav_cmd != NAV_CMD_LOITER_TIME_LIMIT && + _mission_item.nav_cmd != NAV_CMD_LOITER_TURN_COUNT && + _mission_item.nav_cmd != NAV_CMD_LOITER_UNLIMITED) { /* don't reset RTL state on RTL or LOITER items */ _rtl_state = RTL_STATE_NONE; } if (_vstatus.is_rotary_wing) { if (_need_takeoff && ( - _mission_item_triplet.current.nav_cmd == NAV_CMD_TAKEOFF || - _mission_item_triplet.current.nav_cmd == NAV_CMD_WAYPOINT || - _mission_item_triplet.current.nav_cmd == NAV_CMD_RETURN_TO_LAUNCH || - _mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || - _mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_TURN_COUNT || - _mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_UNLIMITED + _mission_item.nav_cmd == NAV_CMD_TAKEOFF || + _mission_item.nav_cmd == NAV_CMD_WAYPOINT || + _mission_item.nav_cmd == NAV_CMD_RETURN_TO_LAUNCH || + _mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || + _mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT || + _mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED )) { /* do special TAKEOFF handling for VTOL */ _need_takeoff = false; /* calculate desired takeoff altitude AMSL */ - float takeoff_alt_amsl = _mission_item_triplet.current.altitude; - if (_mission_item_triplet.current.altitude_is_relative) - takeoff_alt_amsl += _home_pos.altitude; + float takeoff_alt_amsl = _pos_sp_triplet.current.alt; if (_vstatus.condition_landed) { /* takeoff to at least NAV_TAKEOFF_ALT from ground if landed */ @@ -1067,30 +1052,29 @@ Navigator::set_mission_item() } /* check if we really need takeoff */ - if (_vstatus.condition_landed || _global_pos.alt < takeoff_alt_amsl - _mission_item_triplet.current.acceptance_radius) { + if (_vstatus.condition_landed || _global_pos.alt < takeoff_alt_amsl - item.acceptance_radius) { /* force TAKEOFF if landed or waypoint altitude is more than current */ _do_takeoff = true; - /* move current mission item to next */ - memcpy(&_mission_item_triplet.next, &_mission_item_triplet.current, sizeof(mission_item_s)); - _mission_item_triplet.next_valid = true; + /* move current position setpoint to next */ + memcpy(&_pos_sp_triplet.next, &_pos_sp_triplet.current, sizeof(position_setpoint_s)); - /* set current item to TAKEOFF */ - get_takeoff_item(&_mission_item_triplet.current); + /* set current setpoint to takeoff */ - _mission_item_triplet.current.lat = (double)_global_pos.lat / 1e7d; - _mission_item_triplet.current.lon = (double)_global_pos.lon / 1e7d; - _mission_item_triplet.current.altitude = takeoff_alt_amsl; - _mission_item_triplet.current.altitude_is_relative = false; + _pos_sp_triplet.current.lat = (double)_global_pos.lat / 1e7d; + _pos_sp_triplet.current.lon = (double)_global_pos.lon / 1e7d; + _pos_sp_triplet.current.alt = takeoff_alt_amsl; + _pos_sp_triplet.current.yaw = NAN; + _pos_sp_triplet.current.type = SETPOINT_TYPE_TAKEOFF; } - } else if (_mission_item_triplet.current.nav_cmd == NAV_CMD_LAND) { + } else if (item.nav_cmd == NAV_CMD_LAND) { /* will need takeoff after landing */ _need_takeoff = true; } } if (_do_takeoff) { - mavlink_log_info(_mavlink_fd, "[navigator] takeoff to %.1fm", _mission_item_triplet.current.altitude); + mavlink_log_info(_mavlink_fd, "[navigator] takeoff to %.1fm AMSL", _pos_sp_triplet.current.altitude); } else { if (onboard) { mavlink_log_info(_mavlink_fd, "[navigator] heading to onboard WP %d", index); @@ -1100,23 +1084,24 @@ Navigator::set_mission_item() } } else { /* since a mission is not advanced without WPs available, this is not supposed to happen */ - _mission_item_triplet.current_valid = false; + _mission_item_valid = false; + _pos_sp_triplet.current.valid = false; warnx("ERROR: current WP can't be set"); } if (!_do_takeoff) { - ret = _mission.get_next_mission_item(&_mission_item_triplet.next); + mission_item_s item_next; + ret = _mission.get_next_mission_item(&item_next); if (ret == OK) { - add_home_pos_to_rtl(&_mission_item_triplet.next); - _mission_item_triplet.next_valid = true; + position_setpoint_from_mission_item(&_pos_sp_triplet.next, &item_next); } else { /* this will fail for the last WP */ - _mission_item_triplet.next_valid = false; + _pos_sp_triplet.next.valid = false; } } - publish_mission_item_triplet(); + publish_position_setpoint_triplet(); } void @@ -1129,8 +1114,8 @@ Navigator::start_rtl() } else { _rtl_state = RTL_STATE_RETURN; if (_reset_loiter_pos) { - _mission_item_triplet.current.altitude_is_relative = false; - _mission_item_triplet.current.altitude = _global_pos.alt; + _mission_item.altitude_is_relative = false; + _mission_item.altitude = _global_pos.alt; } } } @@ -1144,99 +1129,110 @@ Navigator::set_rtl_item() { switch (_rtl_state) { case RTL_STATE_CLIMB: { - memcpy(&_mission_item_triplet.previous, &_mission_item_triplet.current, sizeof(mission_item_s)); - _mission_item_triplet.previous_valid = _mission_item_triplet.current_valid; - - _mission_item_triplet.current_valid = true; - _mission_item_triplet.next_valid = false; + memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s)); float climb_alt = _home_pos.altitude + _parameters.rtl_alt; - if (_vstatus.condition_landed) + if (_vstatus.condition_landed) { climb_alt = fmaxf(climb_alt, _global_pos.alt + _parameters.rtl_alt); + } + + _mission_item_valid = true; + + _mission_item.lat = (double)_global_pos.lat / 1e7d; + _mission_item.lon = (double)_global_pos.lon / 1e7d; + _mission_item.altitude_is_relative = false; + _mission_item.altitude = climb_alt; + _mission_item.yaw = NAN; + _mission_item.loiter_radius = _parameters.loiter_radius; + _mission_item.loiter_direction = 1; + _mission_item.nav_cmd = NAV_CMD_TAKEOFF; + _mission_item.acceptance_radius = _parameters.acceptance_radius; + _mission_item.time_inside = 0.0f; + _mission_item.pitch_min = 0.0f; + _mission_item.autocontinue = true; + _mission_item.origin = ORIGIN_ONBOARD; + + position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item); + + _pos_sp_triplet.next.valid = false; - _mission_item_triplet.current.altitude_is_relative = false; - _mission_item_triplet.current.lat = (double)_global_pos.lat / 1e7d; - _mission_item_triplet.current.lon = (double)_global_pos.lon / 1e7d; - _mission_item_triplet.current.altitude = climb_alt; - _mission_item_triplet.current.yaw = NAN; - _mission_item_triplet.current.loiter_radius = _parameters.loiter_radius; - _mission_item_triplet.current.loiter_direction = 1; - _mission_item_triplet.current.nav_cmd = NAV_CMD_TAKEOFF; - _mission_item_triplet.current.acceptance_radius = _parameters.acceptance_radius; - _mission_item_triplet.current.time_inside = 0.0f; - _mission_item_triplet.current.pitch_min = 0.0f; - _mission_item_triplet.current.autocontinue = true; - _mission_item_triplet.current.origin = ORIGIN_ONBOARD; mavlink_log_info(_mavlink_fd, "[navigator] RTL: climb to %.1fm", climb_alt - _home_pos.altitude); break; } case RTL_STATE_RETURN: { - memcpy(&_mission_item_triplet.previous, &_mission_item_triplet.current, sizeof(mission_item_s)); - _mission_item_triplet.previous_valid = _mission_item_triplet.current_valid; + memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s)); - _mission_item_triplet.current_valid = true; - _mission_item_triplet.next_valid = false; + _mission_item_valid = true; + + _mission_item.lat = _home_pos.lat; + _mission_item.lon = _home_pos.lon; + // don't change altitude + _mission_item.yaw = NAN; // TODO set heading to home + _mission_item.loiter_radius = _parameters.loiter_radius; + _mission_item.loiter_direction = 1; + _mission_item.nav_cmd = NAV_CMD_WAYPOINT; + _mission_item.acceptance_radius = _parameters.acceptance_radius; + _mission_item.time_inside = 0.0f; + _mission_item.pitch_min = 0.0f; + _mission_item.autocontinue = true; + _mission_item.origin = ORIGIN_ONBOARD; + + position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item); + + _pos_sp_triplet.next.valid = false; - _mission_item_triplet.current.lat = _home_pos.lat; - _mission_item_triplet.current.lon = _home_pos.lon; - // don't change altitude setpoint - _mission_item_triplet.current.yaw = NAN; - _mission_item_triplet.current.loiter_radius = _parameters.loiter_radius; - _mission_item_triplet.current.loiter_direction = 1; - _mission_item_triplet.current.nav_cmd = NAV_CMD_WAYPOINT; - _mission_item_triplet.current.acceptance_radius = _parameters.acceptance_radius; - _mission_item_triplet.current.time_inside = 0.0f; - _mission_item_triplet.current.pitch_min = 0.0f; - _mission_item_triplet.current.autocontinue = true; - _mission_item_triplet.current.origin = ORIGIN_ONBOARD; mavlink_log_info(_mavlink_fd, "[navigator] RTL: return"); break; } case RTL_STATE_DESCEND: { - memcpy(&_mission_item_triplet.previous, &_mission_item_triplet.current, sizeof(mission_item_s)); - _mission_item_triplet.previous_valid = _mission_item_triplet.current_valid; + memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s)); - _mission_item_triplet.current_valid = true; - _mission_item_triplet.next_valid = false; + _mission_item_valid = true; - float descend_alt = _home_pos.altitude + _parameters.land_alt; + _mission_item.lat = _home_pos.lat; + _mission_item.lon = _home_pos.lon; + _mission_item.altitude_is_relative = false; + _mission_item.altitude = _home_pos.altitude + _parameters.land_alt; + _mission_item.yaw = NAN; + _mission_item.loiter_radius = _parameters.loiter_radius; + _mission_item.loiter_direction = 1; + _mission_item.nav_cmd = NAV_CMD_WAYPOINT; + _mission_item.acceptance_radius = _parameters.acceptance_radius; + _mission_item.time_inside = 0.0f; + _mission_item.pitch_min = 0.0f; + _mission_item.autocontinue = true; + _mission_item.origin = ORIGIN_ONBOARD; + + position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item); + + _pos_sp_triplet.next.valid = false; - _mission_item_triplet.current.altitude_is_relative = false; - _mission_item_triplet.current.lat = _home_pos.lat; - _mission_item_triplet.current.lon = _home_pos.lon; - _mission_item_triplet.current.altitude = descend_alt; - _mission_item_triplet.current.yaw = NAN; - _mission_item_triplet.current.loiter_radius = _parameters.loiter_radius; - _mission_item_triplet.current.loiter_direction = 1; - _mission_item_triplet.current.nav_cmd = NAV_CMD_WAYPOINT; - _mission_item_triplet.current.acceptance_radius = _parameters.acceptance_radius; - _mission_item_triplet.current.time_inside = 0.0f; - _mission_item_triplet.current.pitch_min = 0.0f; - _mission_item_triplet.current.autocontinue = true; - _mission_item_triplet.current.origin = ORIGIN_ONBOARD; mavlink_log_info(_mavlink_fd, "[navigator] RTL: descend to %.1fm", descend_alt - _home_pos.altitude); break; } case RTL_STATE_LAND: { - memcpy(&_mission_item_triplet.previous, &_mission_item_triplet.current, sizeof(mission_item_s)); - _mission_item_triplet.previous_valid = _mission_item_triplet.current_valid; + memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s)); - _mission_item_triplet.current_valid = true; - _mission_item_triplet.next_valid = false; + _mission_item_valid = true; + + _mission_item.lat = _home_pos.lat; + _mission_item.lon = _home_pos.lon; + _mission_item.altitude_is_relative = false; + _mission_item.altitude = _home_pos.altitude; + _mission_item.yaw = NAN; + _mission_item.loiter_radius = _parameters.loiter_radius; + _mission_item.loiter_direction = 1; + _mission_item.nav_cmd = NAV_CMD_LAND; + _mission_item.acceptance_radius = _parameters.acceptance_radius; + _mission_item.time_inside = 0.0f; + _mission_item.pitch_min = 0.0f; + _mission_item.autocontinue = true; + _mission_item.origin = ORIGIN_ONBOARD; + + position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item); + + _pos_sp_triplet.next.valid = false; - _mission_item_triplet.current.altitude_is_relative = false; - _mission_item_triplet.current.lat = _home_pos.lat; - _mission_item_triplet.current.lon = _home_pos.lon; - _mission_item_triplet.current.altitude = _home_pos.altitude; - _mission_item_triplet.current.yaw = NAN; - _mission_item_triplet.current.loiter_radius = _parameters.loiter_radius; - _mission_item_triplet.current.loiter_direction = 1; - _mission_item_triplet.current.nav_cmd = NAV_CMD_LAND; - _mission_item_triplet.current.acceptance_radius = _parameters.acceptance_radius; - _mission_item_triplet.current.time_inside = 0.0f; - _mission_item_triplet.current.pitch_min = 0.0f; - _mission_item_triplet.current.autocontinue = true; - _mission_item_triplet.current.origin = ORIGIN_ONBOARD; mavlink_log_info(_mavlink_fd, "[navigator] RTL: land"); break; } @@ -1247,18 +1243,46 @@ Navigator::set_rtl_item() } } - publish_mission_item_triplet(); + publish_position_setpoint_triplet(); +} + +void +Navigator::position_setpoint_from_mission_item(position_setpoint_s *sp, mission_item_s *item) +{ + sp->valid = true; + if (item->nav_cmd == NAV_CMD_RETURN_TO_LAUNCH) { + /* set home position for RTL item */ + sp->lat = _home_pos.lat; + sp->lon = _home_pos.lon; + sp->alt = _home_pos.altitude + _parameters.rtl_alt; + } else { + sp->lat = item->lat; + sp->lon = item->lon; + sp->alt = item->altitude_is_relative ? item->alt + _home_pos.altitude : item->alt; + } + sp->yaw = item->yaw; + if (item->nav_cmd == NAV_CMD_TAKEOFF) { + sp->type = SETPOINT_TYPE_TAKEOFF; + } else if (item->nav_cmd == NAV_CMD_LAND) { + sp->type = SETPOINT_TYPE_LAND; + } else if (item->nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || + item->nav_cmd == NAV_CMD_LOITER_TURN_COUNT || + item->nav_cmd == NAV_CMD_LOITER_UNLIMITED) { + sp->type = SETPOINT_TYPE_LOITER; + } else { + sp->type = SETPOINT_TYPE_NORMAL; + } } bool Navigator::check_mission_item_reached() { /* only check if there is actually a mission item to check */ - if (!_mission_item_triplet.current_valid) { + if (!_mission_item_valid) { return false; } - if (_mission_item_triplet.current.nav_cmd == NAV_CMD_LAND) { + if (_mission_item.nav_cmd == NAV_CMD_LAND) { if (_vstatus.is_rotary_wing) { return _vstatus.condition_landed; } else { @@ -1269,10 +1293,10 @@ Navigator::check_mission_item_reached() } /* XXX TODO count turns */ - if ((_mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_TURN_COUNT || - _mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || - _mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_UNLIMITED) && - _mission_item_triplet.current.loiter_radius > 0.01f) { + if ((_mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT || + _mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || + _mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED) && + _mission_item.loiter_radius > 0.01f) { return false; } @@ -1282,8 +1306,8 @@ Navigator::check_mission_item_reached() if (!_waypoint_position_reached) { float acceptance_radius; - if (_mission_item_triplet.current.nav_cmd == NAV_CMD_WAYPOINT && _mission_item_triplet.current.acceptance_radius > 0.01f) { - acceptance_radius = _mission_item_triplet.current.acceptance_radius; + if (_mission_item.nav_cmd == NAV_CMD_WAYPOINT && _mission_item.acceptance_radius > 0.01f) { + acceptance_radius = _mission_item.acceptance_radius; } else { acceptance_radius = _parameters.acceptance_radius; @@ -1294,11 +1318,11 @@ Navigator::check_mission_item_reached() float dist_z = -1.0f; /* current relative or AMSL altitude depending on mission item altitude_is_relative flag */ - float wp_alt_amsl = _mission_item_triplet.current.altitude; - if (_mission_item_triplet.current.altitude_is_relative) - _mission_item_triplet.current.altitude += _home_pos.altitude; + float wp_alt_amsl = _mission_item.altitude; + if (_mission_item.altitude_is_relative) + _mission_itemt.altitude += _home_pos.altitude; - dist = get_distance_to_point_global_wgs84(_mission_item_triplet.current.lat, _mission_item_triplet.current.lon, wp_alt_amsl, + dist = get_distance_to_point_global_wgs84(_mission_item.lat, _mission_item.lon, wp_alt_amsl, (double)_global_pos.lat / 1e7d, (double)_global_pos.lon / 1e7d, _global_pos.alt, &dist_xy, &dist_z); @@ -1315,9 +1339,9 @@ Navigator::check_mission_item_reached() } if (!_waypoint_yaw_reached) { - if (_vstatus.is_rotary_wing && !_do_takeoff && isfinite(_mission_item_triplet.current.yaw)) { + if (_vstatus.is_rotary_wing && !_do_takeoff && isfinite(_mission_item.yaw)) { /* check yaw if defined only for rotary wing except takeoff */ - float yaw_err = _wrap_pi(_mission_item_triplet.current.yaw - _global_pos.yaw); + float yaw_err = _wrap_pi(_mission_item.yaw - _global_pos.yaw); if (fabsf(yaw_err) < 0.05f) { /* XXX get rid of magic number */ _waypoint_yaw_reached = true; } @@ -1330,14 +1354,14 @@ Navigator::check_mission_item_reached() if (_waypoint_position_reached && _waypoint_yaw_reached) { if (_time_first_inside_orbit == 0) { _time_first_inside_orbit = now; - if (_mission_item_triplet.current.time_inside > 0.01f) { - mavlink_log_info(_mavlink_fd, "[navigator] waypoint reached, wait for %.1fs", _mission_item_triplet.current.time_inside); + if (_mission_item.time_inside > 0.01f) { + mavlink_log_info(_mavlink_fd, "[navigator] waypoint reached, wait for %.1fs", _mission_item.time_inside); } } /* check if the MAV was long enough inside the waypoint orbit */ - if ((now - _time_first_inside_orbit >= (uint64_t)_mission_item_triplet.current.time_inside * 1e6) - || _mission_item_triplet.current.nav_cmd == NAV_CMD_TAKEOFF) { + if ((now - _time_first_inside_orbit >= (uint64_t)_mission_item.time_inside * 1e6) + || _mission_item.nav_cmd == NAV_CMD_TAKEOFF) { _time_first_inside_orbit = 0; _waypoint_yaw_reached = false; _waypoint_position_reached = false; @@ -1389,67 +1413,16 @@ Navigator::on_mission_item_reached() } void -Navigator::get_loiter_item(struct mission_item_s *item) -{ - //item->altitude_is_relative - //item->lat - //item->lon - //item->altitude - //item->yaw - item->loiter_radius = _parameters.loiter_radius; - item->loiter_direction = 1; - item->nav_cmd = NAV_CMD_LOITER_UNLIMITED; - item->acceptance_radius = _parameters.acceptance_radius; - item->time_inside = 0.0f; - item->pitch_min = 0.0f; - item->autocontinue = false; - item->origin = ORIGIN_ONBOARD; - -} - -void -Navigator::get_takeoff_item(mission_item_s *item) -{ - //item->altitude_is_relative - //item->lat - //item->lon - //item->altitude - item->yaw = NAN; - item->loiter_radius = _parameters.loiter_radius; - item->loiter_direction = 1; - item->nav_cmd = NAV_CMD_TAKEOFF; - item->acceptance_radius = _parameters.acceptance_radius; - item->time_inside = 0.0f; - item->pitch_min = 0.0; - item->autocontinue = true; - item->origin = ORIGIN_ONBOARD; -} - -void -Navigator::add_home_pos_to_rtl(struct mission_item_s *new_mission_item) -{ - if (new_mission_item->nav_cmd == NAV_CMD_RETURN_TO_LAUNCH) { - /* append the home position to RTL item */ - new_mission_item->lat = _home_pos.lat; - new_mission_item->lon = _home_pos.lon; - new_mission_item->altitude = _home_pos.altitude + _parameters.rtl_alt; - new_mission_item->altitude_is_relative = false; - new_mission_item->loiter_radius = _parameters.loiter_radius; - new_mission_item->acceptance_radius = _parameters.acceptance_radius; - } -} - -void -Navigator::publish_mission_item_triplet() +Navigator::publish_position_setpoint_triplet() { /* lazily publish the mission triplet only once available */ - if (_triplet_pub > 0) { + if (_pos_sp_triplet_pub > 0) { /* publish the mission triplet */ - orb_publish(ORB_ID(mission_item_triplet), _triplet_pub, &_mission_item_triplet); + orb_publish(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_pub, &_pos_sp_triplet); } else { /* advertise and publish */ - _triplet_pub = orb_advertise(ORB_ID(mission_item_triplet), &_mission_item_triplet); + _pos_sp_triplet_pub = orb_advertise(ORB_ID(position_setpoint_triplet), &_pos_sp_triplet); } } @@ -1534,24 +1507,6 @@ Navigator::publish_control_mode() } } -bool Navigator::cmp_mission_item_equivalent(const struct mission_item_s a, const struct mission_item_s b) { - if (a.altitude_is_relative == b.altitude_is_relative && - fabs(a.lat - b.lat) < FLT_EPSILON && - fabs(a.lon - b.lon) < FLT_EPSILON && - fabsf(a.altitude - b.altitude) < FLT_EPSILON && - fabsf(a.yaw - b.yaw) < FLT_EPSILON && - fabsf(a.loiter_radius - b.loiter_radius) < FLT_EPSILON && - a.loiter_direction == b.loiter_direction && - a.nav_cmd == b.nav_cmd && - fabsf(a.acceptance_radius - b.acceptance_radius) < FLT_EPSILON && - fabsf(a.time_inside - b.time_inside) < FLT_EPSILON && - a.autocontinue == b.autocontinue) { - return true; - } else { - return false; - } -} - void Navigator::add_fence_point(int argc, char *argv[]) { _geofence.addPoint(argc, argv); diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index e0bb81e0eb..de510e0223 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -750,7 +750,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct vehicle_local_position_s local_pos; struct vehicle_local_position_setpoint_s local_pos_sp; struct vehicle_global_position_s global_pos; - struct mission_item_triplet_s triplet; + struct position_setpoint_triplet_s triplet; struct vehicle_gps_position_s gps_pos; struct vehicle_vicon_position_s vicon_pos; struct optical_flow_s flow; diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index 79a820c066..4c84c1f25e 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -117,8 +117,8 @@ ORB_DEFINE(vehicle_local_position_setpoint, struct vehicle_local_position_setpoi #include "topics/vehicle_bodyframe_speed_setpoint.h" ORB_DEFINE(vehicle_bodyframe_speed_setpoint, struct vehicle_bodyframe_speed_setpoint_s); -#include "topics/mission_item_triplet.h" -ORB_DEFINE(mission_item_triplet, struct mission_item_triplet_s); +#include "topics/position_setpoint_triplet.h" +ORB_DEFINE(position_setpoint_triplet, struct position_setpoint_triplet_s); #include "topics/vehicle_global_velocity_setpoint.h" ORB_DEFINE(vehicle_global_velocity_setpoint, struct vehicle_global_velocity_setpoint_s); diff --git a/src/modules/uORB/topics/mission_item_triplet.h b/src/modules/uORB/topics/position_setpoint_triplet.h similarity index 77% rename from src/modules/uORB/topics/mission_item_triplet.h rename to src/modules/uORB/topics/position_setpoint_triplet.h index b35eae6070..a8bd6b8e38 100644 --- a/src/modules/uORB/topics/mission_item_triplet.h +++ b/src/modules/uORB/topics/position_setpoint_triplet.h @@ -46,31 +46,39 @@ #include #include "../uORB.h" -#include "mission.h" - /** * @addtogroup topics * @{ */ +enum SETPOINT_TYPE +{ + SETPOINT_TYPE_NORMAL = 0, + SETPOINT_TYPE_LOITER, + SETPOINT_TYPE_TAKEOFF, + SETPOINT_TYPE_LAND, +}; + +struct position_setpoint_s +{ + bool valid; /**< true if point is valid */ + double lat; /**< latitude, in deg */ + double lon; /**< longitude, in deg */ + float alt; /**< altitude AMSL, in m */ + float yaw; /**< yaw (only for multirotors), in rad [-PI..PI), NaN to hold current yaw */ + enum SETPOINT_TYPE type; /**< setpoint type to adjust behavior of position controller */ +}; + /** * Global position setpoint triplet in WGS84 coordinates. * * This are the three next waypoints (or just the next two or one). */ -struct mission_item_triplet_s +struct position_setpoint_triplet_s { - bool previous_valid; - bool current_valid; /**< flag indicating previous mission item is valid */ - bool next_valid; /**< flag indicating next mission item is valid */ - - struct mission_item_s previous; - struct mission_item_s current; - struct mission_item_s next; - - int previous_index; - int current_index; - int next_index; + struct position_setpoint_s previous; + struct position_setpoint_s current; + struct position_setpoint_s next; }; /** @@ -78,6 +86,6 @@ struct mission_item_triplet_s */ /* register this as object request broker structure */ -ORB_DECLARE(mission_item_triplet); +ORB_DECLARE(position_setpoint_triplet); #endif From 1cffa9d2f77f788f8446e0aceec60b7676a0a65f Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 23 Jan 2014 22:41:26 +0100 Subject: [PATCH 335/447] position_setpoint_triplet refactoring finished --- .../fw_pos_control_l1_main.cpp | 26 +++++++++---------- src/modules/mavlink/orb_listener.c | 15 ++++------- src/modules/mavlink/orb_topics.h | 3 +-- src/modules/mavlink_onboard/orb_topics.h | 2 +- .../mc_pos_control/mc_pos_control_main.cpp | 8 +++--- src/modules/navigator/navigator_main.cpp | 21 +++++++++------ src/modules/sdlog2/sdlog2.c | 13 ++++------ src/modules/sdlog2/sdlog2_messages.h | 9 +++---- .../uORB/topics/position_setpoint_triplet.h | 9 ++++--- 9 files changed, 51 insertions(+), 55 deletions(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 3889012c9d..f3d688646d 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -828,7 +828,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi _att_sp.roll_body = _l1_control.nav_roll(); _att_sp.yaw_body = _l1_control.nav_bearing(); - _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _pos_sp_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_trim), + _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _pos_sp_triplet.current.alt, calculate_target_airspeed(_parameters.airspeed_trim), _airspeed.indicated_airspeed_m_s, eas2tas, false, math::radians(_parameters.pitch_limit_min), _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, @@ -845,7 +845,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* heading hold, along the line connecting this and the last waypoint */ if (!land_noreturn_horizontal) {//set target_bearing in first occurrence - if (pos_sp_triplet.previous_valid) { + if (pos_sp_triplet.previous.valid) { target_bearing = get_bearing_to_next_waypoint(prev_wp(0), prev_wp(1), curr_wp(0), curr_wp(1)); } else { target_bearing = _att.yaw; @@ -877,7 +877,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi // /* do not go down too early */ // if (wp_distance > 50.0f) { -// altitude_error = (_global_triplet.current.altitude + 25.0f) - _global_pos.alt; +// altitude_error = (_global_triplet.current.alt + 25.0f) - _global_pos.alt; // } /* apply minimum pitch (flare) and limit roll if close to touch down, altitude error is negative (going down) */ // XXX this could make a great param @@ -888,12 +888,12 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi float airspeed_approach = 1.3f * _parameters.airspeed_min; float L_wp_distance = get_distance_to_next_waypoint(prev_wp(0), prev_wp(1), curr_wp(0), curr_wp(1)) * _parameters.land_slope_length; - float L_altitude = landingslope.getLandingSlopeAbsoluteAltitude(L_wp_distance, _pos_sp_triplet.current.altitude);//getLandingSlopeAbsoluteAltitude(L_wp_distance, _pos_sp_triplet.current.altitude, landing_slope_angle_rad, horizontal_slope_displacement); - float landing_slope_alt_desired = landingslope.getLandingSlopeAbsoluteAltitude(wp_distance, _pos_sp_triplet.current.altitude);//getLandingSlopeAbsoluteAltitude(wp_distance, _pos_sp_triplet.current.altitude, landing_slope_angle_rad, horizontal_slope_displacement); + float L_altitude = landingslope.getLandingSlopeAbsoluteAltitude(L_wp_distance, _pos_sp_triplet.current.alt);//getLandingSlopeAbsoluteAltitude(L_wp_distance, _pos_sp_triplet.current.alt, landing_slope_angle_rad, horizontal_slope_displacement); + float landing_slope_alt_desired = landingslope.getLandingSlopeAbsoluteAltitude(wp_distance, _pos_sp_triplet.current.alt);//getLandingSlopeAbsoluteAltitude(wp_distance, _pos_sp_triplet.current.alt, landing_slope_angle_rad, horizontal_slope_displacement); - if ( (_global_pos.alt < _pos_sp_triplet.current.altitude + landingslope.flare_relative_alt()) || land_noreturn_vertical) { //checking for land_noreturn to avoid unwanted climb out + if ( (_global_pos.alt < _pos_sp_triplet.current.alt + landingslope.flare_relative_alt()) || land_noreturn_vertical) { //checking for land_noreturn to avoid unwanted climb out /* land with minimal speed */ @@ -912,12 +912,12 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } - float flare_curve_alt = landingslope.getFlareCurveAltitude(wp_distance, _pos_sp_triplet.current.altitude); + float flare_curve_alt = landingslope.getFlareCurveAltitude(wp_distance, _pos_sp_triplet.current.alt); /* avoid climbout */ if (flare_curve_alt_last < flare_curve_alt && land_noreturn_vertical || land_stayonground) { - flare_curve_alt = pos_sp_triplet.current.altitude; + flare_curve_alt = pos_sp_triplet.current.alt; land_stayonground = true; } @@ -1009,7 +1009,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi if (altitude_error > 15.0f) { /* enforce a minimum of 10 degrees pitch up on takeoff, or take parameter */ - _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _pos_sp_triplet.current.altitude, calculate_target_airspeed(1.3f * _parameters.airspeed_min), + _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _pos_sp_triplet.current.alt, calculate_target_airspeed(1.3f * _parameters.airspeed_min), _airspeed.indicated_airspeed_m_s, eas2tas, true, math::max(math::radians(pos_sp_triplet.current.pitch_min), math::radians(10.0f)), _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, @@ -1020,7 +1020,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } else { - _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _pos_sp_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_trim), + _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _pos_sp_triplet.current.alt, calculate_target_airspeed(_parameters.airspeed_trim), _airspeed.indicated_airspeed_m_s, eas2tas, false, math::radians(_parameters.pitch_limit_min), _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, @@ -1042,7 +1042,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi _loiter_hold = false; /* reset land state */ - if (pos_sp_triplet.current.nav_cmd != NAV_CMD_LAND) { + if (pos_sp_triplet.current.type != SETPOINT_TYPE_LAND) { land_noreturn_horizontal = false; land_noreturn_vertical = false; land_stayonground = false; @@ -1051,7 +1051,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } /* reset takeoff/launch state */ - if (pos_sp_triplet.current.nav_cmd != NAV_CMD_TAKEOFF) { + if (pos_sp_triplet.current.type != SETPOINT_TYPE_TAKEOFF) { launch_detected = false; usePreTakeoffThrust = false; } @@ -1283,7 +1283,7 @@ FixedwingPositionControl::task_main() } /* XXX check if radius makes sense here */ - float turn_distance = _l1_control.switch_distance(_pos_sp_triplet.current.acceptance_radius); + float turn_distance = _l1_control.switch_distance(100.0f); /* lazily publish navigation capabilities */ if (turn_distance != _nav_capabilities.turn_distance && turn_distance > 0) { diff --git a/src/modules/mavlink/orb_listener.c b/src/modules/mavlink/orb_listener.c index e491911d3e..0b8ac6d3db 100644 --- a/src/modules/mavlink/orb_listener.c +++ b/src/modules/mavlink/orb_listener.c @@ -411,22 +411,17 @@ void l_global_position_setpoint(const struct listener *l) { struct position_setpoint_triplet_s triplet; - orb_copy(ORB_ID(mission_item_triplet), mavlink_subs.triplet_sub, &triplet); + orb_copy(ORB_ID(position_setpoint_triplet), mavlink_subs.triplet_sub, &triplet); - uint8_t coordinate_frame = MAV_FRAME_GLOBAL; - - if (!triplet.current_valid) + if (!triplet.current.valid) return; - if (triplet.current.altitude_is_relative) - coordinate_frame = MAV_FRAME_GLOBAL_RELATIVE_ALT; - if (gcs_link) mavlink_msg_global_position_setpoint_int_send(MAVLINK_COMM_0, - coordinate_frame, + MAV_FRAME_GLOBAL, (int32_t)(triplet.current.lat * 1e7d), (int32_t)(triplet.current.lon * 1e7d), - (int32_t)(triplet.current.altitude * 1e3f), + (int32_t)(triplet.current.alt * 1e3f), (int16_t)(triplet.current.yaw * M_RAD_TO_DEG_F * 1e2f)); } @@ -804,7 +799,7 @@ uorb_receive_start(void) orb_set_interval(mavlink_subs.local_pos_sub, 1000); /* 1Hz active updates */ /* --- GLOBAL SETPOINT VALUE --- */ - mavlink_subs.triplet_sub = orb_subscribe(ORB_ID(mission_item_triplet)); + mavlink_subs.triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet)); orb_set_interval(mavlink_subs.triplet_sub, 2000); /* 0.5 Hz updates */ /* --- LOCAL SETPOINT VALUE --- */ diff --git a/src/modules/mavlink/orb_topics.h b/src/modules/mavlink/orb_topics.h index 4d428406ac..30ba348cf6 100644 --- a/src/modules/mavlink/orb_topics.h +++ b/src/modules/mavlink/orb_topics.h @@ -51,9 +51,8 @@ #include #include #include -#include +#include #include -#include #include #include #include diff --git a/src/modules/mavlink_onboard/orb_topics.h b/src/modules/mavlink_onboard/orb_topics.h index 86bfa26f25..bbc9f6e66a 100644 --- a/src/modules/mavlink_onboard/orb_topics.h +++ b/src/modules/mavlink_onboard/orb_topics.h @@ -50,7 +50,7 @@ #include #include #include -#include +#include #include #include #include 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 bc20a4f470..d3e39e3a02 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -65,7 +65,7 @@ #include #include #include -#include +#include #include #include #include @@ -117,7 +117,7 @@ private: int _manual_sub; /**< notification of manual control updates */ int _arming_sub; /**< arming status of outputs */ int _local_pos_sub; /**< vehicle local position */ - int _pos_sp_triplet_sub; /**< mission item triplet */ + int _pos_sp_triplet_sub; /**< position setpoint triplet */ orb_advert_t _local_pos_sp_pub; /**< local position setpoint publication */ orb_advert_t _att_sp_pub; /**< attitude setpoint publication */ @@ -687,7 +687,7 @@ MulticopterPositionControl::task_main() if (!_control_mode.flag_control_manual_enabled) { /* use constant descend rate when landing, ignore altitude setpoint */ - if (_pos_sp_triplet.current_valid && _pos_sp_triplet.current.nav_cmd == NAV_CMD_LAND) { + if (_pos_sp_triplet.current.valid && _pos_sp_triplet.current.type == SETPOINT_TYPE_LAND) { _vel_sp(2) = _params.land_speed; } @@ -789,7 +789,7 @@ MulticopterPositionControl::task_main() float tilt = atan2f(math::Vector<2>(thrust_sp(0), thrust_sp(1)).length(), -thrust_sp(2)); float tilt_max = _params.tilt_max; if (!_control_mode.flag_control_manual_enabled) { - if (_pos_sp_triplet.current_valid && _pos_sp_triplet.current.nav_cmd == NAV_CMD_LAND) { + if (_pos_sp_triplet.current.valid && _pos_sp_triplet.current.type == SETPOINT_TYPE_LAND) { /* limit max tilt and min lift when landing */ tilt_max = _params.land_tilt_max; if (thr_min < 0.0f) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index dfd07d3158..ba51b024f6 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -972,10 +972,10 @@ Navigator::start_loiter() /* use current altitude if above min altitude set by parameter */ if (_global_pos.alt < min_alt_amsl) { - _pos_sp_triplet.current.altitude = min_alt_amsl; + _pos_sp_triplet.current.alt = min_alt_amsl; mavlink_log_info(_mavlink_fd, "[navigator] loiter %.1fm higher", (double)(min_alt_amsl - _global_pos.alt)); } else { - _pos_sp_triplet.current.altitude = _global_pos.alt; + _pos_sp_triplet.current.alt = _global_pos.alt; mavlink_log_info(_mavlink_fd, "[navigator] loiter at current altitude"); } @@ -987,6 +987,8 @@ Navigator::start_loiter() } } + _pos_sp_triplet.current.loiter_radius = _parameters.loiter_radius; + _pos_sp_triplet.current.loiter_direction = 1; _pos_sp_triplet.previous.valid = false; _pos_sp_triplet.current.valid = true; _pos_sp_triplet.next.valid = false; @@ -1052,7 +1054,7 @@ Navigator::set_mission_item() } /* check if we really need takeoff */ - if (_vstatus.condition_landed || _global_pos.alt < takeoff_alt_amsl - item.acceptance_radius) { + if (_vstatus.condition_landed || _global_pos.alt < takeoff_alt_amsl - _mission_item.acceptance_radius) { /* force TAKEOFF if landed or waypoint altitude is more than current */ _do_takeoff = true; @@ -1067,14 +1069,14 @@ Navigator::set_mission_item() _pos_sp_triplet.current.yaw = NAN; _pos_sp_triplet.current.type = SETPOINT_TYPE_TAKEOFF; } - } else if (item.nav_cmd == NAV_CMD_LAND) { + } else if (_mission_item.nav_cmd == NAV_CMD_LAND) { /* will need takeoff after landing */ _need_takeoff = true; } } if (_do_takeoff) { - mavlink_log_info(_mavlink_fd, "[navigator] takeoff to %.1fm AMSL", _pos_sp_triplet.current.altitude); + mavlink_log_info(_mavlink_fd, "[navigator] takeoff to %.1fm AMSL", _pos_sp_triplet.current.alt); } else { if (onboard) { mavlink_log_info(_mavlink_fd, "[navigator] heading to onboard WP %d", index); @@ -1207,7 +1209,7 @@ Navigator::set_rtl_item() _pos_sp_triplet.next.valid = false; - mavlink_log_info(_mavlink_fd, "[navigator] RTL: descend to %.1fm", descend_alt - _home_pos.altitude); + mavlink_log_info(_mavlink_fd, "[navigator] RTL: descend to %.1fm AMSL", _mission_item.altitude); break; } case RTL_STATE_LAND: { @@ -1258,9 +1260,12 @@ Navigator::position_setpoint_from_mission_item(position_setpoint_s *sp, mission_ } else { sp->lat = item->lat; sp->lon = item->lon; - sp->alt = item->altitude_is_relative ? item->alt + _home_pos.altitude : item->alt; + sp->alt = item->altitude_is_relative ? item->altitude + _home_pos.altitude : item->altitude; } sp->yaw = item->yaw; + sp->loiter_radius = item->loiter_radius; + sp->loiter_direction = item->loiter_direction; + sp->pitch_min = item->pitch_min; if (item->nav_cmd == NAV_CMD_TAKEOFF) { sp->type = SETPOINT_TYPE_TAKEOFF; } else if (item->nav_cmd == NAV_CMD_LAND) { @@ -1320,7 +1325,7 @@ Navigator::check_mission_item_reached() /* current relative or AMSL altitude depending on mission item altitude_is_relative flag */ float wp_alt_amsl = _mission_item.altitude; if (_mission_item.altitude_is_relative) - _mission_itemt.altitude += _home_pos.altitude; + wp_alt_amsl += _home_pos.altitude; dist = get_distance_to_point_global_wgs84(_mission_item.lat, _mission_item.lon, wp_alt_amsl, (double)_global_pos.lat / 1e7d, (double)_global_pos.lon / 1e7d, _global_pos.alt, diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index de510e0223..46f8ed8278 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -73,7 +73,7 @@ #include #include #include -#include +#include #include #include #include @@ -908,7 +908,7 @@ int sdlog2_thread_main(int argc, char *argv[]) fdsc_count++; /* --- GLOBAL POSITION SETPOINT--- */ - subs.triplet_sub = orb_subscribe(ORB_ID(mission_item_triplet)); + subs.triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet)); fds[fdsc_count].fd = subs.triplet_sub; fds[fdsc_count].events = POLLIN; fdsc_count++; @@ -1263,18 +1263,15 @@ int sdlog2_thread_main(int argc, char *argv[]) /* --- GLOBAL POSITION SETPOINT --- */ if (fds[ifds++].revents & POLLIN) { - orb_copy(ORB_ID(mission_item_triplet), subs.triplet_sub, &buf.triplet); + orb_copy(ORB_ID(position_setpoint_triplet), subs.triplet_sub, &buf.triplet); log_msg.msg_type = LOG_GPSP_MSG; - log_msg.body.log_GPSP.altitude_is_relative = buf.triplet.current.altitude_is_relative; log_msg.body.log_GPSP.lat = (int32_t)(buf.triplet.current.lat * 1e7d); log_msg.body.log_GPSP.lon = (int32_t)(buf.triplet.current.lon * 1e7d); - log_msg.body.log_GPSP.altitude = buf.triplet.current.altitude; + log_msg.body.log_GPSP.alt = buf.triplet.current.alt; log_msg.body.log_GPSP.yaw = buf.triplet.current.yaw; - log_msg.body.log_GPSP.nav_cmd = buf.triplet.current.nav_cmd; + log_msg.body.log_GPSP.type = buf.triplet.current.type; log_msg.body.log_GPSP.loiter_radius = buf.triplet.current.loiter_radius; log_msg.body.log_GPSP.loiter_direction = buf.triplet.current.loiter_direction; - log_msg.body.log_GPSP.acceptance_radius = buf.triplet.current.acceptance_radius; - log_msg.body.log_GPSP.time_inside = buf.triplet.current.time_inside; log_msg.body.log_GPSP.pitch_min = buf.triplet.current.pitch_min; LOGBUFFER_WRITE_AND_COUNT(GPSP); } diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 503dc0afca..98736dd211 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -210,16 +210,13 @@ struct log_GPOS_s { /* --- GPSP - GLOBAL POSITION SETPOINT --- */ #define LOG_GPSP_MSG 17 struct log_GPSP_s { - uint8_t altitude_is_relative; int32_t lat; int32_t lon; - float altitude; + float alt; float yaw; - uint8_t nav_cmd; + uint8_t type; float loiter_radius; int8_t loiter_direction; - float acceptance_radius; - float time_inside; float pitch_min; }; @@ -307,7 +304,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(ARSP, "fff", "RollRateSP,PitchRateSP,YawRateSP"), LOG_FORMAT(FLOW, "hhfffBB", "RawX,RawY,CompX,CompY,Dist,Q,SensID"), LOG_FORMAT(GPOS, "LLffff", "Lat,Lon,Alt,VelN,VelE,VelD"), - LOG_FORMAT(GPSP, "BLLffBfbfff", "AltRel,Lat,Lon,Alt,Yaw,NavCmd,LoitR,LoitDir,AccR,TimeIn,PitMin"), + LOG_FORMAT(GPSP, "LLffBfbf", "Lat,Lon,Alt,Yaw,Type,LoitR,LoitDir,PitMin"), LOG_FORMAT(ESC, "HBBBHHHHHHfH", "Counter,NumESC,Conn,N,Ver,Adr,Volt,Amp,RPM,Temp,SetP,SetPRAW"), LOG_FORMAT(GVSP, "fff", "VX,VY,VZ"), LOG_FORMAT(BATT, "ffff", "V,VFilt,C,Discharged"), diff --git a/src/modules/uORB/topics/position_setpoint_triplet.h b/src/modules/uORB/topics/position_setpoint_triplet.h index a8bd6b8e38..4b57833b63 100644 --- a/src/modules/uORB/topics/position_setpoint_triplet.h +++ b/src/modules/uORB/topics/position_setpoint_triplet.h @@ -61,12 +61,15 @@ enum SETPOINT_TYPE struct position_setpoint_s { - bool valid; /**< true if point is valid */ + bool valid; /**< true if setpoint is valid */ + enum SETPOINT_TYPE type; /**< setpoint type to adjust behavior of position controller */ double lat; /**< latitude, in deg */ double lon; /**< longitude, in deg */ float alt; /**< altitude AMSL, in m */ - float yaw; /**< yaw (only for multirotors), in rad [-PI..PI), NaN to hold current yaw */ - enum SETPOINT_TYPE type; /**< setpoint type to adjust behavior of position controller */ + float yaw; /**< yaw (only for multirotors), in rad [-PI..PI), NaN = hold current yaw */ + float loiter_radius; /**< loiter radius (only for fixed wing), in m */ + int8_t loiter_direction; /**< loiter direction: 1 = CW, -1 = CCW */ + float pitch_min; /**< minimal pitch angle for fixed wing takeoff waypoints */ }; /** From 55f845888bece8aaa49ebc2b5e25663b6c8f3f3c Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 23 Jan 2014 22:41:56 +0100 Subject: [PATCH 336/447] px4fmu-v2_test makefile fixed, CMSIS added --- makefiles/config_px4fmu-v2_test.mk | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/makefiles/config_px4fmu-v2_test.mk b/makefiles/config_px4fmu-v2_test.mk index d5a50cfae8..8623c0584b 100644 --- a/makefiles/config_px4fmu-v2_test.mk +++ b/makefiles/config_px4fmu-v2_test.mk @@ -32,6 +32,11 @@ MODULES += modules/systemlib MODULES += modules/systemlib/mixer MODULES += modules/uORB +# +# Libraries +# +LIBRARIES += lib/mathlib/CMSIS + # # Transitional support - add commands from the NuttX export archive. # From 58792c5ca6e42bc251dd3c92b0e79217ff5d5403 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 24 Jan 2014 00:06:10 +0100 Subject: [PATCH 337/447] Use double for lat/lon in vehicle_global_position topic, use filed names lat, lon, alt, vel_n, vel_e, vel_d for global positions --- src/drivers/frsky_telemetry/frsky_data.c | 6 +-- src/examples/fixedwing_control/main.c | 6 +-- .../att_pos_estimator_ekf/KalmanNav.cpp | 7 ++-- .../attitude_estimator_ekf_main.cpp | 6 +-- src/modules/commander/commander.cpp | 10 ++--- src/modules/fixedwing_backside/fixedwing.cpp | 8 ++-- .../fixedwing_pos_control_main.c | 2 +- .../fw_att_control/fw_att_control_main.cpp | 6 +-- .../fw_pos_control_l1_main.cpp | 2 +- src/modules/mavlink/mavlink_receiver.cpp | 6 +-- src/modules/mavlink/orb_listener.c | 21 +++++----- src/modules/navigator/navigator_main.cpp | 40 +++++++++---------- .../position_estimator_inav_main.c | 14 +++---- src/modules/sdlog2/sdlog2.c | 10 ++--- src/modules/uORB/topics/home_position.h | 2 +- .../uORB/topics/vehicle_global_position.h | 17 ++++---- 16 files changed, 76 insertions(+), 87 deletions(-) diff --git a/src/drivers/frsky_telemetry/frsky_data.c b/src/drivers/frsky_telemetry/frsky_data.c index 63b2d2d29f..e201ecbb3d 100644 --- a/src/drivers/frsky_telemetry/frsky_data.c +++ b/src/drivers/frsky_telemetry/frsky_data.c @@ -230,11 +230,11 @@ void frsky_send_frame2(int uart) struct tm *tm_gps = gmtime(&time_gps); course = (global_pos.yaw + M_PI_F) / M_PI_F * 180.0f; - lat = frsky_format_gps(abs(global_pos.lat) / 10000000.0f); + lat = frsky_format_gps(abs(global_pos.lat)); lat_ns = (global_pos.lat < 0) ? 'S' : 'N'; - lon = frsky_format_gps(abs(global_pos.lon) / 10000000.0f); + lon = frsky_format_gps(abs(global_pos.lon)); lon_ew = (global_pos.lon < 0) ? 'W' : 'E'; - speed = sqrtf(global_pos.vx * global_pos.vx + global_pos.vy * global_pos.vy) + speed = sqrtf(global_pos.vel_n * global_pos.vel_n + global_pos.vel_e * global_pos.vel_e) * 25.0f / 46.0f; alt = global_pos.alt; sec = tm_gps->tm_sec; diff --git a/src/examples/fixedwing_control/main.c b/src/examples/fixedwing_control/main.c index b286e00077..067d773649 100644 --- a/src/examples/fixedwing_control/main.c +++ b/src/examples/fixedwing_control/main.c @@ -181,11 +181,7 @@ void control_heading(const struct vehicle_global_position_s *pos, const struct v * Calculate heading error of current position to desired position */ - /* - * PX4 uses 1e7 scaled integers to represent global coordinates for max resolution, - * so they need to be scaled by 1e7 and converted to IEEE double precision floating point. - */ - float bearing = get_bearing_to_next_waypoint(pos->lat/1e7d, pos->lon/1e7d, sp->lat/1e7d, sp->lon/1e7d); + float bearing = get_bearing_to_next_waypoint(pos->lat, pos->lon, sp->lat, sp->lon); /* calculate heading error */ float yaw_err = att->yaw - bearing; diff --git a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp b/src/modules/att_pos_estimator_ekf/KalmanNav.cpp index aca3fe7b67..8e88130e15 100644 --- a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp +++ b/src/modules/att_pos_estimator_ekf/KalmanNav.cpp @@ -318,10 +318,9 @@ void KalmanNav::updatePublications() _pos.lat = getLatDegE7(); _pos.lon = getLonDegE7(); _pos.alt = float(alt); - _pos.relative_alt = float(alt); // TODO, make relative - _pos.vx = vN; - _pos.vy = vE; - _pos.vz = vD; + _pos.vel_n = vN; + _pos.vel_e = vE; + _pos.vel_d = vD; _pos.yaw = psi; // local position publication diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp index 6a1bec1531..66ec20b954 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp @@ -414,9 +414,9 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds vel_valid = true; if (global_pos_updated) { vel_t = global_pos.timestamp; - vel(0) = global_pos.vx; - vel(1) = global_pos.vy; - vel(2) = global_pos.vz; + vel(0) = global_pos.vel_n; + vel(1) = global_pos.vel_e; + vel(2) = global_pos.vel_d; } } diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 47053838cf..d51bb63ffb 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1009,12 +1009,12 @@ int commander_thread_main(int argc, char *argv[]) /* copy position data to uORB home message, store it locally as well */ - home.lat = (double)global_position.lat / 1e7d; - home.lon = (double)global_position.lon / 1e7d; - home.altitude = (float)global_position.alt; + home.lat = global_position.lat; + home.lon = global_position.lon; + home.alt = global_position.alt; - warnx("home: lat = %.7f, lon = %.7f, alt = %.4f ", home.lat, home.lon, (double)home.altitude); - mavlink_log_info(mavlink_fd, "[cmd] home: %.7f, %.7f, %.4f", home.lat, home.lon, (double)home.altitude); + warnx("home: lat = %.7f, lon = %.7f, alt = %.4f ", home.lat, home.lon, (double)home.alt); + mavlink_log_info(mavlink_fd, "[cmd] home: %.7f, %.7f, %.4f", home.lat, home.lon, (double)home.alt); /* announce new home position */ if (home_pub > 0) { diff --git a/src/modules/fixedwing_backside/fixedwing.cpp b/src/modules/fixedwing_backside/fixedwing.cpp index 108e9896d6..f7c0b61488 100644 --- a/src/modules/fixedwing_backside/fixedwing.cpp +++ b/src/modules/fixedwing_backside/fixedwing.cpp @@ -174,9 +174,9 @@ void BlockMultiModeBacksideAutopilot::update() // of control we will limit the velocity feedback between // the min/max velocity float v = _vLimit.update(sqrtf( - _pos.vx * _pos.vx + + _pos.vel_n * _pos.vel_n + _pos.vy * _pos.vy + - _pos.vz * _pos.vz)); + _pos.vel_d * _pos.vel_d)); // limit velocity command between min/max velocity float vCmd = _vLimit.update(_vCmd.get()); @@ -236,9 +236,9 @@ void BlockMultiModeBacksideAutopilot::update() // for the purpose of control we will limit the velocity feedback between // the min/max velocity float v = _vLimit.update(sqrtf( - _pos.vx * _pos.vx + + _pos.vel_n * _pos.vel_n + _pos.vy * _pos.vy + - _pos.vz * _pos.vz)); + _pos.vel_d * _pos.vel_d)); // pitch channel -> rate of climb // TODO, might want to put a gain on this, otherwise commanding diff --git a/src/modules/fixedwing_pos_control/fixedwing_pos_control_main.c b/src/modules/fixedwing_pos_control/fixedwing_pos_control_main.c index 73df3fb9e0..888dd09423 100644 --- a/src/modules/fixedwing_pos_control/fixedwing_pos_control_main.c +++ b/src/modules/fixedwing_pos_control/fixedwing_pos_control_main.c @@ -299,7 +299,7 @@ int fixedwing_pos_control_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(vehicle_global_position_setpoint), global_setpoint_sub, &global_setpoint); start_pos = global_pos; //for now using the current position as the startpoint (= approx. last waypoint because the setpoint switch occurs at the waypoint) global_sp_updated_set_once = true; - psi_track = get_bearing_to_next_waypoint((double)global_pos.lat / (double)1e7d, (double)global_pos.lon / (double)1e7d, + psi_track = get_bearing_to_next_waypoint(global_pos.lat, global_pos.lon, (double)global_setpoint.lat / (double)1e7d, (double)global_setpoint.lon / (double)1e7d); printf("next wp direction: %0.4f\n", (double)psi_track); diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index e49b3c1405..dc2196de6d 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -704,9 +704,9 @@ FixedwingAttitudeControl::task_main() float speed_body_v = 0.0f; float speed_body_w = 0.0f; if(_att.R_valid) { - speed_body_u = _att.R[0][0] * _global_pos.vx + _att.R[1][0] * _global_pos.vy + _att.R[2][0] * _global_pos.vz; - speed_body_v = _att.R[0][1] * _global_pos.vx + _att.R[1][1] * _global_pos.vy + _att.R[2][1] * _global_pos.vz; - speed_body_w = _att.R[0][2] * _global_pos.vx + _att.R[1][2] * _global_pos.vy + _att.R[2][2] * _global_pos.vz; + speed_body_u = _att.R[0][0] * _global_pos.vel_n + _att.R[1][0] * _global_pos.vel_e + _att.R[2][0] * _global_pos.vel_d; + speed_body_v = _att.R[0][1] * _global_pos.vel_n + _att.R[1][1] * _global_pos.vel_e + _att.R[2][1] * _global_pos.vel_d; + speed_body_w = _att.R[0][2] * _global_pos.vel_n + _att.R[1][2] * _global_pos.vel_e + _att.R[2][2] * _global_pos.vel_d; } else { warnx("Did not get a valid R\n"); } diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index f3d688646d..a62b53221c 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -1262,7 +1262,7 @@ FixedwingPositionControl::task_main() vehicle_airspeed_poll(); // vehicle_baro_poll(); - math::Vector<2> ground_speed(_global_pos.vx, _global_pos.vy); + math::Vector<2> ground_speed(_global_pos.vel_n, _global_pos.vel_e); math::Vector<2> current_position(_global_pos.lat / 1e7f, _global_pos.lon / 1e7f); /* diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 4f9c718d2c..7c23488d7e 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -638,9 +638,9 @@ handle_message(mavlink_message_t *msg) hil_global_pos.lat = hil_state.lat; hil_global_pos.lon = hil_state.lon; hil_global_pos.alt = hil_state.alt / 1000.0f; - hil_global_pos.vx = hil_state.vx / 100.0f; - hil_global_pos.vy = hil_state.vy / 100.0f; - hil_global_pos.vz = hil_state.vz / 100.0f; + hil_global_pos.vel_n = hil_state.vx / 100.0f; + hil_global_pos.vel_e = hil_state.vy / 100.0f; + hil_global_pos.vel_d = hil_state.vz / 100.0f; } else { pub_hil_global_pos = orb_advertise(ORB_ID(vehicle_global_position), &hil_global_pos); diff --git a/src/modules/mavlink/orb_listener.c b/src/modules/mavlink/orb_listener.c index 0b8ac6d3db..7f6237535e 100644 --- a/src/modules/mavlink/orb_listener.c +++ b/src/modules/mavlink/orb_listener.c @@ -67,6 +67,7 @@ extern bool gcs_link; struct vehicle_global_position_s global_pos; struct vehicle_local_position_s local_pos; +struct home_position_s home; struct navigation_capabilities_s nav_cap; struct vehicle_status_s v_status; struct vehicle_control_mode_s control_mode; @@ -247,10 +248,10 @@ l_vehicle_attitude(const struct listener *l) hrt_abstime t = hrt_absolute_time(); if (t >= last_sent_vfr + 100000) { last_sent_vfr = t; - float groundspeed = sqrtf(global_pos.vx * global_pos.vx + global_pos.vy * global_pos.vy); + float groundspeed = sqrtf(global_pos.vel_n * global_pos.vel_n + global_pos.vel_e * global_pos.vel_e); uint16_t heading = _wrap_2pi(att.yaw) * M_RAD_TO_DEG_F; float throttle = armed.armed ? actuators_0.control[3] * 100.0f : 0.0f; - mavlink_msg_vfr_hud_send(MAVLINK_COMM_0, airspeed.true_airspeed_m_s, groundspeed, heading, throttle, global_pos.alt, -global_pos.vz); + mavlink_msg_vfr_hud_send(MAVLINK_COMM_0, airspeed.true_airspeed_m_s, groundspeed, heading, throttle, global_pos.alt, -global_pos.vel_d); } /* send quaternion values if it exists */ @@ -380,13 +381,13 @@ l_global_position(const struct listener *l) mavlink_msg_global_position_int_send(MAVLINK_COMM_0, global_pos.timestamp / 1000, - global_pos.lat, - global_pos.lon, + global_pos.lat * 1e7, + global_pos.lon * 1e7, global_pos.alt * 1000.0f, - global_pos.relative_alt * 1000.0f, - global_pos.vx * 100.0f, - global_pos.vy * 100.0f, - global_pos.vz * 100.0f, + (global_pos.alt - home.alt) * 1000.0f, + global_pos.vel_n * 100.0f, + global_pos.vel_e * 100.0f, + global_pos.vel_d * 100.0f, _wrap_2pi(global_pos.yaw) * M_RAD_TO_DEG_F * 100.0f); } @@ -657,11 +658,9 @@ l_optical_flow(const struct listener *l) void l_home(const struct listener *l) { - struct home_position_s home; - orb_copy(ORB_ID(home_position), mavlink_subs.home_sub, &home); - mavlink_msg_gps_global_origin_send(MAVLINK_COMM_0, (int32_t)(home.lat*1e7d), (int32_t)(home.lon*1e7d), (int32_t)(home.altitude)*1e3f); + mavlink_msg_gps_global_origin_send(MAVLINK_COMM_0, (int32_t)(home.lat*1e7d), (int32_t)(home.lon*1e7d), (int32_t)(home.alt)*1e3f); } void diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index ba51b024f6..cfcc886b68 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -833,11 +833,11 @@ Navigator::status() { warnx("Global position is %svalid", _global_pos.valid ? "" : "in"); if (_global_pos.valid) { - warnx("Longitude %5.5f degrees, latitude %5.5f degrees", _global_pos.lon / 1e7d, _global_pos.lat / 1e7d); + warnx("Longitude %5.5f degrees, latitude %5.5f degrees", _global_pos.lon, _global_pos.lat); warnx("Altitude %5.5f meters, altitude above home %5.5f meters", - (double)_global_pos.alt, (double)_global_pos.relative_alt); - warnx("Ground velocity in m/s, x %5.5f, y %5.5f, z %5.5f", - (double)_global_pos.vx, (double)_global_pos.vy, (double)_global_pos.vz); + (double)_global_pos.alt, (double)(_global_pos.alt - _home_pos.alt)); + warnx("Ground velocity in m/s, N %5.5f, E %5.5f, D %5.5f", + (double)_global_pos.vel_n, (double)_global_pos.vel_e, (double)_global_pos.vel_d); warnx("Compass heading in degrees %5.5f", (double)(_global_pos.yaw * M_RAD_TO_DEG_F)); } if (_fence_valid) { @@ -964,11 +964,11 @@ Navigator::start_loiter() if (_reset_loiter_pos || !_pos_sp_triplet.current.valid) { _reset_loiter_pos = false; - _pos_sp_triplet.current.lat = (double)_global_pos.lat / 1e7d; - _pos_sp_triplet.current.lon = (double)_global_pos.lon / 1e7d; + _pos_sp_triplet.current.lat = _global_pos.lat; + _pos_sp_triplet.current.lon = _global_pos.lon; _pos_sp_triplet.current.yaw = NAN; // NAN means to use current yaw - float min_alt_amsl = _parameters.min_altitude + _home_pos.altitude; + float min_alt_amsl = _parameters.min_altitude + _home_pos.alt; /* use current altitude if above min altitude set by parameter */ if (_global_pos.alt < min_alt_amsl) { @@ -1063,8 +1063,8 @@ Navigator::set_mission_item() /* set current setpoint to takeoff */ - _pos_sp_triplet.current.lat = (double)_global_pos.lat / 1e7d; - _pos_sp_triplet.current.lon = (double)_global_pos.lon / 1e7d; + _pos_sp_triplet.current.lat = _global_pos.lat; + _pos_sp_triplet.current.lon = _global_pos.lon; _pos_sp_triplet.current.alt = takeoff_alt_amsl; _pos_sp_triplet.current.yaw = NAN; _pos_sp_triplet.current.type = SETPOINT_TYPE_TAKEOFF; @@ -1111,7 +1111,7 @@ Navigator::start_rtl() { _do_takeoff = false; if (_rtl_state == RTL_STATE_NONE) { - if (_global_pos.alt < _home_pos.altitude + _parameters.rtl_alt) { + if (_global_pos.alt < _home_pos.alt + _parameters.rtl_alt) { _rtl_state = RTL_STATE_CLIMB; } else { _rtl_state = RTL_STATE_RETURN; @@ -1133,15 +1133,15 @@ Navigator::set_rtl_item() case RTL_STATE_CLIMB: { memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s)); - float climb_alt = _home_pos.altitude + _parameters.rtl_alt; + float climb_alt = _home_pos.alt + _parameters.rtl_alt; if (_vstatus.condition_landed) { climb_alt = fmaxf(climb_alt, _global_pos.alt + _parameters.rtl_alt); } _mission_item_valid = true; - _mission_item.lat = (double)_global_pos.lat / 1e7d; - _mission_item.lon = (double)_global_pos.lon / 1e7d; + _mission_item.lat = _global_pos.lat; + _mission_item.lon = _global_pos.lon; _mission_item.altitude_is_relative = false; _mission_item.altitude = climb_alt; _mission_item.yaw = NAN; @@ -1158,7 +1158,7 @@ Navigator::set_rtl_item() _pos_sp_triplet.next.valid = false; - mavlink_log_info(_mavlink_fd, "[navigator] RTL: climb to %.1fm", climb_alt - _home_pos.altitude); + mavlink_log_info(_mavlink_fd, "[navigator] RTL: climb to %.1fm", climb_alt - _home_pos.alt); break; } case RTL_STATE_RETURN: { @@ -1194,7 +1194,7 @@ Navigator::set_rtl_item() _mission_item.lat = _home_pos.lat; _mission_item.lon = _home_pos.lon; _mission_item.altitude_is_relative = false; - _mission_item.altitude = _home_pos.altitude + _parameters.land_alt; + _mission_item.altitude = _home_pos.alt + _parameters.land_alt; _mission_item.yaw = NAN; _mission_item.loiter_radius = _parameters.loiter_radius; _mission_item.loiter_direction = 1; @@ -1220,7 +1220,7 @@ Navigator::set_rtl_item() _mission_item.lat = _home_pos.lat; _mission_item.lon = _home_pos.lon; _mission_item.altitude_is_relative = false; - _mission_item.altitude = _home_pos.altitude; + _mission_item.altitude = _home_pos.alt; _mission_item.yaw = NAN; _mission_item.loiter_radius = _parameters.loiter_radius; _mission_item.loiter_direction = 1; @@ -1256,11 +1256,11 @@ Navigator::position_setpoint_from_mission_item(position_setpoint_s *sp, mission_ /* set home position for RTL item */ sp->lat = _home_pos.lat; sp->lon = _home_pos.lon; - sp->alt = _home_pos.altitude + _parameters.rtl_alt; + sp->alt = _home_pos.alt + _parameters.rtl_alt; } else { sp->lat = item->lat; sp->lon = item->lon; - sp->alt = item->altitude_is_relative ? item->altitude + _home_pos.altitude : item->altitude; + sp->alt = item->altitude_is_relative ? item->altitude + _home_pos.alt : item->altitude; } sp->yaw = item->yaw; sp->loiter_radius = item->loiter_radius; @@ -1325,10 +1325,10 @@ Navigator::check_mission_item_reached() /* current relative or AMSL altitude depending on mission item altitude_is_relative flag */ float wp_alt_amsl = _mission_item.altitude; if (_mission_item.altitude_is_relative) - wp_alt_amsl += _home_pos.altitude; + wp_alt_amsl += _home_pos.alt; dist = get_distance_to_point_global_wgs84(_mission_item.lat, _mission_item.lon, wp_alt_amsl, - (double)_global_pos.lat / 1e7d, (double)_global_pos.lon / 1e7d, _global_pos.alt, + (double)_global_pos.lat, (double)_global_pos.lon, _global_pos.alt, &dist_xy, &dist_z); if (_do_takeoff) { diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index 02fa6a8f28..af04bb0bc4 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -840,19 +840,15 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (local_pos.xy_global) { double est_lat, est_lon; map_projection_reproject(local_pos.x, local_pos.y, &est_lat, &est_lon); - global_pos.lat = (int32_t)(est_lat * 1e7d); - global_pos.lon = (int32_t)(est_lon * 1e7d); + global_pos.lat = est_lat; + global_pos.lon = est_lon; global_pos.time_gps_usec = gps.time_gps_usec; } /* set valid values even if position is not valid */ if (local_pos.v_xy_valid) { - global_pos.vx = local_pos.vx; - global_pos.vy = local_pos.vy; - } - - if (local_pos.z_valid) { - global_pos.relative_alt = -local_pos.z; + global_pos.vel_n = local_pos.vx; + global_pos.vel_e = local_pos.vy; } if (local_pos.z_global) { @@ -860,7 +856,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } if (local_pos.v_z_valid) { - global_pos.vz = local_pos.vz; + global_pos.vel_d = local_pos.vz; } global_pos.yaw = local_pos.yaw; diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 46f8ed8278..9bac2958e9 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1252,12 +1252,12 @@ int sdlog2_thread_main(int argc, char *argv[]) if (fds[ifds++].revents & POLLIN) { orb_copy(ORB_ID(vehicle_global_position), subs.global_pos_sub, &buf.global_pos); log_msg.msg_type = LOG_GPOS_MSG; - log_msg.body.log_GPOS.lat = buf.global_pos.lat; - log_msg.body.log_GPOS.lon = buf.global_pos.lon; + log_msg.body.log_GPOS.lat = buf.global_pos.lat * 1e7; + log_msg.body.log_GPOS.lon = buf.global_pos.lon * 1e7; log_msg.body.log_GPOS.alt = buf.global_pos.alt; - log_msg.body.log_GPOS.vel_n = buf.global_pos.vx; - log_msg.body.log_GPOS.vel_e = buf.global_pos.vy; - log_msg.body.log_GPOS.vel_d = buf.global_pos.vz; + log_msg.body.log_GPOS.vel_n = buf.global_pos.vel_n; + log_msg.body.log_GPOS.vel_e = buf.global_pos.vel_e; + log_msg.body.log_GPOS.vel_d = buf.global_pos.vel_d; LOGBUFFER_WRITE_AND_COUNT(GPOS); } diff --git a/src/modules/uORB/topics/home_position.h b/src/modules/uORB/topics/home_position.h index 3e2fee84e9..08d11abaea 100644 --- a/src/modules/uORB/topics/home_position.h +++ b/src/modules/uORB/topics/home_position.h @@ -62,7 +62,7 @@ struct home_position_s //bool altitude_is_relative; // TODO what means home relative altitude? we need clear definition of reference altitude then double lat; /**< Latitude in degrees */ double lon; /**< Longitude in degrees */ - float altitude; /**< Altitude in meters */ + float alt; /**< Altitude in meters */ }; /** diff --git a/src/modules/uORB/topics/vehicle_global_position.h b/src/modules/uORB/topics/vehicle_global_position.h index 143734e372..ae771ca008 100644 --- a/src/modules/uORB/topics/vehicle_global_position.h +++ b/src/modules/uORB/topics/vehicle_global_position.h @@ -54,7 +54,7 @@ /** * Fused global position in WGS84. * - * This struct contains the system's believ about its position. It is not the raw GPS + * This struct contains global position estimation. It is not the raw GPS * measurement (@see vehicle_gps_position). This topic is usually published by the position * 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. @@ -65,14 +65,13 @@ struct vehicle_global_position_s uint64_t time_gps_usec; /**< GPS timestamp in microseconds */ bool valid; /**< true if position satisfies validity criteria of estimator */ - int32_t lat; /**< Latitude in 1E7 degrees */ - int32_t lon; /**< Longitude in 1E7 degrees */ - float alt; /**< Altitude in meters */ - float relative_alt; /**< Altitude above home position in meters, */ - float vx; /**< Ground X velocity, m/s in NED */ - float vy; /**< Ground Y velocity, m/s in NED */ - float vz; /**< Ground Z velocity, m/s in NED */ - float yaw; /**< Compass heading in radians -PI..+PI. */ + double lat; /**< Latitude in degrees */ + double lon; /**< Longitude in degrees */ + float alt; /**< Altitude in meters */ + float vel_n; /**< Ground north velocity, m/s */ + float vel_e; /**< Ground east velocity, m/s */ + float vel_d; /**< Ground downside velocity, m/s */ + float yaw; /**< Yaw in radians -PI..+PI. */ }; /** From 06227331ea053d8cc297ecd1bf7afd1da1677c39 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 24 Jan 2014 18:21:28 +0100 Subject: [PATCH 338/447] Checking out registers page state from master, as this is clearly a symptom of a bad merge --- src/modules/px4iofirmware/registers.c | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index 19fc61f78b..2c437d2c0c 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -89,7 +89,9 @@ uint16_t r_page_status[] = { [PX4IO_P_STATUS_IBATT] = 0, [PX4IO_P_STATUS_VSERVO] = 0, [PX4IO_P_STATUS_VRSSI] = 0, - [PX4IO_P_STATUS_PRSSI] = 0 + [PX4IO_P_STATUS_PRSSI] = 0, + [PX4IO_P_STATUS_NRSSI] = 0, + [PX4IO_P_STATUS_RC_DATA] = 0 }; /** @@ -600,7 +602,7 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) if (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] == UINT8_MAX) { disabled = true; - } else if (REG_TO_SIGNED(conf[PX4IO_P_RC_CONFIG_ASSIGNMENT]) < 0 || conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] >= PX4IO_RC_MAPPED_CONTROL_CHANNELS) { + } else if ((int)(conf[PX4IO_P_RC_CONFIG_ASSIGNMENT]) < 0 || conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] >= PX4IO_RC_MAPPED_CONTROL_CHANNELS) { count++; } From ebc7cb03b726ebfb864e770a82b92bb67b6bfd4c Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 25 Jan 2014 23:24:12 +0100 Subject: [PATCH 339/447] =?UTF-8?q?=C2=ABflighttermination=20state=C2=BB?= =?UTF-8?q?=20replaced=20by=20more=20general=20=C2=ABfailsafe=20state?= =?UTF-8?q?=C2=BB?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/modules/commander/commander.cpp | 39 ++- .../commander/state_machine_helper.cpp | 60 ++--- src/modules/commander/state_machine_helper.h | 4 +- .../fw_att_control/fw_att_control_main.cpp | 2 +- src/modules/navigator/navigator_main.cpp | 252 ++++++++++-------- .../uORB/topics/vehicle_control_mode.h | 2 +- src/modules/uORB/topics/vehicle_status.h | 11 +- 7 files changed, 206 insertions(+), 164 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index d51bb63ffb..7715f73e07 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -512,7 +512,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe case VEHICLE_CMD_DO_SET_SERVO: { //xxx: needs its own mavlink command if (armed->armed && cmd->param3 > 0.5) { //xxx: for safety only for now, param3 is unused by VEHICLE_CMD_DO_SET_SERVO - transition_result_t flighttermination_res = flighttermination_state_transition(status, FLIGHTTERMINATION_STATE_ON); + transition_result_t failsafe_res = failsafe_state_transition(status, FAILSAFE_STATE_TERMINATION); result = VEHICLE_CMD_RESULT_ACCEPTED; ret = true; @@ -1112,6 +1112,14 @@ int commander_thread_main(int argc, char *argv[]) mavlink_log_critical(mavlink_fd, "#audio: ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch); } + if (status.failsafe_state != FAILSAFE_STATE_NORMAL) { + /* recover from failsafe */ + transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_NORMAL); + if (res == TRANSITION_CHANGED) { + mavlink_log_critical(mavlink_fd, "#audio: recover from failsafe"); + } + } + /* fill current_status according to mode switches */ check_mode_switches(&sp_man, &status); @@ -1135,32 +1143,23 @@ int commander_thread_main(int argc, char *argv[]) status_changed = true; } if (status.main_state != MAIN_STATE_AUTO && armed.armed) { - transition_result_t res = main_state_transition(&status, MAIN_STATE_AUTO); + transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_RTL); if (res == TRANSITION_CHANGED) { - mavlink_log_critical(mavlink_fd, "#audio: failsafe, switching to RTL mode"); - status.set_nav_state = NAV_STATE_RTL; - status.set_nav_state_timestamp = hrt_absolute_time(); - } else if (status.main_state != MAIN_STATE_SEATBELT) { - res = main_state_transition(&status, MAIN_STATE_SEATBELT); - if (res == TRANSITION_CHANGED) { - mavlink_log_critical(mavlink_fd, "#audio: failsafe, switching to SEATBELT mode"); - } + mavlink_log_critical(mavlink_fd, "#audio: failsafe: RTL"); } + // TODO add other failsafe modes if position estimate not available } } } - /* Flight termination in manual mode if assisted switch is on easy position //xxx hack! */ - if (armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.assisted_switch > STICK_ON_OFF_LIMIT) { - transition_result_t flighttermination_res = flighttermination_state_transition(&status, FLIGHTTERMINATION_STATE_ON); - if (flighttermination_res == TRANSITION_CHANGED) { + // TODO remove this hack + /* flight termination in manual mode if assisted switch is on easy position */ + if (!status.is_rotary_wing && armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.assisted_switch > STICK_ON_OFF_LIMIT) { + if (TRANSITION_CHANGED == failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION)) { tune_positive(); } - } else { - flighttermination_state_transition(&status, FLIGHTTERMINATION_STATE_OFF); } - /* handle commands last, as the system needs to be updated to handle them */ orb_check(cmd_sub, &updated); @@ -1176,12 +1175,12 @@ int commander_thread_main(int argc, char *argv[]) /* check which state machines for changes, clear "changed" flag */ bool arming_state_changed = check_arming_state_changed(); bool main_state_changed = check_main_state_changed(); - bool flighttermination_state_changed = check_flighttermination_state_changed(); + bool failsafe_state_changed = check_failsafe_state_changed(); hrt_abstime t1 = hrt_absolute_time(); - if (arming_state_changed || main_state_changed) { - mavlink_log_info(mavlink_fd, "[cmd] state: arm %d, main %d", status.arming_state, status.main_state); + if (arming_state_changed || main_state_changed || failsafe_state_changed) { + mavlink_log_info(mavlink_fd, "[cmd] state: arm %d, main %d, fs %d", status.arming_state, status.main_state, status.failsafe_state); status_changed = true; } diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 731e0e3ffc..90ca2a6d23 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -63,7 +63,7 @@ static const int ERROR = -1; static bool arming_state_changed = true; static bool main_state_changed = true; -static bool flighttermination_state_changed = true; +static bool failsafe_state_changed = true; transition_result_t arming_state_transition(struct vehicle_status_s *status, const struct safety_s *safety, @@ -287,10 +287,10 @@ check_main_state_changed() } bool -check_flighttermination_state_changed() +check_failsafe_state_changed() { - if (flighttermination_state_changed) { - flighttermination_state_changed = false; + if (failsafe_state_changed) { + failsafe_state_changed = false; return true; } else { @@ -361,41 +361,39 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s /** -* Transition from one flightermination state to another +* Transition from one failsafe state to another */ -transition_result_t flighttermination_state_transition(struct vehicle_status_s *status, flighttermination_state_t new_flighttermination_state) +transition_result_t failsafe_state_transition(struct vehicle_status_s *status, failsafe_state_t new_failsafe_state) { transition_result_t ret = TRANSITION_DENIED; - /* only check transition if the new state is actually different from the current one */ - if (new_flighttermination_state == status->flighttermination_state) { - ret = TRANSITION_NOT_CHANGED; + /* only check transition if the new state is actually different from the current one */ + if (new_failsafe_state == status->failsafe_state) { + ret = TRANSITION_NOT_CHANGED; - } else { + } else if (status->failsafe_state != FAILSAFE_STATE_TERMINATION) { + switch (new_failsafe_state) { + case FAILSAFE_STATE_NORMAL: + ret = TRANSITION_CHANGED; + break; + case FAILSAFE_STATE_RTL: + ret = TRANSITION_CHANGED; + break; + case FAILSAFE_STATE_TERMINATION: + ret = TRANSITION_CHANGED; + break; - switch (new_flighttermination_state) { - case FLIGHTTERMINATION_STATE_ON: - ret = TRANSITION_CHANGED; - status->flighttermination_state = FLIGHTTERMINATION_STATE_ON; - warnx("state machine helper: change to FLIGHTTERMINATION_STATE_ON"); - break; - case FLIGHTTERMINATION_STATE_OFF: - ret = TRANSITION_CHANGED; - status->flighttermination_state = FLIGHTTERMINATION_STATE_OFF; - break; - - default: - break; - } - - if (ret == TRANSITION_CHANGED) { - flighttermination_state_changed = true; - // TODO - //control_mode->flag_control_flighttermination_enabled = status->flighttermination_state == FLIGHTTERMINATION_STATE_ON; - } + default: + break; } - return ret; + if (ret == TRANSITION_CHANGED) { + status->failsafe_state = new_failsafe_state; + failsafe_state_changed = true; + } + } + + return ret; } diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h index e569fb4f37..f04879ff9e 100644 --- a/src/modules/commander/state_machine_helper.h +++ b/src/modules/commander/state_machine_helper.h @@ -67,11 +67,11 @@ transition_result_t main_state_transition(struct vehicle_status_s *current_state bool check_main_state_changed(); -transition_result_t flighttermination_state_transition(struct vehicle_status_s *status, flighttermination_state_t new_flighttermination_state); +transition_result_t failsafe_state_transition(struct vehicle_status_s *status, failsafe_state_t new_failsafe_state); bool check_navigation_state_changed(); -bool check_flighttermination_state_changed(); +bool check_failsafe_state_changed(); void set_navigation_state_changed(); diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index dc2196de6d..17b1028f9a 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -619,7 +619,7 @@ FixedwingAttitudeControl::task_main() } /* Simple handling of failsafe: deploy parachute if failsafe is on */ - if (_vcontrol_mode.flag_control_flighttermination_enabled) { + if (_vcontrol_mode.flag_control_termination_enabled) { _actuators_airframe.control[1] = 1.0f; // warnx("_actuators_airframe.control[1] = 1.0f;"); } else { diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index cfcc886b68..89a62e1661 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -643,87 +643,101 @@ Navigator::task_main() vehicle_status_update(); pub_control_mode = true; - /* Evaluate state machine from commander and set the navigator mode accordingly */ - if (_vstatus.main_state == MAIN_STATE_AUTO && - (_vstatus.arming_state == ARMING_STATE_ARMED || _vstatus.arming_state == ARMING_STATE_ARMED_ERROR)) { - bool stick_mode = false; - if (!_vstatus.rc_signal_lost) { - /* RC signal available, use control switches to set mode */ - /* RETURN switch, overrides MISSION switch */ - if (_vstatus.return_switch == RETURN_SWITCH_RETURN) { - if (myState != NAV_STATE_READY || _rtl_state != RTL_STATE_LAND) { - dispatch(EVENT_RTL_REQUESTED); + /* evaluate state machine from commander and set the navigator mode accordingly */ + if (_vstatus.arming_state == ARMING_STATE_ARMED || _vstatus.arming_state == ARMING_STATE_ARMED_ERROR) { + if (_vstatus.failsafe_state == FAILSAFE_STATE_NORMAL) { + if (_vstatus.main_state == MAIN_STATE_AUTO) { + bool stick_mode = false; + if (!_vstatus.rc_signal_lost) { + /* RC signal available, use control switches to set mode */ + /* RETURN switch, overrides MISSION switch */ + if (_vstatus.return_switch == RETURN_SWITCH_RETURN) { + if (myState != NAV_STATE_READY || _rtl_state != RTL_STATE_LAND) { + dispatch(EVENT_RTL_REQUESTED); + } + stick_mode = true; + } else { + /* MISSION switch */ + if (_vstatus.mission_switch == MISSION_SWITCH_LOITER) { + dispatch(EVENT_LOITER_REQUESTED); + stick_mode = true; + } else if (_vstatus.mission_switch == MISSION_SWITCH_MISSION) { + /* switch to mission only if available */ + if (_mission.current_mission_available()) { + dispatch(EVENT_MISSION_REQUESTED); + } else { + dispatch(EVENT_LOITER_REQUESTED); + } + stick_mode = true; + } + if (!stick_mode && _vstatus.return_switch == RETURN_SWITCH_NORMAL && myState == NAV_STATE_RTL) { + /* RETURN switch is in normal mode, no MISSION switch mapped, interrupt if in RTL state */ + dispatch(EVENT_LOITER_REQUESTED); + stick_mode = true; + } + } + } + + if (!stick_mode) { + if (_vstatus.set_nav_state_timestamp != _set_nav_state_timestamp) { + /* commander requested new navigation mode, try to set it */ + _set_nav_state_timestamp = _vstatus.set_nav_state_timestamp; + + switch (_vstatus.set_nav_state) { + case NAV_STATE_NONE: + /* nothing to do */ + break; + + case NAV_STATE_LOITER: + dispatch(EVENT_LOITER_REQUESTED); + break; + + case NAV_STATE_MISSION: + if (_mission.current_mission_available()) { + dispatch(EVENT_MISSION_REQUESTED); + } else { + dispatch(EVENT_LOITER_REQUESTED); + } + break; + + case NAV_STATE_RTL: + if (myState != NAV_STATE_READY || _rtl_state != RTL_STATE_LAND) { + dispatch(EVENT_RTL_REQUESTED); + } + break; + + default: + warnx("ERROR: Requested navigation state not supported"); + break; + } + + } else { + /* on first switch to AUTO try mission by default, if none is available fallback to loiter */ + if (myState == NAV_STATE_NONE) { + if (_mission.current_mission_available()) { + dispatch(EVENT_MISSION_REQUESTED); + } else { + dispatch(EVENT_LOITER_REQUESTED); + } + } + } } - stick_mode = true; } else { - /* MISSION switch */ - if (_vstatus.mission_switch == MISSION_SWITCH_LOITER) { - dispatch(EVENT_LOITER_REQUESTED); - stick_mode = true; - } else if (_vstatus.mission_switch == MISSION_SWITCH_MISSION) { - /* switch to mission only if available */ - if (_mission.current_mission_available()) { - dispatch(EVENT_MISSION_REQUESTED); - } else { - dispatch(EVENT_LOITER_REQUESTED); - } - stick_mode = true; - } - if (!stick_mode && _vstatus.return_switch == RETURN_SWITCH_NORMAL && myState == NAV_STATE_RTL) { - /* RETURN switch is in normal mode, no MISSION switch mapped, interrupt if in RTL state */ - dispatch(EVENT_LOITER_REQUESTED); - stick_mode = true; - } + /* not in AUTO mode */ + dispatch(EVENT_NONE_REQUESTED); } - } - if (!stick_mode) { - if (_vstatus.set_nav_state_timestamp != _set_nav_state_timestamp) { - /* commander requested new navigation mode, try to set it */ - _set_nav_state_timestamp = _vstatus.set_nav_state_timestamp; + } else if (_vstatus.failsafe_state == FAILSAFE_STATE_RTL) { + /* RTL on failsafe */ + dispatch(EVENT_RTL_REQUESTED); - switch (_vstatus.set_nav_state) { - case NAV_STATE_NONE: - /* nothing to do */ - break; - - case NAV_STATE_LOITER: - dispatch(EVENT_LOITER_REQUESTED); - break; - - case NAV_STATE_MISSION: - if (_mission.current_mission_available()) { - dispatch(EVENT_MISSION_REQUESTED); - } else { - dispatch(EVENT_LOITER_REQUESTED); - } - break; - - case NAV_STATE_RTL: - if (myState != NAV_STATE_READY || _rtl_state != RTL_STATE_LAND) { - dispatch(EVENT_RTL_REQUESTED); - } - break; - - default: - warnx("ERROR: Requested navigation state not supported"); - break; - } - - } else { - /* on first switch to AUTO try mission by default, if none is available fallback to loiter */ - if (myState == NAV_STATE_NONE) { - if (_mission.current_mission_available()) { - dispatch(EVENT_MISSION_REQUESTED); - } else { - dispatch(EVENT_LOITER_REQUESTED); - } - } - } + } else { + /* shouldn't act */ + dispatch(EVENT_NONE_REQUESTED); } } else { - /* not in AUTO */ + /* not armed */ dispatch(EVENT_NONE_REQUESTED); } } @@ -1442,40 +1456,74 @@ Navigator::publish_control_mode() _control_mode.flag_system_hil_enabled = _vstatus.hil_state == HIL_STATE_ON; _control_mode.flag_control_offboard_enabled = false; - _control_mode.flag_control_flighttermination_enabled = false; + _control_mode.flag_control_termination_enabled = false; - switch (_vstatus.main_state) { - case MAIN_STATE_MANUAL: - _control_mode.flag_control_manual_enabled = true; - _control_mode.flag_control_rates_enabled = _vstatus.is_rotary_wing; - _control_mode.flag_control_attitude_enabled = _vstatus.is_rotary_wing; + /* set this flag when navigator has control */ + bool navigator_enabled = false; + + switch (_vstatus.failsafe_state) { + case FAILSAFE_STATE_NORMAL: + switch (_vstatus.main_state) { + case MAIN_STATE_MANUAL: + _control_mode.flag_control_manual_enabled = true; + _control_mode.flag_control_rates_enabled = _vstatus.is_rotary_wing; + _control_mode.flag_control_attitude_enabled = _vstatus.is_rotary_wing; + _control_mode.flag_control_altitude_enabled = false; + _control_mode.flag_control_climb_rate_enabled = false; + _control_mode.flag_control_position_enabled = false; + _control_mode.flag_control_velocity_enabled = false; + break; + + case MAIN_STATE_SEATBELT: + _control_mode.flag_control_manual_enabled = true; + _control_mode.flag_control_rates_enabled = true; + _control_mode.flag_control_attitude_enabled = true; + _control_mode.flag_control_altitude_enabled = true; + _control_mode.flag_control_climb_rate_enabled = true; + _control_mode.flag_control_position_enabled = false; + _control_mode.flag_control_velocity_enabled = false; + break; + + case MAIN_STATE_EASY: + _control_mode.flag_control_manual_enabled = true; + _control_mode.flag_control_rates_enabled = true; + _control_mode.flag_control_attitude_enabled = true; + _control_mode.flag_control_altitude_enabled = true; + _control_mode.flag_control_climb_rate_enabled = true; + _control_mode.flag_control_position_enabled = true; + _control_mode.flag_control_velocity_enabled = true; + break; + + case MAIN_STATE_AUTO: + navigator_enabled = true; + + default: + break; + } + break; + + case FAILSAFE_STATE_RTL: + navigator_enabled = true; + break; + + case FAILSAFE_STATE_TERMINATION: + navigator_enabled = true; + /* disable all controllers on termination */ + _control_mode.flag_control_rates_enabled = false; + _control_mode.flag_control_attitude_enabled = false; + _control_mode.flag_control_position_enabled = false; + _control_mode.flag_control_velocity_enabled = false; _control_mode.flag_control_altitude_enabled = false; _control_mode.flag_control_climb_rate_enabled = false; - _control_mode.flag_control_position_enabled = false; - _control_mode.flag_control_velocity_enabled = false; + _control_mode.flag_control_termination_enabled = true; break; - case MAIN_STATE_SEATBELT: - _control_mode.flag_control_manual_enabled = true; - _control_mode.flag_control_rates_enabled = true; - _control_mode.flag_control_attitude_enabled = true; - _control_mode.flag_control_altitude_enabled = true; - _control_mode.flag_control_climb_rate_enabled = true; - _control_mode.flag_control_position_enabled = false; - _control_mode.flag_control_velocity_enabled = false; + default: break; + } - case MAIN_STATE_EASY: - _control_mode.flag_control_manual_enabled = true; - _control_mode.flag_control_rates_enabled = true; - _control_mode.flag_control_attitude_enabled = true; - _control_mode.flag_control_altitude_enabled = true; - _control_mode.flag_control_climb_rate_enabled = true; - _control_mode.flag_control_position_enabled = true; - _control_mode.flag_control_velocity_enabled = true; - break; - - case MAIN_STATE_AUTO: + /* navigator has control, set control mode flags according to nav state*/ + if (navigator_enabled) { _control_mode.flag_control_manual_enabled = false; if (myState == NAV_STATE_READY) { /* disable all controllers, armed but idle */ @@ -1493,10 +1541,6 @@ Navigator::publish_control_mode() _control_mode.flag_control_altitude_enabled = true; _control_mode.flag_control_climb_rate_enabled = true; } - break; - - default: - break; } _control_mode.timestamp = hrt_absolute_time(); diff --git a/src/modules/uORB/topics/vehicle_control_mode.h b/src/modules/uORB/topics/vehicle_control_mode.h index 26dcbd985c..f9f8414df8 100644 --- a/src/modules/uORB/topics/vehicle_control_mode.h +++ b/src/modules/uORB/topics/vehicle_control_mode.h @@ -92,7 +92,7 @@ struct vehicle_control_mode_s bool flag_control_position_enabled; /**< true if position is controlled */ bool flag_control_altitude_enabled; /**< true if altitude is controlled */ bool flag_control_climb_rate_enabled; /**< true if climb rate is controlled */ - bool flag_control_flighttermination_enabled; /**< true if flighttermination is enabled */ + bool flag_control_termination_enabled; /**< true if flighttermination is enabled */ }; /** diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index 1a9dec5f51..a3a862d858 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -82,9 +82,11 @@ typedef enum { } hil_state_t; typedef enum { - FLIGHTTERMINATION_STATE_OFF = 0, - FLIGHTTERMINATION_STATE_ON -} flighttermination_state_t; + FAILSAFE_STATE_NORMAL = 0, + FAILSAFE_STATE_RTL, + FAILSAFE_STATE_TERMINATION, + FAILSAFE_STATE_MAX +} failsafe_state_t; typedef enum { MODE_SWITCH_MANUAL = 0, @@ -173,6 +175,7 @@ struct vehicle_status_s uint64_t set_nav_state_timestamp; /**< timestamp of latest change of set_nav_state */ arming_state_t arming_state; /**< current arming state */ hil_state_t hil_state; /**< current hil state */ + failsafe_state_t failsafe_state; /**< current failsafe state */ int32_t system_type; /**< system type, inspired by MAVLink's VEHICLE_TYPE enum */ int32_t system_id; /**< system id, inspired by MAVLink's system ID field */ @@ -223,8 +226,6 @@ struct vehicle_status_s uint16_t errors_count2; uint16_t errors_count3; uint16_t errors_count4; - - flighttermination_state_t flighttermination_state; }; /** From 92ddf7903b4a540215905f01acd1819eac1f176d Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 25 Jan 2014 23:37:26 +0100 Subject: [PATCH 340/447] commander: more user-friendly states indication --- src/modules/commander/commander.cpp | 43 ++++++++++++++++++++---- src/modules/navigator/navigator_main.cpp | 2 +- src/modules/uORB/topics/vehicle_status.h | 4 ++- 3 files changed, 40 insertions(+), 9 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 7715f73e07..722230effa 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -575,6 +575,26 @@ int commander_thread_main(int argc, char *argv[]) /* welcome user */ warnx("starting"); + char *main_states_str[MAIN_STATE_MAX]; + main_states_str[0] = "MANUAL"; + main_states_str[1] = "SEATBELT"; + main_states_str[2] = "EASY"; + main_states_str[3] = "AUTO"; + + char *arming_states_str[ARMING_STATE_MAX]; + arming_states_str[0] = "INIT"; + arming_states_str[1] = "STANDBY"; + arming_states_str[2] = "ARMED"; + arming_states_str[3] = "ARMED_ERROR"; + arming_states_str[4] = "STANDBY_ERROR"; + arming_states_str[5] = "REBOOT"; + arming_states_str[6] = "IN_AIR_RESTORE"; + + char *failsafe_states_str[FAILSAFE_STATE_MAX]; + failsafe_states_str[0] = "NORMAL"; + failsafe_states_str[1] = "RTL"; + failsafe_states_str[2] = "TERMINATION"; + /* pthread for slow low prio thread */ pthread_t commander_low_prio_thread; @@ -1108,8 +1128,8 @@ int commander_thread_main(int argc, char *argv[]) } } else if (res == TRANSITION_DENIED) { - warnx("ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch); - mavlink_log_critical(mavlink_fd, "#audio: ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch); + /* DENIED here indicates bug in the commander */ + mavlink_log_critical(mavlink_fd, "ERROR: arming state transition denied"); } if (status.failsafe_state != FAILSAFE_STATE_NORMAL) { @@ -1127,13 +1147,11 @@ int commander_thread_main(int argc, char *argv[]) res = check_main_state_machine(&status); if (res == TRANSITION_CHANGED) { - //mavlink_log_info(mavlink_fd, "[cmd] main state: %d", status.main_state); tune_positive(); } else if (res == TRANSITION_DENIED) { /* DENIED here indicates bug in the commander */ - warnx("ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch); - mavlink_log_critical(mavlink_fd, "#audio: ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch); + mavlink_log_critical(mavlink_fd, "ERROR: main state transition denied"); } } else { @@ -1179,9 +1197,20 @@ int commander_thread_main(int argc, char *argv[]) hrt_abstime t1 = hrt_absolute_time(); - if (arming_state_changed || main_state_changed || failsafe_state_changed) { - mavlink_log_info(mavlink_fd, "[cmd] state: arm %d, main %d, fs %d", status.arming_state, status.main_state, status.failsafe_state); + /* print new state */ + if (arming_state_changed) { status_changed = true; + mavlink_log_info(mavlink_fd, "[cmd] arming state: %s", arming_states_str[status.arming_state]); + } + + if (main_state_changed) { + status_changed = true; + mavlink_log_info(mavlink_fd, "[cmd] main state: %s", main_states_str[status.main_state]); + } + + if (failsafe_state_changed) { + status_changed = true; + mavlink_log_info(mavlink_fd, "[cmd] failsafe state: %s", failsafe_states_str[status.failsafe_state]); } /* publish states (armed, control mode, vehicle status) at least with 5 Hz */ diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 89a62e1661..8ecc28b11d 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -802,7 +802,7 @@ Navigator::task_main() /* notify user about state changes */ if (myState != prevState) { - mavlink_log_info(_mavlink_fd, "[navigator] nav state: %s -> %s", nav_states_str[prevState], nav_states_str[myState]); + mavlink_log_info(_mavlink_fd, "[navigator] nav state: %s", nav_states_str[myState]); prevState = myState; pub_control_mode = true; } diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index a3a862d858..73102090f0 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -64,6 +64,7 @@ typedef enum { MAIN_STATE_SEATBELT, MAIN_STATE_EASY, MAIN_STATE_AUTO, + MAIN_STATE_MAX } main_state_t; typedef enum { @@ -73,7 +74,8 @@ typedef enum { ARMING_STATE_ARMED_ERROR, ARMING_STATE_STANDBY_ERROR, ARMING_STATE_REBOOT, - ARMING_STATE_IN_AIR_RESTORE + ARMING_STATE_IN_AIR_RESTORE, + ARMING_STATE_MAX } arming_state_t; typedef enum { From 062b64a1e21406cf787d93aa53921ce0ef6627fd Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 25 Jan 2014 23:49:33 +0100 Subject: [PATCH 341/447] navigator: RTL on failsafe bug fixed --- src/modules/navigator/navigator_main.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 8ecc28b11d..d72ed70585 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -729,7 +729,9 @@ Navigator::task_main() } else if (_vstatus.failsafe_state == FAILSAFE_STATE_RTL) { /* RTL on failsafe */ - dispatch(EVENT_RTL_REQUESTED); + if (myState != NAV_STATE_READY || _rtl_state != RTL_STATE_LAND) { + dispatch(EVENT_RTL_REQUESTED); + } } else { /* shouldn't act */ From c7f05539382a48d7ecaad3bfdf71261cde2ee8c7 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 26 Jan 2014 11:50:34 +0100 Subject: [PATCH 342/447] cammander: state machine can now deny current state (e.g. when position lock lost during EASY mode), added FAILSAFE_STATE_LAND --- src/modules/commander/commander.cpp | 35 ++++++- .../commander/state_machine_helper.cpp | 99 ++++++++++--------- src/modules/navigator/navigator_main.cpp | 58 ++++++++++- .../uORB/topics/vehicle_control_mode.h | 1 + src/modules/uORB/topics/vehicle_status.h | 7 +- 5 files changed, 148 insertions(+), 52 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 722230effa..bca0569d5f 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1143,7 +1143,7 @@ int commander_thread_main(int argc, char *argv[]) /* fill current_status according to mode switches */ check_mode_switches(&sp_man, &status); - /* evaluate the main state machine */ + /* evaluate the main state machine according to mode switches */ res = check_main_state_machine(&status); if (res == TRANSITION_CHANGED) { @@ -1160,12 +1160,41 @@ int commander_thread_main(int argc, char *argv[]) status.rc_signal_lost = true; status_changed = true; } - if (status.main_state != MAIN_STATE_AUTO && armed.armed) { + + if (status.main_state == MAIN_STATE_AUTO) { + /* check if AUTO mode still allowed */ + transition_result_t res = main_state_transition(&status, MAIN_STATE_AUTO); + if (res == TRANSITION_DENIED) { + /* AUTO mode denied, don't try RTL, switch to failsafe state LAND */ + res = failsafe_state_transition(&status, FAILSAFE_STATE_LAND); + if (res == TRANSITION_CHANGED) { + mavlink_log_critical(mavlink_fd, "#audio: failsafe: LAND"); + } else if (res == TRANSITION_DENIED) { + /* LAND mode denied, switch to failsafe state TERMINATION */ + transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION); + if (res == TRANSITION_CHANGED) { + mavlink_log_critical(mavlink_fd, "#audio: failsafe: TERMINATION"); + } + } + } + + } else if (armed.armed) { + /* failsafe for manual modes */ transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_RTL); if (res == TRANSITION_CHANGED) { mavlink_log_critical(mavlink_fd, "#audio: failsafe: RTL"); + } else if (res == TRANSITION_DENIED) { + /* RTL not allowed (no global position estimate), try LAND */ + res = failsafe_state_transition(&status, FAILSAFE_STATE_LAND); + if (res == TRANSITION_CHANGED) { + mavlink_log_critical(mavlink_fd, "#audio: failsafe: LAND"); + } else if (res == TRANSITION_DENIED) { + res = failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION); + if (res == TRANSITION_CHANGED) { + mavlink_log_critical(mavlink_fd, "#audio: failsafe: TERMINATION"); + } + } } - // TODO add other failsafe modes if position estimate not available } } } diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 90ca2a6d23..77edea546d 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -223,51 +223,50 @@ main_state_transition(struct vehicle_status_s *current_state, main_state_t new_m { transition_result_t ret = TRANSITION_DENIED; - /* only check transition if the new state is actually different from the current one */ - if (new_main_state == current_state->main_state) { - ret = TRANSITION_NOT_CHANGED; + /* transition may be denied even if requested the same state because conditions may be changed */ + switch (new_main_state) { + case MAIN_STATE_MANUAL: + ret = TRANSITION_CHANGED; + break; - } else { + case MAIN_STATE_SEATBELT: - switch (new_main_state) { - case MAIN_STATE_MANUAL: + /* need at minimum altitude estimate */ + if (!current_state->is_rotary_wing || + (current_state->condition_local_altitude_valid || + current_state->condition_global_position_valid)) { ret = TRANSITION_CHANGED; - break; - - case MAIN_STATE_SEATBELT: - - /* need at minimum altitude estimate */ - if (!current_state->is_rotary_wing || - (current_state->condition_local_altitude_valid || - current_state->condition_global_position_valid)) { - ret = TRANSITION_CHANGED; - } - - break; - - case MAIN_STATE_EASY: - - /* need at minimum local position estimate */ - if (current_state->condition_local_position_valid || - current_state->condition_global_position_valid) { - ret = TRANSITION_CHANGED; - } - - break; - - case MAIN_STATE_AUTO: - - /* need global position estimate */ - if (current_state->condition_global_position_valid) { - ret = TRANSITION_CHANGED; - } - - break; } - if (ret == TRANSITION_CHANGED) { + break; + + case MAIN_STATE_EASY: + + /* need at minimum local position estimate */ + if (current_state->condition_local_position_valid || + current_state->condition_global_position_valid) { + ret = TRANSITION_CHANGED; + } + + break; + + case MAIN_STATE_AUTO: + + /* need global position estimate */ + if (current_state->condition_global_position_valid) { + ret = TRANSITION_CHANGED; + } + + break; + } + + if (ret == TRANSITION_CHANGED) { + if (current_state->main_state != new_main_state) { current_state->main_state = new_main_state; main_state_changed = true; + + } else { + ret = TRANSITION_NOT_CHANGED; } } @@ -367,17 +366,22 @@ transition_result_t failsafe_state_transition(struct vehicle_status_s *status, f { transition_result_t ret = TRANSITION_DENIED; - /* only check transition if the new state is actually different from the current one */ - if (new_failsafe_state == status->failsafe_state) { - ret = TRANSITION_NOT_CHANGED; + /* transition may be denied even if requested the same state because conditions may be changed */ + if (status->failsafe_state == FAILSAFE_STATE_TERMINATION) { + /* transitions from TERMINATION to other states not allowed */ + if (new_failsafe_state == FAILSAFE_STATE_TERMINATION) { + ret = TRANSITION_NOT_CHANGED; + } - } else if (status->failsafe_state != FAILSAFE_STATE_TERMINATION) { + } else { switch (new_failsafe_state) { case FAILSAFE_STATE_NORMAL: ret = TRANSITION_CHANGED; break; case FAILSAFE_STATE_RTL: - ret = TRANSITION_CHANGED; + if (status->condition_global_position_valid) { + ret = TRANSITION_CHANGED; + } break; case FAILSAFE_STATE_TERMINATION: ret = TRANSITION_CHANGED; @@ -388,8 +392,13 @@ transition_result_t failsafe_state_transition(struct vehicle_status_s *status, f } if (ret == TRANSITION_CHANGED) { - status->failsafe_state = new_failsafe_state; - failsafe_state_changed = true; + if (status->failsafe_state != new_failsafe_state) { + status->failsafe_state = new_failsafe_state; + failsafe_state_changed = true; + + } else { + ret = TRANSITION_NOT_CHANGED; + } } } diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index d72ed70585..388fefd024 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -216,6 +216,7 @@ private: EVENT_LOITER_REQUESTED, EVENT_MISSION_REQUESTED, EVENT_RTL_REQUESTED, + EVENT_LAND_REQUESTED, EVENT_MISSION_CHANGED, EVENT_HOME_POSITION_CHANGED, MAX_EVENT @@ -292,7 +293,7 @@ private: void start_loiter(); void start_mission(); void start_rtl(); - void finish_rtl(); + void start_land(); /** * Guards offboard mission @@ -733,6 +734,12 @@ Navigator::task_main() dispatch(EVENT_RTL_REQUESTED); } + } else if (_vstatus.failsafe_state == FAILSAFE_STATE_LAND) { + /* LAND on failsafe */ + if (myState != NAV_STATE_READY) { + dispatch(EVENT_LAND_REQUESTED); + } + } else { /* shouldn't act */ dispatch(EVENT_NONE_REQUESTED); @@ -892,6 +899,7 @@ StateTable::Tran const Navigator::myTable[NAV_STATE_MAX][MAX_EVENT] = { /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER}, /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, + /* EVENT_LAND_REQUESTED */ {ACTION(&Navigator::start_land), NAV_STATE_LAND}, /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_NONE}, /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_NONE}, }, @@ -902,6 +910,7 @@ StateTable::Tran const Navigator::myTable[NAV_STATE_MAX][MAX_EVENT] = { /* EVENT_LOITER_REQUESTED */ {NO_ACTION, NAV_STATE_READY}, /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, + /* EVENT_LAND_REQUESTED */ {NO_ACTION, NAV_STATE_READY}, /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_READY}, /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_READY}, }, @@ -912,6 +921,7 @@ StateTable::Tran const Navigator::myTable[NAV_STATE_MAX][MAX_EVENT] = { /* EVENT_LOITER_REQUESTED */ {NO_ACTION, NAV_STATE_LOITER}, /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, + /* EVENT_LAND_REQUESTED */ {ACTION(&Navigator::start_land), NAV_STATE_LAND}, /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_LOITER}, /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_LOITER}, }, @@ -922,6 +932,7 @@ StateTable::Tran const Navigator::myTable[NAV_STATE_MAX][MAX_EVENT] = { /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER}, /* EVENT_MISSION_REQUESTED */ {NO_ACTION, NAV_STATE_MISSION}, /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, + /* EVENT_LAND_REQUESTED */ {ACTION(&Navigator::start_land), NAV_STATE_LAND}, /* EVENT_MISSION_CHANGED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_MISSION}, }, @@ -932,9 +943,21 @@ StateTable::Tran const Navigator::myTable[NAV_STATE_MAX][MAX_EVENT] = { /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER}, /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, /* EVENT_RTL_REQUESTED */ {NO_ACTION, NAV_STATE_RTL}, + /* EVENT_LAND_REQUESTED */ {ACTION(&Navigator::start_land), NAV_STATE_LAND}, /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_RTL}, /* EVENT_HOME_POSITION_CHANGED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, // TODO need to reset rtl_state }, + { + /* STATE_LAND */ + /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE}, + /* EVENT_READY_REQUESTED */ {ACTION(&Navigator::start_ready), NAV_STATE_READY}, + /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER}, + /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, + /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, + /* EVENT_LAND_REQUESTED */ {ACTION(&Navigator::start_land), NAV_STATE_LAND}, + /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_RTL}, + /* EVENT_HOME_POSITION_CHANGED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, + }, }; void @@ -1142,6 +1165,27 @@ Navigator::start_rtl() set_rtl_item(); } +void +Navigator::start_land() +{ + _do_takeoff = false; + _reset_loiter_pos = true; + + _pos_sp_triplet.previous.valid = false; + _pos_sp_triplet.next.valid = false; + + _pos_sp_triplet.current.valid = true; + _pos_sp_triplet.current.type = SETPOINT_TYPE_LAND; + _pos_sp_triplet.current.lat = _global_pos.lat; + _pos_sp_triplet.current.lon = _global_pos.lon; + _pos_sp_triplet.current.alt = _global_pos.alt; + _pos_sp_triplet.current.loiter_direction = 1; + _pos_sp_triplet.current.loiter_radius = _parameters.loiter_radius; + _pos_sp_triplet.current.yaw = NAN; + + mavlink_log_info(_mavlink_fd, "[navigator] LAND started"); +} + void Navigator::set_rtl_item() { @@ -1508,9 +1552,21 @@ Navigator::publish_control_mode() navigator_enabled = true; break; + case FAILSAFE_STATE_LAND: + /* land with or without position control */ + _control_mode.flag_control_manual_enabled = false; + _control_mode.flag_control_rates_enabled = true; + _control_mode.flag_control_attitude_enabled = true; + _control_mode.flag_control_position_enabled = _vstatus.condition_global_position_valid; + _control_mode.flag_control_velocity_enabled = _vstatus.condition_global_position_valid; + _control_mode.flag_control_altitude_enabled = true; + _control_mode.flag_control_climb_rate_enabled = true; + break; + case FAILSAFE_STATE_TERMINATION: navigator_enabled = true; /* disable all controllers on termination */ + _control_mode.flag_control_manual_enabled = false; _control_mode.flag_control_rates_enabled = false; _control_mode.flag_control_attitude_enabled = false; _control_mode.flag_control_position_enabled = false; diff --git a/src/modules/uORB/topics/vehicle_control_mode.h b/src/modules/uORB/topics/vehicle_control_mode.h index f9f8414df8..5aecac8987 100644 --- a/src/modules/uORB/topics/vehicle_control_mode.h +++ b/src/modules/uORB/topics/vehicle_control_mode.h @@ -67,6 +67,7 @@ typedef enum { NAV_STATE_LOITER, NAV_STATE_MISSION, NAV_STATE_RTL, + NAV_STATE_LAND, NAV_STATE_MAX } nav_state_t; diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index 73102090f0..a5988d3ba4 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -84,9 +84,10 @@ typedef enum { } hil_state_t; typedef enum { - FAILSAFE_STATE_NORMAL = 0, - FAILSAFE_STATE_RTL, - FAILSAFE_STATE_TERMINATION, + FAILSAFE_STATE_NORMAL = 0, /**< Normal operation */ + FAILSAFE_STATE_RTL, /**< Return To Launch */ + FAILSAFE_STATE_LAND, /**< Land without position control */ + FAILSAFE_STATE_TERMINATION, /**< Disable motors and use parachute, can't be recovered */ FAILSAFE_STATE_MAX } failsafe_state_t; From 7d2efe9367787cdfc4590f335f600f3b79b2cbc7 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 26 Jan 2014 11:52:33 +0100 Subject: [PATCH 343/447] commander, navigator: minor cleanup (refactoring), code style fixed --- src/modules/commander/commander.cpp | 19 +- .../commander/state_machine_helper.cpp | 24 +- src/modules/navigator/navigator_main.cpp | 358 +++++++++++------- 3 files changed, 243 insertions(+), 158 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index bca0569d5f..15f229bf00 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -371,6 +371,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe mavlink_log_info(mavlink_fd, "[cmd] HIL: FAILED resetting armed state"); } } + if (hil_ret == OK) ret = true; @@ -405,6 +406,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe arming_res = TRANSITION_NOT_CHANGED; } } + if (arming_res == TRANSITION_CHANGED) ret = true; @@ -447,6 +449,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe } } } + if (main_res == TRANSITION_CHANGED) ret = true; @@ -486,8 +489,9 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe break; case VEHICLE_CMD_OVERRIDE_GOTO: { - // TODO listen vehicle_command topic directly from navigator (?) + // TODO listen vehicle_command topic directly from navigator (?) unsigned int mav_goto = cmd->param1; + if (mav_goto == 0) { // MAV_GOTO_DO_HOLD status->set_nav_state = NAV_STATE_LOITER; status->set_nav_state_timestamp = hrt_absolute_time(); @@ -508,7 +512,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe } break; - /* Flight termination */ + /* Flight termination */ case VEHICLE_CMD_DO_SET_SERVO: { //xxx: needs its own mavlink command if (armed->armed && cmd->param3 > 0.5) { //xxx: for safety only for now, param3 is unused by VEHICLE_CMD_DO_SET_SERVO @@ -900,6 +904,7 @@ int commander_thread_main(int argc, char *argv[]) if (updated) { orb_copy(ORB_ID(battery_status), battery_sub, &battery); + /* only consider battery voltage if system has been running 2s and battery voltage is valid */ if (hrt_absolute_time() > start_time + 2000000 && battery.voltage_filtered_v > 0.0f) { status.battery_voltage = battery.voltage_filtered_v; @@ -1135,6 +1140,7 @@ int commander_thread_main(int argc, char *argv[]) if (status.failsafe_state != FAILSAFE_STATE_NORMAL) { /* recover from failsafe */ transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_NORMAL); + if (res == TRANSITION_CHANGED) { mavlink_log_critical(mavlink_fd, "#audio: recover from failsafe"); } @@ -1164,14 +1170,18 @@ int commander_thread_main(int argc, char *argv[]) if (status.main_state == MAIN_STATE_AUTO) { /* check if AUTO mode still allowed */ transition_result_t res = main_state_transition(&status, MAIN_STATE_AUTO); + if (res == TRANSITION_DENIED) { /* AUTO mode denied, don't try RTL, switch to failsafe state LAND */ res = failsafe_state_transition(&status, FAILSAFE_STATE_LAND); + if (res == TRANSITION_CHANGED) { mavlink_log_critical(mavlink_fd, "#audio: failsafe: LAND"); + } else if (res == TRANSITION_DENIED) { /* LAND mode denied, switch to failsafe state TERMINATION */ transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION); + if (res == TRANSITION_CHANGED) { mavlink_log_critical(mavlink_fd, "#audio: failsafe: TERMINATION"); } @@ -1181,15 +1191,20 @@ int commander_thread_main(int argc, char *argv[]) } else if (armed.armed) { /* failsafe for manual modes */ transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_RTL); + if (res == TRANSITION_CHANGED) { mavlink_log_critical(mavlink_fd, "#audio: failsafe: RTL"); + } else if (res == TRANSITION_DENIED) { /* RTL not allowed (no global position estimate), try LAND */ res = failsafe_state_transition(&status, FAILSAFE_STATE_LAND); + if (res == TRANSITION_CHANGED) { mavlink_log_critical(mavlink_fd, "#audio: failsafe: LAND"); + } else if (res == TRANSITION_DENIED) { res = failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION); + if (res == TRANSITION_CHANGED) { mavlink_log_critical(mavlink_fd, "#audio: failsafe: TERMINATION"); } diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 77edea546d..1be0a16b88 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -67,7 +67,7 @@ static bool failsafe_state_changed = true; transition_result_t arming_state_transition(struct vehicle_status_s *status, const struct safety_s *safety, - arming_state_t new_arming_state, struct actuator_armed_s *armed) + arming_state_t new_arming_state, struct actuator_armed_s *armed) { /* * Perform an atomic state update @@ -85,6 +85,7 @@ arming_state_transition(struct vehicle_status_s *status, const struct safety_s * /* enforce lockdown in HIL */ if (status->hil_state == HIL_STATE_ON) { armed->lockdown = true; + } else { armed->lockdown = false; } @@ -219,7 +220,7 @@ check_arming_state_changed() } transition_result_t -main_state_transition(struct vehicle_status_s *current_state, main_state_t new_main_state) +main_state_transition(struct vehicle_status_s *status, main_state_t new_main_state) { transition_result_t ret = TRANSITION_DENIED; @@ -232,9 +233,9 @@ main_state_transition(struct vehicle_status_s *current_state, main_state_t new_m case MAIN_STATE_SEATBELT: /* need at minimum altitude estimate */ - if (!current_state->is_rotary_wing || - (current_state->condition_local_altitude_valid || - current_state->condition_global_position_valid)) { + if (!status->is_rotary_wing || + (status->condition_local_altitude_valid || + status->condition_global_position_valid)) { ret = TRANSITION_CHANGED; } @@ -243,8 +244,8 @@ main_state_transition(struct vehicle_status_s *current_state, main_state_t new_m case MAIN_STATE_EASY: /* need at minimum local position estimate */ - if (current_state->condition_local_position_valid || - current_state->condition_global_position_valid) { + if (status->condition_local_position_valid || + status->condition_global_position_valid) { ret = TRANSITION_CHANGED; } @@ -253,7 +254,7 @@ main_state_transition(struct vehicle_status_s *current_state, main_state_t new_m case MAIN_STATE_AUTO: /* need global position estimate */ - if (current_state->condition_global_position_valid) { + if (status->condition_global_position_valid) { ret = TRANSITION_CHANGED; } @@ -261,8 +262,8 @@ main_state_transition(struct vehicle_status_s *current_state, main_state_t new_m } if (ret == TRANSITION_CHANGED) { - if (current_state->main_state != new_main_state) { - current_state->main_state = new_main_state; + if (status->main_state != new_main_state) { + status->main_state = new_main_state; main_state_changed = true; } else { @@ -378,11 +379,14 @@ transition_result_t failsafe_state_transition(struct vehicle_status_s *status, f case FAILSAFE_STATE_NORMAL: ret = TRANSITION_CHANGED; break; + case FAILSAFE_STATE_RTL: if (status->condition_global_position_valid) { ret = TRANSITION_CHANGED; } + break; + case FAILSAFE_STATE_TERMINATION: ret = TRANSITION_CHANGED; break; diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 388fefd024..9b5cfa6b4b 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -166,12 +166,12 @@ private: bool _mission_item_valid; /**< current mission item valid */ perf_counter_t _loop_perf; /**< loop performance counter */ - + Geofence _geofence; bool _geofence_violation_warning_sent; bool _fence_valid; /**< flag if fence is valid */ - bool _inside_fence; /**< vehicle is inside fence */ + bool _inside_fence; /**< vehicle is inside fence */ struct navigation_capabilities_s _nav_caps; @@ -358,7 +358,7 @@ static const int ERROR = -1; Navigator *g_navigator; } -Navigator::Navigator() : +Navigator::Navigator() : /* state machine transition table */ StateTable(&myTable[0][0], NAV_STATE_MAX, MAX_EVENT), @@ -490,16 +490,20 @@ void Navigator::offboard_mission_update(bool isrotaryWing) { struct mission_s offboard_mission; + if (orb_copy(ORB_ID(mission), _offboard_mission_sub, &offboard_mission) == OK) { /* Check mission feasibility, for now do not handle the return value, * however warnings are issued to the gcs via mavlink from inside the MissionFeasiblityChecker */ dm_item_t dm_current; + if (offboard_mission.dataman_id == 0) { dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0; + } else { dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1; } + missionFeasiblityChecker.checkMissionFeasible(isrotaryWing, dm_current, (size_t)offboard_mission.count, _geofence); _mission.set_offboard_dataman_id(offboard_mission.dataman_id); @@ -516,6 +520,7 @@ void Navigator::onboard_mission_update() { struct mission_s onboard_mission; + if (orb_copy(ORB_ID(mission), _onboard_mission_sub, &onboard_mission) == OK) { _mission.set_current_onboard_mission_index(onboard_mission.current_index); @@ -558,11 +563,13 @@ Navigator::task_main() * else clear geofence data in datamanager */ struct stat buffer; - if( stat (GEOFENCE_FILENAME, &buffer) == 0 ) { + + if (stat(GEOFENCE_FILENAME, &buffer) == 0) { warnx("Try to load geofence.txt"); _geofence.loadFromFile(GEOFENCE_FILENAME); + } else { - if (_geofence.clearDm() > 0 ) + if (_geofence.clearDm() > 0) warnx("Geofence cleared"); else warnx("Could not clear geofence"); @@ -578,7 +585,7 @@ Navigator::task_main() _vstatus_sub = orb_subscribe(ORB_ID(vehicle_status)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); _home_pos_sub = orb_subscribe(ORB_ID(home_position)); - + /* copy all topics first time */ vehicle_status_update(); parameters_update(); @@ -649,6 +656,7 @@ Navigator::task_main() if (_vstatus.failsafe_state == FAILSAFE_STATE_NORMAL) { if (_vstatus.main_state == MAIN_STATE_AUTO) { bool stick_mode = false; + if (!_vstatus.rc_signal_lost) { /* RC signal available, use control switches to set mode */ /* RETURN switch, overrides MISSION switch */ @@ -656,21 +664,27 @@ Navigator::task_main() if (myState != NAV_STATE_READY || _rtl_state != RTL_STATE_LAND) { dispatch(EVENT_RTL_REQUESTED); } + stick_mode = true; + } else { /* MISSION switch */ if (_vstatus.mission_switch == MISSION_SWITCH_LOITER) { dispatch(EVENT_LOITER_REQUESTED); stick_mode = true; + } else if (_vstatus.mission_switch == MISSION_SWITCH_MISSION) { /* switch to mission only if available */ if (_mission.current_mission_available()) { dispatch(EVENT_MISSION_REQUESTED); + } else { dispatch(EVENT_LOITER_REQUESTED); } + stick_mode = true; } + if (!stick_mode && _vstatus.return_switch == RETURN_SWITCH_NORMAL && myState == NAV_STATE_RTL) { /* RETURN switch is in normal mode, no MISSION switch mapped, interrupt if in RTL state */ dispatch(EVENT_LOITER_REQUESTED); @@ -696,15 +710,18 @@ Navigator::task_main() case NAV_STATE_MISSION: if (_mission.current_mission_available()) { dispatch(EVENT_MISSION_REQUESTED); + } else { dispatch(EVENT_LOITER_REQUESTED); } + break; case NAV_STATE_RTL: if (myState != NAV_STATE_READY || _rtl_state != RTL_STATE_LAND) { dispatch(EVENT_RTL_REQUESTED); } + break; default: @@ -717,12 +734,14 @@ Navigator::task_main() if (myState == NAV_STATE_NONE) { if (_mission.current_mission_available()) { dispatch(EVENT_MISSION_REQUESTED); + } else { dispatch(EVENT_LOITER_REQUESTED); } } } } + } else { /* not in AUTO mode */ dispatch(EVENT_NONE_REQUESTED); @@ -765,7 +784,7 @@ Navigator::task_main() /* offboard mission updated */ if (fds[4].revents & POLLIN) { offboard_mission_update(_vstatus.is_rotary_wing); - // XXX check if mission really changed + // XXX check if mission really changed dispatch(EVENT_MISSION_CHANGED); } @@ -786,6 +805,7 @@ Navigator::task_main() /* global position updated */ if (fds[1].revents & POLLIN) { global_position_update(); + /* only check if waypoint has been reached in MISSION and RTL modes */ if (myState == NAV_STATE_MISSION || myState == NAV_STATE_RTL) { if (check_mission_item_reached()) { @@ -794,15 +814,15 @@ Navigator::task_main() } /* Check geofence violation */ - if(!_geofence.inside(&_global_pos)) { + if (!_geofence.inside(&_global_pos)) { //xxx: publish geofence violation here (or change local flag depending on which app handles the flight termination) /* Issue a warning about the geofence violation once */ - if (!_geofence_violation_warning_sent) - { + if (!_geofence_violation_warning_sent) { mavlink_log_critical(_mavlink_fd, "#audio: Geofence violation"); _geofence_violation_warning_sent = true; } + } else { /* Reset the _geofence_violation_warning_sent field */ _geofence_violation_warning_sent = false; @@ -852,48 +872,55 @@ Navigator::start() } void -Navigator::status() +Navigator::status() { warnx("Global position is %svalid", _global_pos.valid ? "" : "in"); + if (_global_pos.valid) { warnx("Longitude %5.5f degrees, latitude %5.5f degrees", _global_pos.lon, _global_pos.lat); warnx("Altitude %5.5f meters, altitude above home %5.5f meters", - (double)_global_pos.alt, (double)(_global_pos.alt - _home_pos.alt)); + (double)_global_pos.alt, (double)(_global_pos.alt - _home_pos.alt)); warnx("Ground velocity in m/s, N %5.5f, E %5.5f, D %5.5f", - (double)_global_pos.vel_n, (double)_global_pos.vel_e, (double)_global_pos.vel_d); + (double)_global_pos.vel_n, (double)_global_pos.vel_e, (double)_global_pos.vel_d); warnx("Compass heading in degrees %5.5f", (double)(_global_pos.yaw * M_RAD_TO_DEG_F)); } + if (_fence_valid) { warnx("Geofence is valid"); // warnx("Vertex longitude latitude"); // for (unsigned i = 0; i < _fence.count; i++) // warnx("%6u %9.5f %8.5f", i, (double)_fence.vertices[i].lon, (double)_fence.vertices[i].lat); + } else { warnx("Geofence not set"); } switch (myState) { - case NAV_STATE_NONE: - warnx("State: None"); - break; - case NAV_STATE_LOITER: - warnx("State: Loiter"); - break; - case NAV_STATE_MISSION: - warnx("State: Mission"); - break; - case NAV_STATE_RTL: - warnx("State: RTL"); - break; - default: - warnx("State: Unknown"); - break; + case NAV_STATE_NONE: + warnx("State: None"); + break; + + case NAV_STATE_LOITER: + warnx("State: Loiter"); + break; + + case NAV_STATE_MISSION: + warnx("State: Mission"); + break; + + case NAV_STATE_RTL: + warnx("State: RTL"); + break; + + default: + warnx("State: Unknown"); + break; } } StateTable::Tran const Navigator::myTable[NAV_STATE_MAX][MAX_EVENT] = { - { - /* STATE_NONE */ + { + /* STATE_NONE */ /* EVENT_NONE_REQUESTED */ {NO_ACTION, NAV_STATE_NONE}, /* EVENT_READY_REQUESTED */ {ACTION(&Navigator::start_ready), NAV_STATE_READY}, /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER}, @@ -903,8 +930,8 @@ StateTable::Tran const Navigator::myTable[NAV_STATE_MAX][MAX_EVENT] = { /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_NONE}, /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_NONE}, }, - { - /* STATE_READY */ + { + /* STATE_READY */ /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE}, /* EVENT_READY_REQUESTED */ {NO_ACTION, NAV_STATE_READY}, /* EVENT_LOITER_REQUESTED */ {NO_ACTION, NAV_STATE_READY}, @@ -915,7 +942,7 @@ StateTable::Tran const Navigator::myTable[NAV_STATE_MAX][MAX_EVENT] = { /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_READY}, }, { - /* STATE_LOITER */ + /* STATE_LOITER */ /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE}, /* EVENT_READY_REQUESTED */ {NO_ACTION, NAV_STATE_LOITER}, /* EVENT_LOITER_REQUESTED */ {NO_ACTION, NAV_STATE_LOITER}, @@ -925,8 +952,8 @@ StateTable::Tran const Navigator::myTable[NAV_STATE_MAX][MAX_EVENT] = { /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_LOITER}, /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_LOITER}, }, - { - /* STATE_MISSION */ + { + /* STATE_MISSION */ /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE}, /* EVENT_READY_REQUESTED */ {ACTION(&Navigator::start_ready), NAV_STATE_READY}, /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER}, @@ -936,8 +963,8 @@ StateTable::Tran const Navigator::myTable[NAV_STATE_MAX][MAX_EVENT] = { /* EVENT_MISSION_CHANGED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_MISSION}, }, - { - /* STATE_RTL */ + { + /* STATE_RTL */ /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE}, /* EVENT_READY_REQUESTED */ {ACTION(&Navigator::start_ready), NAV_STATE_READY}, /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER}, @@ -948,7 +975,7 @@ StateTable::Tran const Navigator::myTable[NAV_STATE_MAX][MAX_EVENT] = { /* EVENT_HOME_POSITION_CHANGED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, // TODO need to reset rtl_state }, { - /* STATE_LAND */ + /* STATE_LAND */ /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE}, /* EVENT_READY_REQUESTED */ {ACTION(&Navigator::start_ready), NAV_STATE_READY}, /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER}, @@ -1013,6 +1040,7 @@ Navigator::start_loiter() if (_global_pos.alt < min_alt_amsl) { _pos_sp_triplet.current.alt = min_alt_amsl; mavlink_log_info(_mavlink_fd, "[navigator] loiter %.1fm higher", (double)(min_alt_amsl - _global_pos.alt)); + } else { _pos_sp_triplet.current.alt = _global_pos.alt; mavlink_log_info(_mavlink_fd, "[navigator] loiter at current altitude"); @@ -1065,22 +1093,22 @@ Navigator::set_mission_item() position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item); if (_mission_item.nav_cmd != NAV_CMD_RETURN_TO_LAUNCH && - _mission_item.nav_cmd != NAV_CMD_LOITER_TIME_LIMIT && - _mission_item.nav_cmd != NAV_CMD_LOITER_TURN_COUNT && - _mission_item.nav_cmd != NAV_CMD_LOITER_UNLIMITED) { + _mission_item.nav_cmd != NAV_CMD_LOITER_TIME_LIMIT && + _mission_item.nav_cmd != NAV_CMD_LOITER_TURN_COUNT && + _mission_item.nav_cmd != NAV_CMD_LOITER_UNLIMITED) { /* don't reset RTL state on RTL or LOITER items */ _rtl_state = RTL_STATE_NONE; } if (_vstatus.is_rotary_wing) { if (_need_takeoff && ( - _mission_item.nav_cmd == NAV_CMD_TAKEOFF || - _mission_item.nav_cmd == NAV_CMD_WAYPOINT || - _mission_item.nav_cmd == NAV_CMD_RETURN_TO_LAUNCH || - _mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || - _mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT || - _mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED - )) { + _mission_item.nav_cmd == NAV_CMD_TAKEOFF || + _mission_item.nav_cmd == NAV_CMD_WAYPOINT || + _mission_item.nav_cmd == NAV_CMD_RETURN_TO_LAUNCH || + _mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || + _mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT || + _mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED + )) { /* do special TAKEOFF handling for VTOL */ _need_takeoff = false; @@ -1108,6 +1136,7 @@ Navigator::set_mission_item() _pos_sp_triplet.current.yaw = NAN; _pos_sp_triplet.current.type = SETPOINT_TYPE_TAKEOFF; } + } else if (_mission_item.nav_cmd == NAV_CMD_LAND) { /* will need takeoff after landing */ _need_takeoff = true; @@ -1116,13 +1145,16 @@ Navigator::set_mission_item() if (_do_takeoff) { mavlink_log_info(_mavlink_fd, "[navigator] takeoff to %.1fm AMSL", _pos_sp_triplet.current.alt); + } else { if (onboard) { mavlink_log_info(_mavlink_fd, "[navigator] heading to onboard WP %d", index); + } else { mavlink_log_info(_mavlink_fd, "[navigator] heading to offboard WP %d", index); } } + } else { /* since a mission is not advanced without WPs available, this is not supposed to happen */ _mission_item_valid = false; @@ -1136,6 +1168,7 @@ Navigator::set_mission_item() if (ret == OK) { position_setpoint_from_mission_item(&_pos_sp_triplet.next, &item_next); + } else { /* this will fail for the last WP */ _pos_sp_triplet.next.valid = false; @@ -1149,17 +1182,21 @@ void Navigator::start_rtl() { _do_takeoff = false; + if (_rtl_state == RTL_STATE_NONE) { if (_global_pos.alt < _home_pos.alt + _parameters.rtl_alt) { _rtl_state = RTL_STATE_CLIMB; + } else { _rtl_state = RTL_STATE_RETURN; + if (_reset_loiter_pos) { _mission_item.altitude_is_relative = false; _mission_item.altitude = _global_pos.alt; } } } + _reset_loiter_pos = true; mavlink_log_info(_mavlink_fd, "[navigator] RTL started"); set_rtl_item(); @@ -1191,118 +1228,123 @@ Navigator::set_rtl_item() { switch (_rtl_state) { case RTL_STATE_CLIMB: { - memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s)); + memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s)); - float climb_alt = _home_pos.alt + _parameters.rtl_alt; - if (_vstatus.condition_landed) { - climb_alt = fmaxf(climb_alt, _global_pos.alt + _parameters.rtl_alt); + float climb_alt = _home_pos.alt + _parameters.rtl_alt; + + if (_vstatus.condition_landed) { + climb_alt = fmaxf(climb_alt, _global_pos.alt + _parameters.rtl_alt); + } + + _mission_item_valid = true; + + _mission_item.lat = _global_pos.lat; + _mission_item.lon = _global_pos.lon; + _mission_item.altitude_is_relative = false; + _mission_item.altitude = climb_alt; + _mission_item.yaw = NAN; + _mission_item.loiter_radius = _parameters.loiter_radius; + _mission_item.loiter_direction = 1; + _mission_item.nav_cmd = NAV_CMD_TAKEOFF; + _mission_item.acceptance_radius = _parameters.acceptance_radius; + _mission_item.time_inside = 0.0f; + _mission_item.pitch_min = 0.0f; + _mission_item.autocontinue = true; + _mission_item.origin = ORIGIN_ONBOARD; + + position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item); + + _pos_sp_triplet.next.valid = false; + + mavlink_log_info(_mavlink_fd, "[navigator] RTL: climb to %.1fm", climb_alt - _home_pos.alt); + break; } - _mission_item_valid = true; - - _mission_item.lat = _global_pos.lat; - _mission_item.lon = _global_pos.lon; - _mission_item.altitude_is_relative = false; - _mission_item.altitude = climb_alt; - _mission_item.yaw = NAN; - _mission_item.loiter_radius = _parameters.loiter_radius; - _mission_item.loiter_direction = 1; - _mission_item.nav_cmd = NAV_CMD_TAKEOFF; - _mission_item.acceptance_radius = _parameters.acceptance_radius; - _mission_item.time_inside = 0.0f; - _mission_item.pitch_min = 0.0f; - _mission_item.autocontinue = true; - _mission_item.origin = ORIGIN_ONBOARD; - - position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item); - - _pos_sp_triplet.next.valid = false; - - mavlink_log_info(_mavlink_fd, "[navigator] RTL: climb to %.1fm", climb_alt - _home_pos.alt); - break; - } case RTL_STATE_RETURN: { - memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s)); + memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s)); - _mission_item_valid = true; + _mission_item_valid = true; - _mission_item.lat = _home_pos.lat; - _mission_item.lon = _home_pos.lon; - // don't change altitude - _mission_item.yaw = NAN; // TODO set heading to home - _mission_item.loiter_radius = _parameters.loiter_radius; - _mission_item.loiter_direction = 1; - _mission_item.nav_cmd = NAV_CMD_WAYPOINT; - _mission_item.acceptance_radius = _parameters.acceptance_radius; - _mission_item.time_inside = 0.0f; - _mission_item.pitch_min = 0.0f; - _mission_item.autocontinue = true; - _mission_item.origin = ORIGIN_ONBOARD; + _mission_item.lat = _home_pos.lat; + _mission_item.lon = _home_pos.lon; + // don't change altitude + _mission_item.yaw = NAN; // TODO set heading to home + _mission_item.loiter_radius = _parameters.loiter_radius; + _mission_item.loiter_direction = 1; + _mission_item.nav_cmd = NAV_CMD_WAYPOINT; + _mission_item.acceptance_radius = _parameters.acceptance_radius; + _mission_item.time_inside = 0.0f; + _mission_item.pitch_min = 0.0f; + _mission_item.autocontinue = true; + _mission_item.origin = ORIGIN_ONBOARD; - position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item); + position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item); - _pos_sp_triplet.next.valid = false; + _pos_sp_triplet.next.valid = false; + + mavlink_log_info(_mavlink_fd, "[navigator] RTL: return"); + break; + } - mavlink_log_info(_mavlink_fd, "[navigator] RTL: return"); - break; - } case RTL_STATE_DESCEND: { - memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s)); + memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s)); - _mission_item_valid = true; + _mission_item_valid = true; - _mission_item.lat = _home_pos.lat; - _mission_item.lon = _home_pos.lon; - _mission_item.altitude_is_relative = false; - _mission_item.altitude = _home_pos.alt + _parameters.land_alt; - _mission_item.yaw = NAN; - _mission_item.loiter_radius = _parameters.loiter_radius; - _mission_item.loiter_direction = 1; - _mission_item.nav_cmd = NAV_CMD_WAYPOINT; - _mission_item.acceptance_radius = _parameters.acceptance_radius; - _mission_item.time_inside = 0.0f; - _mission_item.pitch_min = 0.0f; - _mission_item.autocontinue = true; - _mission_item.origin = ORIGIN_ONBOARD; + _mission_item.lat = _home_pos.lat; + _mission_item.lon = _home_pos.lon; + _mission_item.altitude_is_relative = false; + _mission_item.altitude = _home_pos.alt + _parameters.land_alt; + _mission_item.yaw = NAN; + _mission_item.loiter_radius = _parameters.loiter_radius; + _mission_item.loiter_direction = 1; + _mission_item.nav_cmd = NAV_CMD_WAYPOINT; + _mission_item.acceptance_radius = _parameters.acceptance_radius; + _mission_item.time_inside = 0.0f; + _mission_item.pitch_min = 0.0f; + _mission_item.autocontinue = true; + _mission_item.origin = ORIGIN_ONBOARD; - position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item); + position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item); - _pos_sp_triplet.next.valid = false; + _pos_sp_triplet.next.valid = false; + + mavlink_log_info(_mavlink_fd, "[navigator] RTL: descend to %.1fm AMSL", _mission_item.altitude); + break; + } - mavlink_log_info(_mavlink_fd, "[navigator] RTL: descend to %.1fm AMSL", _mission_item.altitude); - break; - } case RTL_STATE_LAND: { - memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s)); + memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s)); - _mission_item_valid = true; + _mission_item_valid = true; - _mission_item.lat = _home_pos.lat; - _mission_item.lon = _home_pos.lon; - _mission_item.altitude_is_relative = false; - _mission_item.altitude = _home_pos.alt; - _mission_item.yaw = NAN; - _mission_item.loiter_radius = _parameters.loiter_radius; - _mission_item.loiter_direction = 1; - _mission_item.nav_cmd = NAV_CMD_LAND; - _mission_item.acceptance_radius = _parameters.acceptance_radius; - _mission_item.time_inside = 0.0f; - _mission_item.pitch_min = 0.0f; - _mission_item.autocontinue = true; - _mission_item.origin = ORIGIN_ONBOARD; + _mission_item.lat = _home_pos.lat; + _mission_item.lon = _home_pos.lon; + _mission_item.altitude_is_relative = false; + _mission_item.altitude = _home_pos.alt; + _mission_item.yaw = NAN; + _mission_item.loiter_radius = _parameters.loiter_radius; + _mission_item.loiter_direction = 1; + _mission_item.nav_cmd = NAV_CMD_LAND; + _mission_item.acceptance_radius = _parameters.acceptance_radius; + _mission_item.time_inside = 0.0f; + _mission_item.pitch_min = 0.0f; + _mission_item.autocontinue = true; + _mission_item.origin = ORIGIN_ONBOARD; - position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item); + position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item); - _pos_sp_triplet.next.valid = false; + _pos_sp_triplet.next.valid = false; + + mavlink_log_info(_mavlink_fd, "[navigator] RTL: land"); + break; + } - mavlink_log_info(_mavlink_fd, "[navigator] RTL: land"); - break; - } default: { - mavlink_log_critical(_mavlink_fd, "[navigator] error: unknown RTL state: %d", _rtl_state); - start_loiter(); - break; - } + mavlink_log_critical(_mavlink_fd, "[navigator] error: unknown RTL state: %d", _rtl_state); + start_loiter(); + break; + } } publish_position_setpoint_triplet(); @@ -1312,28 +1354,35 @@ void Navigator::position_setpoint_from_mission_item(position_setpoint_s *sp, mission_item_s *item) { sp->valid = true; + if (item->nav_cmd == NAV_CMD_RETURN_TO_LAUNCH) { /* set home position for RTL item */ sp->lat = _home_pos.lat; sp->lon = _home_pos.lon; sp->alt = _home_pos.alt + _parameters.rtl_alt; + } else { sp->lat = item->lat; sp->lon = item->lon; sp->alt = item->altitude_is_relative ? item->altitude + _home_pos.alt : item->altitude; } + sp->yaw = item->yaw; sp->loiter_radius = item->loiter_radius; sp->loiter_direction = item->loiter_direction; sp->pitch_min = item->pitch_min; + if (item->nav_cmd == NAV_CMD_TAKEOFF) { sp->type = SETPOINT_TYPE_TAKEOFF; + } else if (item->nav_cmd == NAV_CMD_LAND) { sp->type = SETPOINT_TYPE_LAND; + } else if (item->nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || - item->nav_cmd == NAV_CMD_LOITER_TURN_COUNT || - item->nav_cmd == NAV_CMD_LOITER_UNLIMITED) { + item->nav_cmd == NAV_CMD_LOITER_TURN_COUNT || + item->nav_cmd == NAV_CMD_LOITER_UNLIMITED) { sp->type = SETPOINT_TYPE_LOITER; + } else { sp->type = SETPOINT_TYPE_NORMAL; } @@ -1350,6 +1399,7 @@ Navigator::check_mission_item_reached() if (_mission_item.nav_cmd == NAV_CMD_LAND) { if (_vstatus.is_rotary_wing) { return _vstatus.condition_landed; + } else { /* For fw there is currently no landing detector: * make sure control is not stopped when overshooting the landing waypoint */ @@ -1364,7 +1414,7 @@ Navigator::check_mission_item_reached() _mission_item.loiter_radius > 0.01f) { return false; - } + } uint64_t now = hrt_absolute_time(); @@ -1384,18 +1434,20 @@ Navigator::check_mission_item_reached() /* current relative or AMSL altitude depending on mission item altitude_is_relative flag */ float wp_alt_amsl = _mission_item.altitude; + if (_mission_item.altitude_is_relative) wp_alt_amsl += _home_pos.alt; dist = get_distance_to_point_global_wgs84(_mission_item.lat, _mission_item.lon, wp_alt_amsl, - (double)_global_pos.lat, (double)_global_pos.lon, _global_pos.alt, - &dist_xy, &dist_z); + (double)_global_pos.lat, (double)_global_pos.lon, _global_pos.alt, + &dist_xy, &dist_z); if (_do_takeoff) { if (_global_pos.alt > wp_alt_amsl - acceptance_radius) { /* require only altitude for takeoff */ _waypoint_position_reached = true; } + } else { if (dist >= 0.0f && dist <= acceptance_radius) { _waypoint_position_reached = true; @@ -1407,9 +1459,11 @@ Navigator::check_mission_item_reached() if (_vstatus.is_rotary_wing && !_do_takeoff && isfinite(_mission_item.yaw)) { /* check yaw if defined only for rotary wing except takeoff */ float yaw_err = _wrap_pi(_mission_item.yaw - _global_pos.yaw); + if (fabsf(yaw_err) < 0.05f) { /* XXX get rid of magic number */ _waypoint_yaw_reached = true; } + } else { _waypoint_yaw_reached = true; } @@ -1419,20 +1473,22 @@ Navigator::check_mission_item_reached() if (_waypoint_position_reached && _waypoint_yaw_reached) { if (_time_first_inside_orbit == 0) { _time_first_inside_orbit = now; + if (_mission_item.time_inside > 0.01f) { mavlink_log_info(_mavlink_fd, "[navigator] waypoint reached, wait for %.1fs", _mission_item.time_inside); } } - + /* check if the MAV was long enough inside the waypoint orbit */ if ((now - _time_first_inside_orbit >= (uint64_t)_mission_item.time_inside * 1e6) - || _mission_item.nav_cmd == NAV_CMD_TAKEOFF) { + || _mission_item.nav_cmd == NAV_CMD_TAKEOFF) { _time_first_inside_orbit = 0; _waypoint_yaw_reached = false; _waypoint_position_reached = false; return true; } } + return false; } @@ -1445,6 +1501,7 @@ Navigator::on_mission_item_reached() /* takeoff completed */ _do_takeoff = false; mavlink_log_info(_mavlink_fd, "[navigator] takeoff completed"); + } else { /* advance by one mission item */ _mission.move_to_next(); @@ -1452,23 +1509,28 @@ Navigator::on_mission_item_reached() if (_mission.current_mission_available()) { set_mission_item(); + } else { /* if no more mission items available then finish mission */ /* loiter at last waypoint */ _reset_loiter_pos = false; mavlink_log_info(_mavlink_fd, "[navigator] mission completed"); + if (_vstatus.condition_landed) { dispatch(EVENT_READY_REQUESTED); + } else { dispatch(EVENT_LOITER_REQUESTED); } } + } else { /* RTL finished */ if (_rtl_state == RTL_STATE_LAND) { /* landed at home position */ mavlink_log_info(_mavlink_fd, "[navigator] RTL completed, landed"); dispatch(EVENT_READY_REQUESTED); + } else { /* next RTL step */ _rtl_state = (RTLState)(_rtl_state + 1); @@ -1546,6 +1608,7 @@ Navigator::publish_control_mode() default: break; } + break; case FAILSAFE_STATE_RTL: @@ -1583,6 +1646,7 @@ Navigator::publish_control_mode() /* navigator has control, set control mode flags according to nav state*/ if (navigator_enabled) { _control_mode.flag_control_manual_enabled = false; + if (myState == NAV_STATE_READY) { /* disable all controllers, armed but idle */ _control_mode.flag_control_rates_enabled = false; @@ -1591,6 +1655,7 @@ Navigator::publish_control_mode() _control_mode.flag_control_velocity_enabled = false; _control_mode.flag_control_altitude_enabled = false; _control_mode.flag_control_climb_rate_enabled = false; + } else { _control_mode.flag_control_rates_enabled = true; _control_mode.flag_control_attitude_enabled = true; @@ -1669,8 +1734,9 @@ int navigator_main(int argc, char *argv[]) } else if (!strcmp(argv[1], "fence")) { navigator::g_navigator->add_fence_point(argc - 2, argv + 2); + } else if (!strcmp(argv[1], "fencefile")) { - navigator::g_navigator->load_fence_from_file(GEOFENCE_FILENAME); + navigator::g_navigator->load_fence_from_file(GEOFENCE_FILENAME); } else { usage(); From b7c69262a7e4d51fb7806ab40a4dbb2b0ea4f75b Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 26 Jan 2014 11:58:50 +0100 Subject: [PATCH 344/447] state_machine_helper: added missed transition to FAILSAFE_STATE_LAND, transition conditions fixed --- src/modules/commander/state_machine_helper.cpp | 13 ++++++++++++- 1 file changed, 12 insertions(+), 1 deletion(-) diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 1be0a16b88..c7256583a3 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -377,17 +377,28 @@ transition_result_t failsafe_state_transition(struct vehicle_status_s *status, f } else { switch (new_failsafe_state) { case FAILSAFE_STATE_NORMAL: + /* always allowed (except from TERMINATION state) */ ret = TRANSITION_CHANGED; break; case FAILSAFE_STATE_RTL: - if (status->condition_global_position_valid) { + /* global position and home position required for RTL */ + if (status->condition_global_position_valid && status->condition_home_position_valid) { + ret = TRANSITION_CHANGED; + } + + break; + + case FAILSAFE_STATE_LAND: + /* at least relative altitude estimate required for landing */ + if (status->condition_local_altitude_valid || status->condition_global_position_valid) { ret = TRANSITION_CHANGED; } break; case FAILSAFE_STATE_TERMINATION: + /* always allowed */ ret = TRANSITION_CHANGED; break; From c841929e3f617e67adec6606b3ec6517aa455834 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 26 Jan 2014 14:12:27 +0100 Subject: [PATCH 345/447] =?UTF-8?q?commander:=20=C2=ABhome=20position=20se?= =?UTF-8?q?t=C2=BB=20condition=20fixed,=20failsafe=20fixes,=20navigator:?= =?UTF-8?q?=20state=20indication=20bugfix,=20control=5Fmode=20fixes?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/modules/commander/commander.cpp | 264 +++++++++++------------ src/modules/navigator/navigator_main.cpp | 34 +-- 2 files changed, 152 insertions(+), 146 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 15f229bf00..0e91728004 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -565,7 +565,6 @@ int commander_thread_main(int argc, char *argv[]) { /* not yet initialized */ commander_initialized = false; - bool home_position_set = false; bool battery_tune_played = false; bool arm_tune_played = false; @@ -597,7 +596,8 @@ int commander_thread_main(int argc, char *argv[]) char *failsafe_states_str[FAILSAFE_STATE_MAX]; failsafe_states_str[0] = "NORMAL"; failsafe_states_str[1] = "RTL"; - failsafe_states_str[2] = "TERMINATION"; + failsafe_states_str[2] = "LAND"; + failsafe_states_str[3] = "TERMINATION"; /* pthread for slow low prio thread */ pthread_t commander_low_prio_thread; @@ -628,6 +628,7 @@ int commander_thread_main(int argc, char *argv[]) status.set_nav_state_timestamp = 0; status.arming_state = ARMING_STATE_INIT; status.hil_state = HIL_STATE_OFF; + status.failsafe_state = FAILSAFE_STATE_NORMAL; /* neither manual nor offboard control commands have been received */ status.offboard_control_signal_found_once = false; @@ -1027,13 +1028,12 @@ int commander_thread_main(int argc, char *argv[]) * position to the current position. */ - if (!home_position_set && gps_position.fix_type >= 3 && - (gps_position.eph_m < hdop_threshold_m) && (gps_position.epv_m < vdop_threshold_m) && // XXX note that vdop is 0 for mtk + if (!status.condition_home_position_valid && gps_position.fix_type >= 3 && + (gps_position.eph_m < hdop_threshold_m) && (gps_position.epv_m < vdop_threshold_m) && (hrt_absolute_time() < gps_position.timestamp_position + POSITION_TIMEOUT) && !armed.armed && global_position.valid) { + /* copy position data to uORB home message, store it locally as well */ - - home.lat = global_position.lat; home.lon = global_position.lon; home.alt = global_position.alt; @@ -1050,164 +1050,160 @@ int commander_thread_main(int argc, char *argv[]) } /* mark home position as set */ - home_position_set = true; + status.condition_home_position_valid = true; tune_positive(); } } - /* ignore RC signals if in offboard control mode */ - if (!status.offboard_control_signal_found_once && sp_man.timestamp != 0) { - /* start RC input check */ - if (hrt_absolute_time() < sp_man.timestamp + RC_TIMEOUT) { - /* handle the case where RC signal was regained */ - if (!status.rc_signal_found_once) { - status.rc_signal_found_once = true; - mavlink_log_critical(mavlink_fd, "#audio: detected RC signal first time"); + /* start RC input check */ + if (sp_man.timestamp != 0 && hrt_absolute_time() < sp_man.timestamp + RC_TIMEOUT) { + /* handle the case where RC signal was regained */ + if (!status.rc_signal_found_once) { + status.rc_signal_found_once = true; + mavlink_log_critical(mavlink_fd, "#audio: detected RC signal first time"); + status_changed = true; + + } else { + if (status.rc_signal_lost) { + mavlink_log_critical(mavlink_fd, "#audio: RC signal regained"); status_changed = true; - - } else { - if (status.rc_signal_lost) { - mavlink_log_critical(mavlink_fd, "#audio: RC signal regained"); - status_changed = true; - } } + } - status.rc_signal_lost = false; + status.rc_signal_lost = false; - transition_result_t res; // store all transitions results here + transition_result_t res; // store all transitions results here - /* arm/disarm by RC */ - res = TRANSITION_NOT_CHANGED; + /* arm/disarm by RC */ + res = TRANSITION_NOT_CHANGED; - /* check if left stick is in lower left position and we are in MANUAL or AUTO_READY mode or (ASSISTED mode and landed) -> disarm - * do it only for rotary wings */ - if (status.is_rotary_wing && - (status.arming_state == ARMING_STATE_ARMED || status.arming_state == ARMING_STATE_ARMED_ERROR) && - (status.main_state == MAIN_STATE_MANUAL || status.condition_landed) && - sp_man.yaw < -STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) { + /* check if left stick is in lower left position and we are in MANUAL or AUTO_READY mode or (ASSISTED mode and landed) -> disarm + * do it only for rotary wings */ + if (status.is_rotary_wing && + (status.arming_state == ARMING_STATE_ARMED || status.arming_state == ARMING_STATE_ARMED_ERROR) && + (status.main_state == MAIN_STATE_MANUAL || status.condition_landed) && + sp_man.yaw < -STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) { - if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { - /* disarm to STANDBY if ARMED or to STANDBY_ERROR if ARMED_ERROR */ - arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR); - res = arming_state_transition(&status, &safety, new_arming_state, &armed); - stick_off_counter = 0; - - } else { - stick_off_counter++; - } - - } else { + if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { + /* disarm to STANDBY if ARMED or to STANDBY_ERROR if ARMED_ERROR */ + arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR); + res = arming_state_transition(&status, &safety, new_arming_state, &armed); stick_off_counter = 0; - } - - /* check if left stick is in lower right position and we're in MANUAL mode -> arm */ - if (status.arming_state == ARMING_STATE_STANDBY && - sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) { - if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) { - if (safety.safety_switch_available && !safety.safety_off) { - print_reject_arm("NOT ARMING: Press safety switch first."); - - } else if (status.main_state != MAIN_STATE_MANUAL) { - print_reject_arm("NOT ARMING: Switch to MANUAL mode first."); - - } else { - res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed); - } - - stick_on_counter = 0; - - } else { - stick_on_counter++; - } } else { - stick_on_counter = 0; - } - - if (res == TRANSITION_CHANGED) { - if (status.arming_state == ARMING_STATE_ARMED) { - mavlink_log_info(mavlink_fd, "[cmd] ARMED by RC"); - - } else { - mavlink_log_info(mavlink_fd, "[cmd] DISARMED by RC"); - } - - } else if (res == TRANSITION_DENIED) { - /* DENIED here indicates bug in the commander */ - mavlink_log_critical(mavlink_fd, "ERROR: arming state transition denied"); - } - - if (status.failsafe_state != FAILSAFE_STATE_NORMAL) { - /* recover from failsafe */ - transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_NORMAL); - - if (res == TRANSITION_CHANGED) { - mavlink_log_critical(mavlink_fd, "#audio: recover from failsafe"); - } - } - - /* fill current_status according to mode switches */ - check_mode_switches(&sp_man, &status); - - /* evaluate the main state machine according to mode switches */ - res = check_main_state_machine(&status); - - if (res == TRANSITION_CHANGED) { - tune_positive(); - - } else if (res == TRANSITION_DENIED) { - /* DENIED here indicates bug in the commander */ - mavlink_log_critical(mavlink_fd, "ERROR: main state transition denied"); + stick_off_counter++; } } else { - if (!status.rc_signal_lost) { - mavlink_log_critical(mavlink_fd, "#audio: CRITICAL: RC SIGNAL LOST"); - status.rc_signal_lost = true; - status_changed = true; - } + stick_off_counter = 0; + } - if (status.main_state == MAIN_STATE_AUTO) { - /* check if AUTO mode still allowed */ - transition_result_t res = main_state_transition(&status, MAIN_STATE_AUTO); + /* check if left stick is in lower right position and we're in MANUAL mode -> arm */ + if (status.arming_state == ARMING_STATE_STANDBY && + sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) { + if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) { + if (safety.safety_switch_available && !safety.safety_off) { + print_reject_arm("NOT ARMING: Press safety switch first."); - if (res == TRANSITION_DENIED) { - /* AUTO mode denied, don't try RTL, switch to failsafe state LAND */ - res = failsafe_state_transition(&status, FAILSAFE_STATE_LAND); + } else if (status.main_state != MAIN_STATE_MANUAL) { + print_reject_arm("NOT ARMING: Switch to MANUAL mode first."); - if (res == TRANSITION_CHANGED) { - mavlink_log_critical(mavlink_fd, "#audio: failsafe: LAND"); - - } else if (res == TRANSITION_DENIED) { - /* LAND mode denied, switch to failsafe state TERMINATION */ - transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION); - - if (res == TRANSITION_CHANGED) { - mavlink_log_critical(mavlink_fd, "#audio: failsafe: TERMINATION"); - } - } + } else { + res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed); } - } else if (armed.armed) { - /* failsafe for manual modes */ - transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_RTL); + stick_on_counter = 0; + + } else { + stick_on_counter++; + } + + } else { + stick_on_counter = 0; + } + + if (res == TRANSITION_CHANGED) { + if (status.arming_state == ARMING_STATE_ARMED) { + mavlink_log_info(mavlink_fd, "[cmd] ARMED by RC"); + + } else { + mavlink_log_info(mavlink_fd, "[cmd] DISARMED by RC"); + } + + } else if (res == TRANSITION_DENIED) { + /* DENIED here indicates bug in the commander */ + mavlink_log_critical(mavlink_fd, "ERROR: arming state transition denied"); + } + + if (status.failsafe_state != FAILSAFE_STATE_NORMAL) { + /* recover from failsafe */ + transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_NORMAL); + + if (res == TRANSITION_CHANGED) { + mavlink_log_critical(mavlink_fd, "#audio: recover from failsafe"); + } + } + + /* fill current_status according to mode switches */ + check_mode_switches(&sp_man, &status); + + /* evaluate the main state machine according to mode switches */ + res = check_main_state_machine(&status); + + if (res == TRANSITION_CHANGED) { + tune_positive(); + + } else if (res == TRANSITION_DENIED) { + /* DENIED here indicates bug in the commander */ + mavlink_log_critical(mavlink_fd, "ERROR: main state transition denied"); + } + + } else { + if (!status.rc_signal_lost) { + mavlink_log_critical(mavlink_fd, "#audio: CRITICAL: RC SIGNAL LOST"); + status.rc_signal_lost = true; + status_changed = true; + } + + if (status.main_state == MAIN_STATE_AUTO) { + /* check if AUTO mode still allowed */ + transition_result_t res = main_state_transition(&status, MAIN_STATE_AUTO); + + if (res == TRANSITION_DENIED) { + /* AUTO mode denied, don't try RTL, switch to failsafe state LAND */ + res = failsafe_state_transition(&status, FAILSAFE_STATE_LAND); if (res == TRANSITION_CHANGED) { - mavlink_log_critical(mavlink_fd, "#audio: failsafe: RTL"); + mavlink_log_critical(mavlink_fd, "#audio: failsafe: LAND"); } else if (res == TRANSITION_DENIED) { - /* RTL not allowed (no global position estimate), try LAND */ - res = failsafe_state_transition(&status, FAILSAFE_STATE_LAND); + /* LAND mode denied, switch to failsafe state TERMINATION */ + transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION); if (res == TRANSITION_CHANGED) { - mavlink_log_critical(mavlink_fd, "#audio: failsafe: LAND"); + mavlink_log_critical(mavlink_fd, "#audio: failsafe: TERMINATION"); + } + } + } - } else if (res == TRANSITION_DENIED) { - res = failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION); + } else if (armed.armed) { + /* failsafe for manual modes */ + transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_RTL); + if (res == TRANSITION_CHANGED) { + mavlink_log_critical(mavlink_fd, "#audio: failsafe: RTL"); - if (res == TRANSITION_CHANGED) { - mavlink_log_critical(mavlink_fd, "#audio: failsafe: TERMINATION"); - } + } else if (res == TRANSITION_DENIED) { + /* RTL not allowed (no global position estimate), try LAND */ + res = failsafe_state_transition(&status, FAILSAFE_STATE_LAND); + + if (res == TRANSITION_CHANGED) { + mavlink_log_critical(mavlink_fd, "#audio: failsafe: LAND"); + + } else if (res == TRANSITION_DENIED) { + res = failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION); + + if (res == TRANSITION_CHANGED) { + mavlink_log_critical(mavlink_fd, "#audio: failsafe: TERMINATION"); } } } diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 9b5cfa6b4b..7c37237ff2 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -411,11 +411,13 @@ Navigator::Navigator() : memset(&_mission_result, 0, sizeof(struct mission_result_s)); memset(&_mission_item, 0, sizeof(struct mission_item_s)); + memset(&nav_states_str, 0, sizeof(nav_states_str)); nav_states_str[0] = "NONE"; nav_states_str[1] = "READY"; nav_states_str[2] = "LOITER"; nav_states_str[3] = "MISSION"; nav_states_str[4] = "RTL"; + nav_states_str[5] = "LAND"; /* Initialize state machine */ myState = NAV_STATE_NONE; @@ -750,6 +752,7 @@ Navigator::task_main() } else if (_vstatus.failsafe_state == FAILSAFE_STATE_RTL) { /* RTL on failsafe */ if (myState != NAV_STATE_READY || _rtl_state != RTL_STATE_LAND) { + dispatch(EVENT_RTL_REQUESTED); } @@ -981,7 +984,7 @@ StateTable::Tran const Navigator::myTable[NAV_STATE_MAX][MAX_EVENT] = { /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER}, /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, - /* EVENT_LAND_REQUESTED */ {ACTION(&Navigator::start_land), NAV_STATE_LAND}, + /* EVENT_LAND_REQUESTED */ {NO_ACTION, NAV_STATE_LAND}, /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_RTL}, /* EVENT_HOME_POSITION_CHANGED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, }, @@ -1220,7 +1223,7 @@ Navigator::start_land() _pos_sp_triplet.current.loiter_radius = _parameters.loiter_radius; _pos_sp_triplet.current.yaw = NAN; - mavlink_log_info(_mavlink_fd, "[navigator] LAND started"); + mavlink_log_info(_mavlink_fd, "[navigator] land started"); } void @@ -1616,14 +1619,7 @@ Navigator::publish_control_mode() break; case FAILSAFE_STATE_LAND: - /* land with or without position control */ - _control_mode.flag_control_manual_enabled = false; - _control_mode.flag_control_rates_enabled = true; - _control_mode.flag_control_attitude_enabled = true; - _control_mode.flag_control_position_enabled = _vstatus.condition_global_position_valid; - _control_mode.flag_control_velocity_enabled = _vstatus.condition_global_position_valid; - _control_mode.flag_control_altitude_enabled = true; - _control_mode.flag_control_climb_rate_enabled = true; + navigator_enabled = true; break; case FAILSAFE_STATE_TERMINATION: @@ -1647,7 +1643,8 @@ Navigator::publish_control_mode() if (navigator_enabled) { _control_mode.flag_control_manual_enabled = false; - if (myState == NAV_STATE_READY) { + switch (myState) { + case NAV_STATE_READY: /* disable all controllers, armed but idle */ _control_mode.flag_control_rates_enabled = false; _control_mode.flag_control_attitude_enabled = false; @@ -1655,14 +1652,27 @@ Navigator::publish_control_mode() _control_mode.flag_control_velocity_enabled = false; _control_mode.flag_control_altitude_enabled = false; _control_mode.flag_control_climb_rate_enabled = false; + break; - } else { + case NAV_STATE_LAND: + /* land with or without position control */ + _control_mode.flag_control_manual_enabled = false; + _control_mode.flag_control_rates_enabled = true; + _control_mode.flag_control_attitude_enabled = true; + _control_mode.flag_control_position_enabled = _vstatus.condition_global_position_valid; + _control_mode.flag_control_velocity_enabled = _vstatus.condition_global_position_valid; + _control_mode.flag_control_altitude_enabled = true; + _control_mode.flag_control_climb_rate_enabled = true; + break; + + default: _control_mode.flag_control_rates_enabled = true; _control_mode.flag_control_attitude_enabled = true; _control_mode.flag_control_position_enabled = true; _control_mode.flag_control_velocity_enabled = true; _control_mode.flag_control_altitude_enabled = true; _control_mode.flag_control_climb_rate_enabled = true; + break; } } From 9e9105048ac70b7abaa40ef8ce3f6f75910ada0a Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 26 Jan 2014 15:46:14 +0100 Subject: [PATCH 346/447] commander, navigator: failsafe fixes, mavlink messages cleanup --- src/modules/commander/commander.cpp | 71 ++++++++++-------------- src/modules/navigator/navigator_main.cpp | 8 +-- 2 files changed, 32 insertions(+), 47 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 0e91728004..4c29d774af 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1080,9 +1080,9 @@ int commander_thread_main(int argc, char *argv[]) /* check if left stick is in lower left position and we are in MANUAL or AUTO_READY mode or (ASSISTED mode and landed) -> disarm * do it only for rotary wings */ if (status.is_rotary_wing && - (status.arming_state == ARMING_STATE_ARMED || status.arming_state == ARMING_STATE_ARMED_ERROR) && - (status.main_state == MAIN_STATE_MANUAL || status.condition_landed) && - sp_man.yaw < -STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) { + (status.arming_state == ARMING_STATE_ARMED || status.arming_state == ARMING_STATE_ARMED_ERROR) && + (status.main_state == MAIN_STATE_MANUAL || status.condition_landed) && + sp_man.yaw < -STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) { if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { /* disarm to STANDBY if ARMED or to STANDBY_ERROR if ARMED_ERROR */ @@ -1100,7 +1100,7 @@ int commander_thread_main(int argc, char *argv[]) /* check if left stick is in lower right position and we're in MANUAL mode -> arm */ if (status.arming_state == ARMING_STATE_STANDBY && - sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) { + sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) { if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) { if (safety.safety_switch_available && !safety.safety_off) { print_reject_arm("NOT ARMING: Press safety switch first."); @@ -1138,10 +1138,6 @@ int commander_thread_main(int argc, char *argv[]) if (status.failsafe_state != FAILSAFE_STATE_NORMAL) { /* recover from failsafe */ transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_NORMAL); - - if (res == TRANSITION_CHANGED) { - mavlink_log_critical(mavlink_fd, "#audio: recover from failsafe"); - } } /* fill current_status according to mode switches */ @@ -1165,47 +1161,40 @@ int commander_thread_main(int argc, char *argv[]) status_changed = true; } - if (status.main_state == MAIN_STATE_AUTO) { - /* check if AUTO mode still allowed */ - transition_result_t res = main_state_transition(&status, MAIN_STATE_AUTO); + if (armed.armed) { + if (status.main_state == MAIN_STATE_AUTO) { + /* check if AUTO mode still allowed */ + transition_result_t res = main_state_transition(&status, MAIN_STATE_AUTO); - if (res == TRANSITION_DENIED) { - /* AUTO mode denied, don't try RTL, switch to failsafe state LAND */ - res = failsafe_state_transition(&status, FAILSAFE_STATE_LAND); + if (res == TRANSITION_DENIED) { + /* AUTO mode denied, don't try RTL, switch to failsafe state LAND */ + res = failsafe_state_transition(&status, FAILSAFE_STATE_LAND); - if (res == TRANSITION_CHANGED) { - mavlink_log_critical(mavlink_fd, "#audio: failsafe: LAND"); + if (res == TRANSITION_DENIED) { + /* LAND not allowed, set TERMINATION state */ + transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION); + } + } - } else if (res == TRANSITION_DENIED) { - /* LAND mode denied, switch to failsafe state TERMINATION */ - transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION); + } else { + /* failsafe for manual modes */ + transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_RTL); - if (res == TRANSITION_CHANGED) { - mavlink_log_critical(mavlink_fd, "#audio: failsafe: TERMINATION"); + if (res == TRANSITION_DENIED) { + /* RTL not allowed (no global position estimate), try LAND */ + res = failsafe_state_transition(&status, FAILSAFE_STATE_LAND); + + if (res == TRANSITION_DENIED) { + /* LAND not allowed, set TERMINATION state */ + res = failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION); } } } - } else if (armed.armed) { - /* failsafe for manual modes */ - transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_RTL); - if (res == TRANSITION_CHANGED) { - mavlink_log_critical(mavlink_fd, "#audio: failsafe: RTL"); - - } else if (res == TRANSITION_DENIED) { - /* RTL not allowed (no global position estimate), try LAND */ - res = failsafe_state_transition(&status, FAILSAFE_STATE_LAND); - - if (res == TRANSITION_CHANGED) { - mavlink_log_critical(mavlink_fd, "#audio: failsafe: LAND"); - - } else if (res == TRANSITION_DENIED) { - res = failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION); - - if (res == TRANSITION_CHANGED) { - mavlink_log_critical(mavlink_fd, "#audio: failsafe: TERMINATION"); - } - } + } else { + if (status.failsafe_state != FAILSAFE_STATE_NORMAL) { + /* reset failsafe when disarmed */ + transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_NORMAL); } } } diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 7c37237ff2..4be9f055b3 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -1072,7 +1072,6 @@ Navigator::start_mission() { _need_takeoff = true; - mavlink_log_info(_mavlink_fd, "[navigator] mission started"); set_mission_item(); } @@ -1201,7 +1200,6 @@ Navigator::start_rtl() } _reset_loiter_pos = true; - mavlink_log_info(_mavlink_fd, "[navigator] RTL started"); set_rtl_item(); } @@ -1222,8 +1220,6 @@ Navigator::start_land() _pos_sp_triplet.current.loiter_direction = 1; _pos_sp_triplet.current.loiter_radius = _parameters.loiter_radius; _pos_sp_triplet.current.yaw = NAN; - - mavlink_log_info(_mavlink_fd, "[navigator] land started"); } void @@ -1259,7 +1255,7 @@ Navigator::set_rtl_item() _pos_sp_triplet.next.valid = false; - mavlink_log_info(_mavlink_fd, "[navigator] RTL: climb to %.1fm", climb_alt - _home_pos.alt); + mavlink_log_info(_mavlink_fd, "[navigator] RTL: climb to %.1fm above home", climb_alt - _home_pos.alt); break; } @@ -1312,7 +1308,7 @@ Navigator::set_rtl_item() _pos_sp_triplet.next.valid = false; - mavlink_log_info(_mavlink_fd, "[navigator] RTL: descend to %.1fm AMSL", _mission_item.altitude); + mavlink_log_info(_mavlink_fd, "[navigator] RTL: descend to %.1fm above home", _mission_item.altitude - _home_pos.alt); break; } From 1e63e8d9321d685e3af09aa16b58c5985e878435 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 27 Jan 2014 12:07:27 +0100 Subject: [PATCH 347/447] navigator: wait before landing in RTL --- src/modules/navigator/navigator_main.cpp | 8 ++++++-- src/modules/navigator/navigator_params.c | 1 + 2 files changed, 7 insertions(+), 2 deletions(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 4be9f055b3..e311c469f5 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -198,6 +198,7 @@ private: float takeoff_alt; float land_alt; float rtl_alt; + float rtl_land_delay; } _parameters; /**< local copies of parameters */ struct { @@ -208,6 +209,7 @@ private: param_t takeoff_alt; param_t land_alt; param_t rtl_alt; + param_t rtl_land_delay; } _parameter_handles; /**< handles for parameters */ enum Event { @@ -406,6 +408,7 @@ Navigator::Navigator() : _parameter_handles.takeoff_alt = param_find("NAV_TAKEOFF_ALT"); _parameter_handles.land_alt = param_find("NAV_LAND_ALT"); _parameter_handles.rtl_alt = param_find("NAV_RTL_ALT"); + _parameter_handles.rtl_land_delay = param_find("NAV_RTL_LAND_T"); memset(&_pos_sp_triplet, 0, sizeof(struct position_setpoint_triplet_s)); memset(&_mission_result, 0, sizeof(struct mission_result_s)); @@ -463,6 +466,7 @@ Navigator::parameters_update() param_get(_parameter_handles.takeoff_alt, &(_parameters.takeoff_alt)); param_get(_parameter_handles.land_alt, &(_parameters.land_alt)); param_get(_parameter_handles.rtl_alt, &(_parameters.rtl_alt)); + param_get(_parameter_handles.rtl_land_delay, &(_parameters.rtl_land_delay)); _mission.set_onboard_mission_allowed((bool)_parameter_handles.onboard_mission_enabled); @@ -1299,9 +1303,9 @@ Navigator::set_rtl_item() _mission_item.loiter_direction = 1; _mission_item.nav_cmd = NAV_CMD_WAYPOINT; _mission_item.acceptance_radius = _parameters.acceptance_radius; - _mission_item.time_inside = 0.0f; + _mission_item.time_inside = _parameters.rtl_land_delay < 0.0 ? 0.0f : _parameters.rtl_land_delay; _mission_item.pitch_min = 0.0f; - _mission_item.autocontinue = true; + _mission_item.autocontinue = _parameters.rtl_land_delay > -0.001f; _mission_item.origin = ORIGIN_ONBOARD; position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item); diff --git a/src/modules/navigator/navigator_params.c b/src/modules/navigator/navigator_params.c index ff819fca40..af1d9d7d58 100644 --- a/src/modules/navigator/navigator_params.c +++ b/src/modules/navigator/navigator_params.c @@ -60,3 +60,4 @@ PARAM_DEFINE_INT32(NAV_ONB_MIS_EN, 0); PARAM_DEFINE_FLOAT(NAV_TAKEOFF_ALT, 10.0f); // default TAKEOFF altitude PARAM_DEFINE_FLOAT(NAV_LAND_ALT, 5.0f); // slow descend from this altitude when landing PARAM_DEFINE_FLOAT(NAV_RTL_ALT, 30.0f); // min altitude for going home in RTL mode +PARAM_DEFINE_FLOAT(NAV_RTL_LAND_T, 10.0f); // delay after descend before landing From ad51e0a08abcd7c9cd1d018f2bcaae3055b7c18e Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 27 Jan 2014 12:09:23 +0100 Subject: [PATCH 348/447] navigator: minor mavlink messages and comments fixes --- src/modules/navigator/navigator_main.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index e311c469f5..dd471928e3 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -1150,7 +1150,7 @@ Navigator::set_mission_item() } if (_do_takeoff) { - mavlink_log_info(_mavlink_fd, "[navigator] takeoff to %.1fm AMSL", _pos_sp_triplet.current.alt); + mavlink_log_info(_mavlink_fd, "[navigator] takeoff to %.1fm above home", _pos_sp_triplet.current.alt - _home_pos.alt); } else { if (onboard) { @@ -1435,7 +1435,7 @@ Navigator::check_mission_item_reached() float dist_xy = -1.0f; float dist_z = -1.0f; - /* current relative or AMSL altitude depending on mission item altitude_is_relative flag */ + /* calculate AMSL altitude for this waypoint */ float wp_alt_amsl = _mission_item.altitude; if (_mission_item.altitude_is_relative) From 20108ed95d5bbae64bfcb95de5404fa97d9d0ac1 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 27 Jan 2014 13:18:54 +0100 Subject: [PATCH 349/447] commander: minor refactoring current_status -> status --- src/modules/commander/commander.cpp | 58 ++++++++++++++--------------- 1 file changed, 29 insertions(+), 29 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 4c29d774af..f579fb52ad 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -199,9 +199,9 @@ void control_status_leds(vehicle_status_s *status, const actuator_armed_s *actua void check_valid(hrt_abstime timestamp, hrt_abstime timeout, bool valid_in, bool *valid_out, bool *changed); -void check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_status_s *current_status); +void check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_status_s *status); -transition_result_t check_main_state_machine(struct vehicle_status_s *current_status); +transition_result_t set_main_state_rc(struct vehicle_status_s *status); void print_reject_mode(const char *msg); @@ -1140,11 +1140,11 @@ int commander_thread_main(int argc, char *argv[]) transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_NORMAL); } - /* fill current_status according to mode switches */ + /* fill status according to mode switches */ check_mode_switches(&sp_man, &status); /* evaluate the main state machine according to mode switches */ - res = check_main_state_machine(&status); + res = set_main_state_rc(&status); if (res == TRANSITION_CHANGED) { tune_positive(); @@ -1418,72 +1418,72 @@ control_status_leds(vehicle_status_s *status, const actuator_armed_s *actuator_a } void -check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_status_s *current_status) +check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_status_s *status) { /* main mode switch */ if (!isfinite(sp_man->mode_switch)) { warnx("mode sw not finite"); - current_status->mode_switch = MODE_SWITCH_MANUAL; + status->mode_switch = MODE_SWITCH_MANUAL; } else if (sp_man->mode_switch > STICK_ON_OFF_LIMIT) { - current_status->mode_switch = MODE_SWITCH_AUTO; + status->mode_switch = MODE_SWITCH_AUTO; } else if (sp_man->mode_switch < -STICK_ON_OFF_LIMIT) { - current_status->mode_switch = MODE_SWITCH_MANUAL; + status->mode_switch = MODE_SWITCH_MANUAL; } else { - current_status->mode_switch = MODE_SWITCH_ASSISTED; + status->mode_switch = MODE_SWITCH_ASSISTED; } /* return switch */ if (!isfinite(sp_man->return_switch)) { - current_status->return_switch = RETURN_SWITCH_NONE; + status->return_switch = RETURN_SWITCH_NONE; } else if (sp_man->return_switch > STICK_ON_OFF_LIMIT) { - current_status->return_switch = RETURN_SWITCH_RETURN; + status->return_switch = RETURN_SWITCH_RETURN; } else { - current_status->return_switch = RETURN_SWITCH_NORMAL; + status->return_switch = RETURN_SWITCH_NORMAL; } /* assisted switch */ if (!isfinite(sp_man->assisted_switch)) { - current_status->assisted_switch = ASSISTED_SWITCH_SEATBELT; + status->assisted_switch = ASSISTED_SWITCH_SEATBELT; } else if (sp_man->assisted_switch > STICK_ON_OFF_LIMIT) { - current_status->assisted_switch = ASSISTED_SWITCH_EASY; + status->assisted_switch = ASSISTED_SWITCH_EASY; } else { - current_status->assisted_switch = ASSISTED_SWITCH_SEATBELT; + status->assisted_switch = ASSISTED_SWITCH_SEATBELT; } /* mission switch */ if (!isfinite(sp_man->mission_switch)) { - current_status->mission_switch = MISSION_SWITCH_NONE; + status->mission_switch = MISSION_SWITCH_NONE; } else if (sp_man->mission_switch > STICK_ON_OFF_LIMIT) { - current_status->mission_switch = MISSION_SWITCH_LOITER; + status->mission_switch = MISSION_SWITCH_LOITER; } else { - current_status->mission_switch = MISSION_SWITCH_MISSION; + status->mission_switch = MISSION_SWITCH_MISSION; } } transition_result_t -check_main_state_machine(struct vehicle_status_s *current_status) +set_main_state_rc(struct vehicle_status_s *status) { /* evaluate the main state machine */ transition_result_t res = TRANSITION_DENIED; - switch (current_status->mode_switch) { + switch (status->mode_switch) { case MODE_SWITCH_MANUAL: - res = main_state_transition(current_status, MAIN_STATE_MANUAL); + res = main_state_transition(status, MAIN_STATE_MANUAL); // TRANSITION_DENIED is not possible here break; case MODE_SWITCH_ASSISTED: - if (current_status->assisted_switch == ASSISTED_SWITCH_EASY) { - res = main_state_transition(current_status, MAIN_STATE_EASY); + if (status->assisted_switch == ASSISTED_SWITCH_EASY) { + res = main_state_transition(status, MAIN_STATE_EASY); if (res != TRANSITION_DENIED) break; // changed successfully or already in this state @@ -1492,34 +1492,34 @@ check_main_state_machine(struct vehicle_status_s *current_status) print_reject_mode("EASY"); } - res = main_state_transition(current_status, MAIN_STATE_SEATBELT); + res = main_state_transition(status, MAIN_STATE_SEATBELT); if (res != TRANSITION_DENIED) break; // changed successfully or already in this mode - if (current_status->assisted_switch != ASSISTED_SWITCH_EASY) // don't print both messages + if (status->assisted_switch != ASSISTED_SWITCH_EASY) // don't print both messages print_reject_mode("SEATBELT"); // else fallback to MANUAL - res = main_state_transition(current_status, MAIN_STATE_MANUAL); + res = main_state_transition(status, MAIN_STATE_MANUAL); // TRANSITION_DENIED is not possible here break; case MODE_SWITCH_AUTO: - res = main_state_transition(current_status, MAIN_STATE_AUTO); + res = main_state_transition(status, MAIN_STATE_AUTO); if (res != TRANSITION_DENIED) break; // changed successfully or already in this state // else fallback to SEATBELT (EASY likely will not work too) print_reject_mode("AUTO"); - res = main_state_transition(current_status, MAIN_STATE_SEATBELT); + res = main_state_transition(status, MAIN_STATE_SEATBELT); if (res != TRANSITION_DENIED) break; // changed successfully or already in this state // else fallback to MANUAL - res = main_state_transition(current_status, MAIN_STATE_MANUAL); + res = main_state_transition(status, MAIN_STATE_MANUAL); // TRANSITION_DENIED is not possible here break; From d1508a7813ad09a173fe314608c25dc8c3cd7a1f Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 27 Jan 2014 20:49:17 +0100 Subject: [PATCH 350/447] vehicle_control_mode publication moved to commander, WIP --- src/modules/commander/commander.cpp | 136 ++++++- .../commander/state_machine_helper.cpp | 4 + src/modules/mavlink/mavlink.c | 4 + src/modules/navigator/navigator_main.cpp | 362 ++++++------------ src/modules/navigator/navigator_state.h | 21 + src/modules/sdlog2/sdlog2.c | 4 +- .../uORB/topics/vehicle_control_mode.h | 19 +- src/modules/uORB/topics/vehicle_status.h | 2 + 8 files changed, 265 insertions(+), 287 deletions(-) create mode 100644 src/modules/navigator/navigator_state.h diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index f579fb52ad..60fb4f486a 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -203,6 +203,8 @@ void check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicl transition_result_t set_main_state_rc(struct vehicle_status_s *status); +void set_control_mode(); + void print_reject_mode(const char *msg); void print_reject_arm(const char *msg); @@ -555,10 +557,8 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe } static struct vehicle_status_s status; - -/* armed topic */ +static struct vehicle_control_mode_s control_mode; static struct actuator_armed_s armed; - static struct safety_s safety; int commander_thread_main(int argc, char *argv[]) @@ -613,16 +613,9 @@ int commander_thread_main(int argc, char *argv[]) mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); - /* Main state machine */ - /* make sure we are in preflight state */ + /* vehicle status topic */ memset(&status, 0, sizeof(status)); status.condition_landed = true; // initialize to safe value - - /* armed topic */ - orb_advert_t armed_pub; - /* Initialize armed with all false */ - memset(&armed, 0, sizeof(armed)); - status.main_state = MAIN_STATE_MANUAL; status.set_nav_state = NAV_STATE_NONE; status.set_nav_state_timestamp = 0; @@ -645,14 +638,20 @@ int commander_thread_main(int argc, char *argv[]) // XXX for now just set sensors as initialized status.condition_system_sensors_initialized = true; - /* advertise to ORB */ - status_pub = orb_advertise(ORB_ID(vehicle_status), &status); - /* publish current state machine */ - - /* publish initial state */ status.counter++; status.timestamp = hrt_absolute_time(); - orb_publish(ORB_ID(vehicle_status), status_pub, &status); + + /* publish initial state */ + status_pub = orb_advertise(ORB_ID(vehicle_status), &status); + + /* armed topic */ + orb_advert_t armed_pub; + /* Initialize armed with all false */ + memset(&armed, 0, sizeof(armed)); + + /* vehicle control mode topic */ + memset(&control_mode, 0, sizeof(control_mode)); + orb_advert_t control_mode_pub = orb_advertise(ORB_ID(vehicle_control_mode), &control_mode); armed_pub = orb_advertise(ORB_ID(actuator_armed), &armed); @@ -1244,8 +1243,13 @@ int commander_thread_main(int argc, char *argv[]) /* publish states (armed, control mode, vehicle status) at least with 5 Hz */ if (counter % (200000 / COMMANDER_MONITORING_INTERVAL) == 0 || status_changed) { + set_control_mode(); + control_mode.timestamp = t1; + orb_publish(ORB_ID(vehicle_control_mode), control_mode_pub, &control_mode); + status.timestamp = t1; orb_publish(ORB_ID(vehicle_status), status_pub, &status); + armed.timestamp = t1; orb_publish(ORB_ID(actuator_armed), armed_pub, &armed); } @@ -1472,7 +1476,7 @@ check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_sta transition_result_t set_main_state_rc(struct vehicle_status_s *status) { - /* evaluate the main state machine */ + /* set main state according to RC switches */ transition_result_t res = TRANSITION_DENIED; switch (status->mode_switch) { @@ -1530,6 +1534,102 @@ set_main_state_rc(struct vehicle_status_s *status) return res; } +void +set_control_mode() +{ + /* set vehicle_control_mode according to main state and failsafe state */ + control_mode.flag_armed = armed.armed; + control_mode.flag_external_manual_override_ok = !status.is_rotary_wing; + control_mode.flag_system_hil_enabled = status.hil_state == HIL_STATE_ON; + + control_mode.flag_control_termination_enabled = false; + + /* set this flag when navigator should act */ + bool navigator_enabled = false; + + switch (status.failsafe_state) { + case FAILSAFE_STATE_NORMAL: + switch (status.main_state) { + case MAIN_STATE_MANUAL: + control_mode.flag_control_manual_enabled = true; + control_mode.flag_control_auto_enabled = false; + control_mode.flag_control_rates_enabled = status.is_rotary_wing; + control_mode.flag_control_attitude_enabled = status.is_rotary_wing; + control_mode.flag_control_altitude_enabled = false; + control_mode.flag_control_climb_rate_enabled = false; + control_mode.flag_control_position_enabled = false; + control_mode.flag_control_velocity_enabled = false; + break; + + case MAIN_STATE_SEATBELT: + control_mode.flag_control_manual_enabled = true; + control_mode.flag_control_auto_enabled = false; + control_mode.flag_control_rates_enabled = true; + control_mode.flag_control_attitude_enabled = true; + control_mode.flag_control_altitude_enabled = true; + control_mode.flag_control_climb_rate_enabled = true; + control_mode.flag_control_position_enabled = false; + control_mode.flag_control_velocity_enabled = false; + break; + + case MAIN_STATE_EASY: + control_mode.flag_control_manual_enabled = true; + control_mode.flag_control_auto_enabled = false; + control_mode.flag_control_rates_enabled = true; + control_mode.flag_control_attitude_enabled = true; + control_mode.flag_control_altitude_enabled = true; + control_mode.flag_control_climb_rate_enabled = true; + control_mode.flag_control_position_enabled = true; + control_mode.flag_control_velocity_enabled = true; + break; + + case MAIN_STATE_AUTO: + navigator_enabled = true; + + default: + break; + } + + break; + + case FAILSAFE_STATE_RTL: + navigator_enabled = true; + break; + + case FAILSAFE_STATE_LAND: + navigator_enabled = true; + break; + + case FAILSAFE_STATE_TERMINATION: + /* disable all controllers on termination */ + control_mode.flag_control_manual_enabled = false; + control_mode.flag_control_auto_enabled = false; + control_mode.flag_control_rates_enabled = false; + control_mode.flag_control_attitude_enabled = false; + control_mode.flag_control_position_enabled = false; + control_mode.flag_control_velocity_enabled = false; + control_mode.flag_control_altitude_enabled = false; + control_mode.flag_control_climb_rate_enabled = false; + control_mode.flag_control_termination_enabled = true; + break; + + default: + break; + } + + /* navigator has control, set control mode flags according to nav state*/ + if (navigator_enabled) { + control_mode.flag_control_manual_enabled = false; + control_mode.flag_control_auto_enabled = true; + control_mode.flag_control_rates_enabled = true; + control_mode.flag_control_attitude_enabled = true; + control_mode.flag_control_position_enabled = true; + control_mode.flag_control_velocity_enabled = true; + control_mode.flag_control_altitude_enabled = true; + control_mode.flag_control_climb_rate_enabled = true; + } +} + void print_reject_mode(const char *msg) { diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index c7256583a3..43d0e023e3 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -384,6 +384,8 @@ transition_result_t failsafe_state_transition(struct vehicle_status_s *status, f case FAILSAFE_STATE_RTL: /* global position and home position required for RTL */ if (status->condition_global_position_valid && status->condition_home_position_valid) { + status->set_nav_state = NAV_STATE_RTL; + status->set_nav_state_timestamp = hrt_absolute_time(); ret = TRANSITION_CHANGED; } @@ -392,6 +394,8 @@ transition_result_t failsafe_state_transition(struct vehicle_status_s *status, f case FAILSAFE_STATE_LAND: /* at least relative altitude estimate required for landing */ if (status->condition_local_altitude_valid || status->condition_global_position_valid) { + status->set_nav_state = NAV_STATE_LAND; + status->set_nav_state_timestamp = hrt_absolute_time(); ret = TRANSITION_CHANGED; } diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c index 4f80917162..5406c35c21 100644 --- a/src/modules/mavlink/mavlink.c +++ b/src/modules/mavlink/mavlink.c @@ -220,6 +220,9 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_base_mode, u } else if (v_status.main_state == MAIN_STATE_AUTO) { *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; + // TODO get nav state + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_MISSION; + /* if (control_mode.nav_state == NAV_STATE_NONE) { // failsafe, shouldn't happen custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY; } else if (control_mode.nav_state == NAV_STATE_READY) { @@ -231,6 +234,7 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_base_mode, u } else if (control_mode.nav_state == NAV_STATE_RTL) { custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_RTL; } + */ } *mavlink_custom_mode = custom_mode.data; diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index dd471928e3..2117755eed 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -84,6 +84,7 @@ #include #include +#include "navigator_state.h" #include "navigator_mission.h" #include "mission_feasibility_checker.h" #include "geofence.h" @@ -151,10 +152,10 @@ private: int _offboard_mission_sub; /**< notification of offboard mission updates */ int _onboard_mission_sub; /**< notification of onboard mission updates */ int _capabilities_sub; /**< notification of vehicle capabilities updates */ + int _control_mode_sub; /**< vehicle control mode subscription */ orb_advert_t _pos_sp_triplet_pub; /**< publish position setpoint triplet */ orb_advert_t _mission_result_pub; /**< publish mission result topic */ - orb_advert_t _control_mode_pub; /**< publish vehicle control mode topic */ struct vehicle_status_s _vstatus; /**< vehicle status */ struct vehicle_control_mode_s _control_mode; /**< vehicle control mode */ @@ -274,6 +275,10 @@ private: */ void vehicle_status_update(); + /** + * Retrieve vehicle control mode + */ + void vehicle_control_mode_update(); /** * Shim for calling task_main from task_create. @@ -341,11 +346,6 @@ private: * Publish a new mission item triplet for position controller */ void publish_position_setpoint_triplet(); - - /** - * Publish vehicle_control_mode topic for controllers - */ - void publish_control_mode(); }; namespace navigator @@ -373,6 +373,7 @@ Navigator::Navigator() : _global_pos_sub(-1), _home_pos_sub(-1), _vstatus_sub(-1), + _control_mode_sub(-1), _params_sub(-1), _offboard_mission_sub(-1), _onboard_mission_sub(-1), @@ -381,7 +382,6 @@ Navigator::Navigator() : /* publications */ _pos_sp_triplet_pub(-1), _mission_result_pub(-1), - _control_mode_pub(-1), /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "navigator")), @@ -541,9 +541,19 @@ Navigator::onboard_mission_update() void Navigator::vehicle_status_update() { - /* try to load initial states */ if (orb_copy(ORB_ID(vehicle_status), _vstatus_sub, &_vstatus) != OK) { - _vstatus.arming_state = ARMING_STATE_STANDBY; /* in case the commander is not be running */ + /* in case the commander is not be running */ + _vstatus.arming_state = ARMING_STATE_STANDBY; + } +} + +void +Navigator::vehicle_control_mode_update() +{ + if (orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode) != OK) { + /* in case the commander is not be running */ + _control_mode.flag_control_auto_enabled = false; + _control_mode.flag_armed = false; } } @@ -589,11 +599,13 @@ Navigator::task_main() _onboard_mission_sub = orb_subscribe(ORB_ID(onboard_mission)); _capabilities_sub = orb_subscribe(ORB_ID(navigation_capabilities)); _vstatus_sub = orb_subscribe(ORB_ID(vehicle_status)); + _control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); _home_pos_sub = orb_subscribe(ORB_ID(home_position)); /* copy all topics first time */ vehicle_status_update(); + vehicle_control_mode_update(); parameters_update(); global_position_update(); home_position_update(); @@ -605,12 +617,11 @@ Navigator::task_main() orb_set_interval(_global_pos_sub, 20); unsigned prevState = NAV_STATE_NONE; - bool pub_control_mode = true; hrt_abstime mavlink_open_time = 0; const hrt_abstime mavlink_open_interval = 500000; /* wakeup source(s) */ - struct pollfd fds[7]; + struct pollfd fds[8]; /* Setup of loop */ fds[0].fd = _params_sub; @@ -627,6 +638,8 @@ Navigator::task_main() fds[5].events = POLLIN; fds[6].fd = _vstatus_sub; fds[6].events = POLLIN; + fds[7].fd = _control_mode_sub; + fds[7].events = POLLIN; while (!_task_should_exit) { @@ -652,127 +665,113 @@ Navigator::task_main() _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); } + /* vehicle control mode updated */ + if (fds[7].revents & POLLIN) { + vehicle_control_mode_update(); + } + /* vehicle status updated */ if (fds[6].revents & POLLIN) { vehicle_status_update(); - pub_control_mode = true; /* evaluate state machine from commander and set the navigator mode accordingly */ - if (_vstatus.arming_state == ARMING_STATE_ARMED || _vstatus.arming_state == ARMING_STATE_ARMED_ERROR) { - if (_vstatus.failsafe_state == FAILSAFE_STATE_NORMAL) { - if (_vstatus.main_state == MAIN_STATE_AUTO) { - bool stick_mode = false; + if (_control_mode.flag_armed && _control_mode.flag_control_auto_enabled) { + bool stick_mode = false; - if (!_vstatus.rc_signal_lost) { - /* RC signal available, use control switches to set mode */ - /* RETURN switch, overrides MISSION switch */ - if (_vstatus.return_switch == RETURN_SWITCH_RETURN) { - if (myState != NAV_STATE_READY || _rtl_state != RTL_STATE_LAND) { - dispatch(EVENT_RTL_REQUESTED); - } - - stick_mode = true; - - } else { - /* MISSION switch */ - if (_vstatus.mission_switch == MISSION_SWITCH_LOITER) { - dispatch(EVENT_LOITER_REQUESTED); - stick_mode = true; - - } else if (_vstatus.mission_switch == MISSION_SWITCH_MISSION) { - /* switch to mission only if available */ - if (_mission.current_mission_available()) { - dispatch(EVENT_MISSION_REQUESTED); - - } else { - dispatch(EVENT_LOITER_REQUESTED); - } - - stick_mode = true; - } - - if (!stick_mode && _vstatus.return_switch == RETURN_SWITCH_NORMAL && myState == NAV_STATE_RTL) { - /* RETURN switch is in normal mode, no MISSION switch mapped, interrupt if in RTL state */ - dispatch(EVENT_LOITER_REQUESTED); - stick_mode = true; - } - } + if (!_vstatus.rc_signal_lost) { + /* RC signal available, use control switches to set mode */ + /* RETURN switch, overrides MISSION switch */ + if (_vstatus.return_switch == RETURN_SWITCH_RETURN) { + if (myState != NAV_STATE_READY || _rtl_state != RTL_STATE_LAND) { + dispatch(EVENT_RTL_REQUESTED); } - if (!stick_mode) { - if (_vstatus.set_nav_state_timestamp != _set_nav_state_timestamp) { - /* commander requested new navigation mode, try to set it */ - _set_nav_state_timestamp = _vstatus.set_nav_state_timestamp; + stick_mode = true; - switch (_vstatus.set_nav_state) { - case NAV_STATE_NONE: - /* nothing to do */ - break; + } else { + /* MISSION switch */ + if (_vstatus.mission_switch == MISSION_SWITCH_LOITER) { + dispatch(EVENT_LOITER_REQUESTED); + stick_mode = true; - case NAV_STATE_LOITER: - dispatch(EVENT_LOITER_REQUESTED); - break; - - case NAV_STATE_MISSION: - if (_mission.current_mission_available()) { - dispatch(EVENT_MISSION_REQUESTED); - - } else { - dispatch(EVENT_LOITER_REQUESTED); - } - - break; - - case NAV_STATE_RTL: - if (myState != NAV_STATE_READY || _rtl_state != RTL_STATE_LAND) { - dispatch(EVENT_RTL_REQUESTED); - } - - break; - - default: - warnx("ERROR: Requested navigation state not supported"); - break; - } + } else if (_vstatus.mission_switch == MISSION_SWITCH_MISSION) { + /* switch to mission only if available */ + if (_mission.current_mission_available()) { + dispatch(EVENT_MISSION_REQUESTED); } else { - /* on first switch to AUTO try mission by default, if none is available fallback to loiter */ - if (myState == NAV_STATE_NONE) { - if (_mission.current_mission_available()) { - dispatch(EVENT_MISSION_REQUESTED); - - } else { - dispatch(EVENT_LOITER_REQUESTED); - } - } + dispatch(EVENT_LOITER_REQUESTED); } + + stick_mode = true; + } + + if (!stick_mode && _vstatus.return_switch == RETURN_SWITCH_NORMAL && myState == NAV_STATE_RTL) { + /* RETURN switch is in normal mode, no MISSION switch mapped, interrupt if in RTL state */ + dispatch(EVENT_LOITER_REQUESTED); + stick_mode = true; + } + } + } + + if (!stick_mode) { + if (_vstatus.set_nav_state_timestamp != _set_nav_state_timestamp) { + /* commander requested new navigation mode, try to set it */ + _set_nav_state_timestamp = _vstatus.set_nav_state_timestamp; + + switch (_vstatus.set_nav_state) { + case NAV_STATE_NONE: + /* nothing to do */ + break; + + case NAV_STATE_LOITER: + dispatch(EVENT_LOITER_REQUESTED); + break; + + case NAV_STATE_MISSION: + if (_mission.current_mission_available()) { + dispatch(EVENT_MISSION_REQUESTED); + + } else { + dispatch(EVENT_LOITER_REQUESTED); + } + + break; + + case NAV_STATE_RTL: + if (myState != NAV_STATE_READY || _rtl_state != RTL_STATE_LAND) { + dispatch(EVENT_RTL_REQUESTED); + } + + break; + + case NAV_STATE_LAND: + if (myState != NAV_STATE_READY) { + dispatch(EVENT_LAND_REQUESTED); + } + + break; + + default: + warnx("ERROR: Requested navigation state not supported"); + break; } } else { - /* not in AUTO mode */ - dispatch(EVENT_NONE_REQUESTED); + /* on first switch to AUTO try mission by default, if none is available fallback to loiter */ + if (myState == NAV_STATE_NONE) { + if (_mission.current_mission_available()) { + dispatch(EVENT_MISSION_REQUESTED); + + } else { + dispatch(EVENT_LOITER_REQUESTED); + } + } } - - } else if (_vstatus.failsafe_state == FAILSAFE_STATE_RTL) { - /* RTL on failsafe */ - if (myState != NAV_STATE_READY || _rtl_state != RTL_STATE_LAND) { - - dispatch(EVENT_RTL_REQUESTED); - } - - } else if (_vstatus.failsafe_state == FAILSAFE_STATE_LAND) { - /* LAND on failsafe */ - if (myState != NAV_STATE_READY) { - dispatch(EVENT_LAND_REQUESTED); - } - - } else { - /* shouldn't act */ - dispatch(EVENT_NONE_REQUESTED); } } else { - /* not armed */ + /* navigator shouldn't act */ dispatch(EVENT_NONE_REQUESTED); } } @@ -840,12 +839,6 @@ Navigator::task_main() if (myState != prevState) { mavlink_log_info(_mavlink_fd, "[navigator] nav state: %s", nav_states_str[myState]); prevState = myState; - pub_control_mode = true; - } - - /* publish control mode if updated */ - if (pub_control_mode) { - publish_control_mode(); } perf_end(_loop_perf); @@ -1556,139 +1549,6 @@ Navigator::publish_position_setpoint_triplet() } } -void -Navigator::publish_control_mode() -{ - /* update vehicle_control_mode topic*/ - _control_mode.main_state = _vstatus.main_state; - _control_mode.nav_state = static_cast(myState); - _control_mode.flag_armed = _vstatus.arming_state == ARMING_STATE_ARMED || _vstatus.arming_state == ARMING_STATE_ARMED_ERROR; - _control_mode.flag_external_manual_override_ok = !_vstatus.is_rotary_wing; - _control_mode.flag_system_hil_enabled = _vstatus.hil_state == HIL_STATE_ON; - - _control_mode.flag_control_offboard_enabled = false; - _control_mode.flag_control_termination_enabled = false; - - /* set this flag when navigator has control */ - bool navigator_enabled = false; - - switch (_vstatus.failsafe_state) { - case FAILSAFE_STATE_NORMAL: - switch (_vstatus.main_state) { - case MAIN_STATE_MANUAL: - _control_mode.flag_control_manual_enabled = true; - _control_mode.flag_control_rates_enabled = _vstatus.is_rotary_wing; - _control_mode.flag_control_attitude_enabled = _vstatus.is_rotary_wing; - _control_mode.flag_control_altitude_enabled = false; - _control_mode.flag_control_climb_rate_enabled = false; - _control_mode.flag_control_position_enabled = false; - _control_mode.flag_control_velocity_enabled = false; - break; - - case MAIN_STATE_SEATBELT: - _control_mode.flag_control_manual_enabled = true; - _control_mode.flag_control_rates_enabled = true; - _control_mode.flag_control_attitude_enabled = true; - _control_mode.flag_control_altitude_enabled = true; - _control_mode.flag_control_climb_rate_enabled = true; - _control_mode.flag_control_position_enabled = false; - _control_mode.flag_control_velocity_enabled = false; - break; - - case MAIN_STATE_EASY: - _control_mode.flag_control_manual_enabled = true; - _control_mode.flag_control_rates_enabled = true; - _control_mode.flag_control_attitude_enabled = true; - _control_mode.flag_control_altitude_enabled = true; - _control_mode.flag_control_climb_rate_enabled = true; - _control_mode.flag_control_position_enabled = true; - _control_mode.flag_control_velocity_enabled = true; - break; - - case MAIN_STATE_AUTO: - navigator_enabled = true; - - default: - break; - } - - break; - - case FAILSAFE_STATE_RTL: - navigator_enabled = true; - break; - - case FAILSAFE_STATE_LAND: - navigator_enabled = true; - break; - - case FAILSAFE_STATE_TERMINATION: - navigator_enabled = true; - /* disable all controllers on termination */ - _control_mode.flag_control_manual_enabled = false; - _control_mode.flag_control_rates_enabled = false; - _control_mode.flag_control_attitude_enabled = false; - _control_mode.flag_control_position_enabled = false; - _control_mode.flag_control_velocity_enabled = false; - _control_mode.flag_control_altitude_enabled = false; - _control_mode.flag_control_climb_rate_enabled = false; - _control_mode.flag_control_termination_enabled = true; - break; - - default: - break; - } - - /* navigator has control, set control mode flags according to nav state*/ - if (navigator_enabled) { - _control_mode.flag_control_manual_enabled = false; - - switch (myState) { - case NAV_STATE_READY: - /* disable all controllers, armed but idle */ - _control_mode.flag_control_rates_enabled = false; - _control_mode.flag_control_attitude_enabled = false; - _control_mode.flag_control_position_enabled = false; - _control_mode.flag_control_velocity_enabled = false; - _control_mode.flag_control_altitude_enabled = false; - _control_mode.flag_control_climb_rate_enabled = false; - break; - - case NAV_STATE_LAND: - /* land with or without position control */ - _control_mode.flag_control_manual_enabled = false; - _control_mode.flag_control_rates_enabled = true; - _control_mode.flag_control_attitude_enabled = true; - _control_mode.flag_control_position_enabled = _vstatus.condition_global_position_valid; - _control_mode.flag_control_velocity_enabled = _vstatus.condition_global_position_valid; - _control_mode.flag_control_altitude_enabled = true; - _control_mode.flag_control_climb_rate_enabled = true; - break; - - default: - _control_mode.flag_control_rates_enabled = true; - _control_mode.flag_control_attitude_enabled = true; - _control_mode.flag_control_position_enabled = true; - _control_mode.flag_control_velocity_enabled = true; - _control_mode.flag_control_altitude_enabled = true; - _control_mode.flag_control_climb_rate_enabled = true; - break; - } - } - - _control_mode.timestamp = hrt_absolute_time(); - - /* lazily publish the mission triplet only once available */ - if (_control_mode_pub > 0) { - /* publish the mission triplet */ - orb_publish(ORB_ID(vehicle_control_mode), _control_mode_pub, &_control_mode); - - } else { - /* advertise and publish */ - _control_mode_pub = orb_advertise(ORB_ID(vehicle_control_mode), &_control_mode); - } -} - void Navigator::add_fence_point(int argc, char *argv[]) { _geofence.addPoint(argc, argv); diff --git a/src/modules/navigator/navigator_state.h b/src/modules/navigator/navigator_state.h new file mode 100644 index 0000000000..6a1475c9b0 --- /dev/null +++ b/src/modules/navigator/navigator_state.h @@ -0,0 +1,21 @@ +/* + * navigator_state.h + * + * Created on: 27.01.2014 + * Author: ton + */ + +#ifndef NAVIGATOR_STATE_H_ +#define NAVIGATOR_STATE_H_ + +typedef enum { + NAV_STATE_NONE = 0, + NAV_STATE_READY, + NAV_STATE_LOITER, + NAV_STATE_MISSION, + NAV_STATE_RTL, + NAV_STATE_LAND, + NAV_STATE_MAX +} nav_state_t; + +#endif /* NAVIGATOR_STATE_H_ */ diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 9bac2958e9..1e032d1c81 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1067,8 +1067,8 @@ int sdlog2_thread_main(int argc, char *argv[]) /* copy VEHICLE CONTROL MODE control mode here to construct STAT message */ orb_copy(ORB_ID(vehicle_control_mode), subs.control_mode_sub, &buf.control_mode); log_msg.msg_type = LOG_STAT_MSG; - log_msg.body.log_STAT.main_state = (uint8_t) buf.control_mode.main_state; - log_msg.body.log_STAT.navigation_state = (uint8_t) buf.control_mode.nav_state; + log_msg.body.log_STAT.main_state = (uint8_t) buf_status.main_state; + //log_msg.body.log_STAT.navigation_state = (uint8_t) buf.control_mode.nav_state; log_msg.body.log_STAT.arming_state = (uint8_t) buf_status.arming_state; log_msg.body.log_STAT.battery_remaining = buf_status.battery_remaining; log_msg.body.log_STAT.battery_warning = (uint8_t) buf_status.battery_warning; diff --git a/src/modules/uORB/topics/vehicle_control_mode.h b/src/modules/uORB/topics/vehicle_control_mode.h index 5aecac8987..7cbb37cd8e 100644 --- a/src/modules/uORB/topics/vehicle_control_mode.h +++ b/src/modules/uORB/topics/vehicle_control_mode.h @@ -61,23 +61,10 @@ * Encodes the complete system state and is set by the commander app. */ -typedef enum { - NAV_STATE_NONE = 0, - NAV_STATE_READY, - NAV_STATE_LOITER, - NAV_STATE_MISSION, - NAV_STATE_RTL, - NAV_STATE_LAND, - NAV_STATE_MAX -} nav_state_t; - struct vehicle_control_mode_s { uint64_t timestamp; /**< in microseconds since system start, is set whenever the writing thread stores new data */ - main_state_t main_state; - nav_state_t nav_state; - bool flag_armed; bool flag_external_manual_override_ok; /**< external override non-fatal for system. Only true for fixed wing */ @@ -86,14 +73,14 @@ struct vehicle_control_mode_s bool flag_system_hil_enabled; bool flag_control_manual_enabled; /**< true if manual input is mixed in */ - bool flag_control_offboard_enabled; /**< true if offboard control input is on */ + bool flag_control_auto_enabled; /**< true if onboard autopilot should act */ bool flag_control_rates_enabled; /**< true if rates are stabilized */ bool flag_control_attitude_enabled; /**< true if attitude stabilization is mixed in */ bool flag_control_velocity_enabled; /**< true if horizontal velocity (implies direction) is controlled */ bool flag_control_position_enabled; /**< true if position is controlled */ bool flag_control_altitude_enabled; /**< true if altitude is controlled */ - bool flag_control_climb_rate_enabled; /**< true if climb rate is controlled */ - bool flag_control_termination_enabled; /**< true if flighttermination is enabled */ + bool flag_control_climb_rate_enabled; /**< true if climb rate is controlled */ + bool flag_control_termination_enabled; /**< true if flighttermination is enabled */ }; /** diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index a5988d3ba4..1b3639e309 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -54,6 +54,8 @@ #include #include "../uORB.h" +#include + /** * @addtogroup topics @{ */ From 7d334ed54f41bd89fcaaddff4091e3eb8901b6b8 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 27 Jan 2014 21:52:23 +0100 Subject: [PATCH 351/447] navigator: RTL fix, more informative log message --- src/modules/navigator/navigator_main.cpp | 14 ++++++++------ 1 file changed, 8 insertions(+), 6 deletions(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 2117755eed..8985b28df5 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -1182,20 +1182,22 @@ Navigator::start_rtl() { _do_takeoff = false; + /* decide if we need climb */ if (_rtl_state == RTL_STATE_NONE) { if (_global_pos.alt < _home_pos.alt + _parameters.rtl_alt) { _rtl_state = RTL_STATE_CLIMB; } else { _rtl_state = RTL_STATE_RETURN; - - if (_reset_loiter_pos) { - _mission_item.altitude_is_relative = false; - _mission_item.altitude = _global_pos.alt; - } } } + /* if switching directly to return state, reset altitude setpoint */ + if (_rtl_state == RTL_STATE_RETURN) { + _mission_item.altitude_is_relative = false; + _mission_item.altitude = _global_pos.alt; + } + _reset_loiter_pos = true; set_rtl_item(); } @@ -1278,7 +1280,7 @@ Navigator::set_rtl_item() _pos_sp_triplet.next.valid = false; - mavlink_log_info(_mavlink_fd, "[navigator] RTL: return"); + mavlink_log_info(_mavlink_fd, "[navigator] RTL: return at %.1fm above home", climb_alt - _home_pos.alt); break; } From 3f79057dd1a6e336c6138553268576345c3bf6e1 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 27 Jan 2014 23:46:02 +0100 Subject: [PATCH 352/447] fw att pos estimator: fix output lat/lon conversion (introduced by changes in 58792c5ca6e42bc251dd3c92b0e79217ff5d5403) --- src/modules/att_pos_estimator_ekf/KalmanNav.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp b/src/modules/att_pos_estimator_ekf/KalmanNav.cpp index 8e88130e15..83145ac72d 100644 --- a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp +++ b/src/modules/att_pos_estimator_ekf/KalmanNav.cpp @@ -315,8 +315,8 @@ void KalmanNav::updatePublications() _pos.timestamp = _pubTimeStamp; _pos.time_gps_usec = _gps.timestamp_position; _pos.valid = true; - _pos.lat = getLatDegE7(); - _pos.lon = getLonDegE7(); + _pos.lat = lat * M_RAD_TO_DEG; + _pos.lon = lon * M_RAD_TO_DEG; _pos.alt = float(alt); _pos.vel_n = vN; _pos.vel_e = vE; From 3fdb082cb89dc82538637b3f060787d929105567 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 28 Jan 2014 00:03:29 +0100 Subject: [PATCH 353/447] mavlink: AUTO states indication fixed --- src/modules/mavlink/mavlink.c | 47 ++++++++++--------- src/modules/mavlink/orb_listener.c | 31 ++---------- src/modules/mavlink/orb_topics.h | 6 +-- src/modules/navigator/navigator_main.cpp | 27 +++++++---- .../uORB/topics/position_setpoint_triplet.h | 3 ++ 5 files changed, 55 insertions(+), 59 deletions(-) diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c index 5406c35c21..4d975066f7 100644 --- a/src/modules/mavlink/mavlink.c +++ b/src/modules/mavlink/mavlink.c @@ -199,8 +199,7 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_base_mode, u } /* arming state */ - if (v_status.arming_state == ARMING_STATE_ARMED - || v_status.arming_state == ARMING_STATE_ARMED_ERROR) { + if (armed.armed) { *mavlink_base_mode |= MAV_MODE_FLAG_SAFETY_ARMED; } @@ -208,33 +207,37 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_base_mode, u *mavlink_base_mode |= MAV_MODE_FLAG_CUSTOM_MODE_ENABLED; union px4_custom_mode custom_mode; custom_mode.data = 0; - if (v_status.main_state == MAIN_STATE_MANUAL) { - *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | (v_status.is_rotary_wing ? MAV_MODE_FLAG_STABILIZE_ENABLED : 0); - custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL; - } else if (v_status.main_state == MAIN_STATE_SEATBELT) { - *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED; - custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_SEATBELT; - } else if (v_status.main_state == MAIN_STATE_EASY) { - *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; - custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_EASY; - } else if (v_status.main_state == MAIN_STATE_AUTO) { + if (pos_sp_triplet.nav_state == NAV_STATE_NONE) { + /* use main state when navigator is not active */ + if (v_status.main_state == MAIN_STATE_MANUAL) { + *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | (v_status.is_rotary_wing ? MAV_MODE_FLAG_STABILIZE_ENABLED : 0); + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL; + } else if (v_status.main_state == MAIN_STATE_SEATBELT) { + *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_SEATBELT; + } else if (v_status.main_state == MAIN_STATE_EASY) { + *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_EASY; + } else if (v_status.main_state == MAIN_STATE_AUTO) { + *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY; + } + } else { + /* use navigation state when navigator is active */ *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; - // TODO get nav state - custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_MISSION; - /* - if (control_mode.nav_state == NAV_STATE_NONE) { // failsafe, shouldn't happen + if (pos_sp_triplet.nav_state == NAV_STATE_READY) { custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY; - } else if (control_mode.nav_state == NAV_STATE_READY) { - custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY; - } else if (control_mode.nav_state == NAV_STATE_LOITER) { + } else if (pos_sp_triplet.nav_state == NAV_STATE_LOITER) { custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LOITER; - } else if (control_mode.nav_state == NAV_STATE_MISSION) { + } else if (pos_sp_triplet.nav_state == NAV_STATE_MISSION) { custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_MISSION; - } else if (control_mode.nav_state == NAV_STATE_RTL) { + } else if (pos_sp_triplet.nav_state == NAV_STATE_RTL) { custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_RTL; + } else if (pos_sp_triplet.nav_state == NAV_STATE_LAND) { + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LAND; } - */ } *mavlink_custom_mode = custom_mode.data; diff --git a/src/modules/mavlink/orb_listener.c b/src/modules/mavlink/orb_listener.c index 7f6237535e..69cd820a02 100644 --- a/src/modules/mavlink/orb_listener.c +++ b/src/modules/mavlink/orb_listener.c @@ -70,7 +70,7 @@ struct vehicle_local_position_s local_pos; struct home_position_s home; struct navigation_capabilities_s nav_cap; struct vehicle_status_s v_status; -struct vehicle_control_mode_s control_mode; +struct position_setpoint_triplet_s pos_sp_triplet; struct rc_channels_s rc; struct rc_input_values rc_raw; struct actuator_armed_s armed; @@ -127,7 +127,6 @@ static void l_vehicle_rates_setpoint(const struct listener *l); static void l_home(const struct listener *l); static void l_airspeed(const struct listener *l); static void l_nav_cap(const struct listener *l); -static void l_control_mode(const struct listener *l); static const struct listener listeners[] = { {l_sensor_combined, &mavlink_subs.sensor_sub, 0}, @@ -154,7 +153,6 @@ static const struct listener listeners[] = { {l_home, &mavlink_subs.home_sub, 0}, {l_airspeed, &mavlink_subs.airspeed_sub, 0}, {l_nav_cap, &mavlink_subs.navigation_capabilities_sub, 0}, - {l_control_mode, &mavlink_subs.control_mode_sub, 0}, }; static const unsigned n_listeners = sizeof(listeners) / sizeof(listeners[0]); @@ -315,6 +313,7 @@ l_vehicle_status(const struct listener *l) /* immediately communicate state changes back to user */ orb_copy(ORB_ID(vehicle_status), status_sub, &v_status); orb_copy(ORB_ID(actuator_armed), mavlink_subs.armed_sub, &armed); + orb_copy(ORB_ID(position_setpoint_triplet), mavlink_subs.position_setpoint_triplet_sub, &pos_sp_triplet); /* enable or disable HIL */ if (v_status.hil_state == HIL_STATE_ON) @@ -682,26 +681,6 @@ l_nav_cap(const struct listener *l) } -void -l_control_mode(const struct listener *l) -{ - orb_copy(ORB_ID(vehicle_control_mode), mavlink_subs.control_mode_sub, &control_mode); - - /* translate the current syste state to mavlink state and mode */ - uint8_t mavlink_state = 0; - uint8_t mavlink_base_mode = 0; - uint32_t mavlink_custom_mode = 0; - get_mavlink_mode_and_state(&mavlink_state, &mavlink_base_mode, &mavlink_custom_mode); - - /* send heartbeat */ - mavlink_msg_heartbeat_send(chan, - mavlink_system.type, - MAV_AUTOPILOT_PX4, - mavlink_base_mode, - mavlink_custom_mode, - mavlink_state); -} - static void * uorb_receive_thread(void *arg) { @@ -777,9 +756,9 @@ uorb_receive_start(void) status_sub = orb_subscribe(ORB_ID(vehicle_status)); orb_set_interval(status_sub, 300); /* max 3.33 Hz updates */ - /* --- CONTROL MODE --- */ - mavlink_subs.control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); - orb_set_interval(mavlink_subs.control_mode_sub, 300); /* max 3.33 Hz updates */ + /* --- POSITION SETPOINT TRIPLET --- */ + mavlink_subs.position_setpoint_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet)); + orb_set_interval(mavlink_subs.position_setpoint_triplet_sub, 0); /* not polled, don't limit */ /* --- RC CHANNELS VALUE --- */ rc_sub = orb_subscribe(ORB_ID(rc_channels)); diff --git a/src/modules/mavlink/orb_topics.h b/src/modules/mavlink/orb_topics.h index 30ba348cf6..89f647bdc8 100644 --- a/src/modules/mavlink/orb_topics.h +++ b/src/modules/mavlink/orb_topics.h @@ -94,7 +94,7 @@ struct mavlink_subscriptions { int home_sub; int airspeed_sub; int navigation_capabilities_sub; - int control_mode_sub; + int position_setpoint_triplet_sub; }; extern struct mavlink_subscriptions mavlink_subs; @@ -111,8 +111,8 @@ extern struct navigation_capabilities_s nav_cap; /** Vehicle status */ extern struct vehicle_status_s v_status; -/** Vehicle control mode */ -extern struct vehicle_control_mode_s control_mode; +/** Position setpoint triplet */ +extern struct position_setpoint_triplet_s pos_sp_triplet; /** RC channels */ extern struct rc_channels_s rc; diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 8985b28df5..5a02bf522f 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -189,6 +189,8 @@ private: uint64_t _set_nav_state_timestamp; /**< timestamp of last handled navigation state request */ + bool _pos_sp_triplet_updated; + char *nav_states_str[NAV_STATE_MAX]; struct { @@ -399,6 +401,7 @@ Navigator::Navigator() : _mission_item_valid(false), _need_takeoff(true), _do_takeoff(false), + _pos_sp_triplet_updated(false), _geofence_violation_warning_sent(false) { _parameter_handles.min_altitude = param_find("NAV_MIN_ALT"); @@ -835,6 +838,11 @@ Navigator::task_main() } } + /* publish position setpoint triplet if updated */ + if (_pos_sp_triplet_updated) { + publish_position_setpoint_triplet(); + } + /* notify user about state changes */ if (myState != prevState) { mavlink_log_info(_mavlink_fd, "[navigator] nav state: %s", nav_states_str[myState]); @@ -999,7 +1007,7 @@ Navigator::start_none() _do_takeoff = false; _rtl_state = RTL_STATE_NONE; - publish_position_setpoint_triplet(); + _pos_sp_triplet_updated = true; } void @@ -1018,7 +1026,7 @@ Navigator::start_ready() _rtl_state = RTL_STATE_NONE; } - publish_position_setpoint_triplet(); + _pos_sp_triplet_updated = true; } void @@ -1061,7 +1069,7 @@ Navigator::start_loiter() _pos_sp_triplet.next.valid = false; _mission_item_valid = false; - publish_position_setpoint_triplet(); + _pos_sp_triplet_updated = true; } void @@ -1174,7 +1182,7 @@ Navigator::set_mission_item() } } - publish_position_setpoint_triplet(); + _pos_sp_triplet_updated = true; } void @@ -1280,7 +1288,7 @@ Navigator::set_rtl_item() _pos_sp_triplet.next.valid = false; - mavlink_log_info(_mavlink_fd, "[navigator] RTL: return at %.1fm above home", climb_alt - _home_pos.alt); + mavlink_log_info(_mavlink_fd, "[navigator] RTL: return at %.1fm above home", _mission_item.altitude - _home_pos.alt); break; } @@ -1345,7 +1353,7 @@ Navigator::set_rtl_item() } } - publish_position_setpoint_triplet(); + _pos_sp_triplet_updated = true; } void @@ -1540,9 +1548,12 @@ Navigator::on_mission_item_reached() void Navigator::publish_position_setpoint_triplet() { - /* lazily publish the mission triplet only once available */ + /* update navigation state */ + _pos_sp_triplet.nav_state = static_cast(myState); + + /* lazily publish the position setpoint triplet only once available */ if (_pos_sp_triplet_pub > 0) { - /* publish the mission triplet */ + /* publish the position setpoint triplet */ orb_publish(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_pub, &_pos_sp_triplet); } else { diff --git a/src/modules/uORB/topics/position_setpoint_triplet.h b/src/modules/uORB/topics/position_setpoint_triplet.h index 4b57833b63..e8d68281a8 100644 --- a/src/modules/uORB/topics/position_setpoint_triplet.h +++ b/src/modules/uORB/topics/position_setpoint_triplet.h @@ -45,6 +45,7 @@ #include #include #include "../uORB.h" +#include /** * @addtogroup topics @@ -82,6 +83,8 @@ struct position_setpoint_triplet_s struct position_setpoint_s previous; struct position_setpoint_s current; struct position_setpoint_s next; + + nav_state_t nav_state; /**< navigation state */ }; /** From f9b5709e9bb96afc134b32b6eb17c7d308afd82b Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 28 Jan 2014 00:23:23 +0100 Subject: [PATCH 354/447] sdlog2: NavState moved to GPSP message --- src/modules/sdlog2/sdlog2.c | 19 +++---------------- src/modules/sdlog2/sdlog2_messages.h | 6 +++--- 2 files changed, 6 insertions(+), 19 deletions(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 1e032d1c81..c3ea30cbfb 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -62,7 +62,6 @@ #include #include -#include #include #include #include @@ -740,7 +739,6 @@ int sdlog2_thread_main(int argc, char *argv[]) /* warning! using union here to save memory, elements should be used separately! */ union { struct vehicle_command_s cmd; - struct vehicle_control_mode_s control_mode; struct sensor_combined_s sensor; struct vehicle_attitude_s att; struct vehicle_attitude_setpoint_s att_sp; @@ -767,7 +765,6 @@ int sdlog2_thread_main(int argc, char *argv[]) struct { int cmd_sub; int status_sub; - int control_mode_sub; int sensor_sub; int att_sub; int att_sp_sub; @@ -847,12 +844,6 @@ int sdlog2_thread_main(int argc, char *argv[]) fds[fdsc_count].events = POLLIN; fdsc_count++; - /* --- VEHICLE CONTROL MODE --- */ - subs.control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); - fds[fdsc_count].fd = subs.control_mode_sub; - fds[fdsc_count].events = POLLIN; - fdsc_count++; - /* --- SENSORS COMBINED --- */ subs.sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); fds[fdsc_count].fd = subs.sensor_sub; @@ -1002,7 +993,7 @@ int sdlog2_thread_main(int argc, char *argv[]) /* decide use usleep() or blocking poll() */ bool use_sleep = sleep_delay > 0 && logging_enabled; - /* poll all topics if logging enabled or only management (first 2) if not */ + /* poll all topics if logging enabled or only management (first 3) if not */ int poll_ret = poll(fds, logging_enabled ? fdsc_count : 3, use_sleep ? 0 : poll_timeout); /* handle the poll result */ @@ -1064,11 +1055,8 @@ int sdlog2_thread_main(int argc, char *argv[]) /* --- VEHICLE STATUS --- */ if (fds[ifds++].revents & POLLIN) { /* don't orb_copy, it's already done few lines above */ - /* copy VEHICLE CONTROL MODE control mode here to construct STAT message */ - orb_copy(ORB_ID(vehicle_control_mode), subs.control_mode_sub, &buf.control_mode); log_msg.msg_type = LOG_STAT_MSG; log_msg.body.log_STAT.main_state = (uint8_t) buf_status.main_state; - //log_msg.body.log_STAT.navigation_state = (uint8_t) buf.control_mode.nav_state; log_msg.body.log_STAT.arming_state = (uint8_t) buf_status.arming_state; log_msg.body.log_STAT.battery_remaining = buf_status.battery_remaining; log_msg.body.log_STAT.battery_warning = (uint8_t) buf_status.battery_warning; @@ -1078,7 +1066,7 @@ int sdlog2_thread_main(int argc, char *argv[]) /* --- GPS POSITION --- */ if (fds[ifds++].revents & POLLIN) { - orb_copy(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf.gps_pos); + /* don't orb_copy, it's already done few lines above */ log_msg.msg_type = LOG_GPS_MSG; log_msg.body.log_GPS.gps_time = buf.gps_pos.time_gps_usec; log_msg.body.log_GPS.fix_type = buf.gps_pos.fix_type; @@ -1094,8 +1082,6 @@ int sdlog2_thread_main(int argc, char *argv[]) LOGBUFFER_WRITE_AND_COUNT(GPS); } - ifds++; // skip CONTROL MODE, already handled - /* --- SENSOR COMBINED --- */ if (fds[ifds++].revents & POLLIN) { orb_copy(ORB_ID(sensor_combined), subs.sensor_sub, &buf.sensor); @@ -1265,6 +1251,7 @@ int sdlog2_thread_main(int argc, char *argv[]) if (fds[ifds++].revents & POLLIN) { orb_copy(ORB_ID(position_setpoint_triplet), subs.triplet_sub, &buf.triplet); log_msg.msg_type = LOG_GPSP_MSG; + log_msg.body.log_GPSP.nav_state = buf.triplet.nav_state; log_msg.body.log_GPSP.lat = (int32_t)(buf.triplet.current.lat * 1e7d); log_msg.body.log_GPSP.lon = (int32_t)(buf.triplet.current.lon * 1e7d); log_msg.body.log_GPSP.alt = buf.triplet.current.alt; diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 98736dd211..1e2441c331 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -149,7 +149,6 @@ struct log_ATTC_s { #define LOG_STAT_MSG 10 struct log_STAT_s { uint8_t main_state; - uint8_t navigation_state; uint8_t arming_state; float battery_remaining; uint8_t battery_warning; @@ -210,6 +209,7 @@ struct log_GPOS_s { /* --- GPSP - GLOBAL POSITION SETPOINT --- */ #define LOG_GPSP_MSG 17 struct log_GPSP_s { + uint8_t nav_state; int32_t lat; int32_t lon; float alt; @@ -297,14 +297,14 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(LPSP, "ffff", "X,Y,Z,Yaw"), LOG_FORMAT(GPS, "QBffLLfffff", "GPSTime,FixType,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog"), LOG_FORMAT(ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"), - LOG_FORMAT(STAT, "BBBfBB", "MainState,NavState,ArmState,BatRem,BatWarn,Landed"), + LOG_FORMAT(STAT, "BBfBB", "MainState,ArmState,BatRem,BatWarn,Landed"), LOG_FORMAT(RC, "ffffffffB", "Ch0,Ch1,Ch2,Ch3,Ch4,Ch5,Ch6,Ch7,Count"), LOG_FORMAT(OUT0, "ffffffff", "Out0,Out1,Out2,Out3,Out4,Out5,Out6,Out7"), LOG_FORMAT(AIRS, "ff", "IndSpeed,TrueSpeed"), LOG_FORMAT(ARSP, "fff", "RollRateSP,PitchRateSP,YawRateSP"), LOG_FORMAT(FLOW, "hhfffBB", "RawX,RawY,CompX,CompY,Dist,Q,SensID"), LOG_FORMAT(GPOS, "LLffff", "Lat,Lon,Alt,VelN,VelE,VelD"), - LOG_FORMAT(GPSP, "LLffBfbf", "Lat,Lon,Alt,Yaw,Type,LoitR,LoitDir,PitMin"), + LOG_FORMAT(GPSP, "LLffBfbf", "NavState,Lat,Lon,Alt,Yaw,Type,LoitR,LoitDir,PitMin"), LOG_FORMAT(ESC, "HBBBHHHHHHfH", "Counter,NumESC,Conn,N,Ver,Adr,Volt,Amp,RPM,Temp,SetP,SetPRAW"), LOG_FORMAT(GVSP, "fff", "VX,VY,VZ"), LOG_FORMAT(BATT, "ffff", "V,VFilt,C,Discharged"), From 984a815b94709916174f6a0beeb8ae8217a9aed1 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 28 Jan 2014 10:08:23 +0100 Subject: [PATCH 355/447] Navigator: make state names generic so that they can be used by the FSM visualisation tool later --- src/modules/commander/commander.cpp | 6 +- src/modules/mavlink/mavlink.c | 10 +- src/modules/navigator/navigator_main.cpp | 144 +++++++++--------- .../uORB/topics/vehicle_control_mode.h | 14 +- 4 files changed, 87 insertions(+), 87 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index f579fb52ad..ad142d09ec 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -493,14 +493,14 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe unsigned int mav_goto = cmd->param1; if (mav_goto == 0) { // MAV_GOTO_DO_HOLD - status->set_nav_state = NAV_STATE_LOITER; + status->set_nav_state = STATE_LOITER; status->set_nav_state_timestamp = hrt_absolute_time(); mavlink_log_critical(mavlink_fd, "#audio: pause mission cmd"); result = VEHICLE_CMD_RESULT_ACCEPTED; ret = true; } else if (mav_goto == 1) { // MAV_GOTO_DO_CONTINUE - status->set_nav_state = NAV_STATE_MISSION; + status->set_nav_state = STATE_MISSION; status->set_nav_state_timestamp = hrt_absolute_time(); mavlink_log_critical(mavlink_fd, "#audio: continue mission cmd"); result = VEHICLE_CMD_RESULT_ACCEPTED; @@ -624,7 +624,7 @@ int commander_thread_main(int argc, char *argv[]) memset(&armed, 0, sizeof(armed)); status.main_state = MAIN_STATE_MANUAL; - status.set_nav_state = NAV_STATE_NONE; + status.set_nav_state = STATE_NONE; status.set_nav_state_timestamp = 0; status.arming_state = ARMING_STATE_INIT; status.hil_state = HIL_STATE_OFF; diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c index 4f80917162..a13d0c537f 100644 --- a/src/modules/mavlink/mavlink.c +++ b/src/modules/mavlink/mavlink.c @@ -220,15 +220,15 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_base_mode, u } else if (v_status.main_state == MAIN_STATE_AUTO) { *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; - if (control_mode.nav_state == NAV_STATE_NONE) { // failsafe, shouldn't happen + if (control_mode.nav_state == STATE_NONE) { // failsafe, shouldn't happen custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY; - } else if (control_mode.nav_state == NAV_STATE_READY) { + } else if (control_mode.nav_state == STATE_READY) { custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY; - } else if (control_mode.nav_state == NAV_STATE_LOITER) { + } else if (control_mode.nav_state == STATE_LOITER) { custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LOITER; - } else if (control_mode.nav_state == NAV_STATE_MISSION) { + } else if (control_mode.nav_state == STATE_MISSION) { custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_MISSION; - } else if (control_mode.nav_state == NAV_STATE_RTL) { + } else if (control_mode.nav_state == STATE_RTL) { custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_RTL; } } diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index dd471928e3..8cc2a1489e 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -188,7 +188,7 @@ private: uint64_t _set_nav_state_timestamp; /**< timestamp of last handled navigation state request */ - char *nav_states_str[NAV_STATE_MAX]; + char *nav_states_str[MAX_STATE]; struct { float min_altitude; @@ -227,7 +227,7 @@ private: /** * State machine transition table */ - static StateTable::Tran const myTable[NAV_STATE_MAX][MAX_EVENT]; + static StateTable::Tran const myTable[MAX_STATE][MAX_EVENT]; enum RTLState { RTL_STATE_NONE = 0, @@ -363,7 +363,7 @@ Navigator *g_navigator; Navigator::Navigator() : /* state machine transition table */ - StateTable(&myTable[0][0], NAV_STATE_MAX, MAX_EVENT), + StateTable(&myTable[0][0], MAX_STATE, MAX_EVENT), _task_should_exit(false), _navigator_task(-1), @@ -423,7 +423,7 @@ Navigator::Navigator() : nav_states_str[5] = "LAND"; /* Initialize state machine */ - myState = NAV_STATE_NONE; + myState = STATE_NONE; start_none(); } @@ -604,7 +604,7 @@ Navigator::task_main() /* rate limit position updates to 50 Hz */ orb_set_interval(_global_pos_sub, 20); - unsigned prevState = NAV_STATE_NONE; + unsigned prevState = STATE_NONE; bool pub_control_mode = true; hrt_abstime mavlink_open_time = 0; const hrt_abstime mavlink_open_interval = 500000; @@ -667,7 +667,7 @@ Navigator::task_main() /* RC signal available, use control switches to set mode */ /* RETURN switch, overrides MISSION switch */ if (_vstatus.return_switch == RETURN_SWITCH_RETURN) { - if (myState != NAV_STATE_READY || _rtl_state != RTL_STATE_LAND) { + if (myState != STATE_READY || _rtl_state != RTL_STATE_LAND) { dispatch(EVENT_RTL_REQUESTED); } @@ -691,7 +691,7 @@ Navigator::task_main() stick_mode = true; } - if (!stick_mode && _vstatus.return_switch == RETURN_SWITCH_NORMAL && myState == NAV_STATE_RTL) { + if (!stick_mode && _vstatus.return_switch == RETURN_SWITCH_NORMAL && myState == STATE_RTL) { /* RETURN switch is in normal mode, no MISSION switch mapped, interrupt if in RTL state */ dispatch(EVENT_LOITER_REQUESTED); stick_mode = true; @@ -705,15 +705,15 @@ Navigator::task_main() _set_nav_state_timestamp = _vstatus.set_nav_state_timestamp; switch (_vstatus.set_nav_state) { - case NAV_STATE_NONE: + case STATE_NONE: /* nothing to do */ break; - case NAV_STATE_LOITER: + case STATE_LOITER: dispatch(EVENT_LOITER_REQUESTED); break; - case NAV_STATE_MISSION: + case STATE_MISSION: if (_mission.current_mission_available()) { dispatch(EVENT_MISSION_REQUESTED); @@ -723,8 +723,8 @@ Navigator::task_main() break; - case NAV_STATE_RTL: - if (myState != NAV_STATE_READY || _rtl_state != RTL_STATE_LAND) { + case STATE_RTL: + if (myState != STATE_READY || _rtl_state != RTL_STATE_LAND) { dispatch(EVENT_RTL_REQUESTED); } @@ -737,7 +737,7 @@ Navigator::task_main() } else { /* on first switch to AUTO try mission by default, if none is available fallback to loiter */ - if (myState == NAV_STATE_NONE) { + if (myState == STATE_NONE) { if (_mission.current_mission_available()) { dispatch(EVENT_MISSION_REQUESTED); @@ -755,14 +755,14 @@ Navigator::task_main() } else if (_vstatus.failsafe_state == FAILSAFE_STATE_RTL) { /* RTL on failsafe */ - if (myState != NAV_STATE_READY || _rtl_state != RTL_STATE_LAND) { + if (myState != STATE_READY || _rtl_state != RTL_STATE_LAND) { dispatch(EVENT_RTL_REQUESTED); } } else if (_vstatus.failsafe_state == FAILSAFE_STATE_LAND) { /* LAND on failsafe */ - if (myState != NAV_STATE_READY) { + if (myState != STATE_READY) { dispatch(EVENT_LAND_REQUESTED); } @@ -814,7 +814,7 @@ Navigator::task_main() global_position_update(); /* only check if waypoint has been reached in MISSION and RTL modes */ - if (myState == NAV_STATE_MISSION || myState == NAV_STATE_RTL) { + if (myState == STATE_MISSION || myState == STATE_RTL) { if (check_mission_item_reached()) { on_mission_item_reached(); } @@ -903,19 +903,19 @@ Navigator::status() } switch (myState) { - case NAV_STATE_NONE: + case STATE_NONE: warnx("State: None"); break; - case NAV_STATE_LOITER: + case STATE_LOITER: warnx("State: Loiter"); break; - case NAV_STATE_MISSION: + case STATE_MISSION: warnx("State: Mission"); break; - case NAV_STATE_RTL: + case STATE_RTL: warnx("State: RTL"); break; @@ -925,72 +925,72 @@ Navigator::status() } } -StateTable::Tran const Navigator::myTable[NAV_STATE_MAX][MAX_EVENT] = { +StateTable::Tran const Navigator::myTable[MAX_STATE][MAX_EVENT] = { { /* STATE_NONE */ - /* EVENT_NONE_REQUESTED */ {NO_ACTION, NAV_STATE_NONE}, - /* EVENT_READY_REQUESTED */ {ACTION(&Navigator::start_ready), NAV_STATE_READY}, - /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER}, - /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, - /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, - /* EVENT_LAND_REQUESTED */ {ACTION(&Navigator::start_land), NAV_STATE_LAND}, - /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_NONE}, - /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_NONE}, + /* EVENT_NONE_REQUESTED */ {NO_ACTION, STATE_NONE}, + /* EVENT_READY_REQUESTED */ {ACTION(&Navigator::start_ready), STATE_READY}, + /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), STATE_LOITER}, + /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), STATE_MISSION}, + /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), STATE_RTL}, + /* EVENT_LAND_REQUESTED */ {ACTION(&Navigator::start_land), STATE_LAND}, + /* EVENT_MISSION_CHANGED */ {NO_ACTION, STATE_NONE}, + /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, STATE_NONE}, }, { /* STATE_READY */ - /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE}, - /* EVENT_READY_REQUESTED */ {NO_ACTION, NAV_STATE_READY}, - /* EVENT_LOITER_REQUESTED */ {NO_ACTION, NAV_STATE_READY}, - /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, - /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, - /* EVENT_LAND_REQUESTED */ {NO_ACTION, NAV_STATE_READY}, - /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_READY}, - /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_READY}, + /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), STATE_NONE}, + /* EVENT_READY_REQUESTED */ {NO_ACTION, STATE_READY}, + /* EVENT_LOITER_REQUESTED */ {NO_ACTION, STATE_READY}, + /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), STATE_MISSION}, + /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), STATE_RTL}, + /* EVENT_LAND_REQUESTED */ {NO_ACTION, STATE_READY}, + /* EVENT_MISSION_CHANGED */ {NO_ACTION, STATE_READY}, + /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, STATE_READY}, }, { /* STATE_LOITER */ - /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE}, - /* EVENT_READY_REQUESTED */ {NO_ACTION, NAV_STATE_LOITER}, - /* EVENT_LOITER_REQUESTED */ {NO_ACTION, NAV_STATE_LOITER}, - /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, - /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, - /* EVENT_LAND_REQUESTED */ {ACTION(&Navigator::start_land), NAV_STATE_LAND}, - /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_LOITER}, - /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_LOITER}, + /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), STATE_NONE}, + /* EVENT_READY_REQUESTED */ {NO_ACTION, STATE_LOITER}, + /* EVENT_LOITER_REQUESTED */ {NO_ACTION, STATE_LOITER}, + /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), STATE_MISSION}, + /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), STATE_RTL}, + /* EVENT_LAND_REQUESTED */ {ACTION(&Navigator::start_land), STATE_LAND}, + /* EVENT_MISSION_CHANGED */ {NO_ACTION, STATE_LOITER}, + /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, STATE_LOITER}, }, { /* STATE_MISSION */ - /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE}, - /* EVENT_READY_REQUESTED */ {ACTION(&Navigator::start_ready), NAV_STATE_READY}, - /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER}, - /* EVENT_MISSION_REQUESTED */ {NO_ACTION, NAV_STATE_MISSION}, - /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, - /* EVENT_LAND_REQUESTED */ {ACTION(&Navigator::start_land), NAV_STATE_LAND}, - /* EVENT_MISSION_CHANGED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, - /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_MISSION}, + /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), STATE_NONE}, + /* EVENT_READY_REQUESTED */ {ACTION(&Navigator::start_ready), STATE_READY}, + /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), STATE_LOITER}, + /* EVENT_MISSION_REQUESTED */ {NO_ACTION, STATE_MISSION}, + /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), STATE_RTL}, + /* EVENT_LAND_REQUESTED */ {ACTION(&Navigator::start_land), STATE_LAND}, + /* EVENT_MISSION_CHANGED */ {ACTION(&Navigator::start_mission), STATE_MISSION}, + /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, STATE_MISSION}, }, { /* STATE_RTL */ - /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE}, - /* EVENT_READY_REQUESTED */ {ACTION(&Navigator::start_ready), NAV_STATE_READY}, - /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER}, - /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, - /* EVENT_RTL_REQUESTED */ {NO_ACTION, NAV_STATE_RTL}, - /* EVENT_LAND_REQUESTED */ {ACTION(&Navigator::start_land), NAV_STATE_LAND}, - /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_RTL}, - /* EVENT_HOME_POSITION_CHANGED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, // TODO need to reset rtl_state + /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), STATE_NONE}, + /* EVENT_READY_REQUESTED */ {ACTION(&Navigator::start_ready), STATE_READY}, + /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), STATE_LOITER}, + /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), STATE_MISSION}, + /* EVENT_RTL_REQUESTED */ {NO_ACTION, STATE_RTL}, + /* EVENT_LAND_REQUESTED */ {ACTION(&Navigator::start_land), STATE_LAND}, + /* EVENT_MISSION_CHANGED */ {NO_ACTION, STATE_RTL}, + /* EVENT_HOME_POSITION_CHANGED */ {ACTION(&Navigator::start_rtl), STATE_RTL}, // TODO need to reset rtl_state }, { /* STATE_LAND */ - /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE}, - /* EVENT_READY_REQUESTED */ {ACTION(&Navigator::start_ready), NAV_STATE_READY}, - /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER}, - /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, - /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, - /* EVENT_LAND_REQUESTED */ {NO_ACTION, NAV_STATE_LAND}, - /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_RTL}, - /* EVENT_HOME_POSITION_CHANGED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, + /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), STATE_NONE}, + /* EVENT_READY_REQUESTED */ {ACTION(&Navigator::start_ready), STATE_READY}, + /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), STATE_LOITER}, + /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), STATE_MISSION}, + /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), STATE_RTL}, + /* EVENT_LAND_REQUESTED */ {NO_ACTION, STATE_LAND}, + /* EVENT_MISSION_CHANGED */ {NO_ACTION, STATE_RTL}, + /* EVENT_HOME_POSITION_CHANGED */ {ACTION(&Navigator::start_rtl), STATE_RTL}, }, }; @@ -1499,7 +1499,7 @@ Navigator::check_mission_item_reached() void Navigator::on_mission_item_reached() { - if (myState == NAV_STATE_MISSION) { + if (myState == STATE_MISSION) { if (_do_takeoff) { /* takeoff completed */ _do_takeoff = false; @@ -1644,7 +1644,7 @@ Navigator::publish_control_mode() _control_mode.flag_control_manual_enabled = false; switch (myState) { - case NAV_STATE_READY: + case STATE_READY: /* disable all controllers, armed but idle */ _control_mode.flag_control_rates_enabled = false; _control_mode.flag_control_attitude_enabled = false; @@ -1654,7 +1654,7 @@ Navigator::publish_control_mode() _control_mode.flag_control_climb_rate_enabled = false; break; - case NAV_STATE_LAND: + case STATE_LAND: /* land with or without position control */ _control_mode.flag_control_manual_enabled = false; _control_mode.flag_control_rates_enabled = true; diff --git a/src/modules/uORB/topics/vehicle_control_mode.h b/src/modules/uORB/topics/vehicle_control_mode.h index 5aecac8987..7869a69585 100644 --- a/src/modules/uORB/topics/vehicle_control_mode.h +++ b/src/modules/uORB/topics/vehicle_control_mode.h @@ -62,13 +62,13 @@ */ typedef enum { - NAV_STATE_NONE = 0, - NAV_STATE_READY, - NAV_STATE_LOITER, - NAV_STATE_MISSION, - NAV_STATE_RTL, - NAV_STATE_LAND, - NAV_STATE_MAX + STATE_NONE = 0, + STATE_READY, + STATE_LOITER, + STATE_MISSION, + STATE_RTL, + STATE_LAND, + MAX_STATE } nav_state_t; struct vehicle_control_mode_s From cd9ec04904fa684f0ecddb3f2a4511346458fe8b Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 28 Jan 2014 10:15:28 +0100 Subject: [PATCH 356/447] Added FSM visualisation script --- Tools/draw_fsm_diagram.py | 202 ++++++++++++++++++++++++++++++++++++++ 1 file changed, 202 insertions(+) create mode 100755 Tools/draw_fsm_diagram.py diff --git a/Tools/draw_fsm_diagram.py b/Tools/draw_fsm_diagram.py new file mode 100755 index 0000000000..a36d39b41e --- /dev/null +++ b/Tools/draw_fsm_diagram.py @@ -0,0 +1,202 @@ +#!/usr/bin/python3 + +"""draw_fsm_diagram.py: Create dot code from a state transition table + +convert dot code to png using graphviz: + +dot fsm.dot -Tpng -o fsm.png +""" + +from optparse import OptionParser +import re +import subprocess + +__author__ = "Julian Oes" + +def get_dot_header(): + + return """digraph finite_state_machine { + graph [ dpi = 300 ]; + ratio = 1.5 + node [shape = circle];""" + +def get_dot_footer(): + + return """}\n""" + +def main(): + + # parse input arguments + parser = OptionParser() + parser.add_option("-i", "--input-file", default=None, help="choose file to parse") + parser.add_option("-d", "--dot-file", default=None, help="choose file for output dot file") + parser.add_option("-t", "--table-file", default=None, help="choose file for output of table") + (options, args) = parser.parse_args() + + # open source file + if options.input_file == None: + exit('please specify file') + f = open(options.input_file,'r') + source = f.read() + + # search for state transition table and extract the table itself + # first look for StateTable::Tran + # then accept anything including newline until { + # but don't accept the definition (without ;) + # then extract anything inside the brackets until }; + match = re.search(r'StateTable::Tran(?:.|\n!;)*\{((?:.|\n)*?)\};', source) + + if not match: + exit('no state transition table found') + + table_source = match.group(1) + + # bookkeeping for error checking + num_errors_found = 0 + + states = [] + events = [] + + # first get all states and events + for table_line in table_source.split('\n'): + + match = re.search(r'/\*\s+STATE_(\w+)\s+\*/', table_line) + if match: + states.append(str(match.group(1))) + # go to next line + continue + + if len(states) == 1: + match = re.search(r'/\*\s+EVENT_(\w+)\s+\*/', table_line) + if match: + events.append(str(match.group(1))) + + print('Found %d states and %d events' % (len(states), len(events))) + + + # keep track of origin state + state = None + + # fill dot code in here + dot_code = '' + + # create table len(states)xlen(events) + transition_table = [[[] for x in range(len(states))] for y in range(len(events))] + + # now fill the transition table and write the dot code + for table_line in table_source.split('\n'): + + # get states + # from: /* STATE_NONE */ + # extract only "NONE" + match = re.search(r'/\*\s+STATE_(\w+)\s+\*/', table_line) + if match: + state = match.group(1) + state_index = states.index(state) + # go to next line + continue + + # can't advance without proper state + if state == None: + continue + + # get event and next state + # from /* EVENT_READY_REQUESTED */ {ACTION(&Navigator::start_ready), STATE_READY} + # extract "READY_REQUESTED" and "READY" if there is ACTION + match_action = re.search(r'/\*\s+EVENT_(\w+)\s+\*/\s+\{ACTION\((?:.|\n)*STATE_(\w+)', table_line) + + # get event and next state + # from /* EVENT_NONE_REQUESTED */ {NO_ACTION, STATE_NONE}, + # extract "NONE_REQUESTED" and "STATE_NONE" if there is NO_ACTION + match_no_action = re.search(r'/\*\s+EVENT_(\w+)\s+\*/\s+\{NO_ACTION(?:.|\n)*STATE_(\w+)', table_line) + + # ignore lines with brackets only + if match_action or match_no_action: + + # only write arrows for actions + if match_action: + event = match_action.group(1) + new_state = match_action.group(2) + dot_code += ' ' + state + ' -> ' + new_state + '[ label = "' + event + '"];\n' + + elif match_no_action: + event = match_no_action.group(1) + new_state = match_no_action.group(2) + + # check for state changes without action + if state != new_state: + print('Error: no action but state change:') + print('State: ' + state + ' changed to: ' + new_state) + print(table_line) + num_errors_found += 1 + + # check for wrong events + if event not in events: + print('Error: unknown event: ' + event) + print(table_line) + num_errors_found += 1 + + # check for wrong new states + if new_state not in states: + print('Error: unknown new state: ' + new_state) + print(table_line) + num_errors_found += 1 + + # save new state in transition table + event_index = events.index(event) + + # bold for action + if match_action: + transition_table[event_index][state_index] = '**' + new_state + '**' + else: + transition_table[event_index][state_index] = new_state + + + + # assemble dot code + dot_code = get_dot_header() + dot_code + get_dot_footer() + + # write or print dot file + if options.dot_file: + f = open(options.dot_file,'w') + f.write(dot_code) + print('Wrote dot file') + else: + print('##########Dot-start##########') + print(dot_code) + print('##########Dot-end############') + + + # assemble doku wiki table + table_code = '| ^ ' + # start with header of all states + for state in states: + table_code += state + ' ^ ' + + table_code += '\n' + + # add events and new states + for event, row in zip(events, transition_table): + table_code += '^ ' + event + ' | ' + for new_state in row: + table_code += new_state + ' | ' + table_code += '\n' + + # write or print wiki table + if options.table_file: + f = open(options.table_file,'w') + f.write(table_code) + print('Wrote table file') + else: + print('##########Table-start########') + print(table_code) + print('##########Table-end##########') + + # report obvous errors + if num_errors_found: + print('Obvious errors found: %d' % num_errors_found) + else: + print('No obvious errors found') + +if __name__ == '__main__': + main() From 6002819f8fccb491dcfbe23de892a827cd9f4618 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 28 Jan 2014 10:21:48 +0100 Subject: [PATCH 357/447] Navigator: FSM bugfix and missing break --- src/modules/navigator/navigator_main.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 8cc2a1489e..2a782ea032 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -989,8 +989,8 @@ StateTable::Tran const Navigator::myTable[MAX_STATE][MAX_EVENT] = { /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), STATE_MISSION}, /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), STATE_RTL}, /* EVENT_LAND_REQUESTED */ {NO_ACTION, STATE_LAND}, - /* EVENT_MISSION_CHANGED */ {NO_ACTION, STATE_RTL}, - /* EVENT_HOME_POSITION_CHANGED */ {ACTION(&Navigator::start_rtl), STATE_RTL}, + /* EVENT_MISSION_CHANGED */ {NO_ACTION, STATE_LAND}, + /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, STATE_LAND}, }, }; @@ -1607,6 +1607,7 @@ Navigator::publish_control_mode() case MAIN_STATE_AUTO: navigator_enabled = true; + break; default: break; From c5a3dd916899ddd8409380c27c05a8b2d903c8f4 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 28 Jan 2014 10:46:44 +0100 Subject: [PATCH 358/447] FSM visualisation script: use argparse instead of optionparse and some minor cleanup --- Tools/draw_fsm_diagram.py | 29 ++++++++++++++--------------- 1 file changed, 14 insertions(+), 15 deletions(-) diff --git a/Tools/draw_fsm_diagram.py b/Tools/draw_fsm_diagram.py index a36d39b41e..fe37b6ba3f 100755 --- a/Tools/draw_fsm_diagram.py +++ b/Tools/draw_fsm_diagram.py @@ -1,15 +1,14 @@ -#!/usr/bin/python3 +#!/usr/bin/env python3 -"""draw_fsm_diagram.py: Create dot code from a state transition table +"""draw_fsm_diagram.py: Create dot code and dokuwiki table from a state transition table convert dot code to png using graphviz: dot fsm.dot -Tpng -o fsm.png """ -from optparse import OptionParser +import argparse import re -import subprocess __author__ = "Julian Oes" @@ -27,16 +26,16 @@ def get_dot_footer(): def main(): # parse input arguments - parser = OptionParser() - parser.add_option("-i", "--input-file", default=None, help="choose file to parse") - parser.add_option("-d", "--dot-file", default=None, help="choose file for output dot file") - parser.add_option("-t", "--table-file", default=None, help="choose file for output of table") - (options, args) = parser.parse_args() + parser = argparse.ArgumentParser(description='Create dot code and dokuwiki table from a state transition table.') + parser.add_argument("-i", "--input-file", default=None, help="choose file to parse") + parser.add_argument("-d", "--dot-file", default=None, help="choose file for output dot file") + parser.add_argument("-t", "--table-file", default=None, help="choose file for output of table") + args = parser.parse_args() # open source file - if options.input_file == None: + if args.input_file == None: exit('please specify file') - f = open(options.input_file,'r') + f = open(args.input_file,'r') source = f.read() # search for state transition table and extract the table itself @@ -157,8 +156,8 @@ def main(): dot_code = get_dot_header() + dot_code + get_dot_footer() # write or print dot file - if options.dot_file: - f = open(options.dot_file,'w') + if args.dot_file: + f = open(args.dot_file,'w') f.write(dot_code) print('Wrote dot file') else: @@ -183,8 +182,8 @@ def main(): table_code += '\n' # write or print wiki table - if options.table_file: - f = open(options.table_file,'w') + if args.table_file: + f = open(args.table_file,'w') f.write(table_code) print('Wrote table file') else: From 1488d32d0f25469da9119f36429c8783256048b1 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 28 Jan 2014 10:49:14 +0100 Subject: [PATCH 359/447] FSM visualisation script: renamed the file --- Tools/{draw_fsm_diagram.py => fsm_visualisation.py} | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) rename Tools/{draw_fsm_diagram.py => fsm_visualisation.py} (98%) diff --git a/Tools/draw_fsm_diagram.py b/Tools/fsm_visualisation.py similarity index 98% rename from Tools/draw_fsm_diagram.py rename to Tools/fsm_visualisation.py index fe37b6ba3f..5532b99bc8 100755 --- a/Tools/draw_fsm_diagram.py +++ b/Tools/fsm_visualisation.py @@ -1,6 +1,6 @@ #!/usr/bin/env python3 -"""draw_fsm_diagram.py: Create dot code and dokuwiki table from a state transition table +"""fsm_visualisation.py: Create dot code and dokuwiki table from a state transition table convert dot code to png using graphviz: From 547080f1882918a65db4e7cba396ae2c72d84544 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 28 Jan 2014 12:29:30 +0100 Subject: [PATCH 360/447] Revert "Navigator: make state names generic so that they can be used by the FSM visualisation tool later" This reverts commit 984a815b94709916174f6a0beeb8ae8217a9aed1. --- src/modules/commander/commander.cpp | 6 +- src/modules/mavlink/mavlink.c | 10 +- src/modules/navigator/navigator_main.cpp | 156 +++++++++--------- .../uORB/topics/vehicle_control_mode.h | 14 +- 4 files changed, 93 insertions(+), 93 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index ad142d09ec..f579fb52ad 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -493,14 +493,14 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe unsigned int mav_goto = cmd->param1; if (mav_goto == 0) { // MAV_GOTO_DO_HOLD - status->set_nav_state = STATE_LOITER; + status->set_nav_state = NAV_STATE_LOITER; status->set_nav_state_timestamp = hrt_absolute_time(); mavlink_log_critical(mavlink_fd, "#audio: pause mission cmd"); result = VEHICLE_CMD_RESULT_ACCEPTED; ret = true; } else if (mav_goto == 1) { // MAV_GOTO_DO_CONTINUE - status->set_nav_state = STATE_MISSION; + status->set_nav_state = NAV_STATE_MISSION; status->set_nav_state_timestamp = hrt_absolute_time(); mavlink_log_critical(mavlink_fd, "#audio: continue mission cmd"); result = VEHICLE_CMD_RESULT_ACCEPTED; @@ -624,7 +624,7 @@ int commander_thread_main(int argc, char *argv[]) memset(&armed, 0, sizeof(armed)); status.main_state = MAIN_STATE_MANUAL; - status.set_nav_state = STATE_NONE; + status.set_nav_state = NAV_STATE_NONE; status.set_nav_state_timestamp = 0; status.arming_state = ARMING_STATE_INIT; status.hil_state = HIL_STATE_OFF; diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c index a13d0c537f..4f80917162 100644 --- a/src/modules/mavlink/mavlink.c +++ b/src/modules/mavlink/mavlink.c @@ -220,15 +220,15 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_base_mode, u } else if (v_status.main_state == MAIN_STATE_AUTO) { *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; - if (control_mode.nav_state == STATE_NONE) { // failsafe, shouldn't happen + if (control_mode.nav_state == NAV_STATE_NONE) { // failsafe, shouldn't happen custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY; - } else if (control_mode.nav_state == STATE_READY) { + } else if (control_mode.nav_state == NAV_STATE_READY) { custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY; - } else if (control_mode.nav_state == STATE_LOITER) { + } else if (control_mode.nav_state == NAV_STATE_LOITER) { custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LOITER; - } else if (control_mode.nav_state == STATE_MISSION) { + } else if (control_mode.nav_state == NAV_STATE_MISSION) { custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_MISSION; - } else if (control_mode.nav_state == STATE_RTL) { + } else if (control_mode.nav_state == NAV_STATE_RTL) { custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_RTL; } } diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 2a782ea032..e6b7ef93d8 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -188,7 +188,7 @@ private: uint64_t _set_nav_state_timestamp; /**< timestamp of last handled navigation state request */ - char *nav_states_str[MAX_STATE]; + char *nav_states_str[NAV_STATE_MAX]; struct { float min_altitude; @@ -227,7 +227,7 @@ private: /** * State machine transition table */ - static StateTable::Tran const myTable[MAX_STATE][MAX_EVENT]; + static StateTable::Tran const myTable[NAV_STATE_MAX][MAX_EVENT]; enum RTLState { RTL_STATE_NONE = 0, @@ -363,7 +363,7 @@ Navigator *g_navigator; Navigator::Navigator() : /* state machine transition table */ - StateTable(&myTable[0][0], MAX_STATE, MAX_EVENT), + StateTable(&myTable[0][0], NAV_STATE_MAX, MAX_EVENT), _task_should_exit(false), _navigator_task(-1), @@ -423,7 +423,7 @@ Navigator::Navigator() : nav_states_str[5] = "LAND"; /* Initialize state machine */ - myState = STATE_NONE; + myState = NAV_STATE_NONE; start_none(); } @@ -604,7 +604,7 @@ Navigator::task_main() /* rate limit position updates to 50 Hz */ orb_set_interval(_global_pos_sub, 20); - unsigned prevState = STATE_NONE; + unsigned prevState = NAV_STATE_NONE; bool pub_control_mode = true; hrt_abstime mavlink_open_time = 0; const hrt_abstime mavlink_open_interval = 500000; @@ -667,7 +667,7 @@ Navigator::task_main() /* RC signal available, use control switches to set mode */ /* RETURN switch, overrides MISSION switch */ if (_vstatus.return_switch == RETURN_SWITCH_RETURN) { - if (myState != STATE_READY || _rtl_state != RTL_STATE_LAND) { + if (myState != NAV_STATE_READY || _rtl_state != RTL_STATE_LAND) { dispatch(EVENT_RTL_REQUESTED); } @@ -691,7 +691,7 @@ Navigator::task_main() stick_mode = true; } - if (!stick_mode && _vstatus.return_switch == RETURN_SWITCH_NORMAL && myState == STATE_RTL) { + if (!stick_mode && _vstatus.return_switch == RETURN_SWITCH_NORMAL && myState == NAV_STATE_RTL) { /* RETURN switch is in normal mode, no MISSION switch mapped, interrupt if in RTL state */ dispatch(EVENT_LOITER_REQUESTED); stick_mode = true; @@ -705,15 +705,15 @@ Navigator::task_main() _set_nav_state_timestamp = _vstatus.set_nav_state_timestamp; switch (_vstatus.set_nav_state) { - case STATE_NONE: + case NAV_STATE_NONE: /* nothing to do */ break; - case STATE_LOITER: + case NAV_STATE_LOITER: dispatch(EVENT_LOITER_REQUESTED); break; - case STATE_MISSION: + case NAV_STATE_MISSION: if (_mission.current_mission_available()) { dispatch(EVENT_MISSION_REQUESTED); @@ -723,8 +723,8 @@ Navigator::task_main() break; - case STATE_RTL: - if (myState != STATE_READY || _rtl_state != RTL_STATE_LAND) { + case NAV_STATE_RTL: + if (myState != NAV_STATE_READY || _rtl_state != RTL_STATE_LAND) { dispatch(EVENT_RTL_REQUESTED); } @@ -737,7 +737,7 @@ Navigator::task_main() } else { /* on first switch to AUTO try mission by default, if none is available fallback to loiter */ - if (myState == STATE_NONE) { + if (myState == NAV_STATE_NONE) { if (_mission.current_mission_available()) { dispatch(EVENT_MISSION_REQUESTED); @@ -755,14 +755,14 @@ Navigator::task_main() } else if (_vstatus.failsafe_state == FAILSAFE_STATE_RTL) { /* RTL on failsafe */ - if (myState != STATE_READY || _rtl_state != RTL_STATE_LAND) { + if (myState != NAV_STATE_READY || _rtl_state != RTL_STATE_LAND) { dispatch(EVENT_RTL_REQUESTED); } } else if (_vstatus.failsafe_state == FAILSAFE_STATE_LAND) { /* LAND on failsafe */ - if (myState != STATE_READY) { + if (myState != NAV_STATE_READY) { dispatch(EVENT_LAND_REQUESTED); } @@ -814,7 +814,7 @@ Navigator::task_main() global_position_update(); /* only check if waypoint has been reached in MISSION and RTL modes */ - if (myState == STATE_MISSION || myState == STATE_RTL) { + if (myState == NAV_STATE_MISSION || myState == NAV_STATE_RTL) { if (check_mission_item_reached()) { on_mission_item_reached(); } @@ -903,19 +903,19 @@ Navigator::status() } switch (myState) { - case STATE_NONE: + case NAV_STATE_NONE: warnx("State: None"); break; - case STATE_LOITER: + case NAV_STATE_LOITER: warnx("State: Loiter"); break; - case STATE_MISSION: + case NAV_STATE_MISSION: warnx("State: Mission"); break; - case STATE_RTL: + case NAV_STATE_RTL: warnx("State: RTL"); break; @@ -925,72 +925,72 @@ Navigator::status() } } -StateTable::Tran const Navigator::myTable[MAX_STATE][MAX_EVENT] = { +StateTable::Tran const Navigator::myTable[NAV_STATE_MAX][MAX_EVENT] = { { - /* STATE_NONE */ - /* EVENT_NONE_REQUESTED */ {NO_ACTION, STATE_NONE}, - /* EVENT_READY_REQUESTED */ {ACTION(&Navigator::start_ready), STATE_READY}, - /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), STATE_LOITER}, - /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), STATE_MISSION}, - /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), STATE_RTL}, - /* EVENT_LAND_REQUESTED */ {ACTION(&Navigator::start_land), STATE_LAND}, - /* EVENT_MISSION_CHANGED */ {NO_ACTION, STATE_NONE}, - /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, STATE_NONE}, + /* NAV_STATE_NONE */ + /* EVENT_NONE_REQUESTED */ {NO_ACTION, NAV_STATE_NONE}, + /* EVENT_READY_REQUESTED */ {ACTION(&Navigator::start_ready), NAV_STATE_READY}, + /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER}, + /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, + /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, + /* EVENT_LAND_REQUESTED */ {ACTION(&Navigator::start_land), NAV_STATE_LAND}, + /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_NONE}, + /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_NONE}, }, { - /* STATE_READY */ - /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), STATE_NONE}, - /* EVENT_READY_REQUESTED */ {NO_ACTION, STATE_READY}, - /* EVENT_LOITER_REQUESTED */ {NO_ACTION, STATE_READY}, - /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), STATE_MISSION}, - /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), STATE_RTL}, - /* EVENT_LAND_REQUESTED */ {NO_ACTION, STATE_READY}, - /* EVENT_MISSION_CHANGED */ {NO_ACTION, STATE_READY}, - /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, STATE_READY}, + /* NAV_STATE_READY */ + /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE}, + /* EVENT_READY_REQUESTED */ {NO_ACTION, NAV_STATE_READY}, + /* EVENT_LOITER_REQUESTED */ {NO_ACTION, NAV_STATE_READY}, + /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, + /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, + /* EVENT_LAND_REQUESTED */ {NO_ACTION, NAV_STATE_READY}, + /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_READY}, + /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_READY}, }, { - /* STATE_LOITER */ - /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), STATE_NONE}, - /* EVENT_READY_REQUESTED */ {NO_ACTION, STATE_LOITER}, - /* EVENT_LOITER_REQUESTED */ {NO_ACTION, STATE_LOITER}, - /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), STATE_MISSION}, - /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), STATE_RTL}, - /* EVENT_LAND_REQUESTED */ {ACTION(&Navigator::start_land), STATE_LAND}, - /* EVENT_MISSION_CHANGED */ {NO_ACTION, STATE_LOITER}, - /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, STATE_LOITER}, + /* NAV_STATE_LOITER */ + /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE}, + /* EVENT_READY_REQUESTED */ {NO_ACTION, NAV_STATE_LOITER}, + /* EVENT_LOITER_REQUESTED */ {NO_ACTION, NAV_STATE_LOITER}, + /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, + /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, + /* EVENT_LAND_REQUESTED */ {ACTION(&Navigator::start_land), NAV_STATE_LAND}, + /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_LOITER}, + /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_LOITER}, }, { - /* STATE_MISSION */ - /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), STATE_NONE}, - /* EVENT_READY_REQUESTED */ {ACTION(&Navigator::start_ready), STATE_READY}, - /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), STATE_LOITER}, - /* EVENT_MISSION_REQUESTED */ {NO_ACTION, STATE_MISSION}, - /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), STATE_RTL}, - /* EVENT_LAND_REQUESTED */ {ACTION(&Navigator::start_land), STATE_LAND}, - /* EVENT_MISSION_CHANGED */ {ACTION(&Navigator::start_mission), STATE_MISSION}, - /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, STATE_MISSION}, + /* NAV_STATE_MISSION */ + /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE}, + /* EVENT_READY_REQUESTED */ {ACTION(&Navigator::start_ready), NAV_STATE_READY}, + /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER}, + /* EVENT_MISSION_REQUESTED */ {NO_ACTION, NAV_STATE_MISSION}, + /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, + /* EVENT_LAND_REQUESTED */ {ACTION(&Navigator::start_land), NAV_STATE_LAND}, + /* EVENT_MISSION_CHANGED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, + /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_MISSION}, }, { - /* STATE_RTL */ - /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), STATE_NONE}, - /* EVENT_READY_REQUESTED */ {ACTION(&Navigator::start_ready), STATE_READY}, - /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), STATE_LOITER}, - /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), STATE_MISSION}, - /* EVENT_RTL_REQUESTED */ {NO_ACTION, STATE_RTL}, - /* EVENT_LAND_REQUESTED */ {ACTION(&Navigator::start_land), STATE_LAND}, - /* EVENT_MISSION_CHANGED */ {NO_ACTION, STATE_RTL}, - /* EVENT_HOME_POSITION_CHANGED */ {ACTION(&Navigator::start_rtl), STATE_RTL}, // TODO need to reset rtl_state + /* NAV_STATE_RTL */ + /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE}, + /* EVENT_READY_REQUESTED */ {ACTION(&Navigator::start_ready), NAV_STATE_READY}, + /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER}, + /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, + /* EVENT_RTL_REQUESTED */ {NO_ACTION, NAV_STATE_RTL}, + /* EVENT_LAND_REQUESTED */ {ACTION(&Navigator::start_land), NAV_STATE_LAND}, + /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_RTL}, + /* EVENT_HOME_POSITION_CHANGED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, // TODO need to reset rtl_state }, { - /* STATE_LAND */ - /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), STATE_NONE}, - /* EVENT_READY_REQUESTED */ {ACTION(&Navigator::start_ready), STATE_READY}, - /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), STATE_LOITER}, - /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), STATE_MISSION}, - /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), STATE_RTL}, - /* EVENT_LAND_REQUESTED */ {NO_ACTION, STATE_LAND}, - /* EVENT_MISSION_CHANGED */ {NO_ACTION, STATE_LAND}, - /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, STATE_LAND}, + /* NAV_STATE_LAND */ + /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE}, + /* EVENT_READY_REQUESTED */ {ACTION(&Navigator::start_ready), NAV_STATE_READY}, + /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER}, + /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, + /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, + /* EVENT_LAND_REQUESTED */ {NO_ACTION, NAV_STATE_LAND}, + /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_LAND}, + /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_LAND}, }, }; @@ -1499,7 +1499,7 @@ Navigator::check_mission_item_reached() void Navigator::on_mission_item_reached() { - if (myState == STATE_MISSION) { + if (myState == NAV_STATE_MISSION) { if (_do_takeoff) { /* takeoff completed */ _do_takeoff = false; @@ -1645,7 +1645,7 @@ Navigator::publish_control_mode() _control_mode.flag_control_manual_enabled = false; switch (myState) { - case STATE_READY: + case NAV_STATE_READY: /* disable all controllers, armed but idle */ _control_mode.flag_control_rates_enabled = false; _control_mode.flag_control_attitude_enabled = false; @@ -1655,7 +1655,7 @@ Navigator::publish_control_mode() _control_mode.flag_control_climb_rate_enabled = false; break; - case STATE_LAND: + case NAV_STATE_LAND: /* land with or without position control */ _control_mode.flag_control_manual_enabled = false; _control_mode.flag_control_rates_enabled = true; diff --git a/src/modules/uORB/topics/vehicle_control_mode.h b/src/modules/uORB/topics/vehicle_control_mode.h index 7869a69585..5aecac8987 100644 --- a/src/modules/uORB/topics/vehicle_control_mode.h +++ b/src/modules/uORB/topics/vehicle_control_mode.h @@ -62,13 +62,13 @@ */ typedef enum { - STATE_NONE = 0, - STATE_READY, - STATE_LOITER, - STATE_MISSION, - STATE_RTL, - STATE_LAND, - MAX_STATE + NAV_STATE_NONE = 0, + NAV_STATE_READY, + NAV_STATE_LOITER, + NAV_STATE_MISSION, + NAV_STATE_RTL, + NAV_STATE_LAND, + NAV_STATE_MAX } nav_state_t; struct vehicle_control_mode_s From 99875bbd28ee813950efb9402b3acce55c34469b Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 28 Jan 2014 12:31:23 +0100 Subject: [PATCH 361/447] FSM visualisation script: cope with prefixed state names --- Tools/fsm_visualisation.py | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/Tools/fsm_visualisation.py b/Tools/fsm_visualisation.py index 5532b99bc8..c678ef0f4b 100755 --- a/Tools/fsm_visualisation.py +++ b/Tools/fsm_visualisation.py @@ -59,7 +59,7 @@ def main(): # first get all states and events for table_line in table_source.split('\n'): - match = re.search(r'/\*\s+STATE_(\w+)\s+\*/', table_line) + match = re.search(r'/\*\s+\w+_STATE_(\w+)\s+\*/', table_line) if match: states.append(str(match.group(1))) # go to next line @@ -86,9 +86,9 @@ def main(): for table_line in table_source.split('\n'): # get states - # from: /* STATE_NONE */ + # from: /* NAV_STATE_NONE */ # extract only "NONE" - match = re.search(r'/\*\s+STATE_(\w+)\s+\*/', table_line) + match = re.search(r'/\*\s+\w+_STATE_(\w+)\s+\*/', table_line) if match: state = match.group(1) state_index = states.index(state) @@ -100,14 +100,14 @@ def main(): continue # get event and next state - # from /* EVENT_READY_REQUESTED */ {ACTION(&Navigator::start_ready), STATE_READY} + # from /* EVENT_READY_REQUESTED */ {ACTION(&Navigator::start_ready), NAV_STATE_READY} # extract "READY_REQUESTED" and "READY" if there is ACTION - match_action = re.search(r'/\*\s+EVENT_(\w+)\s+\*/\s+\{ACTION\((?:.|\n)*STATE_(\w+)', table_line) + match_action = re.search(r'/\*\s+EVENT_(\w+)\s+\*/\s+\{ACTION\((?:.|\n)*\w+_STATE_(\w+)', table_line) # get event and next state - # from /* EVENT_NONE_REQUESTED */ {NO_ACTION, STATE_NONE}, - # extract "NONE_REQUESTED" and "STATE_NONE" if there is NO_ACTION - match_no_action = re.search(r'/\*\s+EVENT_(\w+)\s+\*/\s+\{NO_ACTION(?:.|\n)*STATE_(\w+)', table_line) + # from /* EVENT_NONE_REQUESTED */ {NO_ACTION, NAV_STATE_NONE}, + # extract "NONE_REQUESTED" and "NAV_STATE_NONE" if there is NO_ACTION + match_no_action = re.search(r'/\*\s+EVENT_(\w+)\s+\*/\s+\{NO_ACTION(?:.|\n)*\w+_STATE_(\w+)', table_line) # ignore lines with brackets only if match_action or match_no_action: From 33daf84c00004bc1f56c507abb13a0dd9075c649 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 28 Jan 2014 16:56:53 +0100 Subject: [PATCH 362/447] lib/geo: bugs fixed, added function add_vector_to_global_position() --- src/lib/geo/geo.c | 27 +++++++++++++++++---------- src/lib/geo/geo.h | 6 ++++-- 2 files changed, 21 insertions(+), 12 deletions(-) diff --git a/src/lib/geo/geo.c b/src/lib/geo/geo.c index 08fe2b696e..446cd5bee3 100644 --- a/src/lib/geo/geo.c +++ b/src/lib/geo/geo.c @@ -72,7 +72,6 @@ __EXPORT void map_projection_init(double lat_0, double lon_0) //lat_0, lon_0 are /* calculate local scale by using the relation of true distance and the distance on plane */ //TODO: this is a quick solution, there are probably easier ways to determine the scale /* 1) calculate true distance d on sphere to a point: http://www.movable-type.co.uk/scripts/latlong.html */ - const double r_earth = 6371000; double lat1 = phi_1; double lon1 = lambda_0; @@ -81,7 +80,7 @@ __EXPORT void map_projection_init(double lat_0, double lon_0) //lat_0, lon_0 are double lon2 = lambda_0 + 0.5 / 180 * M_PI; double sin_lat_2 = sin(lat2); double cos_lat_2 = cos(lat2); - double d = acos(sin(lat1) * sin_lat_2 + cos(lat1) * cos_lat_2 * cos(lon2 - lon1)) * r_earth; + double d = acos(sin(lat1) * sin_lat_2 + cos(lat1) * cos_lat_2 * cos(lon2 - lon1)) * CONSTANTS_RADIUS_OF_EARTH; /* 2) calculate distance rho on plane */ double k_bar = 0; @@ -188,8 +187,7 @@ __EXPORT float get_distance_to_next_waypoint(double lat_now, double lon_now, dou double a = sin(d_lat / 2.0d) * sin(d_lat / 2.0d) + sin(d_lon / 2.0d) * sin(d_lon / 2.0d) * cos(lat_now_rad) * cos(lat_next_rad); double c = 2.0d * atan2(sqrt(a), sqrt(1.0d - a)); - const double radius_earth = 6371000.0d; - return radius_earth * c; + return CONSTANTS_RADIUS_OF_EARTH * c; } __EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next) @@ -210,7 +208,7 @@ __EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, doub return theta; } -__EXPORT void get_vector_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next, float* vx, float* vy) +__EXPORT void get_vector_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next, float* v_n, float* v_e) { double lat_now_rad = lat_now * M_DEG_TO_RAD; double lon_now_rad = lon_now * M_DEG_TO_RAD; @@ -221,11 +219,11 @@ __EXPORT void get_vector_to_next_waypoint(double lat_now, double lon_now, double double d_lon = lon_next_rad - lon_now_rad; /* conscious mix of double and float trig function to maximize speed and efficiency */ - *vy = CONSTANTS_RADIUS_OF_EARTH * sin(d_lon) * cos(lat_next_rad); - *vx = CONSTANTS_RADIUS_OF_EARTH * cos(lat_now_rad) * sin(lat_next_rad) - sin(lat_now_rad) * cos(lat_next_rad) * cos(d_lon); + *v_n = CONSTANTS_RADIUS_OF_EARTH * (cos(lat_now_rad) * sin(lat_next_rad) - sin(lat_now_rad) * cos(lat_next_rad) * cos(d_lon)); + *v_e = CONSTANTS_RADIUS_OF_EARTH * sin(d_lon) * cos(lat_next_rad); } -__EXPORT void get_vector_to_next_waypoint_fast(double lat_now, double lon_now, double lat_next, double lon_next, float* vx, float* vy) +__EXPORT void get_vector_to_next_waypoint_fast(double lat_now, double lon_now, double lat_next, double lon_next, float* v_n, float* v_e) { double lat_now_rad = lat_now * M_DEG_TO_RAD; double lon_now_rad = lon_now * M_DEG_TO_RAD; @@ -236,8 +234,17 @@ __EXPORT void get_vector_to_next_waypoint_fast(double lat_now, double lon_now, d double d_lon = lon_next_rad - lon_now_rad; /* conscious mix of double and float trig function to maximize speed and efficiency */ - *vy = CONSTANTS_RADIUS_OF_EARTH * d_lon; - *vx = CONSTANTS_RADIUS_OF_EARTH * cos(lat_now_rad); + *v_n = CONSTANTS_RADIUS_OF_EARTH * d_lat; + *v_e = CONSTANTS_RADIUS_OF_EARTH * d_lon * cos(lat_now_rad); +} + +__EXPORT void add_vector_to_global_position(double lat_now, double lon_now, float v_n, float v_e, double *lat_res, double *lon_res) +{ + double lat_now_rad = lat_now * M_DEG_TO_RAD; + double lon_now_rad = lon_now * M_DEG_TO_RAD; + + *lat_res = (lat_now_rad + v_n / CONSTANTS_RADIUS_OF_EARTH) * M_RAD_TO_DEG; + *lon_res = (lon_now_rad + v_e / (CONSTANTS_RADIUS_OF_EARTH * cos(lat_now_rad))) * M_RAD_TO_DEG; } // Additional functions - @author Doug Weibel diff --git a/src/lib/geo/geo.h b/src/lib/geo/geo.h index 5f4bce6986..94afb4df0a 100644 --- a/src/lib/geo/geo.h +++ b/src/lib/geo/geo.h @@ -115,9 +115,11 @@ __EXPORT float get_distance_to_next_waypoint(double lat_now, double lon_now, dou */ __EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next); -__EXPORT void get_vector_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next, float* vx, float* vy); +__EXPORT void get_vector_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next, float* v_n, float* v_e); -__EXPORT void get_vector_to_next_waypoint_fast(double lat_now, double lon_now, double lat_next, double lon_next, float* vx, float* vy); +__EXPORT void get_vector_to_next_waypoint_fast(double lat_now, double lon_now, double lat_next, double lon_next, float* v_n, float* v_e); + +__EXPORT void add_vector_to_global_position(double lat_now, double lon_now, float v_n, float v_e, double *lat_res, double *lon_res); __EXPORT int get_distance_to_line(struct crosstrack_error_s * crosstrack_error, double lat_now, double lon_now, double lat_start, double lon_start, double lat_end, double lon_end); From 3601b807479ba8ea029b92872fcdd2ad2d051c54 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 28 Jan 2014 16:57:31 +0100 Subject: [PATCH 363/447] mc_pos_control: avoid global to local projections, work in global frame --- .../mc_pos_control/mc_pos_control_main.cpp | 255 +++++++++--------- 1 file changed, 131 insertions(+), 124 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 d3e39e3a02..af2674192a 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -63,8 +63,7 @@ #include #include #include -#include -#include +#include #include #include #include @@ -109,6 +108,7 @@ private: bool _task_should_exit; /**< if true, task should exit */ int _control_task; /**< task handle for task */ + int _mavlink_fd; /**< mavlink fd */ int _att_sub; /**< vehicle attitude subscription */ int _att_sp_sub; /**< vehicle attitude setpoint */ @@ -116,11 +116,11 @@ private: int _params_sub; /**< notification of parameter updates */ int _manual_sub; /**< notification of manual control updates */ int _arming_sub; /**< arming status of outputs */ - int _local_pos_sub; /**< vehicle local position */ + int _global_pos_sub; /**< vehicle local position */ int _pos_sp_triplet_sub; /**< position setpoint triplet */ - orb_advert_t _local_pos_sp_pub; /**< local position setpoint publication */ orb_advert_t _att_sp_pub; /**< attitude setpoint publication */ + orb_advert_t _pos_sp_triplet_pub; /**< position setpoint triplet publication */ orb_advert_t _global_vel_sp_pub; /**< vehicle global velocity setpoint */ struct vehicle_attitude_s _att; /**< vehicle attitude */ @@ -128,8 +128,7 @@ private: struct manual_control_setpoint_s _manual; /**< r/c channel data */ struct vehicle_control_mode_s _control_mode; /**< vehicle control mode */ struct actuator_armed_s _arming; /**< actuator arming status */ - struct vehicle_local_position_s _local_pos; /**< vehicle local position */ - struct vehicle_local_position_setpoint_s _local_pos_sp; /**< vehicle local position */ + struct vehicle_global_position_s _global_pos; /**< vehicle global position */ struct position_setpoint_triplet_s _pos_sp_triplet; /**< vehicle global position setpoint triplet */ struct vehicle_global_velocity_setpoint_s _global_vel_sp; /**< vehicle global velocity setpoint */ @@ -177,9 +176,14 @@ private: math::Vector<3> sp_offs_max; } _params; - math::Vector<3> _pos; + double _lat_sp; + double _lon_sp; + float _alt_sp; + + bool _reset_lat_lon_sp; + bool _reset_alt_sp; + math::Vector<3> _vel; - math::Vector<3> _pos_sp; math::Vector<3> _vel_sp; math::Vector<3> _vel_prev; /**< velocity on previous step */ @@ -200,6 +204,16 @@ private: static float scale_control(float ctl, float end, float dz); + /** + * Reset lat/lon to current position + */ + void reset_lat_lon_sp(); + + /** + * Reset altitude setpoint to current altitude + */ + void reset_alt_sp(); + /** * Shim for calling task_main from task_create. */ @@ -227,6 +241,7 @@ MulticopterPositionControl::MulticopterPositionControl() : _task_should_exit(false), _control_task(-1), + _mavlink_fd(-1), /* subscriptions */ _att_sub(-1), @@ -235,21 +250,27 @@ MulticopterPositionControl::MulticopterPositionControl() : _params_sub(-1), _manual_sub(-1), _arming_sub(-1), - _local_pos_sub(-1), + _global_pos_sub(-1), _pos_sp_triplet_sub(-1), /* publications */ - _local_pos_sp_pub(-1), _att_sp_pub(-1), - _global_vel_sp_pub(-1) + _pos_sp_triplet_pub(-1), + _global_vel_sp_pub(-1), + + _lat_sp(0.0), + _lon_sp(0.0), + _alt_sp(0.0f), + + _reset_lat_lon_sp(true), + _reset_alt_sp(true) { memset(&_att, 0, sizeof(_att)); memset(&_att_sp, 0, sizeof(_att_sp)); memset(&_manual, 0, sizeof(_manual)); memset(&_control_mode, 0, sizeof(_control_mode)); memset(&_arming, 0, sizeof(_arming)); - memset(&_local_pos, 0, sizeof(_local_pos)); - memset(&_local_pos_sp, 0, sizeof(_local_pos_sp)); + memset(&_global_pos, 0, sizeof(_global_pos)); memset(&_pos_sp_triplet, 0, sizeof(_pos_sp_triplet)); memset(&_global_vel_sp, 0, sizeof(_global_vel_sp)); @@ -261,9 +282,7 @@ MulticopterPositionControl::MulticopterPositionControl() : _params.vel_ff.zero(); _params.sp_offs_max.zero(); - _pos.zero(); _vel.zero(); - _pos_sp.zero(); _vel_sp.zero(); _vel_prev.zero(); @@ -405,10 +424,10 @@ MulticopterPositionControl::poll_subscriptions() if (updated) orb_copy(ORB_ID(actuator_armed), _arming_sub, &_arming); - orb_check(_pos_sp_triplet_sub, &updated); + orb_check(_global_pos_sub, &updated); if (updated) - orb_copy(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_sub, &_pos_sp_triplet); + orb_copy(ORB_ID(vehicle_global_position), _global_pos_sub, &_global_pos); } float @@ -431,14 +450,34 @@ MulticopterPositionControl::task_main_trampoline(int argc, char *argv[]) pos_control::g_control->task_main(); } +void +MulticopterPositionControl::reset_lat_lon_sp() +{ + if (_reset_lat_lon_sp) { + _reset_lat_lon_sp = false; + _lat_sp = _global_pos.lat; + _lon_sp = _global_pos.lon; + mavlink_log_info(_mavlink_fd, "[mpc] reset lat/lon sp: %.7f, %.7f", _lat_sp, _lon_sp); + } +} + +void +MulticopterPositionControl::reset_alt_sp() +{ + if (_reset_alt_sp) { + _reset_alt_sp = false; + _alt_sp = _global_pos.alt; + mavlink_log_info(_mavlink_fd, "[mpc] reset alt sp: %.2f", (double)_alt_sp); + } +} + void MulticopterPositionControl::task_main() { warnx("started"); - static int mavlink_fd; - mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); - mavlink_log_info(mavlink_fd, "[mpc] started"); + _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); + mavlink_log_info(_mavlink_fd, "[mpc] started"); /* * do subscriptions @@ -449,7 +488,7 @@ MulticopterPositionControl::task_main() _params_sub = orb_subscribe(ORB_ID(parameter_update)); _manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); _arming_sub = orb_subscribe(ORB_ID(actuator_armed)); - _local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position)); + _global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); _pos_sp_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet)); parameters_update(true); @@ -460,8 +499,6 @@ MulticopterPositionControl::task_main() /* get an initial update for all sensor and status data */ poll_subscriptions(); - bool reset_sp_z = true; - bool reset_sp_xy = true; bool reset_int_z = true; bool reset_int_z_manual = false; bool reset_int_xy = true; @@ -472,11 +509,6 @@ MulticopterPositionControl::task_main() const float alt_ctl_dz = 0.2f; const float pos_ctl_dz = 0.05f; - hrt_abstime ref_timestamp = 0; - int32_t ref_lat = 0.0f; - int32_t ref_lon = 0.0f; - float ref_alt = 0.0f; - math::Vector<3> sp_move_rate; sp_move_rate.zero(); math::Vector<3> thrust_int; @@ -488,7 +520,7 @@ MulticopterPositionControl::task_main() struct pollfd fds[1]; /* Setup of loop */ - fds[0].fd = _local_pos_sub; + fds[0].fd = _global_pos_sub; fds[0].events = POLLIN; while (!_task_should_exit) { @@ -505,8 +537,6 @@ MulticopterPositionControl::task_main() continue; } - orb_copy(ORB_ID(vehicle_local_position), _local_pos_sub, &_local_pos); - poll_subscriptions(); parameters_update(false); @@ -516,8 +546,8 @@ MulticopterPositionControl::task_main() if (_control_mode.flag_armed && !was_armed) { /* reset setpoints and integrals on arming */ - reset_sp_z = true; - reset_sp_xy = true; + _reset_lat_lon_sp = true; + _reset_alt_sp = true; reset_int_z = true; reset_int_xy = true; } @@ -529,60 +559,26 @@ MulticopterPositionControl::task_main() _control_mode.flag_control_climb_rate_enabled || _control_mode.flag_control_velocity_enabled) { - _pos(0) = _local_pos.x; - _pos(1) = _local_pos.y; - _pos(2) = _local_pos.z; - _vel(0) = _local_pos.vx; - _vel(1) = _local_pos.vy; - _vel(2) = _local_pos.vz; + _vel(0) = _global_pos.vel_n; + _vel(1) = _global_pos.vel_e; + _vel(2) = _global_pos.vel_d; sp_move_rate.zero(); - if (_local_pos.ref_timestamp != ref_timestamp) { - /* initialize local projection with new reference */ - double lat_home = _local_pos.ref_lat * 1e-7; - double lon_home = _local_pos.ref_lon * 1e-7; - map_projection_init(lat_home, lon_home); - mavlink_log_info(mavlink_fd, "[mpc] local pos ref: %.7f, %.7f", (double)lat_home, (double)lon_home); - - if (_control_mode.flag_control_manual_enabled && ref_timestamp != 0) { - /* correct setpoint in manual mode to stay in the same point */ - float ref_change_x = 0.0f; - float ref_change_y = 0.0f; - map_projection_project(ref_lat, ref_lon, &ref_change_x, &ref_change_y); - _pos_sp(0) += ref_change_x; - _pos_sp(1) += ref_change_y; - _pos_sp(2) += _local_pos.ref_alt - ref_alt; - } - - ref_timestamp = _local_pos.ref_timestamp; - ref_lat = _local_pos.ref_lat; - ref_lon = _local_pos.ref_lon; - ref_alt = _local_pos.ref_alt; - } - + /* select control source */ if (_control_mode.flag_control_manual_enabled) { /* manual control */ if (_control_mode.flag_control_altitude_enabled) { - /* reset setpoint Z to current altitude if needed */ - if (reset_sp_z) { - reset_sp_z = false; - _pos_sp(2) = _pos(2); - mavlink_log_info(mavlink_fd, "[mpc] reset alt sp: %.2f", (double) - _pos_sp(2)); - } + /* reset alt setpoint to current altitude if needed */ + reset_alt_sp(); /* move altitude setpoint with throttle stick */ sp_move_rate(2) = -scale_control(_manual.throttle - 0.5f, 0.5f, alt_ctl_dz); } if (_control_mode.flag_control_position_enabled) { - /* reset setpoint XY to current position if needed */ - if (reset_sp_xy) { - reset_sp_xy = false; - _pos_sp(0) = _pos(0); - _pos_sp(1) = _pos(1); - mavlink_log_info(mavlink_fd, "[mpc] reset pos sp: %.2f, %.2f", (double)_pos_sp(0), (double)_pos_sp(1)); - } + /* reset lat/lon setpoint to current position if needed */ + reset_lat_lon_sp(); /* move position setpoint with roll/pitch stick */ sp_move_rate(0) = scale_control(-_manual.pitch / _params.rc_scale_pitch, 1.0f, pos_ctl_dz); @@ -602,85 +598,96 @@ MulticopterPositionControl::task_main() sp_move_rate = R_yaw_sp * sp_move_rate.emult(_params.vel_max); /* move position setpoint */ - _pos_sp += sp_move_rate * dt; + add_vector_to_global_position(_lat_sp, _lon_sp, sp_move_rate(0) * dt, sp_move_rate(1) * dt, &_lat_sp, &_lon_sp); + _alt_sp -= sp_move_rate(2) * dt; /* check if position setpoint is too far from actual position */ math::Vector<3> pos_sp_offs; pos_sp_offs.zero(); if (_control_mode.flag_control_position_enabled) { - pos_sp_offs(0) = (_pos_sp(0) - _pos(0)) / _params.sp_offs_max(0); - pos_sp_offs(1) = (_pos_sp(1) - _pos(1)) / _params.sp_offs_max(1); + get_vector_to_next_waypoint_fast(_global_pos.lat, _global_pos.lon, _lat_sp, _lon_sp, &pos_sp_offs.data[0], &pos_sp_offs.data[1]); + pos_sp_offs(0) /= _params.sp_offs_max(0); + pos_sp_offs(1) /= _params.sp_offs_max(1); } if (_control_mode.flag_control_altitude_enabled) { - pos_sp_offs(2) = (_pos_sp(2) - _pos(2)) / _params.sp_offs_max(2); + pos_sp_offs(2) = -(_alt_sp - _global_pos.alt) / _params.sp_offs_max(2); } float pos_sp_offs_norm = pos_sp_offs.length(); if (pos_sp_offs_norm > 1.0f) { pos_sp_offs /= pos_sp_offs_norm; - _pos_sp = _pos + pos_sp_offs.emult(_params.sp_offs_max); + add_vector_to_global_position(_lat_sp, _lon_sp, pos_sp_offs(0) * _params.sp_offs_max(0), pos_sp_offs(1) * _params.sp_offs_max(1), &_lat_sp, &_lon_sp); + _alt_sp = _global_pos.alt - pos_sp_offs(2) * _params.sp_offs_max(2); } - } else { + /* fill position setpoint triplet */ + _pos_sp_triplet.previous.valid = true; + _pos_sp_triplet.current.valid = true; + _pos_sp_triplet.next.valid = true; + + _pos_sp_triplet.current.type = SETPOINT_TYPE_NORMAL; + _pos_sp_triplet.current.lat = _lat_sp; + _pos_sp_triplet.current.lon = _lon_sp; + _pos_sp_triplet.current.alt = _alt_sp; + _pos_sp_triplet.current.yaw = _att_sp.yaw_body; + _pos_sp_triplet.current.loiter_radius = 0.0f; + _pos_sp_triplet.current.loiter_direction = 1.0f; + _pos_sp_triplet.current.pitch_min = 0.0f; + + /* publish position setpoint triplet */ + if (_pos_sp_triplet_pub > 0) { + orb_publish(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_pub, &_pos_sp_triplet); + + } else { + _pos_sp_triplet_pub = orb_advertise(ORB_ID(position_setpoint_triplet), &_pos_sp_triplet); + } + + } else if (_control_mode.flag_control_auto_enabled) { /* AUTO */ + bool updated; + orb_check(_pos_sp_triplet_sub, &updated); + + if (updated) + orb_copy(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_sub, &_pos_sp_triplet); + if (_pos_sp_triplet.current.valid) { - struct position_setpoint_s current_sp = _pos_sp_triplet.current; - - _pos_sp(2) = -(current_sp.alt - ref_alt); - - map_projection_project(current_sp.lat, current_sp.lon, &_pos_sp(0), &_pos_sp(1)); - - if (isfinite(current_sp.yaw)) { - _att_sp.yaw_body = current_sp.yaw; - } - /* in case of interrupted mission don't go to waypoint but stay at current position */ - reset_sp_xy = true; - reset_sp_z = true; + _reset_lat_lon_sp = true; + _reset_alt_sp = true; + + _lat_sp = _pos_sp_triplet.current.lat; + _lon_sp = _pos_sp_triplet.current.lon; + _alt_sp = _pos_sp_triplet.current.alt; } else { /* no waypoint, loiter, reset position setpoint if needed */ - if (reset_sp_xy) { - reset_sp_xy = false; - _pos_sp(0) = _pos(0); - _pos_sp(1) = _pos(1); - } - - if (reset_sp_z) { - reset_sp_z = false; - _pos_sp(2) = _pos(2); - } + reset_lat_lon_sp(); + reset_alt_sp(); } - } - - /* copy resulting setpoint to vehicle_local_position_setpoint topic for logging */ - _local_pos_sp.yaw = _att_sp.yaw_body; - _local_pos_sp.x = _pos_sp(0); - _local_pos_sp.y = _pos_sp(1); - _local_pos_sp.z = _pos_sp(2); - - /* publish local position setpoint */ - if (_local_pos_sp_pub > 0) { - orb_publish(ORB_ID(vehicle_local_position_setpoint), _local_pos_sp_pub, &_local_pos_sp); - } else { - _local_pos_sp_pub = orb_advertise(ORB_ID(vehicle_local_position_setpoint), &_local_pos_sp); + /* no control, reset setpoint */ + reset_lat_lon_sp(); + reset_alt_sp(); } /* run position & altitude controllers, calculate velocity setpoint */ - math::Vector<3> pos_err = _pos_sp - _pos; + math::Vector<3> pos_err; + float err_x, err_y; + get_vector_to_next_waypoint_fast(_global_pos.lat, _global_pos.lon, _lat_sp, _lon_sp, &pos_err.data[0], &pos_err.data[1]); + pos_err(2) = -(_alt_sp - _global_pos.alt); + _vel_sp = pos_err.emult(_params.pos_p) + sp_move_rate.emult(_params.vel_ff); if (!_control_mode.flag_control_altitude_enabled) { - reset_sp_z = true; + _reset_alt_sp = true; _vel_sp(2) = 0.0f; } if (!_control_mode.flag_control_position_enabled) { - reset_sp_xy = true; + _reset_lat_lon_sp = true; _vel_sp(0) = 0.0f; _vel_sp(1) = 0.0f; } @@ -730,7 +737,7 @@ MulticopterPositionControl::task_main() } thrust_int(2) = -i; - mavlink_log_info(mavlink_fd, "[mpc] reset hovering thrust: %.2f", (double)i); + mavlink_log_info(_mavlink_fd, "[mpc] reset hovering thrust: %.2f", (double)i); } } else { @@ -742,7 +749,7 @@ MulticopterPositionControl::task_main() reset_int_xy = false; thrust_int(0) = 0.0f; thrust_int(1) = 0.0f; - mavlink_log_info(mavlink_fd, "[mpc] reset xy vel integral"); + mavlink_log_info(_mavlink_fd, "[mpc] reset xy vel integral"); } } else { @@ -946,8 +953,8 @@ MulticopterPositionControl::task_main() } else { /* position controller disabled, reset setpoints */ - reset_sp_z = true; - reset_sp_xy = true; + _reset_alt_sp = true; + _reset_lat_lon_sp = true; reset_int_z = true; reset_int_xy = true; } @@ -957,7 +964,7 @@ MulticopterPositionControl::task_main() } warnx("stopped"); - mavlink_log_info(mavlink_fd, "[mpc] stopped"); + mavlink_log_info(_mavlink_fd, "[mpc] stopped"); _control_task = -1; _exit(0); From 5ffa28b0e950ac3633df61bcfbdb7e2cefec3be4 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 28 Jan 2014 18:21:37 +0100 Subject: [PATCH 364/447] dlog2: GPSP message fixed --- src/modules/sdlog2/sdlog2_messages.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 1e2441c331..baac2ee3ec 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -304,7 +304,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(ARSP, "fff", "RollRateSP,PitchRateSP,YawRateSP"), LOG_FORMAT(FLOW, "hhfffBB", "RawX,RawY,CompX,CompY,Dist,Q,SensID"), LOG_FORMAT(GPOS, "LLffff", "Lat,Lon,Alt,VelN,VelE,VelD"), - LOG_FORMAT(GPSP, "LLffBfbf", "NavState,Lat,Lon,Alt,Yaw,Type,LoitR,LoitDir,PitMin"), + LOG_FORMAT(GPSP, "BLLffBfbf", "NavState,Lat,Lon,Alt,Yaw,Type,LoitR,LoitDir,PitMin"), LOG_FORMAT(ESC, "HBBBHHHHHHfH", "Counter,NumESC,Conn,N,Ver,Adr,Volt,Amp,RPM,Temp,SetP,SetPRAW"), LOG_FORMAT(GVSP, "fff", "VX,VY,VZ"), LOG_FORMAT(BATT, "ffff", "V,VFilt,C,Discharged"), From 48f1b7e1c77a66973c6bb847290018531a99503c Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 28 Jan 2014 18:39:55 +0100 Subject: [PATCH 365/447] mc_pos_control: fill nav_state = NONE when publishing position setpoint triplet --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 1 + 1 file changed, 1 insertion(+) 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 af2674192a..4ff13d4df2 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -628,6 +628,7 @@ MulticopterPositionControl::task_main() _pos_sp_triplet.current.valid = true; _pos_sp_triplet.next.valid = true; + _pos_sp_triplet.nav_state = NAV_STATE_NONE; _pos_sp_triplet.current.type = SETPOINT_TYPE_NORMAL; _pos_sp_triplet.current.lat = _lat_sp; _pos_sp_triplet.current.lon = _lon_sp; From 6a1a29f77ecc9ded341bfbca037c9a6768ed3fb4 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 28 Jan 2014 20:40:05 +0100 Subject: [PATCH 366/447] global_position topic: added baro_alt, mc_pos_control: SEATBELT mode fixed, use baro/AMSL alt --- src/drivers/frsky_telemetry/frsky_data.c | 2 +- .../att_pos_estimator_ekf/KalmanNav.cpp | 2 +- .../attitude_estimator_ekf_main.cpp | 2 +- src/modules/commander/commander.cpp | 4 +- src/modules/mavlink/mavlink_receiver.cpp | 2 +- .../mc_pos_control/mc_pos_control_main.cpp | 45 ++++++++++++++++--- src/modules/navigator/navigator_main.cpp | 5 ++- .../position_estimator_inav_main.c | 23 ++++------ src/modules/sdlog2/sdlog2.c | 2 + src/modules/sdlog2/sdlog2_messages.h | 4 +- .../uORB/topics/vehicle_global_position.h | 12 +++-- 11 files changed, 69 insertions(+), 34 deletions(-) diff --git a/src/drivers/frsky_telemetry/frsky_data.c b/src/drivers/frsky_telemetry/frsky_data.c index e201ecbb3d..cfcf91e3fb 100644 --- a/src/drivers/frsky_telemetry/frsky_data.c +++ b/src/drivers/frsky_telemetry/frsky_data.c @@ -225,7 +225,7 @@ void frsky_send_frame2(int uart) float course = 0, lat = 0, lon = 0, speed = 0, alt = 0; char lat_ns = 0, lon_ew = 0; int sec = 0; - if (global_pos.valid) { + if (global_pos.global_valid) { time_t time_gps = global_pos.time_gps_usec / 1000000; struct tm *tm_gps = gmtime(&time_gps); diff --git a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp b/src/modules/att_pos_estimator_ekf/KalmanNav.cpp index 8e88130e15..7f0dd92192 100644 --- a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp +++ b/src/modules/att_pos_estimator_ekf/KalmanNav.cpp @@ -314,7 +314,7 @@ void KalmanNav::updatePublications() // global position publication _pos.timestamp = _pubTimeStamp; _pos.time_gps_usec = _gps.timestamp_position; - _pos.valid = true; + _pos.global_valid = true; _pos.lat = getLatDegE7(); _pos.lon = getLonDegE7(); _pos.alt = float(alt); diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp index 66ec20b954..620185fb77 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp @@ -410,7 +410,7 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds vel(2) = gps.vel_d_m_s; } - } else if (ekf_params.acc_comp == 2 && global_pos.valid && hrt_absolute_time() < global_pos.timestamp + 500000) { + } else if (ekf_params.acc_comp == 2 && global_pos.global_valid && hrt_absolute_time() < global_pos.timestamp + 500000) { vel_valid = true; if (global_pos_updated) { vel_t = global_pos.timestamp; diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 60fb4f486a..901f919115 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -871,7 +871,7 @@ int commander_thread_main(int argc, char *argv[]) } /* update condition_global_position_valid */ - check_valid(global_position.timestamp, POSITION_TIMEOUT, global_position.valid, &(status.condition_global_position_valid), &status_changed); + check_valid(global_position.timestamp, POSITION_TIMEOUT, global_position.global_valid, &(status.condition_global_position_valid), &status_changed); /* update local position estimate */ orb_check(local_position_sub, &updated); @@ -1030,7 +1030,7 @@ int commander_thread_main(int argc, char *argv[]) if (!status.condition_home_position_valid && gps_position.fix_type >= 3 && (gps_position.eph_m < hdop_threshold_m) && (gps_position.epv_m < vdop_threshold_m) && (hrt_absolute_time() < gps_position.timestamp_position + POSITION_TIMEOUT) && !armed.armed - && global_position.valid) { + && global_position.global_valid) { /* copy position data to uORB home message, store it locally as well */ home.lat = global_position.lat; diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 7c23488d7e..a371a499ef 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -634,7 +634,7 @@ handle_message(mavlink_message_t *msg) orb_publish(ORB_ID(vehicle_global_position), pub_hil_global_pos, &hil_global_pos); // global position packet hil_global_pos.timestamp = timestamp; - hil_global_pos.valid = true; + hil_global_pos.global_valid = true; hil_global_pos.lat = hil_state.lat; hil_global_pos.lon = hil_state.lon; hil_global_pos.alt = hil_state.alt / 1000.0f; 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 4ff13d4df2..923a9dab05 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -182,6 +182,7 @@ private: bool _reset_lat_lon_sp; bool _reset_alt_sp; + bool _use_global_alt; /**< switch between global (AMSL) and barometric altitudes */ math::Vector<3> _vel; math::Vector<3> _vel_sp; @@ -214,6 +215,11 @@ private: */ void reset_alt_sp(); + /** + * Select between barometric and global (AMSL) altitudes + */ + void select_alt(bool global); + /** * Shim for calling task_main from task_create. */ @@ -263,7 +269,8 @@ MulticopterPositionControl::MulticopterPositionControl() : _alt_sp(0.0f), _reset_lat_lon_sp(true), - _reset_alt_sp(true) + _reset_alt_sp(true), + _use_global_alt(false) { memset(&_att, 0, sizeof(_att)); memset(&_att_sp, 0, sizeof(_att_sp)); @@ -466,8 +473,23 @@ MulticopterPositionControl::reset_alt_sp() { if (_reset_alt_sp) { _reset_alt_sp = false; - _alt_sp = _global_pos.alt; - mavlink_log_info(_mavlink_fd, "[mpc] reset alt sp: %.2f", (double)_alt_sp); + _alt_sp = _use_global_alt ? _global_pos.alt : _global_pos.baro_alt; + mavlink_log_info(_mavlink_fd, "[mpc] reset alt (%s) sp: %.2f", _use_global_alt ? "AMSL" : "baro", (double)_alt_sp); + } +} + +void +MulticopterPositionControl::select_alt(bool global) +{ + if (global != _use_global_alt) { + _use_global_alt = global; + if (global) { + /* switch from barometric to global altitude */ + _alt_sp += _global_pos.alt - _global_pos.baro_alt; + } else { + /* switch from global to barometric altitude */ + _alt_sp += _global_pos.baro_alt - _global_pos.alt; + } } } @@ -565,8 +587,16 @@ MulticopterPositionControl::task_main() sp_move_rate.zero(); + float alt = _global_pos.alt; + /* select control source */ if (_control_mode.flag_control_manual_enabled) { + /* select altitude source and update setpoint */ + select_alt(_global_pos.global_valid); + if (!_use_global_alt) { + alt = _global_pos.baro_alt; + } + /* manual control */ if (_control_mode.flag_control_altitude_enabled) { /* reset alt setpoint to current altitude if needed */ @@ -612,7 +642,7 @@ MulticopterPositionControl::task_main() } if (_control_mode.flag_control_altitude_enabled) { - pos_sp_offs(2) = -(_alt_sp - _global_pos.alt) / _params.sp_offs_max(2); + pos_sp_offs(2) = -(_alt_sp - alt) / _params.sp_offs_max(2); } float pos_sp_offs_norm = pos_sp_offs.length(); @@ -620,7 +650,7 @@ MulticopterPositionControl::task_main() if (pos_sp_offs_norm > 1.0f) { pos_sp_offs /= pos_sp_offs_norm; add_vector_to_global_position(_lat_sp, _lon_sp, pos_sp_offs(0) * _params.sp_offs_max(0), pos_sp_offs(1) * _params.sp_offs_max(1), &_lat_sp, &_lon_sp); - _alt_sp = _global_pos.alt - pos_sp_offs(2) * _params.sp_offs_max(2); + _alt_sp = alt - pos_sp_offs(2) * _params.sp_offs_max(2); } /* fill position setpoint triplet */ @@ -647,6 +677,9 @@ MulticopterPositionControl::task_main() } } else if (_control_mode.flag_control_auto_enabled) { + /* always use AMSL altitude for AUTO */ + select_alt(true); + /* AUTO */ bool updated; orb_check(_pos_sp_triplet_sub, &updated); @@ -678,7 +711,7 @@ MulticopterPositionControl::task_main() math::Vector<3> pos_err; float err_x, err_y; get_vector_to_next_waypoint_fast(_global_pos.lat, _global_pos.lon, _lat_sp, _lon_sp, &pos_err.data[0], &pos_err.data[1]); - pos_err(2) = -(_alt_sp - _global_pos.alt); + pos_err(2) = -(_alt_sp - alt); _vel_sp = pos_err.emult(_params.pos_p) + sp_move_rate.emult(_params.vel_ff); diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 5a02bf522f..170e5df059 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -840,6 +840,7 @@ Navigator::task_main() /* publish position setpoint triplet if updated */ if (_pos_sp_triplet_updated) { + _pos_sp_triplet_updated = false; publish_position_setpoint_triplet(); } @@ -882,9 +883,9 @@ Navigator::start() void Navigator::status() { - warnx("Global position is %svalid", _global_pos.valid ? "" : "in"); + warnx("Global position is %svalid", _global_pos.global_valid ? "" : "in"); - if (_global_pos.valid) { + if (_global_pos.global_valid) { warnx("Longitude %5.5f degrees, latitude %5.5f degrees", _global_pos.lon, _global_pos.lat); warnx("Altitude %5.5f meters, altitude above home %5.5f meters", (double)_global_pos.alt, (double)(_global_pos.alt - _home_pos.alt)); diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index af04bb0bc4..e045ce4cca 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -202,8 +202,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) bool landed = true; hrt_abstime landed_time = 0; - bool flag_armed = false; - uint32_t accel_counter = 0; uint32_t baro_counter = 0; @@ -329,6 +327,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) mavlink_log_info(mavlink_fd, "[inav] baro offs: %.2f", baro_offset); local_pos.z_valid = true; local_pos.v_z_valid = true; + global_pos.baro_valid = true; } } } @@ -379,17 +378,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (updated) { orb_copy(ORB_ID(actuator_armed), armed_sub, &armed); - - /* reset ground level on arm */ - if (armed.armed && !flag_armed) { - flag_armed = armed.armed; - baro_offset -= z_est[0]; - corr_baro = 0.0f; - local_pos.ref_alt -= z_est[0]; - local_pos.ref_timestamp = t; - z_est[0] = 0.0f; - alt_avg = 0.0f; - } } /* sensor combined */ @@ -637,6 +625,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } float dt = t_prev > 0 ? (t - t_prev) / 1000000.0f : 0.0f; + dt = fmaxf(fminf(0.02, dt), 0.005); t_prev = t; /* use GPS if it's valid and reference position initialized */ @@ -679,7 +668,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (use_gps_z) { float offs_corr = corr_gps[2][0] * w_z_gps_p * dt; baro_offset += offs_corr; - baro_counter += offs_corr; + corr_baro += offs_corr; } /* accelerometer bias correction */ @@ -835,7 +824,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) orb_publish(ORB_ID(vehicle_local_position), vehicle_local_position_pub, &local_pos); /* publish global position */ - global_pos.valid = local_pos.xy_global; + global_pos.global_valid = local_pos.xy_global; if (local_pos.xy_global) { double est_lat, est_lon; @@ -855,6 +844,10 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) global_pos.alt = local_pos.ref_alt - local_pos.z; } + if (local_pos.z_valid) { + global_pos.baro_alt = baro_offset - local_pos.z; + } + if (local_pos.v_z_valid) { global_pos.vel_d = local_pos.vz; } diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index c3ea30cbfb..3c218e21fb 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1244,6 +1244,8 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.body.log_GPOS.vel_n = buf.global_pos.vel_n; log_msg.body.log_GPOS.vel_e = buf.global_pos.vel_e; log_msg.body.log_GPOS.vel_d = buf.global_pos.vel_d; + log_msg.body.log_GPOS.baro_alt = buf.global_pos.baro_alt; + log_msg.body.log_GPOS.flags = (buf.global_pos.baro_valid ? 1 : 0) | (buf.global_pos.global_valid ? 2 : 0); LOGBUFFER_WRITE_AND_COUNT(GPOS); } diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index baac2ee3ec..db87e3a6a2 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -204,6 +204,8 @@ struct log_GPOS_s { float vel_n; float vel_e; float vel_d; + float baro_alt; + uint8_t flags; }; /* --- GPSP - GLOBAL POSITION SETPOINT --- */ @@ -303,7 +305,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(AIRS, "ff", "IndSpeed,TrueSpeed"), LOG_FORMAT(ARSP, "fff", "RollRateSP,PitchRateSP,YawRateSP"), LOG_FORMAT(FLOW, "hhfffBB", "RawX,RawY,CompX,CompY,Dist,Q,SensID"), - LOG_FORMAT(GPOS, "LLffff", "Lat,Lon,Alt,VelN,VelE,VelD"), + LOG_FORMAT(GPOS, "LLfffffB", "Lat,Lon,Alt,VelN,VelE,VelD,BaroAlt,Flags"), LOG_FORMAT(GPSP, "BLLffBfbf", "NavState,Lat,Lon,Alt,Yaw,Type,LoitR,LoitDir,PitMin"), LOG_FORMAT(ESC, "HBBBHHHHHHfH", "Counter,NumESC,Conn,N,Ver,Adr,Volt,Amp,RPM,Temp,SetP,SetPRAW"), LOG_FORMAT(GVSP, "fff", "VX,VY,VZ"), diff --git a/src/modules/uORB/topics/vehicle_global_position.h b/src/modules/uORB/topics/vehicle_global_position.h index ae771ca008..ff9e98e1c4 100644 --- a/src/modules/uORB/topics/vehicle_global_position.h +++ b/src/modules/uORB/topics/vehicle_global_position.h @@ -61,17 +61,21 @@ */ struct vehicle_global_position_s { - uint64_t timestamp; /**< time of this estimate, in microseconds since system start */ - uint64_t time_gps_usec; /**< GPS timestamp in microseconds */ - bool valid; /**< true if position satisfies validity criteria of estimator */ + uint64_t timestamp; /**< Time of this estimate, in microseconds since system start */ + bool global_valid; /**< true if position satisfies validity criteria of estimator */ + bool baro_valid; /**< true if baro_alt is valid (vel_d is also valid in this case) */ + + uint64_t time_gps_usec; /**< GPS timestamp in microseconds */ double lat; /**< Latitude in degrees */ double lon; /**< Longitude in degrees */ - float alt; /**< Altitude in meters */ + float alt; /**< Altitude AMSL in meters */ float vel_n; /**< Ground north velocity, m/s */ float vel_e; /**< Ground east velocity, m/s */ float vel_d; /**< Ground downside velocity, m/s */ float yaw; /**< Yaw in radians -PI..+PI. */ + + float baro_alt; /**< Barometric altitude (not raw baro but fused with accelerometer) */ }; /** From 23a87f5a5204580b4fad435a75b38efc9d15c05c Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 28 Jan 2014 21:44:46 +0100 Subject: [PATCH 367/447] navigator: add home position check for RTL state --- src/modules/navigator/navigator_main.cpp | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 170e5df059..73b7c13d0f 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -685,7 +685,8 @@ Navigator::task_main() /* RC signal available, use control switches to set mode */ /* RETURN switch, overrides MISSION switch */ if (_vstatus.return_switch == RETURN_SWITCH_RETURN) { - if (myState != NAV_STATE_READY || _rtl_state != RTL_STATE_LAND) { + /* switch to RTL if not already landed after RTL and home position set */ + if ((myState != NAV_STATE_READY || _rtl_state != RTL_STATE_LAND) && _vstatus.condition_home_position_valid) { dispatch(EVENT_RTL_REQUESTED); } @@ -742,7 +743,7 @@ Navigator::task_main() break; case NAV_STATE_RTL: - if (myState != NAV_STATE_READY || _rtl_state != RTL_STATE_LAND) { + if ((myState != NAV_STATE_READY || _rtl_state != RTL_STATE_LAND) && _vstatus.condition_home_position_valid) { dispatch(EVENT_RTL_REQUESTED); } @@ -815,6 +816,11 @@ Navigator::task_main() if (fds[1].revents & POLLIN) { global_position_update(); + /* publish position setpoint triplet on each position update if navigator active */ + if (_control_mode.flag_armed && _control_mode.flag_control_auto_enabled) { + _pos_sp_triplet_updated = true; + } + /* only check if waypoint has been reached in MISSION and RTL modes */ if (myState == NAV_STATE_MISSION || myState == NAV_STATE_RTL) { if (check_mission_item_reached()) { From 48cec50dd30cd2b3163aedbeb11ae52866e2601b Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 29 Jan 2014 13:12:57 +0100 Subject: [PATCH 368/447] navigator: handle regaining global position lock while LANDing --- src/modules/navigator/navigator_main.cpp | 19 ++++++++++++++----- 1 file changed, 14 insertions(+), 5 deletions(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 73b7c13d0f..80bb1b7526 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -178,6 +178,7 @@ private: class Mission _mission; + bool _global_pos_valid; /**< track changes of global_position.global_valid flag */ bool _reset_loiter_pos; /**< if true then loiter position should be set to current position */ bool _waypoint_position_reached; bool _waypoint_yaw_reached; @@ -393,6 +394,7 @@ Navigator::Navigator() : _fence_valid(false), _inside_fence(true), _mission(), + _global_pos_valid(false), _reset_loiter_pos(true), _waypoint_position_reached(false), _waypoint_yaw_reached(false), @@ -819,12 +821,19 @@ Navigator::task_main() /* publish position setpoint triplet on each position update if navigator active */ if (_control_mode.flag_armed && _control_mode.flag_control_auto_enabled) { _pos_sp_triplet_updated = true; - } - /* only check if waypoint has been reached in MISSION and RTL modes */ - if (myState == NAV_STATE_MISSION || myState == NAV_STATE_RTL) { - if (check_mission_item_reached()) { - on_mission_item_reached(); + if (myState == NAV_STATE_LAND && _global_pos.global_valid && !_global_pos_valid) { + /* got global position when landing, update setpoint */ + start_land(); + } + + _global_pos_valid = _global_pos.global_valid; + + /* check if waypoint has been reached in MISSION, RTL and LAND modes */ + if (myState == NAV_STATE_MISSION || myState == NAV_STATE_RTL || myState == NAV_STATE_LAND) { + if (check_mission_item_reached()) { + on_mission_item_reached(); + } } } From 591b355981c781f6d30a6697b690225031792cfc Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 29 Jan 2014 14:39:36 +0100 Subject: [PATCH 369/447] setpoint type IDLE added (for AUTO_READY state), LAND mode fixed --- .../mc_pos_control/mc_pos_control_main.cpp | 549 +++++++++--------- src/modules/navigator/navigator_main.cpp | 46 +- .../uORB/topics/position_setpoint_triplet.h | 9 +- 3 files changed, 324 insertions(+), 280 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 923a9dab05..fe8377a40d 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -707,269 +707,16 @@ MulticopterPositionControl::task_main() reset_alt_sp(); } - /* run position & altitude controllers, calculate velocity setpoint */ - math::Vector<3> pos_err; - float err_x, err_y; - get_vector_to_next_waypoint_fast(_global_pos.lat, _global_pos.lon, _lat_sp, _lon_sp, &pos_err.data[0], &pos_err.data[1]); - pos_err(2) = -(_alt_sp - alt); + if (_control_mode.flag_control_auto_enabled && _pos_sp_triplet.current.valid && _pos_sp_triplet.current.type == SETPOINT_TYPE_IDLE) { + /* idle state, don't run controller and set zero thrust */ + R.identity(); + memcpy(&_att_sp.R_body[0][0], R.data, sizeof(_att_sp.R_body)); + _att_sp.R_valid = true; - _vel_sp = pos_err.emult(_params.pos_p) + sp_move_rate.emult(_params.vel_ff); - - if (!_control_mode.flag_control_altitude_enabled) { - _reset_alt_sp = true; - _vel_sp(2) = 0.0f; - } - - if (!_control_mode.flag_control_position_enabled) { - _reset_lat_lon_sp = true; - _vel_sp(0) = 0.0f; - _vel_sp(1) = 0.0f; - } - - if (!_control_mode.flag_control_manual_enabled) { - /* use constant descend rate when landing, ignore altitude setpoint */ - if (_pos_sp_triplet.current.valid && _pos_sp_triplet.current.type == SETPOINT_TYPE_LAND) { - _vel_sp(2) = _params.land_speed; - } - - /* limit 3D speed only in AUTO mode */ - float vel_sp_norm = _vel_sp.edivide(_params.vel_max).length(); - - if (vel_sp_norm > 1.0f) { - _vel_sp /= vel_sp_norm; - } - } - - _global_vel_sp.vx = _vel_sp(0); - _global_vel_sp.vy = _vel_sp(1); - _global_vel_sp.vz = _vel_sp(2); - - /* publish velocity setpoint */ - if (_global_vel_sp_pub > 0) { - orb_publish(ORB_ID(vehicle_global_velocity_setpoint), _global_vel_sp_pub, &_global_vel_sp); - - } else { - _global_vel_sp_pub = orb_advertise(ORB_ID(vehicle_global_velocity_setpoint), &_global_vel_sp); - } - - if (_control_mode.flag_control_climb_rate_enabled || _control_mode.flag_control_velocity_enabled) { - /* reset integrals if needed */ - if (_control_mode.flag_control_climb_rate_enabled) { - if (reset_int_z) { - reset_int_z = false; - float i = _params.thr_min; - - if (reset_int_z_manual) { - i = _manual.throttle; - - if (i < _params.thr_min) { - i = _params.thr_min; - - } else if (i > _params.thr_max) { - i = _params.thr_max; - } - } - - thrust_int(2) = -i; - mavlink_log_info(_mavlink_fd, "[mpc] reset hovering thrust: %.2f", (double)i); - } - - } else { - reset_int_z = true; - } - - if (_control_mode.flag_control_velocity_enabled) { - if (reset_int_xy) { - reset_int_xy = false; - thrust_int(0) = 0.0f; - thrust_int(1) = 0.0f; - mavlink_log_info(_mavlink_fd, "[mpc] reset xy vel integral"); - } - - } else { - reset_int_xy = true; - } - - /* velocity error */ - math::Vector<3> vel_err = _vel_sp - _vel; - - /* derivative of velocity error, not includes setpoint acceleration */ - math::Vector<3> vel_err_d = (sp_move_rate - _vel).emult(_params.pos_p) - (_vel - _vel_prev) / dt; - _vel_prev = _vel; - - /* thrust vector in NED frame */ - math::Vector<3> thrust_sp = vel_err.emult(_params.vel_p) + vel_err_d.emult(_params.vel_d) + thrust_int; - - if (!_control_mode.flag_control_velocity_enabled) { - thrust_sp(0) = 0.0f; - thrust_sp(1) = 0.0f; - } - - if (!_control_mode.flag_control_climb_rate_enabled) { - thrust_sp(2) = 0.0f; - } - - /* limit thrust vector and check for saturation */ - bool saturation_xy = false; - bool saturation_z = false; - - /* limit min lift */ - float thr_min = _params.thr_min; - - if (!_control_mode.flag_control_velocity_enabled && thr_min < 0.0f) { - /* don't allow downside thrust direction in manual attitude mode */ - thr_min = 0.0f; - } - - if (-thrust_sp(2) < thr_min) { - thrust_sp(2) = -thr_min; - saturation_z = true; - } - - /* limit max tilt */ - float tilt = atan2f(math::Vector<2>(thrust_sp(0), thrust_sp(1)).length(), -thrust_sp(2)); - float tilt_max = _params.tilt_max; - if (!_control_mode.flag_control_manual_enabled) { - if (_pos_sp_triplet.current.valid && _pos_sp_triplet.current.type == SETPOINT_TYPE_LAND) { - /* limit max tilt and min lift when landing */ - tilt_max = _params.land_tilt_max; - if (thr_min < 0.0f) - thr_min = 0.0f; - } - } - - if (_control_mode.flag_control_velocity_enabled) { - if (tilt > tilt_max && thr_min >= 0.0f) { - /* crop horizontal component */ - float k = tanf(tilt_max) / tanf(tilt); - thrust_sp(0) *= k; - thrust_sp(1) *= k; - saturation_xy = true; - } - } else { - /* thrust compensation for altitude only control mode */ - float att_comp; - - if (_att.R[2][2] > TILT_COS_MAX) { - att_comp = 1.0f / _att.R[2][2]; - } else if (_att.R[2][2] > 0.0f) { - att_comp = ((1.0f / TILT_COS_MAX - 1.0f) / TILT_COS_MAX) * _att.R[2][2] + 1.0f; - saturation_z = true; - } else { - att_comp = 1.0f; - saturation_z = true; - } - - thrust_sp(2) *= att_comp; - } - - /* limit max thrust */ - float thrust_abs = thrust_sp.length(); - - if (thrust_abs > _params.thr_max) { - if (thrust_sp(2) < 0.0f) { - if (-thrust_sp(2) > _params.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; - 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_abs = math::Vector<2>(thrust_sp(0), thrust_sp(1)).length(); - float k = thrust_xy_max / thrust_xy_abs; - thrust_sp(0) *= k; - thrust_sp(1) *= k; - saturation_xy = true; - } - - } else { - /* Z component is negative, going down, simply limit thrust vector */ - float k = _params.thr_max / thrust_abs; - thrust_sp *= k; - saturation_xy = true; - saturation_z = true; - } - - thrust_abs = _params.thr_max; - } - - /* update integrals */ - if (_control_mode.flag_control_velocity_enabled && !saturation_xy) { - thrust_int(0) += vel_err(0) * _params.vel_i(0) * dt; - thrust_int(1) += vel_err(1) * _params.vel_i(1) * dt; - } - - if (_control_mode.flag_control_climb_rate_enabled && !saturation_z) { - thrust_int(2) += vel_err(2) * _params.vel_i(2) * dt; - - /* protection against flipping on ground when landing */ - if (thrust_int(2) > 0.0f) - thrust_int(2) = 0.0f; - } - - /* calculate attitude setpoint from thrust vector */ - if (_control_mode.flag_control_velocity_enabled) { - /* desired body_z axis = -normalize(thrust_vector) */ - math::Vector<3> body_x; - math::Vector<3> body_y; - math::Vector<3> body_z; - - if (thrust_abs > SIGMA) { - body_z = -thrust_sp / thrust_abs; - - } else { - /* no thrust, set Z axis to safe value */ - body_z.zero(); - body_z(2) = 1.0f; - } - - /* vector of desired yaw direction in XY plane, rotated by PI/2 */ - math::Vector<3> y_C(-sinf(_att_sp.yaw_body), cosf(_att_sp.yaw_body), 0.0f); - - if (fabsf(body_z(2)) > SIGMA) { - /* desired body_x axis, orthogonal to body_z */ - body_x = y_C % body_z; - - /* keep nose to front while inverted upside down */ - if (body_z(2) < 0.0f) { - body_x = -body_x; - } - - body_x.normalize(); - - } else { - /* desired thrust is in XY plane, set X downside to construct correct matrix, - * but yaw component will not be used actually */ - body_x.zero(); - body_x(2) = 1.0f; - } - - /* desired body_y axis */ - body_y = body_z % body_x; - - /* fill rotation matrix */ - for (int i = 0; i < 3; i++) { - R(i, 0) = body_x(i); - R(i, 1) = body_y(i); - R(i, 2) = body_z(i); - } - - /* copy rotation matrix to attitude setpoint topic */ - memcpy(&_att_sp.R_body[0][0], R.data, sizeof(_att_sp.R_body)); - _att_sp.R_valid = true; - - /* calculate euler angles, for logging only, must not be used for control */ - math::Vector<3> euler = R.to_euler(); - _att_sp.roll_body = euler(0); - _att_sp.pitch_body = euler(1); - /* yaw already used to construct rot matrix, but actual rotation matrix can have different yaw near singularity */ - } - - _att_sp.thrust = thrust_abs; + _att_sp.roll_body = 0.0f; + _att_sp.pitch_body = 0.0f; + _att_sp.yaw_body = 0.0f; + _att_sp.thrust = 0.0f; _att_sp.timestamp = hrt_absolute_time(); @@ -982,7 +729,283 @@ MulticopterPositionControl::task_main() } } else { - reset_int_z = true; + /* run position & altitude controllers, calculate velocity setpoint */ + math::Vector<3> pos_err; + float err_x, err_y; + get_vector_to_next_waypoint_fast(_global_pos.lat, _global_pos.lon, _lat_sp, _lon_sp, &pos_err.data[0], &pos_err.data[1]); + pos_err(2) = -(_alt_sp - alt); + + _vel_sp = pos_err.emult(_params.pos_p) + sp_move_rate.emult(_params.vel_ff); + + if (!_control_mode.flag_control_altitude_enabled) { + _reset_alt_sp = true; + _vel_sp(2) = 0.0f; + } + + if (!_control_mode.flag_control_position_enabled) { + _reset_lat_lon_sp = true; + _vel_sp(0) = 0.0f; + _vel_sp(1) = 0.0f; + } + + /* use constant descend rate when landing, ignore altitude setpoint */ + if (_control_mode.flag_control_auto_enabled && _pos_sp_triplet.current.valid && _pos_sp_triplet.current.type == SETPOINT_TYPE_LAND) { + _vel_sp(2) = _params.land_speed; + } + + if (!_control_mode.flag_control_manual_enabled) { + /* limit 3D speed only in non-manual modes */ + float vel_sp_norm = _vel_sp.edivide(_params.vel_max).length(); + + if (vel_sp_norm > 1.0f) { + _vel_sp /= vel_sp_norm; + } + } + + _global_vel_sp.vx = _vel_sp(0); + _global_vel_sp.vy = _vel_sp(1); + _global_vel_sp.vz = _vel_sp(2); + + /* publish velocity setpoint */ + if (_global_vel_sp_pub > 0) { + orb_publish(ORB_ID(vehicle_global_velocity_setpoint), _global_vel_sp_pub, &_global_vel_sp); + + } else { + _global_vel_sp_pub = orb_advertise(ORB_ID(vehicle_global_velocity_setpoint), &_global_vel_sp); + } + + if (_control_mode.flag_control_climb_rate_enabled || _control_mode.flag_control_velocity_enabled) { + /* reset integrals if needed */ + if (_control_mode.flag_control_climb_rate_enabled) { + if (reset_int_z) { + reset_int_z = false; + float i = _params.thr_min; + + if (reset_int_z_manual) { + i = _manual.throttle; + + if (i < _params.thr_min) { + i = _params.thr_min; + + } else if (i > _params.thr_max) { + i = _params.thr_max; + } + } + + thrust_int(2) = -i; + mavlink_log_info(_mavlink_fd, "[mpc] reset hovering thrust: %.2f", (double)i); + } + + } else { + reset_int_z = true; + } + + if (_control_mode.flag_control_velocity_enabled) { + if (reset_int_xy) { + reset_int_xy = false; + thrust_int(0) = 0.0f; + thrust_int(1) = 0.0f; + mavlink_log_info(_mavlink_fd, "[mpc] reset xy vel integral"); + } + + } else { + reset_int_xy = true; + } + + /* velocity error */ + math::Vector<3> vel_err = _vel_sp - _vel; + + /* derivative of velocity error, not includes setpoint acceleration */ + math::Vector<3> vel_err_d = (sp_move_rate - _vel).emult(_params.pos_p) - (_vel - _vel_prev) / dt; + _vel_prev = _vel; + + /* thrust vector in NED frame */ + math::Vector<3> thrust_sp = vel_err.emult(_params.vel_p) + vel_err_d.emult(_params.vel_d) + thrust_int; + + if (!_control_mode.flag_control_velocity_enabled) { + thrust_sp(0) = 0.0f; + thrust_sp(1) = 0.0f; + } + + if (!_control_mode.flag_control_climb_rate_enabled) { + thrust_sp(2) = 0.0f; + } + + /* limit thrust vector and check for saturation */ + bool saturation_xy = false; + bool saturation_z = false; + + /* limit min lift */ + float thr_min = _params.thr_min; + + if (!_control_mode.flag_control_velocity_enabled && thr_min < 0.0f) { + /* don't allow downside thrust direction in manual attitude mode */ + thr_min = 0.0f; + } + + if (-thrust_sp(2) < thr_min) { + thrust_sp(2) = -thr_min; + saturation_z = true; + } + + /* limit max tilt */ + float tilt = atan2f(math::Vector<2>(thrust_sp(0), thrust_sp(1)).length(), -thrust_sp(2)); + float tilt_max = _params.tilt_max; + if (!_control_mode.flag_control_manual_enabled) { + if (_pos_sp_triplet.current.valid && _pos_sp_triplet.current.type == SETPOINT_TYPE_LAND) { + /* limit max tilt and min lift when landing */ + tilt_max = _params.land_tilt_max; + if (thr_min < 0.0f) + thr_min = 0.0f; + } + } + + if (_control_mode.flag_control_velocity_enabled) { + if (tilt > tilt_max && thr_min >= 0.0f) { + /* crop horizontal component */ + float k = tanf(tilt_max) / tanf(tilt); + thrust_sp(0) *= k; + thrust_sp(1) *= k; + saturation_xy = true; + } + } else { + /* thrust compensation for altitude only control mode */ + float att_comp; + + if (_att.R[2][2] > TILT_COS_MAX) { + att_comp = 1.0f / _att.R[2][2]; + } else if (_att.R[2][2] > 0.0f) { + att_comp = ((1.0f / TILT_COS_MAX - 1.0f) / TILT_COS_MAX) * _att.R[2][2] + 1.0f; + saturation_z = true; + } else { + att_comp = 1.0f; + saturation_z = true; + } + + thrust_sp(2) *= att_comp; + } + + /* limit max thrust */ + float thrust_abs = thrust_sp.length(); + + if (thrust_abs > _params.thr_max) { + if (thrust_sp(2) < 0.0f) { + if (-thrust_sp(2) > _params.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; + 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_abs = math::Vector<2>(thrust_sp(0), thrust_sp(1)).length(); + float k = thrust_xy_max / thrust_xy_abs; + thrust_sp(0) *= k; + thrust_sp(1) *= k; + saturation_xy = true; + } + + } else { + /* Z component is negative, going down, simply limit thrust vector */ + float k = _params.thr_max / thrust_abs; + thrust_sp *= k; + saturation_xy = true; + saturation_z = true; + } + + thrust_abs = _params.thr_max; + } + + /* update integrals */ + if (_control_mode.flag_control_velocity_enabled && !saturation_xy) { + thrust_int(0) += vel_err(0) * _params.vel_i(0) * dt; + thrust_int(1) += vel_err(1) * _params.vel_i(1) * dt; + } + + if (_control_mode.flag_control_climb_rate_enabled && !saturation_z) { + thrust_int(2) += vel_err(2) * _params.vel_i(2) * dt; + + /* protection against flipping on ground when landing */ + if (thrust_int(2) > 0.0f) + thrust_int(2) = 0.0f; + } + + /* calculate attitude setpoint from thrust vector */ + if (_control_mode.flag_control_velocity_enabled) { + /* desired body_z axis = -normalize(thrust_vector) */ + math::Vector<3> body_x; + math::Vector<3> body_y; + math::Vector<3> body_z; + + if (thrust_abs > SIGMA) { + body_z = -thrust_sp / thrust_abs; + + } else { + /* no thrust, set Z axis to safe value */ + body_z.zero(); + body_z(2) = 1.0f; + } + + /* vector of desired yaw direction in XY plane, rotated by PI/2 */ + math::Vector<3> y_C(-sinf(_att_sp.yaw_body), cosf(_att_sp.yaw_body), 0.0f); + + if (fabsf(body_z(2)) > SIGMA) { + /* desired body_x axis, orthogonal to body_z */ + body_x = y_C % body_z; + + /* keep nose to front while inverted upside down */ + if (body_z(2) < 0.0f) { + body_x = -body_x; + } + + body_x.normalize(); + + } else { + /* desired thrust is in XY plane, set X downside to construct correct matrix, + * but yaw component will not be used actually */ + body_x.zero(); + body_x(2) = 1.0f; + } + + /* desired body_y axis */ + body_y = body_z % body_x; + + /* fill rotation matrix */ + for (int i = 0; i < 3; i++) { + R(i, 0) = body_x(i); + R(i, 1) = body_y(i); + R(i, 2) = body_z(i); + } + + /* copy rotation matrix to attitude setpoint topic */ + memcpy(&_att_sp.R_body[0][0], R.data, sizeof(_att_sp.R_body)); + _att_sp.R_valid = true; + + /* calculate euler angles, for logging only, must not be used for control */ + math::Vector<3> euler = R.to_euler(); + _att_sp.roll_body = euler(0); + _att_sp.pitch_body = euler(1); + /* yaw already used to construct rot matrix, but actual rotation matrix can have different yaw near singularity */ + } + + _att_sp.thrust = thrust_abs; + + _att_sp.timestamp = hrt_absolute_time(); + + /* publish attitude setpoint */ + if (_att_sp_pub > 0) { + orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &_att_sp); + + } else { + _att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_att_sp); + } + + } else { + reset_int_z = true; + } } } else { diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 80bb1b7526..73514185b1 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -1030,8 +1030,11 @@ void Navigator::start_ready() { _pos_sp_triplet.previous.valid = false; - _pos_sp_triplet.current.valid = false; + _pos_sp_triplet.current.valid = true; _pos_sp_triplet.next.valid = false; + + _pos_sp_triplet.current.type = SETPOINT_TYPE_IDLE; + _mission_item_valid = false; _reset_loiter_pos = true; @@ -1229,20 +1232,32 @@ Navigator::start_rtl() void Navigator::start_land() { + /* this state can be requested by commander even if no global position available, + * in his case controller must perform landing without position control */ _do_takeoff = false; _reset_loiter_pos = true; - _pos_sp_triplet.previous.valid = false; - _pos_sp_triplet.next.valid = false; + memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s)); - _pos_sp_triplet.current.valid = true; - _pos_sp_triplet.current.type = SETPOINT_TYPE_LAND; - _pos_sp_triplet.current.lat = _global_pos.lat; - _pos_sp_triplet.current.lon = _global_pos.lon; - _pos_sp_triplet.current.alt = _global_pos.alt; - _pos_sp_triplet.current.loiter_direction = 1; - _pos_sp_triplet.current.loiter_radius = _parameters.loiter_radius; - _pos_sp_triplet.current.yaw = NAN; + _mission_item_valid = true; + + _mission_item.lat = _global_pos.lat; + _mission_item.lon = _global_pos.lon; + _mission_item.altitude_is_relative = false; + _mission_item.altitude = _global_pos.alt; + _mission_item.yaw = NAN; + _mission_item.loiter_radius = _parameters.loiter_radius; + _mission_item.loiter_direction = 1; + _mission_item.nav_cmd = NAV_CMD_LAND; + _mission_item.acceptance_radius = _parameters.acceptance_radius; + _mission_item.time_inside = 0.0f; + _mission_item.pitch_min = 0.0f; + _mission_item.autocontinue = true; + _mission_item.origin = ORIGIN_ONBOARD; + + position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item); + + _pos_sp_triplet.next.valid = false; } void @@ -1546,8 +1561,8 @@ Navigator::on_mission_item_reached() } } - } else { - /* RTL finished */ + } else if (myState == NAV_STATE_RTL) { + /* RTL completed */ if (_rtl_state == RTL_STATE_LAND) { /* landed at home position */ mavlink_log_info(_mavlink_fd, "[navigator] RTL completed, landed"); @@ -1558,6 +1573,11 @@ Navigator::on_mission_item_reached() _rtl_state = (RTLState)(_rtl_state + 1); set_rtl_item(); } + + } else if (myState == NAV_STATE_LAND) { + /* landing completed */ + mavlink_log_info(_mavlink_fd, "[navigator] landing completed"); + dispatch(EVENT_READY_REQUESTED); } } diff --git a/src/modules/uORB/topics/position_setpoint_triplet.h b/src/modules/uORB/topics/position_setpoint_triplet.h index e8d68281a8..cf1ddfc38f 100644 --- a/src/modules/uORB/topics/position_setpoint_triplet.h +++ b/src/modules/uORB/topics/position_setpoint_triplet.h @@ -54,10 +54,11 @@ enum SETPOINT_TYPE { - SETPOINT_TYPE_NORMAL = 0, - SETPOINT_TYPE_LOITER, - SETPOINT_TYPE_TAKEOFF, - SETPOINT_TYPE_LAND, + SETPOINT_TYPE_NORMAL = 0, /**< normal setpoint */ + SETPOINT_TYPE_LOITER, /**< loiter setpoint */ + SETPOINT_TYPE_TAKEOFF, /**< takeoff setpoint */ + SETPOINT_TYPE_LAND, /**< land setpoint, altitude must be ignored, vehicle must descend until landing */ + SETPOINT_TYPE_IDLE, /**< do nothing, switch off motors or keep at idle speed (MC) */ }; struct position_setpoint_s From b81520cf30b329ad4d52f2197f25bfc5c8f5269f Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 29 Jan 2014 16:05:09 +0100 Subject: [PATCH 370/447] Use NAV_STATE_LAND instead RTL_STATE_LAND --- src/modules/navigator/navigator_main.cpp | 91 +++++++++++++----------- 1 file changed, 48 insertions(+), 43 deletions(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 73514185b1..46e7c2e188 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -237,8 +237,7 @@ private: RTL_STATE_NONE = 0, RTL_STATE_CLIMB, RTL_STATE_RETURN, - RTL_STATE_DESCEND, - RTL_STATE_LAND + RTL_STATE_DESCEND }; enum RTLState _rtl_state; @@ -304,6 +303,7 @@ private: void start_mission(); void start_rtl(); void start_land(); + void start_land_home(); /** * Guards offboard mission @@ -688,7 +688,8 @@ Navigator::task_main() /* RETURN switch, overrides MISSION switch */ if (_vstatus.return_switch == RETURN_SWITCH_RETURN) { /* switch to RTL if not already landed after RTL and home position set */ - if ((myState != NAV_STATE_READY || _rtl_state != RTL_STATE_LAND) && _vstatus.condition_home_position_valid) { + if (!(_rtl_state == RTL_STATE_DESCEND && (myState == NAV_STATE_READY || myState == NAV_STATE_LAND)) && + _vstatus.condition_home_position_valid) { dispatch(EVENT_RTL_REQUESTED); } @@ -745,7 +746,8 @@ Navigator::task_main() break; case NAV_STATE_RTL: - if ((myState != NAV_STATE_READY || _rtl_state != RTL_STATE_LAND) && _vstatus.condition_home_position_valid) { + if (!(_rtl_state == RTL_STATE_DESCEND && (myState == NAV_STATE_READY || myState == NAV_STATE_LAND)) && + _vstatus.condition_home_position_valid) { dispatch(EVENT_RTL_REQUESTED); } @@ -994,7 +996,7 @@ StateTable::Tran const Navigator::myTable[NAV_STATE_MAX][MAX_EVENT] = { /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER}, /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, /* EVENT_RTL_REQUESTED */ {NO_ACTION, NAV_STATE_RTL}, - /* EVENT_LAND_REQUESTED */ {ACTION(&Navigator::start_land), NAV_STATE_LAND}, + /* EVENT_LAND_REQUESTED */ {ACTION(&Navigator::start_land_home), NAV_STATE_LAND}, /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_RTL}, /* EVENT_HOME_POSITION_CHANGED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, // TODO need to reset rtl_state }, @@ -1040,8 +1042,8 @@ Navigator::start_ready() _reset_loiter_pos = true; _do_takeoff = false; - if (_rtl_state != RTL_STATE_LAND) { - /* allow RTL if landed not at home */ + if (_rtl_state != RTL_STATE_DESCEND) { + /* reset RTL state if landed not at home */ _rtl_state = RTL_STATE_NONE; } @@ -1074,11 +1076,6 @@ Navigator::start_loiter() } _pos_sp_triplet.current.type = SETPOINT_TYPE_NORMAL; - - if (_rtl_state == RTL_STATE_LAND) { - /* if RTL landing was interrupted, avoid landing from MIN_ALT on next RTL */ - _rtl_state = RTL_STATE_DESCEND; - } } _pos_sp_triplet.current.loiter_radius = _parameters.loiter_radius; @@ -1260,6 +1257,36 @@ Navigator::start_land() _pos_sp_triplet.next.valid = false; } +void +Navigator::start_land_home() +{ + /* land to home position, should be called when hovering above home, from RTL state */ + _do_takeoff = false; + _reset_loiter_pos = true; + + memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s)); + + _mission_item_valid = true; + + _mission_item.lat = _home_pos.lat; + _mission_item.lon = _home_pos.lon; + _mission_item.altitude_is_relative = false; + _mission_item.altitude = _home_pos.alt; + _mission_item.yaw = NAN; + _mission_item.loiter_radius = _parameters.loiter_radius; + _mission_item.loiter_direction = 1; + _mission_item.nav_cmd = NAV_CMD_LAND; + _mission_item.acceptance_radius = _parameters.acceptance_radius; + _mission_item.time_inside = 0.0f; + _mission_item.pitch_min = 0.0f; + _mission_item.autocontinue = true; + _mission_item.origin = ORIGIN_ONBOARD; + + position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item); + + _pos_sp_triplet.next.valid = false; +} + void Navigator::set_rtl_item() { @@ -1350,33 +1377,6 @@ Navigator::set_rtl_item() break; } - case RTL_STATE_LAND: { - memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s)); - - _mission_item_valid = true; - - _mission_item.lat = _home_pos.lat; - _mission_item.lon = _home_pos.lon; - _mission_item.altitude_is_relative = false; - _mission_item.altitude = _home_pos.alt; - _mission_item.yaw = NAN; - _mission_item.loiter_radius = _parameters.loiter_radius; - _mission_item.loiter_direction = 1; - _mission_item.nav_cmd = NAV_CMD_LAND; - _mission_item.acceptance_radius = _parameters.acceptance_radius; - _mission_item.time_inside = 0.0f; - _mission_item.pitch_min = 0.0f; - _mission_item.autocontinue = true; - _mission_item.origin = ORIGIN_ONBOARD; - - position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item); - - _pos_sp_triplet.next.valid = false; - - mavlink_log_info(_mavlink_fd, "[navigator] RTL: land"); - break; - } - default: { mavlink_log_critical(_mavlink_fd, "[navigator] error: unknown RTL state: %d", _rtl_state); start_loiter(); @@ -1563,10 +1563,15 @@ Navigator::on_mission_item_reached() } else if (myState == NAV_STATE_RTL) { /* RTL completed */ - if (_rtl_state == RTL_STATE_LAND) { - /* landed at home position */ - mavlink_log_info(_mavlink_fd, "[navigator] RTL completed, landed"); - dispatch(EVENT_READY_REQUESTED); + if (_rtl_state == RTL_STATE_DESCEND) { + /* hovering above home position, land if needed or loiter */ + mavlink_log_info(_mavlink_fd, "[navigator] RTL completed"); + if (_mission_item.autocontinue) { + dispatch(EVENT_LAND_REQUESTED); + + } else { + dispatch(EVENT_LOITER_REQUESTED); + } } else { /* next RTL step */ From 6f559b279e3d03dbf28eff436b41f3b022c5fa82 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 29 Jan 2014 17:59:02 +0100 Subject: [PATCH 371/447] mc_att_control: yaw deadzone increased --- src/modules/mc_att_control/mc_att_control_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 245ac024b8..44baccefc0 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -85,7 +85,7 @@ extern "C" __EXPORT int mc_att_control_main(int argc, char *argv[]); #define MIN_TAKEOFF_THROTTLE 0.3f -#define YAW_DEADZONE 0.01f +#define YAW_DEADZONE 0.05f #define RATES_I_LIMIT 0.5f class MulticopterAttitudeControl From 7d2f2523f86b4622815a323969a77b245a4ceaa3 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 29 Jan 2014 22:11:38 +0100 Subject: [PATCH 372/447] navigator: reset mission item timer on state and mission item changes --- src/modules/navigator/navigator_main.cpp | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 061baf0c5f..e961a8f942 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -865,6 +865,9 @@ Navigator::task_main() if (myState != prevState) { mavlink_log_info(_mavlink_fd, "[navigator] nav state: %s", nav_states_str[myState]); prevState = myState; + + /* reset time counter on state changes */ + _time_first_inside_orbit = 0; } perf_end(_loop_perf); @@ -1112,6 +1115,9 @@ Navigator::set_mission_item() ret = _mission.get_current_mission_item(&_mission_item, &onboard, &index); if (ret == OK) { + /* reset time counter for new item */ + _time_first_inside_orbit = 0; + _mission_item_valid = true; position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item); @@ -1290,6 +1296,9 @@ Navigator::start_land_home() void Navigator::set_rtl_item() { + /*reset time counter for new RTL item */ + _time_first_inside_orbit = 0; + switch (_rtl_state) { case RTL_STATE_CLIMB: { memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s)); From 13a5b5b4a3d8e487fb836e14185d61c03493ef0a Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 30 Jan 2014 16:02:17 +0100 Subject: [PATCH 373/447] mc_att_control: major cleanup and code reorganization --- .../mc_att_control/mc_att_control_main.cpp | 716 ++++++++++-------- 1 file changed, 383 insertions(+), 333 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 44baccefc0..e37fe18b8f 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -113,28 +113,39 @@ private: bool _task_should_exit; /**< if true, sensor task should exit */ int _control_task; /**< task handle for sensor task */ - int _att_sub; /**< vehicle attitude subscription */ - int _att_sp_sub; /**< vehicle attitude setpoint */ - int _control_mode_sub; /**< vehicle control mode subscription */ - int _params_sub; /**< notification of parameter updates */ - int _manual_sub; /**< notification of manual control updates */ - int _arming_sub; /**< arming status of outputs */ + int _v_att_sub; /**< vehicle attitude subscription */ + int _v_att_sp_sub; /**< vehicle attitude setpoint subscription */ + int _v_rates_sp_sub; /**< vehicle rates setpoint subscription */ + int _v_control_mode_sub; /**< vehicle control mode subscription */ + int _params_sub; /**< parameter updates subscription */ + int _manual_control_sp_sub; /**< manual control setpoint subscription */ + int _armed_sub; /**< arming status subscription */ orb_advert_t _att_sp_pub; /**< attitude setpoint publication */ - orb_advert_t _rates_sp_pub; /**< rate setpoint publication */ - orb_advert_t _actuators_0_pub; /**< actuator control group 0 setpoint */ + orb_advert_t _v_rates_sp_pub; /**< rate setpoint publication */ + orb_advert_t _actuators_0_pub; /**< attitude actuator controls publication */ - struct vehicle_attitude_s _att; /**< vehicle attitude */ - struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */ - struct manual_control_setpoint_s _manual; /**< r/c channel data */ - struct vehicle_control_mode_s _control_mode; /**< vehicle control mode */ - struct actuator_controls_s _actuators; /**< actuator control inputs */ - struct actuator_armed_s _arming; /**< actuator arming status */ - struct vehicle_rates_setpoint_s _rates_sp; /**< vehicle rates setpoint */ + struct vehicle_attitude_s _v_att; /**< vehicle attitude */ + struct vehicle_attitude_setpoint_s _v_att_sp; /**< vehicle attitude setpoint */ + struct vehicle_rates_setpoint_s _v_rates_sp; /**< vehicle rates setpoint */ + struct manual_control_setpoint_s _manual_control_sp; /**< manual control setpoint */ + struct vehicle_control_mode_s _v_control_mode; /**< vehicle control mode */ + struct actuator_controls_s _actuators; /**< actuator controls */ + struct actuator_armed_s _armed; /**< actuator arming status */ perf_counter_t _loop_perf; /**< loop performance counter */ - math::Vector<3> _rates_prev; /**< angular rates on previous step */ + math::Matrix<3, 3> _R_sp; /**< attitude setpoint rotation matrix */ + math::Matrix<3, 3> _R; /**< rotation matrix for current state */ + math::Vector<3> _rates_prev; /**< angular rates on previous step */ + math::Vector<3> _rates_sp; /**< angular rates setpoint */ + math::Vector<3> _rates_int; /**< angular rates integral error */ + float _thrust_sp; /**< thrust setpoint */ + math::Vector<3> _att_control; /**< attitude control vector */ + + math::Matrix<3, 3> I; /**< identity matrix */ + + bool _reset_yaw_sp; /**< reset yaw setpoint flag */ struct { param_t att_p; @@ -160,9 +171,9 @@ private: int parameters_update(); /** - * Update control outputs + * Check for parameter update and handle it. */ - void control_update(); + void parameter_update_poll(); /** * Check for changes in vehicle control mode. @@ -175,15 +186,30 @@ private: void vehicle_manual_poll(); /** - * Check for set triplet updates. + * Check for attitude setpoint updates. */ - void vehicle_setpoint_poll(); + void vehicle_attitude_setpoint_poll(); + + /** + * Check for rates setpoint updates. + */ + void vehicle_rates_setpoint_poll(); /** * Check for arming status updates. */ void arming_status_poll(); + /** + * Attitude controller. + */ + void control_attitude(float dt); + + /** + * Attitude rates controller. + */ + void control_attitude_rates(float dt); + /** * Shim for calling task_main from task_create. */ @@ -195,7 +221,7 @@ private: void task_main() __attribute__((noreturn)); }; -namespace att_control +namespace mc_att_control { /* oddly, ERROR is not defined for c++ */ @@ -213,34 +239,42 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : _control_task(-1), /* subscriptions */ - _att_sub(-1), - _att_sp_sub(-1), - _control_mode_sub(-1), + _v_att_sub(-1), + _v_att_sp_sub(-1), + _v_control_mode_sub(-1), _params_sub(-1), - _manual_sub(-1), - _arming_sub(-1), + _manual_control_sp_sub(-1), + _armed_sub(-1), /* publications */ _att_sp_pub(-1), - _rates_sp_pub(-1), + _v_rates_sp_pub(-1), _actuators_0_pub(-1), /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "fw att control")) { - memset(&_att, 0, sizeof(_att)); - memset(&_att_sp, 0, sizeof(_att_sp)); - memset(&_manual, 0, sizeof(_manual)); - memset(&_control_mode, 0, sizeof(_control_mode)); - memset(&_arming, 0, sizeof(_arming)); + memset(&_v_att, 0, sizeof(_v_att)); + memset(&_v_att_sp, 0, sizeof(_v_att_sp)); + memset(&_manual_control_sp, 0, sizeof(_manual_control_sp)); + memset(&_v_control_mode, 0, sizeof(_v_control_mode)); + memset(&_armed, 0, sizeof(_armed)); _params.p.zero(); _params.rate_p.zero(); _params.rate_i.zero(); _params.rate_d.zero(); + _R_sp.identity(); + _R.identity(); _rates_prev.zero(); + _rates_sp.zero(); + _rates_int.zero(); + _thrust_sp = 0.0f; + _att_control.zero(); + + I.identity(); _params_handles.att_p = param_find("MC_ATT_P"); _params_handles.yaw_p = param_find("MC_YAW_P"); @@ -276,7 +310,7 @@ MulticopterAttitudeControl::~MulticopterAttitudeControl() } while (_control_task != -1); } - att_control::g_control = nullptr; + mc_att_control::g_control = nullptr; } int @@ -312,42 +346,68 @@ MulticopterAttitudeControl::parameters_update() } void -MulticopterAttitudeControl::vehicle_control_mode_poll() +MulticopterAttitudeControl::parameter_update_poll() { - bool control_mode_updated; + bool updated; /* Check HIL state if vehicle status has changed */ - orb_check(_control_mode_sub, &control_mode_updated); + orb_check(_params_sub, &updated); - if (control_mode_updated) { + if (updated) { + struct parameter_update_s param_update; + orb_copy(ORB_ID(parameter_update), _params_sub, ¶m_update); + parameters_update(); + } +} - orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode); +void +MulticopterAttitudeControl::vehicle_control_mode_poll() +{ + bool updated; + + /* Check HIL state if vehicle status has changed */ + orb_check(_v_control_mode_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(vehicle_control_mode), _v_control_mode_sub, &_v_control_mode); } } void MulticopterAttitudeControl::vehicle_manual_poll() { - bool manual_updated; + bool updated; /* get pilots inputs */ - orb_check(_manual_sub, &manual_updated); + orb_check(_manual_control_sp_sub, &updated); - if (manual_updated) { + if (updated) { - orb_copy(ORB_ID(manual_control_setpoint), _manual_sub, &_manual); + orb_copy(ORB_ID(manual_control_setpoint), _manual_control_sp_sub, &_manual_control_sp); } } void -MulticopterAttitudeControl::vehicle_setpoint_poll() +MulticopterAttitudeControl::vehicle_attitude_setpoint_poll() { /* check if there is a new setpoint */ - bool att_sp_updated; - orb_check(_att_sp_sub, &att_sp_updated); + bool updated; + orb_check(_v_att_sp_sub, &updated); - if (att_sp_updated) { - orb_copy(ORB_ID(vehicle_attitude_setpoint), _att_sp_sub, &_att_sp); + if (updated) { + orb_copy(ORB_ID(vehicle_attitude_setpoint), _v_att_sp_sub, &_v_att_sp); + } +} + +void +MulticopterAttitudeControl::vehicle_rates_setpoint_poll() +{ + /* check if there is a new setpoint */ + bool updated; + orb_check(_v_rates_sp_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(vehicle_rates_setpoint), _v_rates_sp_sub, &_v_rates_sp); } } @@ -355,351 +415,341 @@ void MulticopterAttitudeControl::arming_status_poll() { /* check if there is a new setpoint */ - bool arming_updated; - orb_check(_arming_sub, &arming_updated); + bool updated; + orb_check(_armed_sub, &updated); - if (arming_updated) { - orb_copy(ORB_ID(actuator_armed), _arming_sub, &_arming); + if (updated) { + orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed); + } +} + +/* + * Attitude controller. + * Input: 'manual_control_setpoint' and 'vehicle_attitude_setpoint' topics (depending on mode) + * Output: '_rates_sp' vector, '_thrust_sp', 'vehicle_attitude_setpoint' topic (for manual modes) + */ +void +MulticopterAttitudeControl::control_attitude(float dt) +{ + float yaw_sp_move_rate = 0.0f; + bool publish_att_sp = false; + + if (_v_control_mode.flag_control_manual_enabled) { + /* manual input, set or modify attitude setpoint */ + + if (_v_control_mode.flag_control_velocity_enabled || _v_control_mode.flag_control_climb_rate_enabled) { + /* in assisted modes poll 'vehicle_attitude_setpoint' topic and modify it */ + vehicle_attitude_setpoint_poll(); + } + + if (!_v_control_mode.flag_control_climb_rate_enabled) { + /* pass throttle directly if not in altitude stabilized mode */ + _v_att_sp.thrust = _manual_control_sp.throttle; + publish_att_sp = true; + } + + if (!_armed.armed) { + /* reset yaw setpoint when disarmed */ + _reset_yaw_sp = true; + } + + /* move yaw setpoint in all modes */ + if (_v_att_sp.thrust < 0.1f) { + // TODO + //if (_status.condition_landed) { + /* reset yaw setpoint if on ground */ + // reset_yaw_sp = true; + //} + } else { + if (_manual_control_sp.yaw < -YAW_DEADZONE || YAW_DEADZONE < _manual_control_sp.yaw) { + /* move yaw setpoint */ + yaw_sp_move_rate = _manual_control_sp.yaw; + _v_att_sp.yaw_body = _wrap_pi(_v_att_sp.yaw_body + yaw_sp_move_rate * dt); + _v_att_sp.R_valid = false; + publish_att_sp = true; + } + } + + /* reset yaw setpint to current position if needed */ + if (_reset_yaw_sp) { + _reset_yaw_sp = false; + _v_att_sp.yaw_body = _v_att.yaw; + _v_att_sp.R_valid = false; + publish_att_sp = true; + } + + if (!_v_control_mode.flag_control_velocity_enabled) { + /* update attitude setpoint if not in position control mode */ + _v_att_sp.roll_body = _manual_control_sp.roll; + _v_att_sp.pitch_body = _manual_control_sp.pitch; + _v_att_sp.R_valid = false; + publish_att_sp = true; + } + + } else { + /* in non-manual mode use 'vehicle_attitude_setpoint' topic */ + vehicle_attitude_setpoint_poll(); + + /* reset yaw setpoint after non-manual control mode */ + _reset_yaw_sp = true; + } + + _thrust_sp = _v_att_sp.thrust; + + /* construct attitude setpoint rotation matrix */ + if (_v_att_sp.R_valid) { + /* rotation matrix in _att_sp is valid, use it */ + _R_sp.set(&_v_att_sp.R_body[0][0]); + + } else { + /* rotation matrix in _att_sp is not valid, use euler angles instead */ + _R_sp.from_euler(_v_att_sp.roll_body, _v_att_sp.pitch_body, _v_att_sp.yaw_body); + + /* copy rotation matrix back to setpoint struct */ + memcpy(&_v_att_sp.R_body[0][0], &_R_sp.data[0][0], sizeof(_v_att_sp.R_body)); + _v_att_sp.R_valid = true; + } + + /* publish the attitude setpoint if needed */ + if (publish_att_sp) { + _v_att_sp.timestamp = hrt_absolute_time(); + + if (_att_sp_pub > 0) { + orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &_v_att_sp); + + } else { + _att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_v_att_sp); + } + } + + /* rotation matrix for current state */ + _R.set(_v_att.R); + + /* all input data is ready, run controller itself */ + + /* try to move thrust vector shortest way, because yaw response is slower than roll/pitch */ + math::Vector<3> R_z(_R(0, 2), _R(1, 2), _R(2, 2)); + math::Vector<3> R_sp_z(_R_sp(0, 2), _R_sp(1, 2), _R_sp(2, 2)); + + /* axis and sin(angle) of desired rotation */ + math::Vector<3> e_R = _R.transposed() * (R_z % R_sp_z); + + /* calculate angle error */ + float e_R_z_sin = e_R.length(); + float e_R_z_cos = R_z * R_sp_z; + + /* calculate weight for yaw control */ + float yaw_w = _R_sp(2, 2) * _R_sp(2, 2); + + /* calculate rotation matrix after roll/pitch only rotation */ + math::Matrix<3, 3> R_rp; + + if (e_R_z_sin > 0.0f) { + /* get axis-angle representation */ + float e_R_z_angle = atan2f(e_R_z_sin, e_R_z_cos); + math::Vector<3> e_R_z_axis = e_R / e_R_z_sin; + + e_R = e_R_z_axis * e_R_z_angle; + + /* cross product matrix for e_R_axis */ + math::Matrix<3, 3> e_R_cp; + e_R_cp.zero(); + e_R_cp(0, 1) = -e_R_z_axis(2); + e_R_cp(0, 2) = e_R_z_axis(1); + e_R_cp(1, 0) = e_R_z_axis(2); + e_R_cp(1, 2) = -e_R_z_axis(0); + e_R_cp(2, 0) = -e_R_z_axis(1); + e_R_cp(2, 1) = e_R_z_axis(0); + + /* rotation matrix for roll/pitch only rotation */ + R_rp = _R * (I + e_R_cp * e_R_z_sin + e_R_cp * e_R_cp * (1.0f - e_R_z_cos)); + + } else { + /* zero roll/pitch rotation */ + R_rp = _R; + } + + /* R_rp and _R_sp has the same Z axis, calculate yaw error */ + math::Vector<3> R_sp_x(_R_sp(0, 0), _R_sp(1, 0), _R_sp(2, 0)); + math::Vector<3> R_rp_x(R_rp(0, 0), R_rp(1, 0), R_rp(2, 0)); + e_R(2) = atan2f((R_rp_x % R_sp_x) * R_sp_z, R_rp_x * R_sp_x) * yaw_w; + + if (e_R_z_cos < 0.0f) { + /* for large thrust vector rotations use another rotation method: + * calculate angle and axis for R -> R_sp rotation directly */ + math::Quaternion q; + q.from_dcm(_R.transposed() * _R_sp); + math::Vector<3> e_R_d = q.imag(); + e_R_d.normalize(); + e_R_d *= 2.0f * atan2f(e_R_d.length(), q(0)); + + /* use fusion of Z axis based rotation and direct rotation */ + float direct_w = e_R_z_cos * e_R_z_cos * yaw_w; + e_R = e_R * (1.0f - direct_w) + e_R_d * direct_w; + } + + /* calculate angular rates setpoint */ + _rates_sp = _params.p.emult(e_R); + + /* feed forward yaw setpoint rate */ + _rates_sp(2) += yaw_sp_move_rate * yaw_w; +} + +/* + * Attitude rates controller. + * Input: '_rates_sp' vector, '_thrust_sp' + * Output: '_att_control' vector + */ +void +MulticopterAttitudeControl::control_attitude_rates(float dt) +{ + /* reset integral if disarmed */ + if (!_armed.armed) { + _rates_int.zero(); + } + + /* current body angular rates */ + math::Vector<3> rates; + rates(0) = _v_att.rollspeed; + rates(1) = _v_att.pitchspeed; + rates(2) = _v_att.yawspeed; + + /* 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; + _rates_prev = rates; + + /* update integral only if not saturated on low limit */ + if (_thrust_sp > 0.1f && _att_control.length() < _thrust_sp) { + for (int i = 0; i < 3; i++) { + float rate_i = _rates_int(i) + _params.rate_i(i) * rates_err(i) * dt; + + if (isfinite(rate_i) && rate_i > -RATES_I_LIMIT && rate_i < RATES_I_LIMIT && + _att_control(i) > -RATES_I_LIMIT && _att_control(i) < RATES_I_LIMIT) { + _rates_int(i) = rate_i; + } + } } } void MulticopterAttitudeControl::task_main_trampoline(int argc, char *argv[]) { - att_control::g_control->task_main(); + mc_att_control::g_control->task_main(); } void MulticopterAttitudeControl::task_main() { - /* inform about start */ warnx("started"); fflush(stdout); /* * do subscriptions */ - _att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); - _att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); - _control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); + _v_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); + _v_rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint)); + _v_att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); + _v_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); - _manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); - _arming_sub = orb_subscribe(ORB_ID(actuator_armed)); + _manual_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); + _armed_sub = orb_subscribe(ORB_ID(actuator_armed)); - /* rate limit attitude updates to 100Hz */ - orb_set_interval(_att_sub, 10); + /* rate limit attitude updates to 200Hz, failsafe against spam, normally runs at the same rate as attitude estimator */ + orb_set_interval(_v_att_sub, 5); + /* initialize parameters cache */ parameters_update(); - /* initialize values of critical structs until first regular update */ - _arming.armed = false; + /* wakeup source: vehicle attitude */ + struct pollfd fds[1]; - /* get an initial update for all sensor and status data */ - vehicle_setpoint_poll(); - vehicle_control_mode_poll(); - vehicle_manual_poll(); - arming_status_poll(); - - /* setpoint rotation matrix */ - math::Matrix<3, 3> R_sp; - R_sp.identity(); - - /* rotation matrix for current state */ - math::Matrix<3, 3> R; - R.identity(); - - /* current angular rates */ - math::Vector<3> rates; - rates.zero(); - - /* angular rates integral error */ - math::Vector<3> rates_int; - rates_int.zero(); - - /* identity matrix */ - math::Matrix<3, 3> I; - I.identity(); - - math::Quaternion q; - - bool reset_yaw_sp = true; - - /* wakeup source(s) */ - struct pollfd fds[2]; - - /* Setup of loop */ - fds[0].fd = _params_sub; + fds[0].fd = _v_att_sub; fds[0].events = POLLIN; - fds[1].fd = _att_sub; - fds[1].events = POLLIN; while (!_task_should_exit) { - /* wait for up to 500ms for data */ + /* wait for up to 100ms for data */ int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100); - /* timed out - periodic check for _task_should_exit, etc. */ + /* timed out - periodic check for _task_should_exit */ if (pret == 0) continue; /* this is undesirable but not much we can do - might want to flag unhappy status */ if (pret < 0) { warn("poll error %d, %d", pret, errno); + /* sleep a bit before next try */ + usleep(100000); continue; } perf_begin(_loop_perf); - /* only update parameters if they changed */ + /* run controller on attitude changes */ if (fds[0].revents & POLLIN) { - /* copy the topic to clear updated flag */ - struct parameter_update_s update; - orb_copy(ORB_ID(parameter_update), _params_sub, &update); - - parameters_update(); - } - - /* only run controller if attitude changed */ - if (fds[1].revents & POLLIN) { static uint64_t last_run = 0; float dt = (hrt_absolute_time() - last_run) / 1000000.0f; last_run = hrt_absolute_time(); - /* guard against too large dt's */ - if (dt > 0.02f) + /* guard against too small (< 2ms) and too large (> 20ms) dt's */ + if (dt < 0.002f) { + dt = 0.002f; + + } else if (dt > 0.02f) { dt = 0.02f; + } /* copy attitude topic */ - orb_copy(ORB_ID(vehicle_attitude), _att_sub, &_att); + orb_copy(ORB_ID(vehicle_attitude), _v_att_sub, &_v_att); - vehicle_setpoint_poll(); + /* check for updates in other topics */ + parameter_update_poll(); vehicle_control_mode_poll(); arming_status_poll(); vehicle_manual_poll(); - float yaw_sp_move_rate = 0.0f; - bool publish_att_sp = false; + if (_v_control_mode.flag_control_attitude_enabled) { + control_attitude(dt); - /* define which input is the dominating control input */ - if (_control_mode.flag_control_manual_enabled) { - /* manual input */ - if (!_control_mode.flag_control_climb_rate_enabled) { - /* pass throttle directly if not in altitude control mode */ - _att_sp.thrust = _manual.throttle; - } + /* 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 (!_arming.armed) { - /* reset yaw setpoint when disarmed */ - reset_yaw_sp = true; - } - - if (_control_mode.flag_control_attitude_enabled) { - /* control attitude, update attitude setpoint depending on mode */ - - if (_att_sp.thrust < 0.1f) { - // TODO - //if (_status.condition_landed) { - /* reset yaw setpoint if on ground */ - // reset_yaw_sp = true; - //} - } else { - if (_manual.yaw < -YAW_DEADZONE || YAW_DEADZONE < _manual.yaw) { - /* move yaw setpoint */ - yaw_sp_move_rate = _manual.yaw; - _att_sp.yaw_body = _wrap_pi(_att_sp.yaw_body + yaw_sp_move_rate * dt); - _att_sp.R_valid = false; - publish_att_sp = true; - } - } - - /* reset yaw setpint to current position if needed */ - if (reset_yaw_sp) { - reset_yaw_sp = false; - _att_sp.yaw_body = _att.yaw; - _att_sp.R_valid = false; - publish_att_sp = true; - } - - if (!_control_mode.flag_control_velocity_enabled) { - /* update attitude setpoint if not in position control mode */ - _att_sp.roll_body = _manual.roll; - _att_sp.pitch_body = _manual.pitch; - _att_sp.R_valid = false; - publish_att_sp = true; - } + if (_v_rates_sp_pub > 0) { + orb_publish(ORB_ID(vehicle_rates_setpoint), _v_rates_sp_pub, &_v_rates_sp); } else { - /* manual rate inputs (ACRO) */ - // TODO - /* reset yaw setpoint after ACRO */ - reset_yaw_sp = true; + _v_rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &_v_rates_sp); } } else { - /* reset yaw setpoint after non-manual control */ - reset_yaw_sp = true; + /* attitude controller disabled */ + // TODO poll 'attitude_rates_setpoint' topic + _rates_sp.zero(); + _thrust_sp = 0.0f; } - if (_att_sp.R_valid) { - /* rotation matrix in _att_sp is valid, use it */ - R_sp.set(&_att_sp.R_body[0][0]); + if (_v_control_mode.flag_control_rates_enabled) { + control_attitude_rates(dt); - } else { - /* rotation matrix in _att_sp is not valid, use euler angles instead */ - R_sp.from_euler(_att_sp.roll_body, _att_sp.pitch_body, _att_sp.yaw_body); + /* publish actuator controls */ + _actuators.control[0] = (isfinite(_att_control(0))) ? _att_control(0) : 0.0f; + _actuators.control[1] = (isfinite(_att_control(1))) ? _att_control(1) : 0.0f; + _actuators.control[2] = (isfinite(_att_control(2))) ? _att_control(2) : 0.0f; + _actuators.control[3] = (isfinite(_thrust_sp)) ? _thrust_sp : 0.0f; + _actuators.timestamp = hrt_absolute_time(); - /* copy rotation matrix back to setpoint struct */ - memcpy(&_att_sp.R_body[0][0], &R_sp.data[0][0], sizeof(_att_sp.R_body)); - _att_sp.R_valid = true; - } - - if (publish_att_sp) { - /* publish the attitude setpoint */ - _att_sp.timestamp = hrt_absolute_time(); - - if (_att_sp_pub > 0) { - orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &_att_sp); + if (_actuators_0_pub > 0) { + orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators); } else { - _att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_att_sp); + _actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators); } } - - /* rotation matrix for current state */ - R.set(_att.R); - - /* current body angular rates */ - rates(0) = _att.rollspeed; - rates(1) = _att.pitchspeed; - rates(2) = _att.yawspeed; - - /* try to move thrust vector shortest way, because yaw response is slower than roll/pitch */ - math::Vector<3> R_z(R(0, 2), R(1, 2), R(2, 2)); - math::Vector<3> R_sp_z(R_sp(0, 2), R_sp(1, 2), R_sp(2, 2)); - - /* axis and sin(angle) of desired rotation */ - math::Vector<3> e_R = R.transposed() * (R_z % R_sp_z); - - /* calculate angle error */ - float e_R_z_sin = e_R.length(); - float e_R_z_cos = R_z * R_sp_z; - - /* calculate weight for yaw control */ - float yaw_w = R_sp(2, 2) * R_sp(2, 2); - - /* calculate rotation matrix after roll/pitch only rotation */ - math::Matrix<3, 3> R_rp; - - if (e_R_z_sin > 0.0f) { - /* get axis-angle representation */ - float e_R_z_angle = atan2f(e_R_z_sin, e_R_z_cos); - math::Vector<3> e_R_z_axis = e_R / e_R_z_sin; - - e_R = e_R_z_axis * e_R_z_angle; - - /* cross product matrix for e_R_axis */ - math::Matrix<3, 3> e_R_cp; - e_R_cp.zero(); - e_R_cp(0, 1) = -e_R_z_axis(2); - e_R_cp(0, 2) = e_R_z_axis(1); - e_R_cp(1, 0) = e_R_z_axis(2); - e_R_cp(1, 2) = -e_R_z_axis(0); - e_R_cp(2, 0) = -e_R_z_axis(1); - e_R_cp(2, 1) = e_R_z_axis(0); - - /* rotation matrix for roll/pitch only rotation */ - R_rp = R * (I + e_R_cp * e_R_z_sin + e_R_cp * e_R_cp * (1.0f - e_R_z_cos)); - - } else { - /* zero roll/pitch rotation */ - R_rp = R; - } - - /* R_rp and R_sp has the same Z axis, calculate yaw error */ - math::Vector<3> R_sp_x(R_sp(0, 0), R_sp(1, 0), R_sp(2, 0)); - math::Vector<3> R_rp_x(R_rp(0, 0), R_rp(1, 0), R_rp(2, 0)); - e_R(2) = atan2f((R_rp_x % R_sp_x) * R_sp_z, R_rp_x * R_sp_x) * yaw_w; - - if (e_R_z_cos < 0.0f) { - /* for large thrust vector rotations use another rotation method: - * calculate angle and axis for R -> R_sp rotation directly */ - q.from_dcm(R.transposed() * R_sp); - math::Vector<3> e_R_d = q.imag(); - e_R_d.normalize(); - e_R_d *= 2.0f * atan2f(e_R_d.length(), q(0)); - - /* use fusion of Z axis based rotation and direct rotation */ - float direct_w = e_R_z_cos * e_R_z_cos * yaw_w; - e_R = e_R * (1.0f - direct_w) + e_R_d * direct_w; - } - - /* angular rates setpoint*/ - math::Vector<3> rates_sp = _params.p.emult(e_R); - - /* feed forward yaw setpoint rate */ - rates_sp(2) += yaw_sp_move_rate * yaw_w; - - /* reset integral if disarmed */ - // TODO add LANDED flag here - if (!_arming.armed) { - rates_int.zero(); - } - - /* rate controller */ - math::Vector<3> rates_err = rates_sp - rates; - math::Vector<3> control = _params.rate_p.emult(rates_err) + _params.rate_d.emult(_rates_prev - rates) / fmaxf(dt, 0.003f) + rates_int; - _rates_prev = rates; - - /* update integral */ - for (int i = 0; i < 3; i++) { - float rate_i = rates_int(i) + _params.rate_i(i) * rates_err(i) * dt; - - if (isfinite(rate_i)) { - if (rate_i > -RATES_I_LIMIT && rate_i < RATES_I_LIMIT && control(i) > -RATES_I_LIMIT && control(i) < RATES_I_LIMIT) { - rates_int(i) = rate_i; - } - } - } - - /* publish the attitude rates setpoint */ - _rates_sp.roll = rates_sp(0); - _rates_sp.pitch = rates_sp(1); - _rates_sp.yaw = rates_sp(2); - _rates_sp.thrust = _att_sp.thrust; - _rates_sp.timestamp = hrt_absolute_time(); - - if (_rates_sp_pub > 0) { - orb_publish(ORB_ID(vehicle_rates_setpoint), _rates_sp_pub, &_rates_sp); - - } else { - _rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &_rates_sp); - } - - /* publish the attitude controls */ - if (_control_mode.flag_control_rates_enabled) { - _actuators.control[0] = (isfinite(control(0))) ? control(0) : 0.0f; - _actuators.control[1] = (isfinite(control(1))) ? control(1) : 0.0f; - _actuators.control[2] = (isfinite(control(2))) ? control(2) : 0.0f; - _actuators.control[3] = (isfinite(_rates_sp.thrust)) ? _rates_sp.thrust : 0.0f; - _actuators.timestamp = hrt_absolute_time(); - - } else { - /* controller disabled, publish zero attitude controls */ - _actuators.control[0] = 0.0f; - _actuators.control[1] = 0.0f; - _actuators.control[2] = 0.0f; - _actuators.control[3] = 0.0f; - _actuators.timestamp = hrt_absolute_time(); - } - - if (_actuators_0_pub > 0) { - /* publish the attitude setpoint */ - orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators); - - } else { - /* advertise and publish */ - _actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators); - } } perf_end(_loop_perf); @@ -739,17 +789,17 @@ int mc_att_control_main(int argc, char *argv[]) if (!strcmp(argv[1], "start")) { - if (att_control::g_control != nullptr) + if (mc_att_control::g_control != nullptr) errx(1, "already running"); - att_control::g_control = new MulticopterAttitudeControl; + mc_att_control::g_control = new MulticopterAttitudeControl; - if (att_control::g_control == nullptr) + if (mc_att_control::g_control == nullptr) errx(1, "alloc failed"); - if (OK != att_control::g_control->start()) { - delete att_control::g_control; - att_control::g_control = nullptr; + if (OK != mc_att_control::g_control->start()) { + delete mc_att_control::g_control; + mc_att_control::g_control = nullptr; err(1, "start failed"); } @@ -757,16 +807,16 @@ int mc_att_control_main(int argc, char *argv[]) } if (!strcmp(argv[1], "stop")) { - if (att_control::g_control == nullptr) + if (mc_att_control::g_control == nullptr) errx(1, "not running"); - delete att_control::g_control; - att_control::g_control = nullptr; + delete mc_att_control::g_control; + mc_att_control::g_control = nullptr; exit(0); } if (!strcmp(argv[1], "status")) { - if (att_control::g_control) { + if (mc_att_control::g_control) { errx(0, "running"); } else { From 9cbc31b588f3bac6d3b03870801c97068a4b4ecc Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 30 Jan 2014 19:44:06 +0100 Subject: [PATCH 374/447] mavlink: revert stack size 2048 to fix suspending in HIL mode --- src/modules/mavlink/mavlink.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c index 9bb92cbf5a..4d975066f7 100644 --- a/src/modules/mavlink/mavlink.c +++ b/src/modules/mavlink/mavlink.c @@ -794,7 +794,7 @@ int mavlink_main(int argc, char *argv[]) mavlink_task = task_spawn_cmd("mavlink", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, - 1200, + 2048, mavlink_thread_main, (const char **)argv); From 498155cf676623abd955ba66bb3e90ab538383d2 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 31 Jan 2014 00:46:28 +0100 Subject: [PATCH 375/447] mc_att_control: yaw dead zone fixed, added MC_YAW_FF (yaw feed-forward) parameter --- .../mc_att_control/mc_att_control_main.cpp | 26 ++++++++++++++++--- .../mc_att_control/mc_att_control_params.c | 1 + 2 files changed, 24 insertions(+), 3 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 e37fe18b8f..dc3da0f467 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -156,6 +156,9 @@ private: param_t yaw_rate_p; param_t yaw_rate_i; param_t yaw_rate_d; + param_t yaw_ff; + + param_t rc_scale_yaw; } _params_handles; /**< handles for interesting parameters */ struct { @@ -163,6 +166,9 @@ private: math::Vector<3> rate_p; /**< P gain for angular rate error */ math::Vector<3> rate_i; /**< I gain for angular rate error */ math::Vector<3> rate_d; /**< D gain for angular rate error */ + float yaw_ff; /**< yaw control feed-forward */ + + float rc_scale_yaw; } _params; /** @@ -284,6 +290,9 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : _params_handles.yaw_rate_p = param_find("MC_YAWRATE_P"); _params_handles.yaw_rate_i = param_find("MC_YAWRATE_I"); _params_handles.yaw_rate_d = param_find("MC_YAWRATE_D"); + _params_handles.yaw_ff = param_find("MC_YAW_FF"); + + _params_handles.rc_scale_yaw = param_find("RC_SCALE_YAW"); /* fetch initial parameter values */ parameters_update(); @@ -342,6 +351,10 @@ MulticopterAttitudeControl::parameters_update() param_get(_params_handles.yaw_rate_d, &v); _params.rate_d(2) = v; + param_get(_params_handles.yaw_ff, &_params.yaw_ff); + + param_get(_params_handles.rc_scale_yaw, &_params.rc_scale_yaw); + return OK; } @@ -461,9 +474,16 @@ MulticopterAttitudeControl::control_attitude(float dt) // reset_yaw_sp = true; //} } else { - if (_manual_control_sp.yaw < -YAW_DEADZONE || YAW_DEADZONE < _manual_control_sp.yaw) { + float yaw_dz_scaled = YAW_DEADZONE * _params.rc_scale_yaw; + if (_params.rc_scale_yaw > 0.001f && fabs(_manual_control_sp.yaw) > yaw_dz_scaled) { /* move yaw setpoint */ - yaw_sp_move_rate = _manual_control_sp.yaw; + yaw_sp_move_rate = _manual_control_sp.yaw / _params.rc_scale_yaw; + if (_manual_control_sp.yaw > 0.0f) { + yaw_sp_move_rate -= YAW_DEADZONE; + } else { + yaw_sp_move_rate += YAW_DEADZONE; + } + yaw_sp_move_rate *= _params.rc_scale_yaw; _v_att_sp.yaw_body = _wrap_pi(_v_att_sp.yaw_body + yaw_sp_move_rate * dt); _v_att_sp.R_valid = false; publish_att_sp = true; @@ -592,7 +612,7 @@ MulticopterAttitudeControl::control_attitude(float dt) _rates_sp = _params.p.emult(e_R); /* feed forward yaw setpoint rate */ - _rates_sp(2) += yaw_sp_move_rate * yaw_w; + _rates_sp(2) += yaw_sp_move_rate * yaw_w * _params.yaw_ff; } /* 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 a170365eec..0a89c3f4e5 100644 --- a/src/modules/mc_att_control/mc_att_control_params.c +++ b/src/modules/mc_att_control/mc_att_control_params.c @@ -51,3 +51,4 @@ PARAM_DEFINE_FLOAT(MC_ATTRATE_D, 0.002f); PARAM_DEFINE_FLOAT(MC_YAWRATE_P, 0.3f); PARAM_DEFINE_FLOAT(MC_YAWRATE_I, 0.0f); PARAM_DEFINE_FLOAT(MC_YAWRATE_D, 0.0f); +PARAM_DEFINE_FLOAT(MC_YAW_FF, 0.5f); From af3b24dc001bfe84ad783a8b85e3b576330ce257 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 31 Jan 2014 00:47:31 +0100 Subject: [PATCH 376/447] mc_pos_control: removed unused parameter reading RC_SCALE_YAW --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 4 ---- 1 file changed, 4 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 fe8377a40d..e9b92119d6 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -153,7 +153,6 @@ private: param_t rc_scale_pitch; param_t rc_scale_roll; - param_t rc_scale_yaw; } _params_handles; /**< handles for interesting parameters */ struct { @@ -165,7 +164,6 @@ private: float rc_scale_pitch; float rc_scale_roll; - float rc_scale_yaw; math::Vector<3> pos_p; math::Vector<3> vel_p; @@ -312,7 +310,6 @@ MulticopterPositionControl::MulticopterPositionControl() : _params_handles.land_tilt_max = param_find("MPC_LAND_TILT"); _params_handles.rc_scale_pitch = param_find("RC_SCALE_PITCH"); _params_handles.rc_scale_roll = param_find("RC_SCALE_ROLL"); - _params_handles.rc_scale_yaw = param_find("RC_SCALE_YAW"); /* fetch initial parameter values */ parameters_update(true); @@ -361,7 +358,6 @@ MulticopterPositionControl::parameters_update(bool force) param_get(_params_handles.land_tilt_max, &_params.land_tilt_max); param_get(_params_handles.rc_scale_pitch, &_params.rc_scale_pitch); param_get(_params_handles.rc_scale_roll, &_params.rc_scale_roll); - param_get(_params_handles.rc_scale_yaw, &_params.rc_scale_yaw); float v; param_get(_params_handles.xy_p, &v); From 88d3f875ffd36a3e05899222aa2eb27848dcf6d1 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 31 Jan 2014 08:48:00 +0100 Subject: [PATCH 377/447] attitude_estimator_ekf: enable acceleration compensation by default --- .../attitude_estimator_ekf/attitude_estimator_ekf_params.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c index 44f47b47ca..4154e3db4a 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c @@ -68,7 +68,7 @@ PARAM_DEFINE_FLOAT(ATT_YAW_OFF3, 0.0f); /* magnetic declination, in degrees */ PARAM_DEFINE_FLOAT(ATT_MAG_DECL, 0.0f); -PARAM_DEFINE_INT32(ATT_ACC_COMP, 0); +PARAM_DEFINE_INT32(ATT_ACC_COMP, 2); int parameters_init(struct attitude_estimator_ekf_param_handles *h) { From 282e0bb6703e6f013eee690bda97e09045837fd3 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 31 Jan 2014 11:35:11 +0100 Subject: [PATCH 378/447] mc_att_control: separate gains for roll and pitch --- .../mc_att_control/mc_att_control_main.cpp | 76 +++++++++++-------- .../mc_att_control/mc_att_control_params.c | 14 ++-- 2 files changed, 53 insertions(+), 37 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 dc3da0f467..4086a227a0 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -148,11 +148,15 @@ private: bool _reset_yaw_sp; /**< reset yaw setpoint flag */ struct { - param_t att_p; + param_t roll_p; + param_t roll_rate_p; + param_t roll_rate_i; + param_t roll_rate_d; + param_t pitch_p; + param_t pitch_rate_p; + param_t pitch_rate_i; + param_t pitch_rate_d; param_t yaw_p; - param_t att_rate_p; - param_t att_rate_i; - param_t att_rate_d; param_t yaw_rate_p; param_t yaw_rate_i; param_t yaw_rate_d; @@ -162,7 +166,7 @@ private: } _params_handles; /**< handles for interesting parameters */ struct { - math::Vector<3> p; /**< P gain for angular error */ + math::Vector<3> att_p; /**< P gain for angular error */ math::Vector<3> rate_p; /**< P gain for angular rate error */ math::Vector<3> rate_i; /**< I gain for angular rate error */ math::Vector<3> rate_d; /**< D gain for angular rate error */ @@ -267,7 +271,7 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : memset(&_v_control_mode, 0, sizeof(_v_control_mode)); memset(&_armed, 0, sizeof(_armed)); - _params.p.zero(); + _params.att_p.zero(); _params.rate_p.zero(); _params.rate_i.zero(); _params.rate_d.zero(); @@ -282,15 +286,19 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : I.identity(); - _params_handles.att_p = param_find("MC_ATT_P"); - _params_handles.yaw_p = param_find("MC_YAW_P"); - _params_handles.att_rate_p = param_find("MC_ATTRATE_P"); - _params_handles.att_rate_i = param_find("MC_ATTRATE_I"); - _params_handles.att_rate_d = param_find("MC_ATTRATE_D"); - _params_handles.yaw_rate_p = param_find("MC_YAWRATE_P"); - _params_handles.yaw_rate_i = param_find("MC_YAWRATE_I"); - _params_handles.yaw_rate_d = param_find("MC_YAWRATE_D"); - _params_handles.yaw_ff = param_find("MC_YAW_FF"); + _params_handles.roll_p = param_find("MC_ROLL_P"); + _params_handles.roll_rate_p = param_find("MC_ROLLRATE_P"); + _params_handles.roll_rate_i = param_find("MC_ROLLRATE_I"); + _params_handles.roll_rate_d = param_find("MC_ROLLRATE_D"); + _params_handles.pitch_p = param_find("MC_PITCH_P"); + _params_handles.pitch_rate_p = param_find("MC_PITCHRATE_P"); + _params_handles.pitch_rate_i = param_find("MC_PITCHRATE_I"); + _params_handles.pitch_rate_d = param_find("MC_PITCHRATE_D"); + _params_handles.yaw_p = param_find("MC_YAW_P"); + _params_handles.yaw_rate_p = param_find("MC_YAWRATE_P"); + _params_handles.yaw_rate_i = param_find("MC_YAWRATE_I"); + _params_handles.yaw_rate_d = param_find("MC_YAWRATE_D"); + _params_handles.yaw_ff = param_find("MC_YAW_FF"); _params_handles.rc_scale_yaw = param_find("RC_SCALE_YAW"); @@ -327,27 +335,33 @@ MulticopterAttitudeControl::parameters_update() { float v; - param_get(_params_handles.att_p, &v); - _params.p(0) = v; - _params.p(1) = v; - param_get(_params_handles.yaw_p, &v); - _params.p(2) = v; - - param_get(_params_handles.att_rate_p, &v); + /* roll */ + param_get(_params_handles.roll_p, &v); + _params.att_p(0) = v; + param_get(_params_handles.roll_rate_p, &v); _params.rate_p(0) = v; + 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; + + /* pitch */ + param_get(_params_handles.pitch_p, &v); + _params.att_p(1) = v; + param_get(_params_handles.pitch_rate_p, &v); _params.rate_p(1) = v; + 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; + + /* yaw */ + param_get(_params_handles.yaw_p, &v); + _params.att_p(2) = v; param_get(_params_handles.yaw_rate_p, &v); _params.rate_p(2) = v; - - param_get(_params_handles.att_rate_i, &v); - _params.rate_i(0) = v; - _params.rate_i(1) = v; param_get(_params_handles.yaw_rate_i, &v); _params.rate_i(2) = v; - - param_get(_params_handles.att_rate_d, &v); - _params.rate_d(0) = v; - _params.rate_d(1) = v; param_get(_params_handles.yaw_rate_d, &v); _params.rate_d(2) = v; @@ -609,7 +623,7 @@ MulticopterAttitudeControl::control_attitude(float dt) } /* calculate angular rates setpoint */ - _rates_sp = _params.p.emult(e_R); + _rates_sp = _params.att_p.emult(e_R); /* feed forward yaw setpoint rate */ _rates_sp(2) += yaw_sp_move_rate * yaw_w * _params.yaw_ff; 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 0a89c3f4e5..27a45b6bbc 100644 --- a/src/modules/mc_att_control/mc_att_control_params.c +++ b/src/modules/mc_att_control/mc_att_control_params.c @@ -41,13 +41,15 @@ #include -PARAM_DEFINE_FLOAT(MC_ATT_P, 6.0f); -PARAM_DEFINE_FLOAT(MC_ATT_I, 0.0f); +PARAM_DEFINE_FLOAT(MC_ROLL_P, 6.0f); +PARAM_DEFINE_FLOAT(MC_ROLLRATE_P, 0.1f); +PARAM_DEFINE_FLOAT(MC_ROLLRATE_I, 0.0f); +PARAM_DEFINE_FLOAT(MC_ROLLRATE_D, 0.002f); +PARAM_DEFINE_FLOAT(MC_PITCH_P, 6.0f); +PARAM_DEFINE_FLOAT(MC_PITCHRATE_P, 0.1f); +PARAM_DEFINE_FLOAT(MC_PITCHRATE_I, 0.0f); +PARAM_DEFINE_FLOAT(MC_PITCHRATE_D, 0.002f); PARAM_DEFINE_FLOAT(MC_YAW_P, 2.0f); -PARAM_DEFINE_FLOAT(MC_YAW_I, 0.0f); -PARAM_DEFINE_FLOAT(MC_ATTRATE_P, 0.1f); -PARAM_DEFINE_FLOAT(MC_ATTRATE_I, 0.0f); -PARAM_DEFINE_FLOAT(MC_ATTRATE_D, 0.002f); PARAM_DEFINE_FLOAT(MC_YAWRATE_P, 0.3f); PARAM_DEFINE_FLOAT(MC_YAWRATE_I, 0.0f); PARAM_DEFINE_FLOAT(MC_YAWRATE_D, 0.0f); From 103bece7266bbcbcd3f31aaf165ea6f902dec9f4 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 31 Jan 2014 11:36:25 +0100 Subject: [PATCH 379/447] rc scripts cleanup, avoid duplicating parameters, use inheritance instead --- .../px4fmu_common/init.d/10015_tbs_discovery | 14 ++-- ROMFS/px4fmu_common/init.d/10016_3dr_iris | 14 ++-- ROMFS/px4fmu_common/init.d/1001_rc_quad_x.hil | 34 +------- .../{12001_octo_cox_pwm => 12001_octo_cox} | 18 ++-- ROMFS/px4fmu_common/init.d/4001_quad_x | 52 ++++++++++++ ROMFS/px4fmu_common/init.d/4008_ardrone | 57 ------------- ROMFS/px4fmu_common/init.d/4009_ardrone_flow | 83 ------------------- ROMFS/px4fmu_common/init.d/4010_dji_f330 | 14 ++-- ROMFS/px4fmu_common/init.d/4011_dji_f450 | 14 ++-- ROMFS/px4fmu_common/init.d/4012_hk_x550 | 14 ++-- ROMFS/px4fmu_common/init.d/5001_quad_+ | 10 +++ ROMFS/px4fmu_common/init.d/5001_quad_+_pwm | 37 --------- ROMFS/px4fmu_common/init.d/6001_hexa_x | 12 +++ ROMFS/px4fmu_common/init.d/6001_hexa_x_pwm | 37 --------- ROMFS/px4fmu_common/init.d/7001_hexa_+ | 10 +++ ROMFS/px4fmu_common/init.d/7001_hexa_+_pwm | 37 --------- ROMFS/px4fmu_common/init.d/8001_octo_x | 12 +++ ROMFS/px4fmu_common/init.d/8001_octo_x_pwm | 37 --------- ROMFS/px4fmu_common/init.d/9001_octo_+ | 10 +++ ROMFS/px4fmu_common/init.d/9001_octo_+_pwm | 37 --------- ROMFS/px4fmu_common/init.d/rc.autostart | 21 ++--- 21 files changed, 164 insertions(+), 410 deletions(-) rename ROMFS/px4fmu_common/init.d/{12001_octo_cox_pwm => 12001_octo_cox} (63%) create mode 100644 ROMFS/px4fmu_common/init.d/4001_quad_x delete mode 100644 ROMFS/px4fmu_common/init.d/4008_ardrone delete mode 100644 ROMFS/px4fmu_common/init.d/4009_ardrone_flow create mode 100644 ROMFS/px4fmu_common/init.d/5001_quad_+ delete mode 100644 ROMFS/px4fmu_common/init.d/5001_quad_+_pwm create mode 100644 ROMFS/px4fmu_common/init.d/6001_hexa_x delete mode 100644 ROMFS/px4fmu_common/init.d/6001_hexa_x_pwm create mode 100644 ROMFS/px4fmu_common/init.d/7001_hexa_+ delete mode 100644 ROMFS/px4fmu_common/init.d/7001_hexa_+_pwm create mode 100644 ROMFS/px4fmu_common/init.d/8001_octo_x delete mode 100644 ROMFS/px4fmu_common/init.d/8001_octo_x_pwm create mode 100644 ROMFS/px4fmu_common/init.d/9001_octo_+ delete mode 100644 ROMFS/px4fmu_common/init.d/9001_octo_+_pwm diff --git a/ROMFS/px4fmu_common/init.d/10015_tbs_discovery b/ROMFS/px4fmu_common/init.d/10015_tbs_discovery index 56c74a3b52..49a50e1309 100644 --- a/ROMFS/px4fmu_common/init.d/10015_tbs_discovery +++ b/ROMFS/px4fmu_common/init.d/10015_tbs_discovery @@ -10,13 +10,15 @@ then # # Default parameters for this platform # - param set MC_ATT_P 5.0 - param set MC_ATT_I 0.0 + param set MC_ROLL_P 5.0 + param set MC_ROLLRATE_P 0.17 + param set MC_ROLLRATE_I 0.0 + param set MC_ROLLRATE_D 0.006 + param set MC_PITCH_P 5.0 + param set MC_PITCHRATE_P 0.17 + param set MC_PITCHRATE_I 0.0 + param set MC_PITCHRATE_D 0.006 param set MC_YAW_P 0.5 - param set MC_YAW_I 0.15 - param set MC_ATTRATE_P 0.17 - param set MC_ATTRATE_I 0.0 - param set MC_ATTRATE_D 0.006 param set MC_YAWRATE_P 0.2 param set MC_YAWRATE_I 0.0 param set MC_YAWRATE_D 0.0 diff --git a/ROMFS/px4fmu_common/init.d/10016_3dr_iris b/ROMFS/px4fmu_common/init.d/10016_3dr_iris index a3bcb63eb8..6c9cb6983a 100644 --- a/ROMFS/px4fmu_common/init.d/10016_3dr_iris +++ b/ROMFS/px4fmu_common/init.d/10016_3dr_iris @@ -10,13 +10,15 @@ then # # Default parameters for this platform # - param set MC_ATT_P 9.0 - param set MC_ATT_I 0.0 + param set MC_ROLL_P 9.0 + param set MC_ROLLRATE_P 0.13 + param set MC_ROLLRATE_I 0.0 + param set MC_ROLLRATE_D 0.004 + param set MC_PITCH_P 9.0 + param set MC_PITCHRATE_P 0.13 + param set MC_PITCHRATE_I 0.0 + param set MC_PITCHRATE_D 0.004 param set MC_YAW_P 0.5 - param set MC_YAW_I 0.15 - param set MC_ATTRATE_P 0.13 - param set MC_ATTRATE_I 0.0 - param set MC_ATTRATE_D 0.004 param set MC_YAWRATE_P 0.2 param set MC_YAWRATE_I 0.0 param set MC_YAWRATE_D 0.0 diff --git a/ROMFS/px4fmu_common/init.d/1001_rc_quad_x.hil b/ROMFS/px4fmu_common/init.d/1001_rc_quad_x.hil index 2d497374a5..84e2bc5d47 100644 --- a/ROMFS/px4fmu_common/init.d/1001_rc_quad_x.hil +++ b/ROMFS/px4fmu_common/init.d/1001_rc_quad_x.hil @@ -5,38 +5,6 @@ # Maintainers: Anton Babushkin # -if [ $DO_AUTOCONFIG == yes ] -then - # - # Default parameters for this platform - # - param set MC_ATT_P 7.0 - param set MC_ATT_I 0.0 - param set MC_YAW_P 2.0 - param set MC_YAW_I 0.0 - param set MC_ATTRATE_P 0.12 - param set MC_ATTRATE_I 0.0 - param set MC_ATTRATE_D 0.004 - param set MC_YAWRATE_P 0.3 - param set MC_YAWRATE_I 0.2 - param set MC_YAWRATE_D 0.005 - - param set MPC_TILT_MAX 1.0 - param set MPC_THR_MAX 1.0 - param set MPC_THR_MIN 0.1 - param set MPC_XY_P 1.0 - param set MPC_XY_VEL_D 0.01 - param set MPC_XY_VEL_I 0.02 - param set MPC_XY_VEL_MAX 5 - param set MPC_XY_VEL_P 0.1 - param set MPC_Z_P 1.0 - param set MPC_Z_VEL_D 0.0 - param set MPC_Z_VEL_I 0.02 - param set MPC_Z_VEL_MAX 3 - param set MPC_Z_VEL_P 0.1 -fi +sh /etc/init.d/4001_quad_x set HIL yes - -set VEHICLE_TYPE mc -set MIXER FMU_quad_x diff --git a/ROMFS/px4fmu_common/init.d/12001_octo_cox_pwm b/ROMFS/px4fmu_common/init.d/12001_octo_cox similarity index 63% rename from ROMFS/px4fmu_common/init.d/12001_octo_cox_pwm rename to ROMFS/px4fmu_common/init.d/12001_octo_cox index 5f3cec4e0a..2bea6d4899 100644 --- a/ROMFS/px4fmu_common/init.d/12001_octo_cox_pwm +++ b/ROMFS/px4fmu_common/init.d/12001_octo_cox @@ -10,15 +10,15 @@ then # # Default parameters for this platform # - param set MC_ATT_P 7.0 - param set MC_ATT_I 0.0 - param set MC_ATT_D 0.0 - param set MC_ATTRATE_P 0.12 - param set MC_ATTRATE_I 0.0 - param set MC_ATTRATE_D 0.004 - param set MC_YAWPOS_P 2.0 - param set MC_YAWPOS_I 0.0 - param set MC_YAWPOS_D 0.0 + param set MC_ROLL_P 7.0 + param set MC_ROLLRATE_P 0.12 + param set MC_ROLLRATE_I 0.0 + param set MC_ROLLRATE_D 0.004 + param set MC_PITCH_P 7.0 + param set MC_PITCHRATE_P 0.12 + param set MC_PITCHRATE_I 0.0 + param set MC_PITCHRATE_D 0.004 + param set MC_YAW_P 2.0 param set MC_YAWRATE_P 0.3 param set MC_YAWRATE_I 0.2 param set MC_YAWRATE_D 0.005 diff --git a/ROMFS/px4fmu_common/init.d/4001_quad_x b/ROMFS/px4fmu_common/init.d/4001_quad_x new file mode 100644 index 0000000000..ca4694d81c --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/4001_quad_x @@ -0,0 +1,52 @@ +#!nsh +# +# Generic 10” Quad X geometry +# +# Maintainers: Lorenz Meier , Anton Babushkin +# + +if [ $DO_AUTOCONFIG == yes ] +then + # + # Default parameters for this platform + # + param set MC_ROLL_P 7.0 + param set MC_ROLLRATE_P 0.12 + param set MC_ROLLRATE_I 0.0 + param set MC_ROLLRATE_D 0.004 + param set MC_PITCH_P 7.0 + param set MC_PITCHRATE_P 0.12 + param set MC_PITCHRATE_I 0.0 + param set MC_PITCHRATE_D 0.004 + param set MC_YAW_P 2.0 + param set MC_YAWRATE_P 0.3 + param set MC_YAWRATE_I 0.2 + param set MC_YAWRATE_D 0.005 + + param set MPC_THR_MAX 1.0 + param set MPC_THR_MIN 0.1 + param set MPC_XY_P 1.0 + param set MPC_XY_VEL_P 0.1 + param set MPC_XY_VEL_I 0.02 + param set MPC_XY_VEL_D 0.01 + param set MPC_XY_VEL_MAX 5 + param set MPC_XY_FF 0.5 + param set MPC_Z_P 1.0 + param set MPC_Z_VEL_P 0.1 + param set MPC_Z_VEL_I 0.02 + param set MPC_Z_VEL_D 0.0 + param set MPC_Z_VEL_MAX 3 + param set MPC_Z_FF 0.5 + param set MPC_TILT_MAX 1.0 + param set MPC_LAND_SPEED 1.0 + param set MPC_LAND_TILT 0.3 +fi + +set VEHICLE_TYPE mc +set MIXER FMU_quad_x + +set PWM_OUTPUTS 1234 +set PWM_RATE 400 +set PWM_DISARMED 900 +set PWM_MIN 1000 +set PWM_MAX 2000 diff --git a/ROMFS/px4fmu_common/init.d/4008_ardrone b/ROMFS/px4fmu_common/init.d/4008_ardrone deleted file mode 100644 index f6f09ac220..0000000000 --- a/ROMFS/px4fmu_common/init.d/4008_ardrone +++ /dev/null @@ -1,57 +0,0 @@ -#!nsh - -echo "[init] 08_ardrone: PX4FMU on PX4IOAR carrier board" - -# -# Load default params for this platform -# -if param compare SYS_AUTOCONFIG 1 -then - # Set all params here, then disable autoconfig - param set MC_ATTRATE_D 0 - param set MC_ATTRATE_I 0 - param set MC_ATTRATE_P 0.13 - param set MC_ATT_D 0 - param set MC_ATT_I 0.3 - param set MC_ATT_P 5 - param set MC_YAWPOS_D 0.1 - param set MC_YAWPOS_I 0.15 - param set MC_YAWPOS_P 1 - param set MC_YAWRATE_D 0 - param set MC_YAWRATE_I 0 - param set MC_YAWRATE_P 0.15 - - param set SYS_AUTOCONFIG 0 - param save -fi - -# -# Force some key parameters to sane values -# MAV_TYPE 2 = quadrotor -# -param set MAV_TYPE 2 -param set BAT_V_SCALING 0.008381 - -# -# Start MAVLink -# -mavlink start -d /dev/ttyS0 -b 57600 -usleep 5000 - -# -# Configure PX4FMU for operation with PX4IOAR -# -fmu mode_gpio_serial - -# -# Fire up the AR.Drone interface. -# -ardrone_interface start -d /dev/ttyS1 - -# -# Start common for all multirotors apps -# -sh /etc/init.d/rc.multirotor - -# Exit, because /dev/ttyS0 is needed for MAVLink -exit diff --git a/ROMFS/px4fmu_common/init.d/4009_ardrone_flow b/ROMFS/px4fmu_common/init.d/4009_ardrone_flow deleted file mode 100644 index e2cb8833dc..0000000000 --- a/ROMFS/px4fmu_common/init.d/4009_ardrone_flow +++ /dev/null @@ -1,83 +0,0 @@ -#!nsh - -echo "[init] 09_ardrone_flow: PX4FMU on PX4IOAR carrier board with PX4FLOW" - -# -# Load default params for this platform -# -if param compare SYS_AUTOCONFIG 1 -then - # Set all params here, then disable autoconfig - param set MC_ATTRATE_D 0 - param set MC_ATTRATE_I 0 - param set MC_ATTRATE_P 0.13 - param set MC_ATT_D 0 - param set MC_ATT_I 0.3 - param set MC_ATT_P 5 - param set MC_YAWPOS_D 0.1 - param set MC_YAWPOS_I 0.15 - param set MC_YAWPOS_P 1 - param set MC_YAWRATE_D 0 - param set MC_YAWRATE_I 0 - param set MC_YAWRATE_P 0.15 - - param set SYS_AUTOCONFIG 0 - param save -fi - -# -# Force some key parameters to sane values -# MAV_TYPE 2 = quadrotor -# -param set MAV_TYPE 2 -param set BAT_V_SCALING 0.008381 - -# -# Start MAVLink and MAVLink Onboard (PX4FLOW Sensor) -# -mavlink start -d /dev/ttyS0 -b 57600 -mavlink_onboard start -d /dev/ttyS3 -b 115200 -usleep 5000 - -# -# Configure PX4FMU for operation with PX4IOAR -# -fmu mode_gpio_serial - -# -# Fire up the AR.Drone interface. -# -ardrone_interface start -d /dev/ttyS1 - -# -# Start the sensors. -# -sh /etc/init.d/rc.sensors - -# -# Start the attitude estimator -# -attitude_estimator_ekf start - -# -# Start the position estimator -# -flow_position_estimator start - -# -# Fire up the multi rotor attitude controller -# -mc_att_control_vector start - -# -# Fire up the flow position controller -# -flow_position_control start - -# -# Fire up the flow speed controller -# -flow_speed_control start - -# Exit, because /dev/ttyS0 is needed for MAVLink -exit diff --git a/ROMFS/px4fmu_common/init.d/4010_dji_f330 b/ROMFS/px4fmu_common/init.d/4010_dji_f330 index e0cf92d97f..6acdc9f81d 100644 --- a/ROMFS/px4fmu_common/init.d/4010_dji_f330 +++ b/ROMFS/px4fmu_common/init.d/4010_dji_f330 @@ -10,13 +10,15 @@ then # # Default parameters for this platform # - param set MC_ATT_P 7.0 - param set MC_ATT_I 0.0 + param set MC_ROLL_P 7.0 + param set MC_ROLLRATE_P 0.12 + param set MC_ROLLRATE_I 0.0 + param set MC_ROLLRATE_D 0.004 + param set MC_PITCH_P 7.0 + param set MC_PITCHRATE_P 0.12 + param set MC_PITCHRATE_I 0.0 + param set MC_PITCHRATE_D 0.004 param set MC_YAW_P 2.8 - param set MC_YAW_I 0.0 - param set MC_ATTRATE_P 0.12 - param set MC_ATTRATE_I 0.0 - param set MC_ATTRATE_D 0.004 param set MC_YAWRATE_P 0.2 param set MC_YAWRATE_I 0.05 param set MC_YAWRATE_D 0.0 diff --git a/ROMFS/px4fmu_common/init.d/4011_dji_f450 b/ROMFS/px4fmu_common/init.d/4011_dji_f450 index ced69783db..cca635e661 100644 --- a/ROMFS/px4fmu_common/init.d/4011_dji_f450 +++ b/ROMFS/px4fmu_common/init.d/4011_dji_f450 @@ -10,13 +10,15 @@ then # # Default parameters for this platform # - param set MC_ATT_P 7.0 - param set MC_ATT_I 0.0 + param set MC_ROLL_P 7.0 + param set MC_ROLLRATE_P 0.12 + param set MC_ROLLRATE_I 0.0 + param set MC_ROLLRATE_D 0.004 + param set MC_PITCH_P 7.0 + param set MC_PITCHRATE_P 0.12 + param set MC_PITCHRATE_I 0.0 + param set MC_PITCHRATE_D 0.004 param set MC_YAW_P 2.0 - param set MC_YAW_I 0.0 - param set MC_ATTRATE_P 0.12 - param set MC_ATTRATE_I 0.0 - param set MC_ATTRATE_D 0.004 param set MC_YAWRATE_P 0.3 param set MC_YAWRATE_I 0.2 param set MC_YAWRATE_D 0.005 diff --git a/ROMFS/px4fmu_common/init.d/4012_hk_x550 b/ROMFS/px4fmu_common/init.d/4012_hk_x550 index e1423e0082..4fea828a75 100644 --- a/ROMFS/px4fmu_common/init.d/4012_hk_x550 +++ b/ROMFS/px4fmu_common/init.d/4012_hk_x550 @@ -10,13 +10,15 @@ then # # Default parameters for this platform # - param set MC_ATT_P 5.5 - param set MC_ATT_I 0 + param set MC_ROLL_P 5.5 + param set MC_ROLLRATE_P 0.14 + param set MC_ROLLRATE_I 0 + param set MC_ROLLRATE_D 0.006 + param set MC_PITCH_P 5.5 + param set MC_PITCHRATE_P 0.14 + param set MC_PITCHRATE_I 0 + param set MC_PITCHRATE_D 0.006 param set MC_YAW_P 0.6 - param set MC_YAW_I 0 - param set MC_ATTRATE_P 0.14 - param set MC_ATTRATE_I 0 - param set MC_ATTRATE_D 0.006 param set MC_YAWRATE_P 0.08 param set MC_YAWRATE_I 0 param set MC_YAWRATE_D 0 diff --git a/ROMFS/px4fmu_common/init.d/5001_quad_+ b/ROMFS/px4fmu_common/init.d/5001_quad_+ new file mode 100644 index 0000000000..7f5a6fc078 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/5001_quad_+ @@ -0,0 +1,10 @@ +#!nsh +# +# Generic 10” Quad + geometry +# +# Maintainers: Lorenz Meier , Anton Babushkin +# + +sh /etc/init.d/4001_quad_x + +set MIXER FMU_quad_+ diff --git a/ROMFS/px4fmu_common/init.d/5001_quad_+_pwm b/ROMFS/px4fmu_common/init.d/5001_quad_+_pwm deleted file mode 100644 index 2e5f6ca4f5..0000000000 --- a/ROMFS/px4fmu_common/init.d/5001_quad_+_pwm +++ /dev/null @@ -1,37 +0,0 @@ -#!nsh -# -# Generic 10” Quad + geometry -# -# Maintainers: Lorenz Meier -# - -if [ $DO_AUTOCONFIG == yes ] -then - # - # Default parameters for this platform - # - param set MC_ATT_P 7.0 - param set MC_ATT_I 0.0 - param set MC_ATT_D 0.0 - param set MC_ATTRATE_P 0.12 - param set MC_ATTRATE_I 0.0 - param set MC_ATTRATE_D 0.004 - param set MC_YAWPOS_P 2.0 - param set MC_YAWPOS_I 0.0 - param set MC_YAWPOS_D 0.0 - param set MC_YAWRATE_P 0.3 - param set MC_YAWRATE_I 0.2 - param set MC_YAWRATE_D 0.005 - - # TODO add default MPC parameters -fi - -set VEHICLE_TYPE mc -set MIXER FMU_quad_+ - -set PWM_OUTPUTS 1234 -set PWM_RATE 400 -# DJI ESC range -set PWM_DISARMED 900 -set PWM_MIN 1200 -set PWM_MAX 1900 diff --git a/ROMFS/px4fmu_common/init.d/6001_hexa_x b/ROMFS/px4fmu_common/init.d/6001_hexa_x new file mode 100644 index 0000000000..e72e15dd4d --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/6001_hexa_x @@ -0,0 +1,12 @@ +#!nsh +# +# Generic 10” Hexa X geometry +# +# Maintainers: Lorenz Meier , Anton Babushkin +# + +sh /etc/init.d/4001_quad_x + +set MIXER FMU_hexa_x + +set PWM_OUTPUTS 123456 diff --git a/ROMFS/px4fmu_common/init.d/6001_hexa_x_pwm b/ROMFS/px4fmu_common/init.d/6001_hexa_x_pwm deleted file mode 100644 index ddec8f36ef..0000000000 --- a/ROMFS/px4fmu_common/init.d/6001_hexa_x_pwm +++ /dev/null @@ -1,37 +0,0 @@ -#!nsh -# -# Generic 10” Hexa X geometry -# -# Maintainers: Lorenz Meier -# - -if [ $DO_AUTOCONFIG == yes ] -then - # - # Default parameters for this platform - # - param set MC_ATT_P 7.0 - param set MC_ATT_I 0.0 - param set MC_ATT_D 0.0 - param set MC_ATTRATE_P 0.12 - param set MC_ATTRATE_I 0.0 - param set MC_ATTRATE_D 0.004 - param set MC_YAWPOS_P 2.0 - param set MC_YAWPOS_I 0.0 - param set MC_YAWPOS_D 0.0 - param set MC_YAWRATE_P 0.3 - param set MC_YAWRATE_I 0.2 - param set MC_YAWRATE_D 0.005 - - # TODO add default MPC parameters -fi - -set VEHICLE_TYPE mc -set MIXER FMU_hexa_x - -set PWM_OUTPUTS 1234 -set PWM_RATE 400 -# DJI ESC range -set PWM_DISARMED 900 -set PWM_MIN 1200 -set PWM_MAX 1900 diff --git a/ROMFS/px4fmu_common/init.d/7001_hexa_+ b/ROMFS/px4fmu_common/init.d/7001_hexa_+ new file mode 100644 index 0000000000..dade0630da --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/7001_hexa_+ @@ -0,0 +1,10 @@ +#!nsh +# +# Generic 10” Hexa + geometry +# +# Maintainers: Lorenz Meier , Anton Babushkin +# + +sh /etc/init.d/6001_hexa_x + +set MIXER FMU_hexa_+ diff --git a/ROMFS/px4fmu_common/init.d/7001_hexa_+_pwm b/ROMFS/px4fmu_common/init.d/7001_hexa_+_pwm deleted file mode 100644 index 106e0fb54c..0000000000 --- a/ROMFS/px4fmu_common/init.d/7001_hexa_+_pwm +++ /dev/null @@ -1,37 +0,0 @@ -#!nsh -# -# Generic 10” Hexa + geometry -# -# Maintainers: Lorenz Meier -# - -if [ $DO_AUTOCONFIG == yes ] -then - # - # Default parameters for this platform - # - param set MC_ATT_P 7.0 - param set MC_ATT_I 0.0 - param set MC_ATT_D 0.0 - param set MC_ATTRATE_P 0.12 - param set MC_ATTRATE_I 0.0 - param set MC_ATTRATE_D 0.004 - param set MC_YAWPOS_P 2.0 - param set MC_YAWPOS_I 0.0 - param set MC_YAWPOS_D 0.0 - param set MC_YAWRATE_P 0.3 - param set MC_YAWRATE_I 0.2 - param set MC_YAWRATE_D 0.005 - - # TODO add default MPC parameters -fi - -set VEHICLE_TYPE mc -set MIXER FMU_hexa_+ - -set PWM_OUTPUTS 1234 -set PWM_RATE 400 -# DJI ESC range -set PWM_DISARMED 900 -set PWM_MIN 1200 -set PWM_MAX 1900 diff --git a/ROMFS/px4fmu_common/init.d/8001_octo_x b/ROMFS/px4fmu_common/init.d/8001_octo_x new file mode 100644 index 0000000000..af632ed908 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/8001_octo_x @@ -0,0 +1,12 @@ +#!nsh +# +# Generic 10” Octo X geometry +# +# Maintainers: Lorenz Meier , Anton Babushkin +# + +sh /etc/init.d/4001_quad_x + +set MIXER FMU_octo_x + +set PWM_OUTPUTS 12345678 diff --git a/ROMFS/px4fmu_common/init.d/8001_octo_x_pwm b/ROMFS/px4fmu_common/init.d/8001_octo_x_pwm deleted file mode 100644 index f0eea339b8..0000000000 --- a/ROMFS/px4fmu_common/init.d/8001_octo_x_pwm +++ /dev/null @@ -1,37 +0,0 @@ -#!nsh -# -# Generic 10” Octo X geometry -# -# Maintainers: Lorenz Meier -# - -if [ $DO_AUTOCONFIG == yes ] -then - # - # Default parameters for this platform - # - param set MC_ATT_P 7.0 - param set MC_ATT_I 0.0 - param set MC_ATT_D 0.0 - param set MC_ATTRATE_P 0.12 - param set MC_ATTRATE_I 0.0 - param set MC_ATTRATE_D 0.004 - param set MC_YAWPOS_P 2.0 - param set MC_YAWPOS_I 0.0 - param set MC_YAWPOS_D 0.0 - param set MC_YAWRATE_P 0.3 - param set MC_YAWRATE_I 0.2 - param set MC_YAWRATE_D 0.005 - - # TODO add default MPC parameters -fi - -set VEHICLE_TYPE mc -set MIXER FMU_octo_x - -set PWM_OUTPUTS 1234 -set PWM_RATE 400 -# DJI ESC range -set PWM_DISARMED 900 -set PWM_MIN 1200 -set PWM_MAX 1900 diff --git a/ROMFS/px4fmu_common/init.d/9001_octo_+ b/ROMFS/px4fmu_common/init.d/9001_octo_+ new file mode 100644 index 0000000000..9bf5e09328 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/9001_octo_+ @@ -0,0 +1,10 @@ +#!nsh +# +# Generic 10” Octo + geometry +# +# Maintainers: Lorenz Meier , Anton Babushkin +# + +sh /etc/init.d/8001_octo_x + +set MIXER FMU_octo_+ diff --git a/ROMFS/px4fmu_common/init.d/9001_octo_+_pwm b/ROMFS/px4fmu_common/init.d/9001_octo_+_pwm deleted file mode 100644 index 992a7aeba6..0000000000 --- a/ROMFS/px4fmu_common/init.d/9001_octo_+_pwm +++ /dev/null @@ -1,37 +0,0 @@ -#!nsh -# -# Generic 10” Octo + geometry -# -# Maintainers: Lorenz Meier -# - -if [ $DO_AUTOCONFIG == yes ] -then - # - # Default parameters for this platform - # - param set MC_ATT_P 7.0 - param set MC_ATT_I 0.0 - param set MC_ATT_D 0.0 - param set MC_ATTRATE_P 0.12 - param set MC_ATTRATE_I 0.0 - param set MC_ATTRATE_D 0.004 - param set MC_YAWPOS_P 2.0 - param set MC_YAWPOS_I 0.0 - param set MC_YAWPOS_D 0.0 - param set MC_YAWRATE_P 0.3 - param set MC_YAWRATE_I 0.2 - param set MC_YAWRATE_D 0.005 - - # TODO add default MPC parameters -fi - -set VEHICLE_TYPE mc -set MIXER FMU_octo_+ - -set PWM_OUTPUTS 1234 -set PWM_RATE 400 -# DJI ESC range -set PWM_DISARMED 900 -set PWM_MIN 1200 -set PWM_MAX 1900 diff --git a/ROMFS/px4fmu_common/init.d/rc.autostart b/ROMFS/px4fmu_common/init.d/rc.autostart index 34da2dfef2..030806fd7e 100644 --- a/ROMFS/px4fmu_common/init.d/rc.autostart +++ b/ROMFS/px4fmu_common/init.d/rc.autostart @@ -101,14 +101,9 @@ fi # Quad X # -if param compare SYS_AUTOSTART 4008 8 +if param compare SYS_AUTOSTART 4001 then - #sh /etc/init.d/4008_ardrone -fi - -if param compare SYS_AUTOSTART 4009 9 -then - #sh /etc/init.d/4009_ardrone_flow + sh /etc/init.d/4001_quad_x fi if param compare SYS_AUTOSTART 4010 10 @@ -132,7 +127,7 @@ fi if param compare SYS_AUTOSTART 5001 then - sh /etc/init.d/5001_quad_+_pwm + sh /etc/init.d/5001_quad_+ fi # @@ -141,7 +136,7 @@ fi if param compare SYS_AUTOSTART 6001 then - sh /etc/init.d/6001_hexa_x_pwm + sh /etc/init.d/6001_hexa_x fi # @@ -150,7 +145,7 @@ fi if param compare SYS_AUTOSTART 7001 then - sh /etc/init.d/7001_hexa_+_pwm + sh /etc/init.d/7001_hexa_+ fi # @@ -159,7 +154,7 @@ fi if param compare SYS_AUTOSTART 8001 then - sh /etc/init.d/8001_octo_x_pwm + sh /etc/init.d/8001_octo_x fi # @@ -168,7 +163,7 @@ fi if param compare SYS_AUTOSTART 9001 then - sh /etc/init.d/9001_octo_+_pwm + sh /etc/init.d/9001_octo_+ fi # @@ -191,5 +186,5 @@ fi if param compare SYS_AUTOSTART 12001 then - sh /etc/init.d/12001_octo_cox_pwm + sh /etc/init.d/12001_octo_cox fi From 9f88cbdc0b850f4136fd68a379663619fb42e150 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 31 Jan 2014 12:18:52 +0100 Subject: [PATCH 380/447] autostart scripts: default MPC parameters updated --- .../px4fmu_common/init.d/10015_tbs_discovery | 18 +++++++++++ ROMFS/px4fmu_common/init.d/10016_3dr_iris | 18 ++++++----- ROMFS/px4fmu_common/init.d/12001_octo_cox | 31 ++----------------- ROMFS/px4fmu_common/init.d/4010_dji_f330 | 18 ++++++----- ROMFS/px4fmu_common/init.d/4011_dji_f450 | 18 ++++++++++- 5 files changed, 59 insertions(+), 44 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/10015_tbs_discovery b/ROMFS/px4fmu_common/init.d/10015_tbs_discovery index 49a50e1309..3a8f14a9ed 100644 --- a/ROMFS/px4fmu_common/init.d/10015_tbs_discovery +++ b/ROMFS/px4fmu_common/init.d/10015_tbs_discovery @@ -22,6 +22,24 @@ then param set MC_YAWRATE_P 0.2 param set MC_YAWRATE_I 0.0 param set MC_YAWRATE_D 0.0 + + param set MPC_THR_MAX 1.0 + param set MPC_THR_MIN 0.1 + param set MPC_XY_P 1.0 + param set MPC_XY_VEL_P 0.1 + param set MPC_XY_VEL_I 0.02 + param set MPC_XY_VEL_D 0.01 + param set MPC_XY_VEL_MAX 5 + param set MPC_XY_FF 0.5 + param set MPC_Z_P 1.0 + param set MPC_Z_VEL_P 0.1 + param set MPC_Z_VEL_I 0.02 + param set MPC_Z_VEL_D 0.0 + param set MPC_Z_VEL_MAX 3 + param set MPC_Z_FF 0.5 + param set MPC_TILT_MAX 1.0 + param set MPC_LAND_SPEED 1.0 + param set MPC_LAND_TILT 0.3 fi set VEHICLE_TYPE mc diff --git a/ROMFS/px4fmu_common/init.d/10016_3dr_iris b/ROMFS/px4fmu_common/init.d/10016_3dr_iris index 6c9cb6983a..2ce0334b4c 100644 --- a/ROMFS/px4fmu_common/init.d/10016_3dr_iris +++ b/ROMFS/px4fmu_common/init.d/10016_3dr_iris @@ -23,19 +23,23 @@ then param set MC_YAWRATE_I 0.0 param set MC_YAWRATE_D 0.0 - param set MPC_TILT_MAX 1.0 param set MPC_THR_MAX 1.0 param set MPC_THR_MIN 0.1 param set MPC_XY_P 1.0 - param set MPC_XY_VEL_D 0.01 - param set MPC_XY_VEL_I 0.02 - param set MPC_XY_VEL_MAX 5 param set MPC_XY_VEL_P 0.1 + param set MPC_XY_VEL_I 0.02 + param set MPC_XY_VEL_D 0.01 + param set MPC_XY_VEL_MAX 5 + param set MPC_XY_FF 0.5 param set MPC_Z_P 1.0 - param set MPC_Z_VEL_D 0.0 - param set MPC_Z_VEL_I 0.02 - param set MPC_Z_VEL_MAX 3 param set MPC_Z_VEL_P 0.1 + param set MPC_Z_VEL_I 0.02 + param set MPC_Z_VEL_D 0.0 + param set MPC_Z_VEL_MAX 3 + param set MPC_Z_FF 0.5 + param set MPC_TILT_MAX 1.0 + param set MPC_LAND_SPEED 1.0 + param set MPC_LAND_TILT 0.3 param set BAT_V_SCALING 0.00989 param set BAT_C_SCALING 0.0124 diff --git a/ROMFS/px4fmu_common/init.d/12001_octo_cox b/ROMFS/px4fmu_common/init.d/12001_octo_cox index 2bea6d4899..77813268a6 100644 --- a/ROMFS/px4fmu_common/init.d/12001_octo_cox +++ b/ROMFS/px4fmu_common/init.d/12001_octo_cox @@ -2,36 +2,9 @@ # # Generic 10” Octo coaxial geometry # -# Maintainers: Lorenz Meier +# Maintainers: Lorenz Meier , Anton Babushkin # -if [ $DO_AUTOCONFIG == yes ] -then - # - # Default parameters for this platform - # - param set MC_ROLL_P 7.0 - param set MC_ROLLRATE_P 0.12 - param set MC_ROLLRATE_I 0.0 - param set MC_ROLLRATE_D 0.004 - param set MC_PITCH_P 7.0 - param set MC_PITCHRATE_P 0.12 - param set MC_PITCHRATE_I 0.0 - param set MC_PITCHRATE_D 0.004 - param set MC_YAW_P 2.0 - param set MC_YAWRATE_P 0.3 - param set MC_YAWRATE_I 0.2 - param set MC_YAWRATE_D 0.005 - - # TODO add default MPC parameters -fi +sh /etc/init.d/8001_octo_x -set VEHICLE_TYPE mc set MIXER FMU_octo_cox - -set PWM_OUTPUTS 1234 -set PWM_RATE 400 -# DJI ESC range -set PWM_DISARMED 900 -set PWM_MIN 1200 -set PWM_MAX 1900 diff --git a/ROMFS/px4fmu_common/init.d/4010_dji_f330 b/ROMFS/px4fmu_common/init.d/4010_dji_f330 index 6acdc9f81d..c78da2d6c5 100644 --- a/ROMFS/px4fmu_common/init.d/4010_dji_f330 +++ b/ROMFS/px4fmu_common/init.d/4010_dji_f330 @@ -23,19 +23,23 @@ then param set MC_YAWRATE_I 0.05 param set MC_YAWRATE_D 0.0 - param set MPC_TILT_MAX 1.0 param set MPC_THR_MAX 1.0 param set MPC_THR_MIN 0.1 param set MPC_XY_P 1.0 - param set MPC_XY_VEL_D 0.01 - param set MPC_XY_VEL_I 0.02 - param set MPC_XY_VEL_MAX 5 param set MPC_XY_VEL_P 0.1 + param set MPC_XY_VEL_I 0.02 + param set MPC_XY_VEL_D 0.01 + param set MPC_XY_VEL_MAX 5 + param set MPC_XY_FF 0.5 param set MPC_Z_P 1.0 - param set MPC_Z_VEL_D 0.0 - param set MPC_Z_VEL_I 0.02 - param set MPC_Z_VEL_MAX 3 param set MPC_Z_VEL_P 0.1 + param set MPC_Z_VEL_I 0.02 + param set MPC_Z_VEL_D 0.0 + param set MPC_Z_VEL_MAX 3 + param set MPC_Z_FF 0.5 + param set MPC_TILT_MAX 1.0 + param set MPC_LAND_SPEED 1.0 + param set MPC_LAND_TILT 0.3 fi set VEHICLE_TYPE mc diff --git a/ROMFS/px4fmu_common/init.d/4011_dji_f450 b/ROMFS/px4fmu_common/init.d/4011_dji_f450 index cca635e661..8027b9d42a 100644 --- a/ROMFS/px4fmu_common/init.d/4011_dji_f450 +++ b/ROMFS/px4fmu_common/init.d/4011_dji_f450 @@ -23,7 +23,23 @@ then param set MC_YAWRATE_I 0.2 param set MC_YAWRATE_D 0.005 - # TODO add default MPC parameters + param set MPC_THR_MAX 1.0 + param set MPC_THR_MIN 0.1 + param set MPC_XY_P 1.0 + param set MPC_XY_VEL_P 0.1 + param set MPC_XY_VEL_I 0.02 + param set MPC_XY_VEL_D 0.01 + param set MPC_XY_VEL_MAX 5 + param set MPC_XY_FF 0.5 + param set MPC_Z_P 1.0 + param set MPC_Z_VEL_P 0.1 + param set MPC_Z_VEL_I 0.02 + param set MPC_Z_VEL_D 0.0 + param set MPC_Z_VEL_MAX 3 + param set MPC_Z_FF 0.5 + param set MPC_TILT_MAX 1.0 + param set MPC_LAND_SPEED 1.0 + param set MPC_LAND_TILT 0.3 fi set VEHICLE_TYPE mc From 5a7a356c2038544efbde562398aa5a50cb7e19d9 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 31 Jan 2014 12:24:29 +0100 Subject: [PATCH 381/447] autostart 4012_hk_x550 script updated --- ROMFS/px4fmu_common/init.d/4012_hk_x550 | 21 ++++++++++++++++++++- 1 file changed, 20 insertions(+), 1 deletion(-) diff --git a/ROMFS/px4fmu_common/init.d/4012_hk_x550 b/ROMFS/px4fmu_common/init.d/4012_hk_x550 index 4fea828a75..98e1f80aa4 100644 --- a/ROMFS/px4fmu_common/init.d/4012_hk_x550 +++ b/ROMFS/px4fmu_common/init.d/4012_hk_x550 @@ -23,7 +23,23 @@ then param set MC_YAWRATE_I 0 param set MC_YAWRATE_D 0 - # TODO add default MPC parameters + param set MPC_THR_MAX 1.0 + param set MPC_THR_MIN 0.1 + param set MPC_XY_P 1.0 + param set MPC_XY_VEL_P 0.1 + param set MPC_XY_VEL_I 0.02 + param set MPC_XY_VEL_D 0.01 + param set MPC_XY_VEL_MAX 5 + param set MPC_XY_FF 0.5 + param set MPC_Z_P 1.0 + param set MPC_Z_VEL_P 0.1 + param set MPC_Z_VEL_I 0.02 + param set MPC_Z_VEL_D 0.0 + param set MPC_Z_VEL_MAX 3 + param set MPC_Z_FF 0.5 + param set MPC_TILT_MAX 1.0 + param set MPC_LAND_SPEED 1.0 + param set MPC_LAND_TILT 0.3 fi set VEHICLE_TYPE mc @@ -31,3 +47,6 @@ set MIXER FMU_quad_x set PWM_OUTPUTS 1234 set PWM_RATE 400 +set PWM_DISARMED 900 +set PWM_MIN 1000 +set PWM_MAX 2000 From d933d523eb74ee2290c56afcd11fe8e85c6e702b Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 31 Jan 2014 16:46:54 +0100 Subject: [PATCH 382/447] mc_att_control: att rate integral fix --- src/modules/mc_att_control/mc_att_control_main.cpp | 12 +++++++----- 1 file changed, 7 insertions(+), 5 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 4086a227a0..a0accb8552 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -654,13 +654,15 @@ MulticopterAttitudeControl::control_attitude_rates(float dt) _rates_prev = rates; /* update integral only if not saturated on low limit */ - if (_thrust_sp > 0.1f && _att_control.length() < _thrust_sp) { + if (_thrust_sp > 0.1f) { for (int i = 0; i < 3; i++) { - float rate_i = _rates_int(i) + _params.rate_i(i) * rates_err(i) * dt; + if (fabsf(_att_control(i)) < _thrust_sp) { + float rate_i = _rates_int(i) + _params.rate_i(i) * rates_err(i) * dt; - if (isfinite(rate_i) && rate_i > -RATES_I_LIMIT && rate_i < RATES_I_LIMIT && - _att_control(i) > -RATES_I_LIMIT && _att_control(i) < RATES_I_LIMIT) { - _rates_int(i) = rate_i; + if (isfinite(rate_i) && rate_i > -RATES_I_LIMIT && rate_i < RATES_I_LIMIT && + _att_control(i) > -RATES_I_LIMIT && _att_control(i) < RATES_I_LIMIT) { + _rates_int(i) = rate_i; + } } } } From 0be7bd3166969294bdd56b853b65248442219b80 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 1 Feb 2014 00:07:50 +0100 Subject: [PATCH 383/447] mc_pos_control: max position setpoint offset limiting fixed --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 2 +- 1 file changed, 1 insertion(+), 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 e9b92119d6..057cb051d8 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -645,7 +645,7 @@ MulticopterPositionControl::task_main() if (pos_sp_offs_norm > 1.0f) { pos_sp_offs /= pos_sp_offs_norm; - add_vector_to_global_position(_lat_sp, _lon_sp, pos_sp_offs(0) * _params.sp_offs_max(0), pos_sp_offs(1) * _params.sp_offs_max(1), &_lat_sp, &_lon_sp); + add_vector_to_global_position(_global_pos.lat, _global_pos.lon, pos_sp_offs(0) * _params.sp_offs_max(0), pos_sp_offs(1) * _params.sp_offs_max(1), &_lat_sp, &_lon_sp); _alt_sp = alt - pos_sp_offs(2) * _params.sp_offs_max(2); } From bb8be966fcaa484c0f8209da41760d4bc24d6c5f Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 1 Feb 2014 10:20:48 +0100 Subject: [PATCH 384/447] mc_pos_control: more safe tilt limiting --- .../mc_pos_control/mc_pos_control_main.cpp | 44 +++++++++++-------- 1 file changed, 26 insertions(+), 18 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 057cb051d8..3194534b9a 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -839,30 +839,38 @@ MulticopterPositionControl::task_main() thr_min = 0.0f; } + float tilt_max = _params.tilt_max; + + /* adjust limits for landing mode */ + if (_control_mode.flag_control_auto_enabled && _pos_sp_triplet.current.valid && + _pos_sp_triplet.current.type == SETPOINT_TYPE_LAND) { + /* limit max tilt and min lift when landing */ + tilt_max = _params.land_tilt_max; + if (thr_min < 0.0f) + thr_min = 0.0f; + } + + /* limit min lift */ if (-thrust_sp(2) < thr_min) { thrust_sp(2) = -thr_min; saturation_z = true; } - /* limit max tilt */ - float tilt = atan2f(math::Vector<2>(thrust_sp(0), thrust_sp(1)).length(), -thrust_sp(2)); - float tilt_max = _params.tilt_max; - if (!_control_mode.flag_control_manual_enabled) { - if (_pos_sp_triplet.current.valid && _pos_sp_triplet.current.type == SETPOINT_TYPE_LAND) { - /* limit max tilt and min lift when landing */ - tilt_max = _params.land_tilt_max; - if (thr_min < 0.0f) - thr_min = 0.0f; - } - } - if (_control_mode.flag_control_velocity_enabled) { - if (tilt > tilt_max && thr_min >= 0.0f) { - /* crop horizontal component */ - float k = tanf(tilt_max) / tanf(tilt); - thrust_sp(0) *= k; - thrust_sp(1) *= k; - saturation_xy = true; + /* limit max tilt */ + if (thr_min >= 0.0f && tilt_max < M_PI / 2 - 0.05f) { + /* absolute horizontal thrust */ + float thrust_sp_xy_len = math::Vector<2>(thrust_sp(0), thrust_sp(1)).length(); + if (thrust_sp_xy_len > 0.01f) { + /* max horizontal thrust for given vertical thrust*/ + float thrust_xy_max = -thrust_sp(2) * tanf(tilt_max); + if (thrust_sp_xy_len > thrust_xy_max) { + float k = thrust_xy_max / thrust_sp_xy_len; + thrust_sp(0) *= k; + thrust_sp(1) *= k; + saturation_xy = true; + } + } } } else { /* thrust compensation for altitude only control mode */ From 8a203951594282a297b2af402a82b85f0f927619 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 1 Feb 2014 10:29:51 +0100 Subject: [PATCH 385/447] mc_pos_control: fixed yaw setpoint in AUTO --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 8 +++++++- 1 file changed, 7 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 3194534b9a..0d65b5b03c 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -688,10 +688,16 @@ MulticopterPositionControl::task_main() _reset_lat_lon_sp = true; _reset_alt_sp = true; + /* update position setpoint */ _lat_sp = _pos_sp_triplet.current.lat; _lon_sp = _pos_sp_triplet.current.lon; _alt_sp = _pos_sp_triplet.current.alt; + /* update yaw setpoint if needed */ + if (isfinite(_pos_sp_triplet.current.yaw)) { + _att_sp.yaw_body = _pos_sp_triplet.current.yaw; + } + } else { /* no waypoint, loiter, reset position setpoint if needed */ reset_lat_lon_sp(); @@ -711,7 +717,7 @@ MulticopterPositionControl::task_main() _att_sp.roll_body = 0.0f; _att_sp.pitch_body = 0.0f; - _att_sp.yaw_body = 0.0f; + _att_sp.yaw_body = _att.yaw; _att_sp.thrust = 0.0f; _att_sp.timestamp = hrt_absolute_time(); From f835980b468fe44c49051df0b58f830bafb256f5 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 1 Feb 2014 10:32:46 +0100 Subject: [PATCH 386/447] mc_pos_control: more correct control flags usage --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 12 ++++-------- 1 file changed, 4 insertions(+), 8 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 0d65b5b03c..4fb9bd663e 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -672,7 +672,7 @@ MulticopterPositionControl::task_main() _pos_sp_triplet_pub = orb_advertise(ORB_ID(position_setpoint_triplet), &_pos_sp_triplet); } - } else if (_control_mode.flag_control_auto_enabled) { + } else { /* always use AMSL altitude for AUTO */ select_alt(true); @@ -703,13 +703,9 @@ MulticopterPositionControl::task_main() reset_lat_lon_sp(); reset_alt_sp(); } - } else { - /* no control, reset setpoint */ - reset_lat_lon_sp(); - reset_alt_sp(); } - if (_control_mode.flag_control_auto_enabled && _pos_sp_triplet.current.valid && _pos_sp_triplet.current.type == SETPOINT_TYPE_IDLE) { + if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid && _pos_sp_triplet.current.type == SETPOINT_TYPE_IDLE) { /* idle state, don't run controller and set zero thrust */ R.identity(); memcpy(&_att_sp.R_body[0][0], R.data, sizeof(_att_sp.R_body)); @@ -751,7 +747,7 @@ MulticopterPositionControl::task_main() } /* use constant descend rate when landing, ignore altitude setpoint */ - if (_control_mode.flag_control_auto_enabled && _pos_sp_triplet.current.valid && _pos_sp_triplet.current.type == SETPOINT_TYPE_LAND) { + if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid && _pos_sp_triplet.current.type == SETPOINT_TYPE_LAND) { _vel_sp(2) = _params.land_speed; } @@ -848,7 +844,7 @@ MulticopterPositionControl::task_main() float tilt_max = _params.tilt_max; /* adjust limits for landing mode */ - if (_control_mode.flag_control_auto_enabled && _pos_sp_triplet.current.valid && + if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid && _pos_sp_triplet.current.type == SETPOINT_TYPE_LAND) { /* limit max tilt and min lift when landing */ tilt_max = _params.land_tilt_max; From 8897894b19e8de4ad7960a0fa552ed12fc2f0200 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 1 Feb 2014 11:14:21 +0100 Subject: [PATCH 387/447] commander, navigator, mc_att_control, mc_pos_control: code style fixed --- src/modules/commander/commander.cpp | 2 + .../commander/state_machine_helper.cpp | 2 + .../mc_att_control/mc_att_control_main.cpp | 6 ++- .../mc_pos_control/mc_pos_control_main.cpp | 11 ++++- src/modules/navigator/navigator_main.cpp | 5 ++- src/modules/navigator/navigator_mission.cpp | 44 ++++++++++++------- 6 files changed, 50 insertions(+), 20 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index c39833713d..e9da69232d 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -369,6 +369,7 @@ int arm() if (arming_res == TRANSITION_CHANGED) { mavlink_log_info(mavlink_fd, "[cmd] ARMED by commandline"); return 0; + } else { return 1; } @@ -381,6 +382,7 @@ int disarm() if (arming_res == TRANSITION_CHANGED) { mavlink_log_info(mavlink_fd, "[cmd] ARMED by commandline"); return 0; + } else { return 1; } diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 43d0e023e3..e5d77b246a 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -382,6 +382,7 @@ transition_result_t failsafe_state_transition(struct vehicle_status_s *status, f break; case FAILSAFE_STATE_RTL: + /* global position and home position required for RTL */ if (status->condition_global_position_valid && status->condition_home_position_valid) { status->set_nav_state = NAV_STATE_RTL; @@ -392,6 +393,7 @@ transition_result_t failsafe_state_transition(struct vehicle_status_s *status, f break; case FAILSAFE_STATE_LAND: + /* at least relative altitude estimate required for landing */ if (status->condition_local_altitude_valid || status->condition_global_position_valid) { status->set_nav_state = NAV_STATE_LAND; 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 a0accb8552..db5e2e9bb0 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -489,14 +489,18 @@ MulticopterAttitudeControl::control_attitude(float dt) //} } else { float yaw_dz_scaled = YAW_DEADZONE * _params.rc_scale_yaw; + if (_params.rc_scale_yaw > 0.001f && fabs(_manual_control_sp.yaw) > yaw_dz_scaled) { /* move yaw setpoint */ yaw_sp_move_rate = _manual_control_sp.yaw / _params.rc_scale_yaw; + if (_manual_control_sp.yaw > 0.0f) { yaw_sp_move_rate -= YAW_DEADZONE; + } else { yaw_sp_move_rate += YAW_DEADZONE; } + yaw_sp_move_rate *= _params.rc_scale_yaw; _v_att_sp.yaw_body = _wrap_pi(_v_att_sp.yaw_body + yaw_sp_move_rate * dt); _v_att_sp.R_valid = false; @@ -660,7 +664,7 @@ MulticopterAttitudeControl::control_attitude_rates(float dt) float rate_i = _rates_int(i) + _params.rate_i(i) * rates_err(i) * dt; if (isfinite(rate_i) && rate_i > -RATES_I_LIMIT && rate_i < RATES_I_LIMIT && - _att_control(i) > -RATES_I_LIMIT && _att_control(i) < RATES_I_LIMIT) { + _att_control(i) > -RATES_I_LIMIT && _att_control(i) < RATES_I_LIMIT) { _rates_int(i) = rate_i; } } 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 4fb9bd663e..25d34c872d 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -479,9 +479,11 @@ MulticopterPositionControl::select_alt(bool global) { if (global != _use_global_alt) { _use_global_alt = global; + if (global) { /* switch from barometric to global altitude */ _alt_sp += _global_pos.alt - _global_pos.baro_alt; + } else { /* switch from global to barometric altitude */ _alt_sp += _global_pos.baro_alt - _global_pos.alt; @@ -589,6 +591,7 @@ MulticopterPositionControl::task_main() if (_control_mode.flag_control_manual_enabled) { /* select altitude source and update setpoint */ select_alt(_global_pos.global_valid); + if (!_use_global_alt) { alt = _global_pos.baro_alt; } @@ -845,9 +848,10 @@ MulticopterPositionControl::task_main() /* adjust limits for landing mode */ if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid && - _pos_sp_triplet.current.type == SETPOINT_TYPE_LAND) { + _pos_sp_triplet.current.type == SETPOINT_TYPE_LAND) { /* limit max tilt and min lift when landing */ tilt_max = _params.land_tilt_max; + if (thr_min < 0.0f) thr_min = 0.0f; } @@ -863,9 +867,11 @@ MulticopterPositionControl::task_main() if (thr_min >= 0.0f && tilt_max < M_PI / 2 - 0.05f) { /* absolute horizontal thrust */ float thrust_sp_xy_len = math::Vector<2>(thrust_sp(0), thrust_sp(1)).length(); + if (thrust_sp_xy_len > 0.01f) { /* max horizontal thrust for given vertical thrust*/ float thrust_xy_max = -thrust_sp(2) * tanf(tilt_max); + if (thrust_sp_xy_len > thrust_xy_max) { float k = thrust_xy_max / thrust_sp_xy_len; thrust_sp(0) *= k; @@ -874,15 +880,18 @@ MulticopterPositionControl::task_main() } } } + } else { /* thrust compensation for altitude only control mode */ float att_comp; if (_att.R[2][2] > TILT_COS_MAX) { att_comp = 1.0f / _att.R[2][2]; + } else if (_att.R[2][2] > 0.0f) { att_comp = ((1.0f / TILT_COS_MAX - 1.0f) / TILT_COS_MAX) * _att.R[2][2] + 1.0f; saturation_z = true; + } else { att_comp = 1.0f; saturation_z = true; diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index e961a8f942..6e4b5f0a06 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -689,7 +689,7 @@ Navigator::task_main() if (_vstatus.return_switch == RETURN_SWITCH_RETURN) { /* switch to RTL if not already landed after RTL and home position set */ if (!(_rtl_state == RTL_STATE_DESCEND && (myState == NAV_STATE_READY || myState == NAV_STATE_LAND)) && - _vstatus.condition_home_position_valid) { + _vstatus.condition_home_position_valid) { dispatch(EVENT_RTL_REQUESTED); } @@ -747,7 +747,7 @@ Navigator::task_main() case NAV_STATE_RTL: if (!(_rtl_state == RTL_STATE_DESCEND && (myState == NAV_STATE_READY || myState == NAV_STATE_LAND)) && - _vstatus.condition_home_position_valid) { + _vstatus.condition_home_position_valid) { dispatch(EVENT_RTL_REQUESTED); } @@ -1575,6 +1575,7 @@ Navigator::on_mission_item_reached() if (_rtl_state == RTL_STATE_DESCEND) { /* hovering above home position, land if needed or loiter */ mavlink_log_info(_mavlink_fd, "[navigator] RTL completed"); + if (_mission_item.autocontinue) { dispatch(EVENT_LAND_REQUESTED); diff --git a/src/modules/navigator/navigator_mission.cpp b/src/modules/navigator/navigator_mission.cpp index 6576aae707..e72caf98e9 100644 --- a/src/modules/navigator/navigator_mission.cpp +++ b/src/modules/navigator/navigator_mission.cpp @@ -52,8 +52,8 @@ static const int ERROR = -1; -Mission::Mission() : - +Mission::Mission() : + _offboard_dataman_id(-1), _current_offboard_mission_index(0), _current_onboard_mission_index(0), @@ -65,7 +65,7 @@ Mission::Mission() : Mission::~Mission() { - + } void @@ -126,33 +126,39 @@ Mission::get_current_mission_item(struct mission_item_s *new_mission_item, bool { /* try onboard mission first */ if (current_onboard_mission_available()) { - + const ssize_t len = sizeof(struct mission_item_s); + if (dm_read(DM_KEY_WAYPOINTS_ONBOARD, _current_onboard_mission_index, new_mission_item, len) != len) { /* not supposed to happen unless the datamanager can't access the SD card, etc. */ return ERROR; } + _current_mission_type = MISSION_TYPE_ONBOARD; *onboard = true; *index = _current_onboard_mission_index; - /* otherwise fallback to offboard */ + /* otherwise fallback to offboard */ + } else if (current_offboard_mission_available()) { dm_item_t dm_current; if (_offboard_dataman_id == 0) { dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0; + } else { dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1; } const ssize_t len = sizeof(struct mission_item_s); + if (dm_read(dm_current, _current_offboard_mission_index, new_mission_item, len) != len) { /* not supposed to happen unless the datamanager can't access the SD card, etc. */ _current_mission_type = MISSION_TYPE_NONE; return ERROR; } + _current_mission_type = MISSION_TYPE_OFFBOARD; *onboard = false; *index = _current_offboard_mission_index; @@ -171,25 +177,29 @@ Mission::get_next_mission_item(struct mission_item_s *new_mission_item) { /* try onboard mission first */ if (next_onboard_mission_available()) { - + const ssize_t len = sizeof(struct mission_item_s); + if (dm_read(DM_KEY_WAYPOINTS_ONBOARD, _current_onboard_mission_index + 1, new_mission_item, len) != len) { /* not supposed to happen unless the datamanager can't access the SD card, etc. */ return ERROR; } - /* otherwise fallback to offboard */ + /* otherwise fallback to offboard */ + } else if (next_offboard_mission_available()) { dm_item_t dm_current; if (_offboard_dataman_id == 0) { dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0; + } else { dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1; } const ssize_t len = sizeof(struct mission_item_s); + if (dm_read(dm_current, _current_offboard_mission_index + 1, new_mission_item, len) != len) { /* not supposed to happen unless the datamanager can't access the SD card, etc. */ return ERROR; @@ -244,14 +254,16 @@ void Mission::move_to_next() { switch (_current_mission_type) { - case MISSION_TYPE_ONBOARD: - _current_onboard_mission_index++; - break; - case MISSION_TYPE_OFFBOARD: - _current_offboard_mission_index++; - break; - case MISSION_TYPE_NONE: - default: - break; + case MISSION_TYPE_ONBOARD: + _current_onboard_mission_index++; + break; + + case MISSION_TYPE_OFFBOARD: + _current_offboard_mission_index++; + break; + + case MISSION_TYPE_NONE: + default: + break; } } \ No newline at end of file From e8dd70e815033ec1a291ac5190dacfeb7d728dba Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 1 Feb 2014 11:58:34 +0100 Subject: [PATCH 388/447] fw: fix global position lat/lon read-in (lat/lon have changed to double lately) --- .../fw_pos_control_l1/fw_pos_control_l1_main.cpp | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index a62b53221c..3ef1871a8b 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -785,26 +785,26 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi _tecs.set_speed_weight(_parameters.speed_weight); /* current waypoint (the one currently heading for) */ - math::Vector<2> next_wp(pos_sp_triplet.current.lat, pos_sp_triplet.current.lon); + math::Vector<2> next_wp((float)pos_sp_triplet.current.lat, (float)pos_sp_triplet.current.lon); /* current waypoint (the one currently heading for) */ - math::Vector<2> curr_wp(pos_sp_triplet.current.lat, pos_sp_triplet.current.lon); + math::Vector<2> curr_wp((float)pos_sp_triplet.current.lat, (float)pos_sp_triplet.current.lon); /* previous waypoint */ math::Vector<2> prev_wp; if (pos_sp_triplet.previous.valid) { - prev_wp(0) = pos_sp_triplet.previous.lat; - prev_wp(1) = pos_sp_triplet.previous.lon; + prev_wp(0) = (float)pos_sp_triplet.previous.lat; + prev_wp(1) = (float)pos_sp_triplet.previous.lon; } else { /* * No valid previous waypoint, go for the current wp. * This is automatically handled by the L1 library. */ - prev_wp(0) = pos_sp_triplet.current.lat; - prev_wp(1) = pos_sp_triplet.current.lon; + prev_wp(0) = (float)pos_sp_triplet.current.lat; + prev_wp(1) = (float)pos_sp_triplet.current.lon; } @@ -1263,7 +1263,7 @@ FixedwingPositionControl::task_main() // vehicle_baro_poll(); math::Vector<2> ground_speed(_global_pos.vel_n, _global_pos.vel_e); - math::Vector<2> current_position(_global_pos.lat / 1e7f, _global_pos.lon / 1e7f); + math::Vector<2> current_position((float)_global_pos.lat, (float)_global_pos.lon); /* * Attempt to control position, on success (= sensors present and not in manual mode), From 589f6cdb44642f4b12aef9deffc8af11fa53b026 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 1 Feb 2014 11:59:39 +0100 Subject: [PATCH 389/447] navigator: in start_loiter set _pos_sp_triplet.current.type to SETPOINT_TYPE_LOITER instead of SETPOINT_TYPE_NORMAL --- src/modules/navigator/navigator_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index e961a8f942..dace00ec5b 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -1078,7 +1078,7 @@ Navigator::start_loiter() mavlink_log_info(_mavlink_fd, "[navigator] loiter at current altitude"); } - _pos_sp_triplet.current.type = SETPOINT_TYPE_NORMAL; + _pos_sp_triplet.current.type = SETPOINT_TYPE_LOITER; } _pos_sp_triplet.current.loiter_radius = _parameters.loiter_radius; From c2911cbecf06b18d581fd3e5407a1f525bd63d4b Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 1 Feb 2014 12:32:03 +0100 Subject: [PATCH 390/447] add malolo (flightgear HIL flying wing) autostart config --- .../init.d/1005_rc_fw_Malolo1.hil | 61 +++++++++++++++++++ ROMFS/px4fmu_common/init.d/rc.autostart | 5 ++ 2 files changed, 66 insertions(+) create mode 100644 ROMFS/px4fmu_common/init.d/1005_rc_fw_Malolo1.hil diff --git a/ROMFS/px4fmu_common/init.d/1005_rc_fw_Malolo1.hil b/ROMFS/px4fmu_common/init.d/1005_rc_fw_Malolo1.hil new file mode 100644 index 0000000000..cc13110e40 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/1005_rc_fw_Malolo1.hil @@ -0,0 +1,61 @@ +#!nsh +# +# HIL Malolo 1 (Flightgear) +# +# Maintainers: Thomas Gubler +# + +echo "HIL Malolo 1 starting.." + +if [ $DO_AUTOCONFIG == yes ] +then + # Set all params here, then disable autoconfig + + param set FW_AIRSPD_MIN 12 + param set FW_AIRSPD_TRIM 25 + param set FW_ATT_TC 0.3 + param set FW_L1_DAMPING 0.74 + param set FW_L1_PERIOD 15 + param set FW_PR_FF 0.8 + param set FW_PR_I 0.05 + param set FW_PR_IMAX 0.2 + param set FW_PR_P 0.1 + param set FW_P_LIM_MAX 45 + param set FW_P_LIM_MIN -45 + param set FW_P_RMAX_NEG 0 + param set FW_P_RMAX_POS 0 + param set FW_P_ROLLFF 0 + param set FW_RR_FF 0.6 + param set FW_RR_I 0.02 + param set FW_RR_IMAX 0.2 + param set FW_RR_P 0.1 + param set FW_R_LIM 45 + param set FW_R_RMAX 0 + param set FW_THR_CRUISE 0.6 + param set FW_THR_MAX 1 + param set FW_THR_MIN 0 + param set FW_T_CLMB_MAX 5 + param set FW_T_HRATE_P 0.02 + param set FW_T_PTCH_DAMP 0 + param set FW_T_RLL2THR 15 + param set FW_T_SINK_MAX 5 + param set FW_T_SINK_MIN 2 + param set FW_T_SRATE_P 0.01 + param set FW_T_TIME_CONST 3 + param set FW_T_VERT_ACC 7 + param set FW_YCO_VMIN 1000 + param set FW_YR_FF 0.0 + param set FW_YR_I 0 + param set FW_YR_IMAX 0.2 + param set FW_YR_P 0.0 + param set FW_Y_RMAX 0 + + param set SYS_AUTOCONFIG 0 + param save +fi + +set HIL yes + +set VEHICLE_TYPE fw +# Set the AERT mixer for HIL (even if the malolo is a flying wing) +set MIXER FMU_AERT diff --git a/ROMFS/px4fmu_common/init.d/rc.autostart b/ROMFS/px4fmu_common/init.d/rc.autostart index 030806fd7e..7fa2afbc8b 100644 --- a/ROMFS/px4fmu_common/init.d/rc.autostart +++ b/ROMFS/px4fmu_common/init.d/rc.autostart @@ -46,6 +46,11 @@ then sh /etc/init.d/1004_rc_fw_Rascal110.hil fi +if param compare SYS_AUTOSTART 1005 +then + sh /etc/init.d/1005_rc_fw_Malolo1.hil +fi + # # Standard plane # From 48f777d071652e382f930a4554ed809a60b7b0c5 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 1 Feb 2014 13:54:39 +0100 Subject: [PATCH 391/447] commander sets vstatus.condition_landed = false for fw systems (until we have a landing detector): solves multiple issues with the state machine in the navigator app --- src/modules/commander/commander.cpp | 8 ++++++++ src/modules/navigator/navigator_main.cpp | 9 +-------- 2 files changed, 9 insertions(+), 8 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index e9da69232d..aca3f62565 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -926,10 +926,12 @@ int commander_thread_main(int argc, char *argv[]) check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.xy_valid, &(status.condition_local_position_valid), &status_changed); check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.z_valid, &(status.condition_local_altitude_valid), &status_changed); + static bool published_condition_landed_fw = false; if (status.is_rotary_wing && status.condition_local_altitude_valid) { if (status.condition_landed != local_position.landed) { status.condition_landed = local_position.landed; status_changed = true; + published_condition_landed_fw = false; //make sure condition_landed is published again if the system type changes if (status.condition_landed) { mavlink_log_critical(mavlink_fd, "#audio: LANDED"); @@ -938,6 +940,12 @@ int commander_thread_main(int argc, char *argv[]) mavlink_log_critical(mavlink_fd, "#audio: IN AIR"); } } + } else { + if (!published_condition_landed_fw) { + status.condition_landed = false; // Fixedwing does not have a landing detector currently + published_condition_landed_fw = true; + status_changed = true; + } } /* update battery status */ diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 114776327e..3ef2009c71 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -1443,14 +1443,7 @@ Navigator::check_mission_item_reached() } if (_mission_item.nav_cmd == NAV_CMD_LAND) { - if (_vstatus.is_rotary_wing) { - return _vstatus.condition_landed; - - } else { - /* For fw there is currently no landing detector: - * make sure control is not stopped when overshooting the landing waypoint */ - return false; - } + return _vstatus.condition_landed; } /* XXX TODO count turns */ From 723259d46dd7d205e48a9cd3ece4ab1628b7a3e6 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 1 Feb 2014 14:36:34 +0100 Subject: [PATCH 392/447] update malolo parameters related to RTL --- ROMFS/px4fmu_common/init.d/1005_rc_fw_Malolo1.hil | 3 +++ 1 file changed, 3 insertions(+) diff --git a/ROMFS/px4fmu_common/init.d/1005_rc_fw_Malolo1.hil b/ROMFS/px4fmu_common/init.d/1005_rc_fw_Malolo1.hil index cc13110e40..36d32f3bea 100644 --- a/ROMFS/px4fmu_common/init.d/1005_rc_fw_Malolo1.hil +++ b/ROMFS/px4fmu_common/init.d/1005_rc_fw_Malolo1.hil @@ -49,6 +49,9 @@ then param set FW_YR_IMAX 0.2 param set FW_YR_P 0.0 param set FW_Y_RMAX 0 + param set NAV_LAND_ALT 90 + param set NAV_RTL_ALT 100 + param set NAV_RTL_LAND_T -1 param set SYS_AUTOCONFIG 0 param save From 44cd82e2fef20a3fc5aa61711b4cc06012a1e21d Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 1 Feb 2014 14:40:23 +0100 Subject: [PATCH 393/447] Set default autoland wait time to -1 (infinite wait) --- src/modules/navigator/navigator_params.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/navigator/navigator_params.c b/src/modules/navigator/navigator_params.c index af1d9d7d58..d5e00e35db 100644 --- a/src/modules/navigator/navigator_params.c +++ b/src/modules/navigator/navigator_params.c @@ -60,4 +60,4 @@ PARAM_DEFINE_INT32(NAV_ONB_MIS_EN, 0); PARAM_DEFINE_FLOAT(NAV_TAKEOFF_ALT, 10.0f); // default TAKEOFF altitude PARAM_DEFINE_FLOAT(NAV_LAND_ALT, 5.0f); // slow descend from this altitude when landing PARAM_DEFINE_FLOAT(NAV_RTL_ALT, 30.0f); // min altitude for going home in RTL mode -PARAM_DEFINE_FLOAT(NAV_RTL_LAND_T, 10.0f); // delay after descend before landing +PARAM_DEFINE_FLOAT(NAV_RTL_LAND_T, -1.0f); // delay after descend before landing, if set to -1 the system will not land but loiter at NAV_LAND_ALT From 0982b081b8d67f6dea8240a6c2a4226bcce57932 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 1 Feb 2014 15:46:04 +0100 Subject: [PATCH 394/447] ROMFS cleanup to eleminate excessive comments and resulting flash usage --- .../init.d/1000_rc_fw_easystar.hil | 2 - ROMFS/px4fmu_common/init.d/10016_3dr_iris | 2 +- ROMFS/px4fmu_common/init.d/1001_rc_quad_x.hil | 2 +- .../px4fmu_common/init.d/1002_rc_fw_state.hil | 45 ------------------- ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil | 2 +- .../init.d/1004_rc_fw_Rascal110.hil | 2 +- .../init.d/1005_rc_fw_Malolo1.hil | 2 +- ROMFS/px4fmu_common/init.d/12001_octo_cox | 2 +- ROMFS/px4fmu_common/init.d/2101_hk_bixler | 5 --- ROMFS/px4fmu_common/init.d/2102_3dr_skywalker | 7 --- ROMFS/px4fmu_common/init.d/3030_io_camflyer | 4 +- ROMFS/px4fmu_common/init.d/3031_phantom | 2 +- ROMFS/px4fmu_common/init.d/3032_skywalker_x5 | 2 +- ROMFS/px4fmu_common/init.d/3033_wingwing | 2 +- ROMFS/px4fmu_common/init.d/3034_fx79 | 2 +- ROMFS/px4fmu_common/init.d/4001_quad_x | 2 +- ROMFS/px4fmu_common/init.d/4010_dji_f330 | 2 +- ROMFS/px4fmu_common/init.d/4011_dji_f450 | 2 +- ROMFS/px4fmu_common/init.d/4012_hk_x550 | 42 +---------------- ROMFS/px4fmu_common/init.d/5001_quad_+ | 2 +- ROMFS/px4fmu_common/init.d/6001_hexa_x | 2 +- ROMFS/px4fmu_common/init.d/7001_hexa_+ | 2 +- ROMFS/px4fmu_common/init.d/8001_octo_x | 2 +- ROMFS/px4fmu_common/init.d/9001_octo_+ | 2 +- ROMFS/px4fmu_common/init.d/rc.autostart | 5 --- ROMFS/px4fmu_common/init.d/rc.fw_apps | 11 ----- ROMFS/px4fmu_common/init.d/rc.io | 1 - ROMFS/px4fmu_common/init.d/rc.mc_apps | 17 +------ ROMFS/px4fmu_common/init.d/rc.sensors | 4 -- ROMFS/px4fmu_common/init.d/rcS | 3 +- 30 files changed, 24 insertions(+), 158 deletions(-) delete mode 100644 ROMFS/px4fmu_common/init.d/1002_rc_fw_state.hil 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 ebe8a1a1ea..c8030d3965 100644 --- a/ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil +++ b/ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil @@ -26,8 +26,6 @@ then param set FW_R_P 100 param set FW_R_RMAX 100 param set FW_THR_CRUISE 0.65 - param set FW_THR_MAX 1 - param set FW_THR_MIN 0 param set FW_T_SINK_MAX 5.0 param set FW_T_SINK_MIN 4.0 param set FW_Y_ROLLFF 1.1 diff --git a/ROMFS/px4fmu_common/init.d/10016_3dr_iris b/ROMFS/px4fmu_common/init.d/10016_3dr_iris index 2ce0334b4c..1c7ecb7129 100644 --- a/ROMFS/px4fmu_common/init.d/10016_3dr_iris +++ b/ROMFS/px4fmu_common/init.d/10016_3dr_iris @@ -2,7 +2,7 @@ # # 3DR Iris Quadcopter # -# Maintainers: Anton Babushkin +# Anton Babushkin # if [ $DO_AUTOCONFIG == yes ] diff --git a/ROMFS/px4fmu_common/init.d/1001_rc_quad_x.hil b/ROMFS/px4fmu_common/init.d/1001_rc_quad_x.hil index 84e2bc5d47..45880f44be 100644 --- a/ROMFS/px4fmu_common/init.d/1001_rc_quad_x.hil +++ b/ROMFS/px4fmu_common/init.d/1001_rc_quad_x.hil @@ -2,7 +2,7 @@ # # HIL Quadcopter X # -# Maintainers: Anton Babushkin +# Anton Babushkin # sh /etc/init.d/4001_quad_x diff --git a/ROMFS/px4fmu_common/init.d/1002_rc_fw_state.hil b/ROMFS/px4fmu_common/init.d/1002_rc_fw_state.hil deleted file mode 100644 index 46da24d35f..0000000000 --- a/ROMFS/px4fmu_common/init.d/1002_rc_fw_state.hil +++ /dev/null @@ -1,45 +0,0 @@ -#!nsh -# -# HIL Rascal 110 (Flightgear) -# -# Maintainers: Thomas Gubler -# - -echo "HIL Rascal 110 starting.." - -if [ $DO_AUTOCONFIG == yes ] -then - # Set all params here, then disable autoconfig - - param set FW_P_D 0 - param set FW_P_I 0 - param set FW_P_IMAX 15 - param set FW_P_LIM_MAX 50 - param set FW_P_LIM_MIN -50 - param set FW_P_P 60 - param set FW_P_RMAX_NEG 0 - param set FW_P_RMAX_POS 0 - param set FW_P_ROLLFF 1.1 - param set FW_R_D 0 - param set FW_R_I 5 - param set FW_R_IMAX 20 - param set FW_R_P 100 - param set FW_R_RMAX 100 - param set FW_THR_CRUISE 0.65 - param set FW_THR_MAX 1 - param set FW_THR_MIN 0 - param set FW_T_SINK_MAX 5.0 - param set FW_T_SINK_MIN 4.0 - param set FW_Y_ROLLFF 1.1 - param set FW_L1_PERIOD 16 - param set RC_SCALE_ROLL 1.0 - param set RC_SCALE_PITCH 1.0 - - param set SYS_AUTOCONFIG 0 - param save -fi - -set HIL yes - -set VEHICLE_TYPE fw -set MIXER FMU_AERT diff --git a/ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil b/ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil index e958448912..2b26810e71 100644 --- a/ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil +++ b/ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil @@ -2,7 +2,7 @@ # # HIL Quadcopter + # -# Maintainers: Anton Babushkin +# Anton Babushkin # sh /etc/init.d/1001_rc_quad_x.hil diff --git a/ROMFS/px4fmu_common/init.d/1004_rc_fw_Rascal110.hil b/ROMFS/px4fmu_common/init.d/1004_rc_fw_Rascal110.hil index 46da24d35f..fd7a6a3dab 100644 --- a/ROMFS/px4fmu_common/init.d/1004_rc_fw_Rascal110.hil +++ b/ROMFS/px4fmu_common/init.d/1004_rc_fw_Rascal110.hil @@ -2,7 +2,7 @@ # # HIL Rascal 110 (Flightgear) # -# Maintainers: Thomas Gubler +# Thomas Gubler # echo "HIL Rascal 110 starting.." diff --git a/ROMFS/px4fmu_common/init.d/1005_rc_fw_Malolo1.hil b/ROMFS/px4fmu_common/init.d/1005_rc_fw_Malolo1.hil index 36d32f3bea..124bf63abf 100644 --- a/ROMFS/px4fmu_common/init.d/1005_rc_fw_Malolo1.hil +++ b/ROMFS/px4fmu_common/init.d/1005_rc_fw_Malolo1.hil @@ -2,7 +2,7 @@ # # HIL Malolo 1 (Flightgear) # -# Maintainers: Thomas Gubler +# Thomas Gubler # echo "HIL Malolo 1 starting.." diff --git a/ROMFS/px4fmu_common/init.d/12001_octo_cox b/ROMFS/px4fmu_common/init.d/12001_octo_cox index 77813268a6..8a813595e4 100644 --- a/ROMFS/px4fmu_common/init.d/12001_octo_cox +++ b/ROMFS/px4fmu_common/init.d/12001_octo_cox @@ -2,7 +2,7 @@ # # Generic 10” Octo coaxial geometry # -# Maintainers: Lorenz Meier , Anton Babushkin +# Lorenz Meier , Anton Babushkin # sh /etc/init.d/8001_octo_x diff --git a/ROMFS/px4fmu_common/init.d/2101_hk_bixler b/ROMFS/px4fmu_common/init.d/2101_hk_bixler index 1ed923b19c..de5e5a8d3a 100644 --- a/ROMFS/px4fmu_common/init.d/2101_hk_bixler +++ b/ROMFS/px4fmu_common/init.d/2101_hk_bixler @@ -1,10 +1,5 @@ #!nsh -echo "[init] PX4FMU v1, v2 with or without IO on 3DR SkyWalker" - -# -# Load default params for this platform -# if [ $DO_AUTOCONFIG == yes ] then # Set all params here, then disable autoconfig diff --git a/ROMFS/px4fmu_common/init.d/2102_3dr_skywalker b/ROMFS/px4fmu_common/init.d/2102_3dr_skywalker index 1ed923b19c..07f215f6cf 100644 --- a/ROMFS/px4fmu_common/init.d/2102_3dr_skywalker +++ b/ROMFS/px4fmu_common/init.d/2102_3dr_skywalker @@ -1,10 +1,5 @@ #!nsh -echo "[init] PX4FMU v1, v2 with or without IO on 3DR SkyWalker" - -# -# Load default params for this platform -# if [ $DO_AUTOCONFIG == yes ] then # Set all params here, then disable autoconfig @@ -23,8 +18,6 @@ then param set FW_R_P 100 param set FW_R_RMAX 100 param set FW_THR_CRUISE 0.65 - param set FW_THR_MAX 1 - param set FW_THR_MIN 0 param set FW_T_SINK_MAX 5.0 param set FW_T_SINK_MIN 4.0 param set FW_Y_ROLLFF 1.1 diff --git a/ROMFS/px4fmu_common/init.d/3030_io_camflyer b/ROMFS/px4fmu_common/init.d/3030_io_camflyer index cbcc6189b6..3b7323ac4e 100644 --- a/ROMFS/px4fmu_common/init.d/3030_io_camflyer +++ b/ROMFS/px4fmu_common/init.d/3030_io_camflyer @@ -1,7 +1,5 @@ #!nsh -echo "[init] PX4FMU v1, v2 with or without IO on Camflyer" - if [ $DO_AUTOCONFIG == yes ] then # @@ -26,7 +24,7 @@ then param set FW_R_P 60 param set FW_R_RMAX 60 param set FW_THR_CRUISE 0.65 - param set FW_THR_MAX 0.7 + param set FW_THR_MAX 1.0 param set FW_THR_MIN 0 param set FW_T_SINK_MAX 5 param set FW_T_SINK_MIN 2 diff --git a/ROMFS/px4fmu_common/init.d/3031_phantom b/ROMFS/px4fmu_common/init.d/3031_phantom index 4ebbe9c61e..a4ff61d937 100644 --- a/ROMFS/px4fmu_common/init.d/3031_phantom +++ b/ROMFS/px4fmu_common/init.d/3031_phantom @@ -2,7 +2,7 @@ # # Phantom FPV Flying Wing # -# Maintainers: Simon Wilks +# Simon Wilks # if [ $DO_AUTOCONFIG == yes ] diff --git a/ROMFS/px4fmu_common/init.d/3032_skywalker_x5 b/ROMFS/px4fmu_common/init.d/3032_skywalker_x5 index 143310af95..c1e78b6f1e 100644 --- a/ROMFS/px4fmu_common/init.d/3032_skywalker_x5 +++ b/ROMFS/px4fmu_common/init.d/3032_skywalker_x5 @@ -2,7 +2,7 @@ # # Skywalker X5 Flying Wing # -# Maintainers: Thomas Gubler , Julian Oes +# Thomas Gubler , Julian Oes # if [ $DO_AUTOCONFIG == yes ] diff --git a/ROMFS/px4fmu_common/init.d/3033_wingwing b/ROMFS/px4fmu_common/init.d/3033_wingwing index e537632784..2f68797999 100644 --- a/ROMFS/px4fmu_common/init.d/3033_wingwing +++ b/ROMFS/px4fmu_common/init.d/3033_wingwing @@ -2,7 +2,7 @@ # # Wing Wing (aka Z-84) Flying Wing # -# Maintainers: Simon Wilks +# Simon Wilks # if [ $DO_AUTOCONFIG == yes ] diff --git a/ROMFS/px4fmu_common/init.d/3034_fx79 b/ROMFS/px4fmu_common/init.d/3034_fx79 index 8d179d1fd4..bc02d87f3d 100644 --- a/ROMFS/px4fmu_common/init.d/3034_fx79 +++ b/ROMFS/px4fmu_common/init.d/3034_fx79 @@ -2,7 +2,7 @@ # # FX-79 Buffalo Flying Wing # -# Maintainers: Simon Wilks +# Simon Wilks # if [ $DO_AUTOCONFIG == yes ] diff --git a/ROMFS/px4fmu_common/init.d/4001_quad_x b/ROMFS/px4fmu_common/init.d/4001_quad_x index ca4694d81c..42b64a68ea 100644 --- a/ROMFS/px4fmu_common/init.d/4001_quad_x +++ b/ROMFS/px4fmu_common/init.d/4001_quad_x @@ -2,7 +2,7 @@ # # Generic 10” Quad X geometry # -# Maintainers: Lorenz Meier , Anton Babushkin +# Lorenz Meier # if [ $DO_AUTOCONFIG == yes ] diff --git a/ROMFS/px4fmu_common/init.d/4010_dji_f330 b/ROMFS/px4fmu_common/init.d/4010_dji_f330 index c78da2d6c5..7829460339 100644 --- a/ROMFS/px4fmu_common/init.d/4010_dji_f330 +++ b/ROMFS/px4fmu_common/init.d/4010_dji_f330 @@ -2,7 +2,7 @@ # # DJI Flame Wheel F330 Quadcopter # -# Maintainers: Anton Babushkin +# Anton Babushkin # if [ $DO_AUTOCONFIG == yes ] diff --git a/ROMFS/px4fmu_common/init.d/4011_dji_f450 b/ROMFS/px4fmu_common/init.d/4011_dji_f450 index 8027b9d42a..8eb53d1720 100644 --- a/ROMFS/px4fmu_common/init.d/4011_dji_f450 +++ b/ROMFS/px4fmu_common/init.d/4011_dji_f450 @@ -2,7 +2,7 @@ # # DJI Flame Wheel F450 Quadcopter # -# Maintainers: Lorenz Meier +# Lorenz Meier # if [ $DO_AUTOCONFIG == yes ] diff --git a/ROMFS/px4fmu_common/init.d/4012_hk_x550 b/ROMFS/px4fmu_common/init.d/4012_hk_x550 index 98e1f80aa4..3d8e9fcf70 100644 --- a/ROMFS/px4fmu_common/init.d/4012_hk_x550 +++ b/ROMFS/px4fmu_common/init.d/4012_hk_x550 @@ -2,48 +2,10 @@ # # HobbyKing X550 Quadcopter # -# Maintainers: Todd Stellanova +# Todd Stellanova # -if [ $DO_AUTOCONFIG == yes ] -then - # - # Default parameters for this platform - # - param set MC_ROLL_P 5.5 - param set MC_ROLLRATE_P 0.14 - param set MC_ROLLRATE_I 0 - param set MC_ROLLRATE_D 0.006 - param set MC_PITCH_P 5.5 - param set MC_PITCHRATE_P 0.14 - param set MC_PITCHRATE_I 0 - param set MC_PITCHRATE_D 0.006 - param set MC_YAW_P 0.6 - param set MC_YAWRATE_P 0.08 - param set MC_YAWRATE_I 0 - param set MC_YAWRATE_D 0 - - param set MPC_THR_MAX 1.0 - param set MPC_THR_MIN 0.1 - param set MPC_XY_P 1.0 - param set MPC_XY_VEL_P 0.1 - param set MPC_XY_VEL_I 0.02 - param set MPC_XY_VEL_D 0.01 - param set MPC_XY_VEL_MAX 5 - param set MPC_XY_FF 0.5 - param set MPC_Z_P 1.0 - param set MPC_Z_VEL_P 0.1 - param set MPC_Z_VEL_I 0.02 - param set MPC_Z_VEL_D 0.0 - param set MPC_Z_VEL_MAX 3 - param set MPC_Z_FF 0.5 - param set MPC_TILT_MAX 1.0 - param set MPC_LAND_SPEED 1.0 - param set MPC_LAND_TILT 0.3 -fi - -set VEHICLE_TYPE mc -set MIXER FMU_quad_x +sh /etc/init.d/4001_quad_x set PWM_OUTPUTS 1234 set PWM_RATE 400 diff --git a/ROMFS/px4fmu_common/init.d/5001_quad_+ b/ROMFS/px4fmu_common/init.d/5001_quad_+ index 7f5a6fc078..ff11bccfe6 100644 --- a/ROMFS/px4fmu_common/init.d/5001_quad_+ +++ b/ROMFS/px4fmu_common/init.d/5001_quad_+ @@ -2,7 +2,7 @@ # # Generic 10” Quad + geometry # -# Maintainers: Lorenz Meier , Anton Babushkin +# Anton Babushkin # sh /etc/init.d/4001_quad_x diff --git a/ROMFS/px4fmu_common/init.d/6001_hexa_x b/ROMFS/px4fmu_common/init.d/6001_hexa_x index e72e15dd4d..270f51a582 100644 --- a/ROMFS/px4fmu_common/init.d/6001_hexa_x +++ b/ROMFS/px4fmu_common/init.d/6001_hexa_x @@ -2,7 +2,7 @@ # # Generic 10” Hexa X geometry # -# Maintainers: Lorenz Meier , Anton Babushkin +# Anton Babushkin # sh /etc/init.d/4001_quad_x diff --git a/ROMFS/px4fmu_common/init.d/7001_hexa_+ b/ROMFS/px4fmu_common/init.d/7001_hexa_+ index dade0630da..45279ec390 100644 --- a/ROMFS/px4fmu_common/init.d/7001_hexa_+ +++ b/ROMFS/px4fmu_common/init.d/7001_hexa_+ @@ -2,7 +2,7 @@ # # Generic 10” Hexa + geometry # -# Maintainers: Lorenz Meier , Anton Babushkin +# Anton Babushkin # sh /etc/init.d/6001_hexa_x diff --git a/ROMFS/px4fmu_common/init.d/8001_octo_x b/ROMFS/px4fmu_common/init.d/8001_octo_x index af632ed908..6fa9626684 100644 --- a/ROMFS/px4fmu_common/init.d/8001_octo_x +++ b/ROMFS/px4fmu_common/init.d/8001_octo_x @@ -2,7 +2,7 @@ # # Generic 10” Octo X geometry # -# Maintainers: Lorenz Meier , Anton Babushkin +# Anton Babushkin # sh /etc/init.d/4001_quad_x diff --git a/ROMFS/px4fmu_common/init.d/9001_octo_+ b/ROMFS/px4fmu_common/init.d/9001_octo_+ index 9bf5e09328..fa3869f9f9 100644 --- a/ROMFS/px4fmu_common/init.d/9001_octo_+ +++ b/ROMFS/px4fmu_common/init.d/9001_octo_+ @@ -2,7 +2,7 @@ # # Generic 10” Octo + geometry # -# Maintainers: Lorenz Meier , Anton Babushkin +# Anton Babushkin # sh /etc/init.d/8001_octo_x diff --git a/ROMFS/px4fmu_common/init.d/rc.autostart b/ROMFS/px4fmu_common/init.d/rc.autostart index 7fa2afbc8b..8d2fc5772d 100644 --- a/ROMFS/px4fmu_common/init.d/rc.autostart +++ b/ROMFS/px4fmu_common/init.d/rc.autostart @@ -31,11 +31,6 @@ then sh /etc/init.d/1001_rc_quad_x.hil fi -if param compare SYS_AUTOSTART 1002 -then - sh /etc/init.d/1002_rc_fw_state.hil -fi - if param compare SYS_AUTOSTART 1003 then sh /etc/init.d/1003_rc_quad_+.hil diff --git a/ROMFS/px4fmu_common/init.d/rc.fw_apps b/ROMFS/px4fmu_common/init.d/rc.fw_apps index d354fb06fa..72327c554e 100644 --- a/ROMFS/px4fmu_common/init.d/rc.fw_apps +++ b/ROMFS/px4fmu_common/init.d/rc.fw_apps @@ -3,17 +3,6 @@ # Standard apps for fixed wing # -# -# Start the attitude and position estimator -# att_pos_estimator_ekf start - -# -# Start attitude controller -# fw_att_control start - -# -# Start the position controller -# fw_pos_control_l1 start diff --git a/ROMFS/px4fmu_common/init.d/rc.io b/ROMFS/px4fmu_common/init.d/rc.io index c9d964f8e2..6e8fdbc0f7 100644 --- a/ROMFS/px4fmu_common/init.d/rc.io +++ b/ROMFS/px4fmu_common/init.d/rc.io @@ -4,7 +4,6 @@ # # Allow PX4IO to recover from midair restarts. -# this is very unlikely, but quite safe and robust. # px4io recovery diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_apps b/ROMFS/px4fmu_common/init.d/rc.mc_apps index 96fe32c8a7..ed3939757a 100644 --- a/ROMFS/px4fmu_common/init.d/rc.mc_apps +++ b/ROMFS/px4fmu_common/init.d/rc.mc_apps @@ -1,24 +1,11 @@ #!nsh # -# Standard apps for multirotors +# Standard apps for multirotors: +# att & pos estimator, att & pos control. # -# -# Start the attitude estimator -# attitude_estimator_ekf start - -# -# Start position estimator -# position_estimator_inav start -# -# Start attitude control -# mc_att_control start - -# -# Start position control -# mc_pos_control start diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors index badbf92c3c..235be24316 100644 --- a/ROMFS/px4fmu_common/init.d/rc.sensors +++ b/ROMFS/px4fmu_common/init.d/rc.sensors @@ -3,10 +3,6 @@ # Standard startup script for PX4FMU onboard sensor drivers. # -# -# Start sensor drivers here. -# - ms5611 start adc start diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 88077c67f0..84dc7ec640 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -3,8 +3,7 @@ # PX4FMU startup script. # -# Default to auto-start mode. An init script on the microSD card -# can change this to prevent automatic startup of the flight script. +# Default to auto-start mode. # set MODE autostart From 2908ffb09293a9d65708a8e80de50bbdee69b9e2 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 1 Feb 2014 15:46:51 +0100 Subject: [PATCH 395/447] Trim down FMUv1 config, remove not commonly used modules from default set --- makefiles/config_px4fmu-v1_default.mk | 4 ---- 1 file changed, 4 deletions(-) diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk index 51be7e1a12..aff614cbb3 100644 --- a/makefiles/config_px4fmu-v1_default.mk +++ b/makefiles/config_px4fmu-v1_default.mk @@ -21,7 +21,6 @@ MODULES += drivers/px4fmu MODULES += drivers/boards/px4fmu-v1 MODULES += drivers/ardrone_interface MODULES += drivers/l3gd20 -#MODULES += drivers/bma180 MODULES += drivers/mpu6000 MODULES += drivers/hmc5883 MODULES += drivers/ms5611 @@ -44,7 +43,6 @@ MODULES += modules/sensors # MODULES += systemcmds/mtd MODULES += systemcmds/bl_update -MODULES += systemcmds/boardinfo MODULES += systemcmds/i2c MODULES += systemcmds/mixer MODULES += systemcmds/param @@ -65,14 +63,12 @@ MODULES += systemcmds/hw_ver MODULES += modules/commander MODULES += modules/navigator MODULES += modules/mavlink -MODULES += modules/mavlink_onboard MODULES += modules/gpio_led # # Estimation modules (EKF/ SO3 / other filters) # MODULES += modules/attitude_estimator_ekf -MODULES += modules/attitude_estimator_so3 MODULES += modules/att_pos_estimator_ekf MODULES += modules/position_estimator_inav #MODULES += examples/flow_position_estimator From b1539d8a74a1874abcb5e64f1813bd6ba9f0353f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 1 Feb 2014 15:56:57 +0100 Subject: [PATCH 396/447] Removed further unneeded commands and other things eating space --- ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil | 5 +---- ROMFS/px4fmu_common/init.d/10015_tbs_discovery | 2 +- ROMFS/px4fmu_common/init.d/1004_rc_fw_Rascal110.hil | 3 --- 3 files changed, 2 insertions(+), 8 deletions(-) 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 c8030d3965..d7e524b414 100644 --- a/ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil +++ b/ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil @@ -2,7 +2,7 @@ # # HILStar / X-Plane # -# Maintainers: Thomas Gubler +# Lorenz Meier # echo "HIL Rascal 110 starting.." @@ -32,9 +32,6 @@ then param set FW_L1_PERIOD 16 param set RC_SCALE_ROLL 1.0 param set RC_SCALE_PITCH 1.0 - - param set SYS_AUTOCONFIG 0 - param save fi set HIL yes diff --git a/ROMFS/px4fmu_common/init.d/10015_tbs_discovery b/ROMFS/px4fmu_common/init.d/10015_tbs_discovery index 3a8f14a9ed..a3d7c3d977 100644 --- a/ROMFS/px4fmu_common/init.d/10015_tbs_discovery +++ b/ROMFS/px4fmu_common/init.d/10015_tbs_discovery @@ -2,7 +2,7 @@ # # Team Blacksheep Discovery Quadcopter # -# Maintainers: Simon Wilks +# Simon Wilks # if [ $DO_AUTOCONFIG == yes ] diff --git a/ROMFS/px4fmu_common/init.d/1004_rc_fw_Rascal110.hil b/ROMFS/px4fmu_common/init.d/1004_rc_fw_Rascal110.hil index fd7a6a3dab..c639bfc254 100644 --- a/ROMFS/px4fmu_common/init.d/1004_rc_fw_Rascal110.hil +++ b/ROMFS/px4fmu_common/init.d/1004_rc_fw_Rascal110.hil @@ -34,9 +34,6 @@ then param set FW_L1_PERIOD 16 param set RC_SCALE_ROLL 1.0 param set RC_SCALE_PITCH 1.0 - - param set SYS_AUTOCONFIG 0 - param save fi set HIL yes From aa5d8a8732b1bed690360c25d725f2c13458528a Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 1 Feb 2014 16:06:09 +0100 Subject: [PATCH 397/447] remove printf in launchdetector --- src/lib/launchdetection/LaunchDetector.cpp | 4 ---- 1 file changed, 4 deletions(-) diff --git a/src/lib/launchdetection/LaunchDetector.cpp b/src/lib/launchdetection/LaunchDetector.cpp index 67b32ad82c..df9f2fe95c 100644 --- a/src/lib/launchdetection/LaunchDetector.cpp +++ b/src/lib/launchdetection/LaunchDetector.cpp @@ -83,14 +83,10 @@ bool LaunchDetector::getLaunchDetected() void LaunchDetector::updateParams() { - warnx(" LaunchDetector::updateParams()"); launchdetection_on.update(); throttlePreTakeoff.update(); for (uint8_t i = 0; i < sizeof(launchMethods)/sizeof(LaunchMethod); i++) { launchMethods[i]->updateParams(); - warnx("updating component %d", i); } - - warnx(" LaunchDetector::updateParams() ended"); } From efc62a6c864766ce211906860d6325e4ca089241 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 2 Feb 2014 00:43:41 +0100 Subject: [PATCH 398/447] fw landing: improve slope altitude calc to avoid climbout after waypoint. Throttle cut is now defined via altitude --- .../fw_pos_control_l1_main.cpp | 25 ++++++++++--------- .../fw_pos_control_l1_params.c | 2 +- .../fw_pos_control_l1/landingslope.cpp | 22 ++++++++++++---- src/modules/fw_pos_control_l1/landingslope.h | 21 ++++++++++++---- 4 files changed, 47 insertions(+), 23 deletions(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 3ef1871a8b..45fdaa355e 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -236,7 +236,7 @@ private: float land_slope_length; float land_H1_virt; float land_flare_alt_relative; - float land_thrust_lim_horizontal_distance; + float land_thrust_lim_alt_relative; float land_heading_hold_horizontal_distance; } _parameters; /**< local copies of interesting parameters */ @@ -281,7 +281,7 @@ private: param_t land_slope_length; param_t land_H1_virt; param_t land_flare_alt_relative; - param_t land_thrust_lim_horizontal_distance; + param_t land_thrust_lim_alt_relative; param_t land_heading_hold_horizontal_distance; } _parameter_handles; /**< handles for interesting parameters */ @@ -434,7 +434,7 @@ FixedwingPositionControl::FixedwingPositionControl() : _parameter_handles.land_slope_length = param_find("FW_LND_SLLR"); _parameter_handles.land_H1_virt = param_find("FW_LND_HVIRT"); _parameter_handles.land_flare_alt_relative = param_find("FW_LND_FLALT"); - _parameter_handles.land_thrust_lim_horizontal_distance = param_find("FW_LND_TLDIST"); + _parameter_handles.land_thrust_lim_alt_relative = param_find("FW_LND_TLALT"); _parameter_handles.land_heading_hold_horizontal_distance = param_find("FW_LND_HHDIST"); _parameter_handles.time_const = param_find("FW_T_TIME_CONST"); @@ -523,7 +523,7 @@ FixedwingPositionControl::parameters_update() param_get(_parameter_handles.land_slope_length, &(_parameters.land_slope_length)); param_get(_parameter_handles.land_H1_virt, &(_parameters.land_H1_virt)); param_get(_parameter_handles.land_flare_alt_relative, &(_parameters.land_flare_alt_relative)); - param_get(_parameter_handles.land_thrust_lim_horizontal_distance, &(_parameters.land_thrust_lim_horizontal_distance)); + param_get(_parameter_handles.land_thrust_lim_alt_relative, &(_parameters.land_thrust_lim_alt_relative)); param_get(_parameter_handles.land_heading_hold_horizontal_distance, &(_parameters.land_heading_hold_horizontal_distance)); _l1_control.set_l1_damping(_parameters.l1_damping); @@ -558,7 +558,7 @@ FixedwingPositionControl::parameters_update() } /* Update the landing slope */ - landingslope.update(math::radians(_parameters.land_slope_angle), _parameters.land_flare_alt_relative, _parameters.land_thrust_lim_horizontal_distance, _parameters.land_H1_virt); + landingslope.update(math::radians(_parameters.land_slope_angle), _parameters.land_flare_alt_relative, _parameters.land_thrust_lim_alt_relative, _parameters.land_H1_virt); /* Update and publish the navigation capabilities */ _nav_capabilities.landing_slope_angle_rad = landingslope.landing_slope_angle_rad(); @@ -836,6 +836,8 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } else if (pos_sp_triplet.current.type == SETPOINT_TYPE_LAND) { + float bearing_lastwp_currwp = get_bearing_to_next_waypoint(prev_wp(0), prev_wp(1), curr_wp(0), curr_wp(1)); + /* Horizontal landing control */ /* switch to heading hold for the last meters, continue heading hold after */ float wp_distance = get_distance_to_next_waypoint(current_position(0), current_position(1), curr_wp(0), curr_wp(1)); @@ -846,7 +848,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi if (!land_noreturn_horizontal) {//set target_bearing in first occurrence if (pos_sp_triplet.previous.valid) { - target_bearing = get_bearing_to_next_waypoint(prev_wp(0), prev_wp(1), curr_wp(0), curr_wp(1)); + target_bearing = bearing_lastwp_currwp; } else { target_bearing = _att.yaw; } @@ -888,10 +890,9 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi float airspeed_approach = 1.3f * _parameters.airspeed_min; float L_wp_distance = get_distance_to_next_waypoint(prev_wp(0), prev_wp(1), curr_wp(0), curr_wp(1)) * _parameters.land_slope_length; - float L_altitude = landingslope.getLandingSlopeAbsoluteAltitude(L_wp_distance, _pos_sp_triplet.current.alt);//getLandingSlopeAbsoluteAltitude(L_wp_distance, _pos_sp_triplet.current.alt, landing_slope_angle_rad, horizontal_slope_displacement); - float landing_slope_alt_desired = landingslope.getLandingSlopeAbsoluteAltitude(wp_distance, _pos_sp_triplet.current.alt);//getLandingSlopeAbsoluteAltitude(wp_distance, _pos_sp_triplet.current.alt, landing_slope_angle_rad, horizontal_slope_displacement); - - + float L_altitude = landingslope.getLandingSlopeAbsoluteAltitude(L_wp_distance, _pos_sp_triplet.current.alt); + float bearing_airplane_currwp = get_bearing_to_next_waypoint(current_position(0), current_position(1), curr_wp(0), curr_wp(1)); + float landing_slope_alt_desired = landingslope.getLandingSlopeAbsoluteAltitudeSave(wp_distance, bearing_lastwp_currwp, bearing_airplane_currwp, _pos_sp_triplet.current.alt); if ( (_global_pos.alt < _pos_sp_triplet.current.alt + landingslope.flare_relative_alt()) || land_noreturn_vertical) { //checking for land_noreturn to avoid unwanted climb out @@ -903,7 +904,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* kill the throttle if param requests it */ throttle_max = _parameters.throttle_max; - if (wp_distance < landingslope.motor_lim_horizontal_distance() || land_motor_lim) { + if (_global_pos.alt < _pos_sp_triplet.current.alt + landingslope.motor_lim_relative_alt() || land_motor_lim) { throttle_max = math::min(throttle_max, _parameters.throttle_land_max); if (!land_motor_lim) { land_motor_lim = true; @@ -912,7 +913,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } - float flare_curve_alt = landingslope.getFlareCurveAltitude(wp_distance, _pos_sp_triplet.current.alt); + float flare_curve_alt = landingslope.getFlareCurveAltitudeSave(wp_distance, bearing_lastwp_currwp, bearing_airplane_currwp, _pos_sp_triplet.current.alt); /* avoid climbout */ if (flare_curve_alt_last < flare_curve_alt && land_noreturn_vertical || land_stayonground) 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 7954d75c20..ee8721ff92 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 @@ -172,5 +172,5 @@ PARAM_DEFINE_FLOAT(FW_LND_ANG, 10.0f); PARAM_DEFINE_FLOAT(FW_LND_SLLR, 0.9f); PARAM_DEFINE_FLOAT(FW_LND_HVIRT, 10.0f); PARAM_DEFINE_FLOAT(FW_LND_FLALT, 15.0f); -PARAM_DEFINE_FLOAT(FW_LND_TLDIST, 30.0f); +PARAM_DEFINE_FLOAT(FW_LND_TLALT, 5.0f); PARAM_DEFINE_FLOAT(FW_LND_HHDIST, 15.0f); diff --git a/src/modules/fw_pos_control_l1/landingslope.cpp b/src/modules/fw_pos_control_l1/landingslope.cpp index b139a63978..e5f7023ae1 100644 --- a/src/modules/fw_pos_control_l1/landingslope.cpp +++ b/src/modules/fw_pos_control_l1/landingslope.cpp @@ -48,13 +48,13 @@ void Landingslope::update(float landing_slope_angle_rad, float flare_relative_alt, - float motor_lim_horizontal_distance, + float motor_lim_relative_alt, float H1_virt) { _landing_slope_angle_rad = landing_slope_angle_rad; _flare_relative_alt = flare_relative_alt; - _motor_lim_horizontal_distance = motor_lim_horizontal_distance; + _motor_lim_relative_alt = motor_lim_relative_alt; _H1_virt = H1_virt; calculateSlopeValues(); @@ -74,9 +74,21 @@ float Landingslope::getLandingSlopeAbsoluteAltitude(float wp_distance, float wp_ return Landingslope::getLandingSlopeAbsoluteAltitude(wp_distance, wp_altitude, _horizontal_slope_displacement, _landing_slope_angle_rad); } -float Landingslope::getFlareCurveAltitude(float wp_landing_distance, float wp_landing_altitude) +float Landingslope::getLandingSlopeAbsoluteAltitudeSave(float wp_distance, float bearing_lastwp_currwp, float bearing_airplane_currwp, float wp_altitude) { - return wp_landing_altitude + _H0 * expf(-math::max(0.0f, _flare_length - wp_landing_distance)/_flare_constant) - _H1_virt; - + /* If airplane is in front of waypoint return slope altitude, else return waypoint altitude */ + if (fabsf(bearing_airplane_currwp - bearing_lastwp_currwp) < math::radians(90.0f)) + return getLandingSlopeAbsoluteAltitude(wp_distance, wp_altitude); + else + return wp_altitude; +} + +float Landingslope::getFlareCurveAltitudeSave(float wp_landing_distance, float bearing_lastwp_currwp, float bearing_airplane_currwp, float wp_landing_altitude) +{ + /* If airplane is in front of waypoint return flare curve altitude, else return waypoint altitude */ + if (fabsf(bearing_airplane_currwp - bearing_lastwp_currwp) < math::radians(90.0f)) + return wp_landing_altitude + _H0 * expf(-math::max(0.0f, _flare_length - wp_landing_distance)/_flare_constant) - _H1_virt; + else + return wp_landing_altitude; } diff --git a/src/modules/fw_pos_control_l1/landingslope.h b/src/modules/fw_pos_control_l1/landingslope.h index 1a149fc7cd..76d65a55f2 100644 --- a/src/modules/fw_pos_control_l1/landingslope.h +++ b/src/modules/fw_pos_control_l1/landingslope.h @@ -49,7 +49,7 @@ private: /* see Documentation/fw_landing.png for a plot of the landing slope */ float _landing_slope_angle_rad; /**< phi in the plot */ float _flare_relative_alt; /**< h_flare,rel in the plot */ - float _motor_lim_horizontal_distance; + float _motor_lim_relative_alt; float _H1_virt; /**< H1 in the plot */ float _H0; /**< h_flare,rel + H1 in the plot */ float _d1; /**< d1 in the plot */ @@ -63,7 +63,18 @@ public: Landingslope() {} ~Landingslope() {} - float getLandingSlopeAbsoluteAltitude(float wp_landing_distance, float wp_landing_altitude); + /** + * + * @return Absolute altitude of point on landing slope at distance to landing waypoint=wp_landing_distance + */ + float getLandingSlopeAbsoluteAltitude(float wp_distance, float wp_altitude); + + /** + * + * @return Absolute altitude of point on landing slope at distance to landing waypoint=wp_landing_distance + * Performs check if aircraft is in front of waypoint to avoid climbout + */ + float getLandingSlopeAbsoluteAltitudeSave(float wp_landing_distance, float bearing_lastwp_currwp, float bearing_airplane_currwp, float wp_landing_altitude); /** * @@ -85,17 +96,17 @@ public: } - float getFlareCurveAltitude(float wp_distance, float wp_altitude); + float getFlareCurveAltitudeSave(float wp_distance, float bearing_lastwp_currwp, float bearing_airplane_currwp, float wp_altitude); void update(float landing_slope_angle_rad, float flare_relative_alt, - float motor_lim_horizontal_distance, + float motor_lim_relative_alt, float H1_virt); inline float landing_slope_angle_rad() {return _landing_slope_angle_rad;} inline float flare_relative_alt() {return _flare_relative_alt;} - inline float motor_lim_horizontal_distance() {return _motor_lim_horizontal_distance;} + inline float motor_lim_relative_alt() {return _motor_lim_relative_alt;} inline float H1_virt() {return _H1_virt;} inline float H0() {return _H0;} inline float d1() {return _d1;} From b92b08ae62b2da027e9fcd15a3a45a9125ea8355 Mon Sep 17 00:00:00 2001 From: Jean Cyr Date: Sun, 2 Feb 2014 00:42:15 -0500 Subject: [PATCH 399/447] Optimize and update data manager docs Move repeated code to common function Update missing and misleading comments Fix data manager test compile errors No functional changes --- src/modules/dataman/dataman.c | 178 +++++++++++++++++----------- src/systemcmds/tests/test_dataman.c | 8 +- 2 files changed, 112 insertions(+), 74 deletions(-) diff --git a/src/modules/dataman/dataman.c b/src/modules/dataman/dataman.c index dc2d6c312a..fa88dfaffb 100644 --- a/src/modules/dataman/dataman.c +++ b/src/modules/dataman/dataman.c @@ -126,45 +126,46 @@ static const char *k_data_manager_device_path = "/fs/microsd/dataman"; /* The data manager work queues */ typedef struct { - sq_queue_t q; - sem_t mutex; /* Mutual exclusion on work queue adds and deletes */ - unsigned size; - unsigned max_size; + sq_queue_t q; /* Nuttx queue */ + sem_t mutex; /* Mutual exclusion on work queue adds and deletes */ + unsigned size; /* Current size of queue */ + unsigned max_size; /* Maximum queue size reached */ } work_q_t; -static work_q_t g_free_q; -static work_q_t g_work_q; +static work_q_t g_free_q; /* queue of free work items. So that we don't always need to call malloc and free*/ +static work_q_t g_work_q; /* pending work items. To be consumed by worker thread */ -sem_t g_work_queued_sema; +sem_t g_work_queued_sema; /* To notify worker thread a work item has been queued */ sem_t g_init_sema; static bool g_task_should_exit; /**< if true, dataman task should exit */ -#define DM_SECTOR_HDR_SIZE 4 -static const unsigned k_sector_size = DM_MAX_DATA_SIZE + DM_SECTOR_HDR_SIZE; +#define DM_SECTOR_HDR_SIZE 4 /* data manager per item header overhead */ +static const unsigned k_sector_size = DM_MAX_DATA_SIZE + DM_SECTOR_HDR_SIZE; /* total item sorage space */ static void init_q(work_q_t *q) { - sq_init(&(q->q)); - sem_init(&(q->mutex), 1, 1); - q->size = q->max_size = 0; + sq_init(&(q->q)); /* Initialize the NuttX queue structure */ + sem_init(&(q->mutex), 1, 1); /* Queue is initially unlocked */ + q->size = q->max_size = 0; /* Queue is initially empty */ } -static void destroy_q(work_q_t *q) +static inline void +destroy_q(work_q_t *q) { - sem_destroy(&(q->mutex)); + sem_destroy(&(q->mutex)); /* Destroy the queue lock */ } static inline void lock_queue(work_q_t *q) { - sem_wait(&(q->mutex)); + sem_wait(&(q->mutex)); /* Acquire the queue lock */ } static inline void unlock_queue(work_q_t *q) { - sem_post(&(q->mutex)); + sem_post(&(q->mutex)); /* Release the queue lock */ } static work_q_item_t * @@ -172,54 +173,47 @@ create_work_item(void) { work_q_item_t *item; + /* Try to reuse item from free item queue */ lock_queue(&g_free_q); if ((item = (work_q_item_t *)sq_remfirst(&(g_free_q.q)))) g_free_q.size--; unlock_queue(&g_free_q); + /* If we there weren't any free items then obtain memory for a new one */ if (item == NULL) item = (work_q_item_t *)malloc(sizeof(work_q_item_t)); + /* If we got one then lock the item*/ if (item) sem_init(&item->wait_sem, 1, 0); /* Caller will wait on this... initially locked */ + /* return the item pointer, or NULL if all failed */ return item; } /* Work queue management functions */ -static void -enqueue_work_item(work_q_item_t *item) -{ - /* put the work item on the work queue */ - lock_queue(&g_work_q); - sq_addlast(&item->link, &(g_work_q.q)); - if (++g_work_q.size > g_work_q.max_size) - g_work_q.max_size = g_work_q.size; - - unlock_queue(&g_work_q); - - /* tell the work thread that work is available */ - sem_post(&g_work_queued_sema); -} - -static void +static inline void destroy_work_item(work_q_item_t *item) { - sem_destroy(&item->wait_sem); + sem_destroy(&item->wait_sem); /* Destroy the item lock */ + /* Return the item to the free item queue for later reuse */ lock_queue(&g_free_q); sq_addfirst(&item->link, &(g_free_q.q)); + /* Update the queue size and potentially the maximum queue size */ if (++g_free_q.size > g_free_q.max_size) g_free_q.max_size = g_free_q.size; unlock_queue(&g_free_q); } -static work_q_item_t * +static inline work_q_item_t * dequeue_work_item(void) { work_q_item_t *work; + + /* retrieve the 1st item on the work queue */ lock_queue(&g_work_q); if ((work = (work_q_item_t *)sq_remfirst(&g_work_q.q))) @@ -229,6 +223,32 @@ dequeue_work_item(void) return work; } +static int +enqueue_work_item_and_wait_for_result(work_q_item_t *item) +{ + /* put the work item at the end of the work queue */ + lock_queue(&g_work_q); + sq_addlast(&item->link, &(g_work_q.q)); + + /* Adjust the queue size and potentially the maximum queue size */ + if (++g_work_q.size > g_work_q.max_size) + g_work_q.max_size = g_work_q.size; + + unlock_queue(&g_work_q); + + /* tell the work thread that work is available */ + sem_post(&g_work_queued_sema); + + /* wait for the result */ + sem_wait(&item->wait_sem); + + int result = item->result; + + destroy_work_item(item); + + return result; +} + /* Calculate the offset in file of specific item */ static int calculate_offset(dm_item_t item, unsigned char index) @@ -250,6 +270,8 @@ calculate_offset(dm_item_t item, unsigned char index) * * byte 0: Length of user data item * byte 1: Persistence of this data item + * byte 2: Unused (for future use) + * byte 3: Unused (for future use) * byte DM_SECTOR_HDR_SIZE... : data item value * * The total size must not exceed k_sector_size @@ -266,6 +288,7 @@ _write(dm_item_t item, unsigned char index, dm_persitence_t persistence, const v /* Get the offset for this item */ offset = calculate_offset(item, index); + /* If item type or index out of range, return error */ if (offset < 0) return -1; @@ -283,10 +306,12 @@ _write(dm_item_t item, unsigned char index, dm_persitence_t persistence, const v len = -1; + /* Seek to the right spot in the data manager file and write the data item */ if (lseek(g_task_fd, offset, SEEK_SET) == offset) if ((len = write(g_task_fd, buffer, count)) == count) - fsync(g_task_fd); + fsync(g_task_fd); /* Make sure data is written to physical media */ + /* Make sure the write succeeded */ if (len != count) return -1; @@ -304,6 +329,7 @@ _read(dm_item_t item, unsigned char index, void *buf, size_t count) /* Get the offset for this item */ offset = calculate_offset(item, index); + /* If item type or index out of range, return error */ if (offset < 0) return -1; @@ -316,14 +342,17 @@ _read(dm_item_t item, unsigned char index, void *buf, size_t count) if (lseek(g_task_fd, offset, SEEK_SET) == offset) len = read(g_task_fd, buffer, count + DM_SECTOR_HDR_SIZE); - /* Check for length issues */ + /* Check for read error */ if (len < 0) return -1; + /* A zero length entry is a empty entry */ if (len == 0) buffer[0] = 0; + /* See if we got data */ if (buffer[0] > 0) { + /* We got more than requested!!! */ if (buffer[0] > count) return -1; @@ -340,11 +369,14 @@ _clear(dm_item_t item) { int i, result = 0; + /* Get the offset of 1st item of this type */ int offset = calculate_offset(item, 0); + /* Check for item type out of range */ if (offset < 0) return -1; + /* Clear all items of this type */ for (i = 0; (unsigned)i < g_per_item_max_index[item]; i++) { char buf[1]; @@ -353,9 +385,11 @@ _clear(dm_item_t item) break; } + /* Avoid SD flash wear by only doing writes where necessary */ if (read(g_task_fd, buf, 1) < 1) break; + /* If item has length greater than 0 it needs to be overwritten */ if (buf[0]) { if (lseek(g_task_fd, offset, SEEK_SET) != offset) { result = -1; @@ -373,6 +407,7 @@ _clear(dm_item_t item) offset += k_sector_size; } + /* Make sure data is actually written to physical media */ fsync(g_task_fd); return result; } @@ -452,12 +487,13 @@ dm_write(dm_item_t item, unsigned char index, dm_persitence_t persistence, const { work_q_item_t *work; + /* Make sure data manager has been started and is not shutting down */ if ((g_fd < 0) || g_task_should_exit) return -1; - /* Will return with queues locked */ + /* get a work item and queue up a write request */ if ((work = create_work_item()) == NULL) - return -1; /* queues unlocked on failure */ + return -1; work->func = dm_write_func; work->write_params.item = item; @@ -465,12 +501,9 @@ dm_write(dm_item_t item, unsigned char index, dm_persitence_t persistence, const work->write_params.persistence = persistence; work->write_params.buf = buf; work->write_params.count = count; - enqueue_work_item(work); - sem_wait(&work->wait_sem); - ssize_t result = work->result; - destroy_work_item(work); - return result; + /* Enqueue the item on the work queue and wait for the worker thread to complete processing it */ + return (ssize_t)enqueue_work_item_and_wait_for_result(work); } /* Retrieve from the data manager file */ @@ -479,24 +512,22 @@ dm_read(dm_item_t item, unsigned char index, void *buf, size_t count) { work_q_item_t *work; + /* Make sure data manager has been started and is not shutting down */ if ((g_fd < 0) || g_task_should_exit) return -1; - /* Will return with queues locked */ + /* get a work item and queue up a read request */ if ((work = create_work_item()) == NULL) - return -1; /* queues unlocked on failure */ + return -1; work->func = dm_read_func; work->read_params.item = item; work->read_params.index = index; work->read_params.buf = buf; work->read_params.count = count; - enqueue_work_item(work); - sem_wait(&work->wait_sem); - ssize_t result = work->result; - destroy_work_item(work); - return result; + /* Enqueue the item on the work queue and wait for the worker thread to complete processing it */ + return (ssize_t)enqueue_work_item_and_wait_for_result(work); } __EXPORT int @@ -504,21 +535,19 @@ dm_clear(dm_item_t item) { work_q_item_t *work; + /* Make sure data manager has been started and is not shutting down */ if ((g_fd < 0) || g_task_should_exit) return -1; - /* Will return with queues locked */ + /* get a work item and queue up a clear request */ if ((work = create_work_item()) == NULL) - return -1; /* queues unlocked on failure */ + return -1; work->func = dm_clear_func; work->clear_params.item = item; - enqueue_work_item(work); - sem_wait(&work->wait_sem); - int result = work->result; - destroy_work_item(work); - return result; + /* Enqueue the item on the work queue and wait for the worker thread to complete processing it */ + return enqueue_work_item_and_wait_for_result(work); } /* Tell the data manager about the type of the last reset */ @@ -527,21 +556,19 @@ dm_restart(dm_reset_reason reason) { work_q_item_t *work; + /* Make sure data manager has been started and is not shutting down */ if ((g_fd < 0) || g_task_should_exit) return -1; - /* Will return with queues locked */ + /* get a work item and queue up a restart request */ if ((work = create_work_item()) == NULL) - return -1; /* queues unlocked on failure */ + return -1; work->func = dm_restart_func; work->restart_params.reason = reason; - enqueue_work_item(work); - sem_wait(&work->wait_sem); - int result = work->result; - destroy_work_item(work); - return result; + /* Enqueue the item on the work queue and wait for the worker thread to complete processing it */ + return enqueue_work_item_and_wait_for_result(work); } static int @@ -570,24 +597,29 @@ task_main(int argc, char *argv[]) sem_init(&g_work_queued_sema, 1, 0); + /* Open or create the data manager file */ g_task_fd = open(k_data_manager_device_path, O_RDWR | O_CREAT | O_BINARY); if (g_task_fd < 0) { warnx("Could not open data manager file %s", k_data_manager_device_path); - sem_post(&g_init_sema); + sem_post(&g_init_sema); /* Don't want to hang startup */ return -1; } if (lseek(g_task_fd, max_offset, SEEK_SET) != max_offset) { close(g_task_fd); warnx("Could not seek data manager file %s", k_data_manager_device_path); - sem_post(&g_init_sema); + sem_post(&g_init_sema); /* Don't want to hang startup */ return -1; } fsync(g_task_fd); + /* We use two file descriptors, one for the caller context and one for the worker thread */ + /* They are actually the same but we need to some way to reject caller request while the */ + /* worker thread is shutting down but still processing requests */ g_fd = g_task_fd; warnx("Initialized, data manager file '%s' size is %d bytes", k_data_manager_device_path, max_offset); + /* Tell startup that the worker thread has completed its initialization */ sem_post(&g_init_sema); /* Start the endless loop, waiting for then processing work requests */ @@ -595,7 +627,7 @@ task_main(int argc, char *argv[]) /* do we need to exit ??? */ if ((g_task_should_exit) && (g_fd >= 0)) { - /* Close the file handle to stop further queueing */ + /* Close the file handle to stop further queuing */ g_fd = -1; } @@ -607,6 +639,7 @@ task_main(int argc, char *argv[]) /* Empty the work queue */ while ((work = dequeue_work_item())) { + /* handle each work item with the appropriate handler */ switch (work->func) { case dm_write_func: g_func_counts[dm_write_func]++; @@ -647,7 +680,7 @@ task_main(int argc, char *argv[]) close(g_task_fd); g_task_fd = -1; - /* Empty the work queue */ + /* The work queue is now empty, empty the free queue */ for (;;) { if ((work = (work_q_item_t *)sq_remfirst(&(g_free_q.q))) == NULL) break; @@ -669,7 +702,7 @@ start(void) sem_init(&g_init_sema, 1, 0); - /* start the task */ + /* start the worker thread */ if ((task = task_spawn_cmd("dataman", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 2048, task_main, NULL)) <= 0) { warn("task start failed"); return -1; @@ -704,7 +737,7 @@ stop(void) static void usage(void) { - errx(1, "usage: dataman {start|stop|status}"); + errx(1, "usage: dataman {start|stop|status|poweronrestart|inflightrestart}"); } int @@ -726,6 +759,7 @@ dataman_main(int argc, char *argv[]) exit(0); } + /* Worker thread should be running for all other commands */ if (g_fd < 0) errx(1, "not running"); @@ -733,6 +767,10 @@ dataman_main(int argc, char *argv[]) stop(); else if (!strcmp(argv[1], "status")) status(); + else if (!strcmp(argv[1], "poweronrestart")) + dm_restart(DM_INIT_REASON_POWER_ON); + else if (!strcmp(argv[1], "inflightrestart")) + dm_restart(DM_INIT_REASON_IN_FLIGHT); else usage(); diff --git a/src/systemcmds/tests/test_dataman.c b/src/systemcmds/tests/test_dataman.c index 5b121e34ed..1f844a97de 100644 --- a/src/systemcmds/tests/test_dataman.c +++ b/src/systemcmds/tests/test_dataman.c @@ -89,7 +89,7 @@ task_main(int argc, char *argv[]) unsigned hash = i ^ my_id; unsigned len = (hash & 63) + 2; - int ret = dm_write(DM_KEY_WAYPOINTS_OFFBOARD, hash, DM_PERSIST_IN_FLIGHT_RESET, buffer, len); + int ret = dm_write(DM_KEY_WAYPOINTS_OFFBOARD_1, hash, DM_PERSIST_IN_FLIGHT_RESET, buffer, len); warnx("ret: %d", ret); if (ret != len) { warnx("%d write failed, index %d, length %d", my_id, hash, len); @@ -103,7 +103,7 @@ task_main(int argc, char *argv[]) for (unsigned i = 0; i < NUM_MISSIONS_SUPPORTED; i++) { unsigned hash = i ^ my_id; unsigned len2, len = (hash & 63) + 2; - if ((len2 = dm_read(DM_KEY_WAYPOINTS_OFFBOARD, hash, buffer, sizeof(buffer))) < 2) { + if ((len2 = dm_read(DM_KEY_WAYPOINTS_OFFBOARD_1, hash, buffer, sizeof(buffer))) < 2) { warnx("%d read failed length test, index %d", my_id, hash); goto fail; } @@ -163,7 +163,7 @@ int test_dataman(int argc, char *argv[]) free(sems); dm_restart(DM_INIT_REASON_IN_FLIGHT); for (i = 0; i < NUM_MISSIONS_SUPPORTED; i++) { - if (dm_read(DM_KEY_WAYPOINTS_OFFBOARD, i, buffer, sizeof(buffer)) != 0) + if (dm_read(DM_KEY_WAYPOINTS_OFFBOARD_1, i, buffer, sizeof(buffer)) != 0) break; } if (i >= NUM_MISSIONS_SUPPORTED) { @@ -173,7 +173,7 @@ int test_dataman(int argc, char *argv[]) } dm_restart(DM_INIT_REASON_POWER_ON); for (i = 0; i < NUM_MISSIONS_SUPPORTED; i++) { - if (dm_read(DM_KEY_WAYPOINTS_OFFBOARD, i, buffer, sizeof(buffer)) != 0) { + if (dm_read(DM_KEY_WAYPOINTS_OFFBOARD_1, i, buffer, sizeof(buffer)) != 0) { warnx("Restart power-on failed"); return -1; } From 150bac4b51492b556b4e5e60236b3c239a1921db Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 2 Feb 2014 15:17:49 +0100 Subject: [PATCH 400/447] Sensors messaging cleanup --- src/modules/sensors/sensors.cpp | 39 +++++++++++++++++---------------- 1 file changed, 20 insertions(+), 19 deletions(-) diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index bd665b592b..b176d7417a 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -1,7 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: Lorenz Meier + * Copyright (c) 2012-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 @@ -636,41 +635,43 @@ Sensors::parameters_update() if (!rc_valid) warnx("WARNING WARNING WARNING\n\nRC CALIBRATION NOT SANE!\n\n"); + const char *paramerr = "FAIL PARM LOAD"; + /* channel mapping */ if (param_get(_parameter_handles.rc_map_roll, &(_parameters.rc_map_roll)) != OK) { - warnx("Failed getting roll chan index"); + warnx(paramerr); } if (param_get(_parameter_handles.rc_map_pitch, &(_parameters.rc_map_pitch)) != OK) { - warnx("Failed getting pitch chan index"); + warnx(paramerr); } if (param_get(_parameter_handles.rc_map_yaw, &(_parameters.rc_map_yaw)) != OK) { - warnx("Failed getting yaw chan index"); + warnx(paramerr); } if (param_get(_parameter_handles.rc_map_throttle, &(_parameters.rc_map_throttle)) != OK) { - warnx("Failed getting throttle chan index"); + warnx(paramerr); } if (param_get(_parameter_handles.rc_map_mode_sw, &(_parameters.rc_map_mode_sw)) != OK) { - warnx("Failed getting mode sw chan index"); + warnx(paramerr); } if (param_get(_parameter_handles.rc_map_return_sw, &(_parameters.rc_map_return_sw)) != OK) { - warnx("Failed getting return sw chan index"); + warnx(paramerr); } if (param_get(_parameter_handles.rc_map_assisted_sw, &(_parameters.rc_map_assisted_sw)) != OK) { - warnx("Failed getting assisted sw chan index"); + warnx(paramerr); } if (param_get(_parameter_handles.rc_map_mission_sw, &(_parameters.rc_map_mission_sw)) != OK) { - warnx("Failed getting mission sw chan index"); + warnx(paramerr); } if (param_get(_parameter_handles.rc_map_flaps, &(_parameters.rc_map_flaps)) != OK) { - warnx("Failed getting flaps chan index"); + warnx(paramerr); } // if (param_get(_parameter_handles.rc_map_offboard_ctrl_mode_sw, &(_parameters.rc_map_offboard_ctrl_mode_sw)) != OK) { @@ -742,12 +743,12 @@ Sensors::parameters_update() /* scaling of ADC ticks to battery voltage */ if (param_get(_parameter_handles.battery_voltage_scaling, &(_parameters.battery_voltage_scaling)) != OK) { - warnx("Failed updating voltage scaling param"); + warnx(paramerr); } /* scaling of ADC ticks to battery current */ if (param_get(_parameter_handles.battery_current_scaling, &(_parameters.battery_current_scaling)) != OK) { - warnx("Failed updating current scaling param"); + warnx(paramerr); } param_get(_parameter_handles.board_rotation, &(_parameters.board_rotation)); @@ -1650,17 +1651,17 @@ int sensors_main(int argc, char *argv[]) if (!strcmp(argv[1], "start")) { if (sensors::g_sensors != nullptr) - errx(0, "sensors task already running"); + errx(0, "already running"); sensors::g_sensors = new Sensors; if (sensors::g_sensors == nullptr) - errx(1, "sensors task alloc failed"); + errx(1, "alloc failed"); if (OK != sensors::g_sensors->start()) { delete sensors::g_sensors; sensors::g_sensors = nullptr; - err(1, "sensors task start failed"); + err(1, "start failed"); } exit(0); @@ -1668,7 +1669,7 @@ int sensors_main(int argc, char *argv[]) if (!strcmp(argv[1], "stop")) { if (sensors::g_sensors == nullptr) - errx(1, "sensors task not running"); + errx(1, "not running"); delete sensors::g_sensors; sensors::g_sensors = nullptr; @@ -1677,10 +1678,10 @@ int sensors_main(int argc, char *argv[]) if (!strcmp(argv[1], "status")) { if (sensors::g_sensors) { - errx(0, "task is running"); + errx(0, "is running"); } else { - errx(1, "task is not running"); + errx(1, "not running"); } } From 3d21a73ddf18b89552aa9bd65965ff6b311487b8 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 2 Feb 2014 22:04:11 +0100 Subject: [PATCH 401/447] navigator: fixed infinite RTL->LOITER->RTL... loop on failsafe --- src/modules/navigator/navigator_main.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 3ef2009c71..e0d5df5f78 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -688,7 +688,8 @@ Navigator::task_main() /* RETURN switch, overrides MISSION switch */ if (_vstatus.return_switch == RETURN_SWITCH_RETURN) { /* switch to RTL if not already landed after RTL and home position set */ - if (!(_rtl_state == RTL_STATE_DESCEND && (myState == NAV_STATE_READY || myState == NAV_STATE_LAND)) && + if (!(_rtl_state == RTL_STATE_DESCEND && + (myState == NAV_STATE_READY || myState == NAV_STATE_LAND || myState == NAV_STATE_LOITER)) && _vstatus.condition_home_position_valid) { dispatch(EVENT_RTL_REQUESTED); } @@ -746,7 +747,8 @@ Navigator::task_main() break; case NAV_STATE_RTL: - if (!(_rtl_state == RTL_STATE_DESCEND && (myState == NAV_STATE_READY || myState == NAV_STATE_LAND)) && + if (!(_rtl_state == RTL_STATE_DESCEND && + (myState == NAV_STATE_READY || myState == NAV_STATE_LAND || myState == NAV_STATE_LOITER)) && _vstatus.condition_home_position_valid) { dispatch(EVENT_RTL_REQUESTED); } From 8de35025b5b28a0f40b5c17ca9baa5acefdc20ca Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 2 Feb 2014 22:06:40 +0100 Subject: [PATCH 402/447] navigator: avoid climbing up for LOITER after RTL --- src/modules/navigator/navigator_main.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index e0d5df5f78..5139ae6cd2 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -1575,6 +1575,7 @@ Navigator::on_mission_item_reached() dispatch(EVENT_LAND_REQUESTED); } else { + _reset_loiter_pos = false; dispatch(EVENT_LOITER_REQUESTED); } From d5caffa8450798f4bf65ff518395152136b75df4 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 4 Feb 2014 16:52:47 +0100 Subject: [PATCH 403/447] startup: make sdlog2 working again on FMUv1 --- ROMFS/px4fmu_common/init.d/rc.logging | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.logging b/ROMFS/px4fmu_common/init.d/rc.logging index dcf5bbced8..c5aef8273c 100644 --- a/ROMFS/px4fmu_common/init.d/rc.logging +++ b/ROMFS/px4fmu_common/init.d/rc.logging @@ -8,7 +8,7 @@ then if hw_ver compare PX4FMU_V1 then echo "Start sdlog2 at 50Hz" - sdlog2 start -r 50 -a -b 16 -t + sdlog2 start -r 50 -a -b 8 -t else echo "Start sdlog2 at 200Hz" sdlog2 start -r 200 -a -b 16 -t From b457e714972649f5a39fa3f2f9463bdfe6971fcc Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 4 Feb 2014 16:53:36 +0100 Subject: [PATCH 404/447] position_estimator_inav: lower RAM --- .../position_estimator_inav/position_estimator_inav_main.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index e045ce4cca..87f2c3330e 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -133,7 +133,7 @@ int position_estimator_inav_main(int argc, char *argv[]) thread_should_exit = false; position_estimator_inav_task = task_spawn_cmd("position_estimator_inav", - SCHED_RR, SCHED_PRIORITY_MAX - 5, 4096, + SCHED_RR, SCHED_PRIORITY_MAX - 5, 3000, position_estimator_inav_thread_main, (argv) ? (const char **) &argv[2] : (const char **) NULL); exit(0); From 4499919f76376f5f9904d672ad5fd85e465badac Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 4 Feb 2014 18:27:06 +0100 Subject: [PATCH 405/447] Fix for hexa mixer --- ROMFS/px4fmu_common/init.d/6001_hexa_x | 4 +++- ROMFS/px4fmu_common/mixers/FMU_hex_+.mix | 11 ----------- ROMFS/px4fmu_common/mixers/FMU_hex_x.mix | 11 ----------- 3 files changed, 3 insertions(+), 23 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/6001_hexa_x b/ROMFS/px4fmu_common/init.d/6001_hexa_x index 270f51a582..fd57565860 100644 --- a/ROMFS/px4fmu_common/init.d/6001_hexa_x +++ b/ROMFS/px4fmu_common/init.d/6001_hexa_x @@ -9,4 +9,6 @@ sh /etc/init.d/4001_quad_x set MIXER FMU_hexa_x -set PWM_OUTPUTS 123456 +# We only can run one channel group with one rate, +# so all 8 at 400 Hz +set PWM_OUTPUTS 12345678 diff --git a/ROMFS/px4fmu_common/mixers/FMU_hex_+.mix b/ROMFS/px4fmu_common/mixers/FMU_hex_+.mix index f8f9f0e4dc..b5e38ce9ef 100644 --- a/ROMFS/px4fmu_common/mixers/FMU_hex_+.mix +++ b/ROMFS/px4fmu_common/mixers/FMU_hex_+.mix @@ -5,14 +5,3 @@ This file defines a single mixer for a hexacopter in the + configuration. All co are mixed 100%. R: 6+ 10000 10000 10000 0 - -Gimbal / payload mixer for last two channels ------------------------------------------------------ - -M: 1 -O: 10000 10000 0 -10000 10000 -S: 0 6 10000 10000 0 -10000 10000 - -M: 1 -O: 10000 10000 0 -10000 10000 -S: 0 7 10000 10000 0 -10000 10000 diff --git a/ROMFS/px4fmu_common/mixers/FMU_hex_x.mix b/ROMFS/px4fmu_common/mixers/FMU_hex_x.mix index 26b40b9e95..8e8d122adc 100644 --- a/ROMFS/px4fmu_common/mixers/FMU_hex_x.mix +++ b/ROMFS/px4fmu_common/mixers/FMU_hex_x.mix @@ -5,14 +5,3 @@ This file defines a single mixer for a hexacopter in the X configuration. All c are mixed 100%. R: 6x 10000 10000 10000 0 - -Gimbal / payload mixer for last two channels ------------------------------------------------------ - -M: 1 -O: 10000 10000 0 -10000 10000 -S: 0 6 10000 10000 0 -10000 10000 - -M: 1 -O: 10000 10000 0 -10000 10000 -S: 0 7 10000 10000 0 -10000 10000 From d92903720d09c2d3ed273620ac8a817cff3ee42d Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 5 Feb 2014 10:22:10 +0100 Subject: [PATCH 406/447] Revert "position_estimator_inav: lower RAM" This reverts commit b457e714972649f5a39fa3f2f9463bdfe6971fcc. --- .../position_estimator_inav/position_estimator_inav_main.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index 87f2c3330e..e045ce4cca 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -133,7 +133,7 @@ int position_estimator_inav_main(int argc, char *argv[]) thread_should_exit = false; position_estimator_inav_task = task_spawn_cmd("position_estimator_inav", - SCHED_RR, SCHED_PRIORITY_MAX - 5, 3000, + SCHED_RR, SCHED_PRIORITY_MAX - 5, 4096, position_estimator_inav_thread_main, (argv) ? (const char **) &argv[2] : (const char **) NULL); exit(0); From 15f021eec2042b81f286ac26a7dc9eb86cd89442 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 5 Feb 2014 19:03:26 +0100 Subject: [PATCH 407/447] Startup: Hex vs Hexa --- ROMFS/px4fmu_common/init.d/rcS | 2 +- ROMFS/px4fmu_common/mixers/{FMU_hex_+.mix => FMU_hexa_+.mix} | 0 ROMFS/px4fmu_common/mixers/{FMU_hex_x.mix => FMU_hexa_x.mix} | 0 3 files changed, 1 insertion(+), 1 deletion(-) rename ROMFS/px4fmu_common/mixers/{FMU_hex_+.mix => FMU_hexa_+.mix} (100%) rename ROMFS/px4fmu_common/mixers/{FMU_hex_x.mix => FMU_hexa_x.mix} (100%) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 84dc7ec640..178bcaeba5 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -474,7 +474,7 @@ then set MAV_TYPE 2 # Use mixer to detect vehicle type - if [ $MIXER == FMU_hex_x -o $MIXER == FMU_hex_+ ] + if [ $MIXER == FMU_hexa_x -o $MIXER == FMU_hexa_+ ] then set MAV_TYPE 13 fi diff --git a/ROMFS/px4fmu_common/mixers/FMU_hex_+.mix b/ROMFS/px4fmu_common/mixers/FMU_hexa_+.mix similarity index 100% rename from ROMFS/px4fmu_common/mixers/FMU_hex_+.mix rename to ROMFS/px4fmu_common/mixers/FMU_hexa_+.mix diff --git a/ROMFS/px4fmu_common/mixers/FMU_hex_x.mix b/ROMFS/px4fmu_common/mixers/FMU_hexa_x.mix similarity index 100% rename from ROMFS/px4fmu_common/mixers/FMU_hex_x.mix rename to ROMFS/px4fmu_common/mixers/FMU_hexa_x.mix From 0f0c5f74b3341f12f38ebe06f3a8ec402ba49f43 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 5 Feb 2014 19:44:31 +0100 Subject: [PATCH 408/447] Startup: Raise min throttle --- ROMFS/px4fmu_common/init.d/4001_quad_x | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ROMFS/px4fmu_common/init.d/4001_quad_x b/ROMFS/px4fmu_common/init.d/4001_quad_x index 42b64a68ea..0f1288dec3 100644 --- a/ROMFS/px4fmu_common/init.d/4001_quad_x +++ b/ROMFS/px4fmu_common/init.d/4001_quad_x @@ -48,5 +48,5 @@ set MIXER FMU_quad_x set PWM_OUTPUTS 1234 set PWM_RATE 400 set PWM_DISARMED 900 -set PWM_MIN 1000 +set PWM_MIN 1100 set PWM_MAX 2000 From 3392086f8f317823061363296c8d3e18be4ee88e Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 5 Feb 2014 21:19:25 +0100 Subject: [PATCH 409/447] Mixer load: don't upload empty mixers from non-existing files --- src/modules/systemlib/mixer/mixer_load.c | 6 ++++-- src/systemcmds/mixer/mixer.cpp | 3 ++- 2 files changed, 6 insertions(+), 3 deletions(-) diff --git a/src/modules/systemlib/mixer/mixer_load.c b/src/modules/systemlib/mixer/mixer_load.c index a55ddf8a35..b05273c0de 100644 --- a/src/modules/systemlib/mixer/mixer_load.c +++ b/src/modules/systemlib/mixer/mixer_load.c @@ -52,7 +52,8 @@ int load_mixer_file(const char *fname, char *buf, unsigned maxlen) /* open the mixer definition file */ fp = fopen(fname, "r"); if (fp == NULL) { - return 1; + warnx("file not found"); + return -1; } /* read valid lines from the file into a buffer */ @@ -88,7 +89,8 @@ int load_mixer_file(const char *fname, char *buf, unsigned maxlen) /* if the line is too long to fit in the buffer, bail */ if ((strlen(line) + strlen(buf) + 1) >= maxlen) { - return 1; + warnx("line too long"); + return -1; } /* add the line to the buffer */ diff --git a/src/systemcmds/mixer/mixer.cpp b/src/systemcmds/mixer/mixer.cpp index 6da39d371b..2f5ed3265d 100644 --- a/src/systemcmds/mixer/mixer.cpp +++ b/src/systemcmds/mixer/mixer.cpp @@ -102,7 +102,8 @@ load(const char *devname, const char *fname) if (ioctl(dev, MIXERIOCRESET, 0)) err(1, "can't reset mixers on %s", devname); - load_mixer_file(fname, &buf[0], sizeof(buf)); + if (load_mixer_file(fname, &buf[0], sizeof(buf)) < 0) + err(1, "can't load mixer: %s", fname); /* XXX pass the buffer to the device */ int ret = ioctl(dev, MIXERIOCLOADBUF, (unsigned long)buf); From 70e1bfa4d69e967b309d177060804e7567f148b2 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 7 Feb 2014 22:28:42 +0100 Subject: [PATCH 410/447] Startup scripts: use rc.mc_defaults for default MC parameters --- .../px4fmu_common/init.d/10015_tbs_discovery | 41 +--------------- ROMFS/px4fmu_common/init.d/10016_3dr_iris | 38 +-------------- ROMFS/px4fmu_common/init.d/1001_rc_quad_x.hil | 4 +- ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil | 4 +- ROMFS/px4fmu_common/init.d/12001_octo_cox | 4 +- ROMFS/px4fmu_common/init.d/4001_quad_x | 44 +---------------- ROMFS/px4fmu_common/init.d/4010_dji_f330 | 47 +------------------ ROMFS/px4fmu_common/init.d/4011_dji_f450 | 47 +------------------ ROMFS/px4fmu_common/init.d/4012_hk_x550 | 6 --- ROMFS/px4fmu_common/init.d/5001_quad_+ | 4 +- ROMFS/px4fmu_common/init.d/6001_hexa_x | 4 +- ROMFS/px4fmu_common/init.d/7001_hexa_+ | 6 ++- ROMFS/px4fmu_common/init.d/8001_octo_x | 4 +- ROMFS/px4fmu_common/init.d/9001_octo_+ | 4 +- ROMFS/px4fmu_common/init.d/rc.mc_defaults | 45 ++++++++++++++++++ 15 files changed, 79 insertions(+), 223 deletions(-) create mode 100644 ROMFS/px4fmu_common/init.d/rc.mc_defaults diff --git a/ROMFS/px4fmu_common/init.d/10015_tbs_discovery b/ROMFS/px4fmu_common/init.d/10015_tbs_discovery index a3d7c3d977..b09765e8e7 100644 --- a/ROMFS/px4fmu_common/init.d/10015_tbs_discovery +++ b/ROMFS/px4fmu_common/init.d/10015_tbs_discovery @@ -5,45 +5,8 @@ # Simon Wilks # -if [ $DO_AUTOCONFIG == yes ] -then - # - # Default parameters for this platform - # - param set MC_ROLL_P 5.0 - param set MC_ROLLRATE_P 0.17 - param set MC_ROLLRATE_I 0.0 - param set MC_ROLLRATE_D 0.006 - param set MC_PITCH_P 5.0 - param set MC_PITCHRATE_P 0.17 - param set MC_PITCHRATE_I 0.0 - param set MC_PITCHRATE_D 0.006 - param set MC_YAW_P 0.5 - param set MC_YAWRATE_P 0.2 - param set MC_YAWRATE_I 0.0 - param set MC_YAWRATE_D 0.0 +sh /etc/init.d/rc.mc_defaults - param set MPC_THR_MAX 1.0 - param set MPC_THR_MIN 0.1 - param set MPC_XY_P 1.0 - param set MPC_XY_VEL_P 0.1 - param set MPC_XY_VEL_I 0.02 - param set MPC_XY_VEL_D 0.01 - param set MPC_XY_VEL_MAX 5 - param set MPC_XY_FF 0.5 - param set MPC_Z_P 1.0 - param set MPC_Z_VEL_P 0.1 - param set MPC_Z_VEL_I 0.02 - param set MPC_Z_VEL_D 0.0 - param set MPC_Z_VEL_MAX 3 - param set MPC_Z_FF 0.5 - param set MPC_TILT_MAX 1.0 - param set MPC_LAND_SPEED 1.0 - param set MPC_LAND_TILT 0.3 -fi - -set VEHICLE_TYPE mc set MIXER FMU_quad_w -set PWM_OUTPUTS 1234 -set PWM_RATE 400 +set PWM_OUTPUTS 1234 \ No newline at end of file diff --git a/ROMFS/px4fmu_common/init.d/10016_3dr_iris b/ROMFS/px4fmu_common/init.d/10016_3dr_iris index 1c7ecb7129..42d5169935 100644 --- a/ROMFS/px4fmu_common/init.d/10016_3dr_iris +++ b/ROMFS/px4fmu_common/init.d/10016_3dr_iris @@ -5,51 +5,17 @@ # Anton Babushkin # +sh /etc/init.d/rc.mc_defaults + if [ $DO_AUTOCONFIG == yes ] then # # Default parameters for this platform # - param set MC_ROLL_P 9.0 - param set MC_ROLLRATE_P 0.13 - param set MC_ROLLRATE_I 0.0 - param set MC_ROLLRATE_D 0.004 - param set MC_PITCH_P 9.0 - param set MC_PITCHRATE_P 0.13 - param set MC_PITCHRATE_I 0.0 - param set MC_PITCHRATE_D 0.004 - param set MC_YAW_P 0.5 - param set MC_YAWRATE_P 0.2 - param set MC_YAWRATE_I 0.0 - param set MC_YAWRATE_D 0.0 - - param set MPC_THR_MAX 1.0 - param set MPC_THR_MIN 0.1 - param set MPC_XY_P 1.0 - param set MPC_XY_VEL_P 0.1 - param set MPC_XY_VEL_I 0.02 - param set MPC_XY_VEL_D 0.01 - param set MPC_XY_VEL_MAX 5 - param set MPC_XY_FF 0.5 - param set MPC_Z_P 1.0 - param set MPC_Z_VEL_P 0.1 - param set MPC_Z_VEL_I 0.02 - param set MPC_Z_VEL_D 0.0 - param set MPC_Z_VEL_MAX 3 - param set MPC_Z_FF 0.5 - param set MPC_TILT_MAX 1.0 - param set MPC_LAND_SPEED 1.0 - param set MPC_LAND_TILT 0.3 - param set BAT_V_SCALING 0.00989 param set BAT_C_SCALING 0.0124 fi -set VEHICLE_TYPE mc set MIXER FMU_quad_w set PWM_OUTPUTS 1234 -set PWM_RATE 400 -set PWM_DISARMED 900 -set PWM_MIN 1000 -set PWM_MAX 2000 diff --git a/ROMFS/px4fmu_common/init.d/1001_rc_quad_x.hil b/ROMFS/px4fmu_common/init.d/1001_rc_quad_x.hil index 45880f44be..7a7a9542c0 100644 --- a/ROMFS/px4fmu_common/init.d/1001_rc_quad_x.hil +++ b/ROMFS/px4fmu_common/init.d/1001_rc_quad_x.hil @@ -5,6 +5,8 @@ # Anton Babushkin # -sh /etc/init.d/4001_quad_x +sh /etc/init.d/rc.mc_defaults + +set MIXER FMU_quad_x set HIL yes diff --git a/ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil b/ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil index 2b26810e71..c47500c7ac 100644 --- a/ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil +++ b/ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil @@ -5,6 +5,8 @@ # Anton Babushkin # -sh /etc/init.d/1001_rc_quad_x.hil +sh /etc/init.d/rc.mc_defaults set MIXER FMU_quad_+ + +set HIL yes \ No newline at end of file diff --git a/ROMFS/px4fmu_common/init.d/12001_octo_cox b/ROMFS/px4fmu_common/init.d/12001_octo_cox index 8a813595e4..c5d3e78078 100644 --- a/ROMFS/px4fmu_common/init.d/12001_octo_cox +++ b/ROMFS/px4fmu_common/init.d/12001_octo_cox @@ -5,6 +5,8 @@ # Lorenz Meier , Anton Babushkin # -sh /etc/init.d/8001_octo_x +sh /etc/init.d/rc.mc_defaults set MIXER FMU_octo_cox + +set PWM_OUTPUTS 12345678 \ No newline at end of file diff --git a/ROMFS/px4fmu_common/init.d/4001_quad_x b/ROMFS/px4fmu_common/init.d/4001_quad_x index 0f1288dec3..fa751f1e31 100644 --- a/ROMFS/px4fmu_common/init.d/4001_quad_x +++ b/ROMFS/px4fmu_common/init.d/4001_quad_x @@ -5,48 +5,8 @@ # Lorenz Meier # -if [ $DO_AUTOCONFIG == yes ] -then - # - # Default parameters for this platform - # - param set MC_ROLL_P 7.0 - param set MC_ROLLRATE_P 0.12 - param set MC_ROLLRATE_I 0.0 - param set MC_ROLLRATE_D 0.004 - param set MC_PITCH_P 7.0 - param set MC_PITCHRATE_P 0.12 - param set MC_PITCHRATE_I 0.0 - param set MC_PITCHRATE_D 0.004 - param set MC_YAW_P 2.0 - param set MC_YAWRATE_P 0.3 - param set MC_YAWRATE_I 0.2 - param set MC_YAWRATE_D 0.005 - - param set MPC_THR_MAX 1.0 - param set MPC_THR_MIN 0.1 - param set MPC_XY_P 1.0 - param set MPC_XY_VEL_P 0.1 - param set MPC_XY_VEL_I 0.02 - param set MPC_XY_VEL_D 0.01 - param set MPC_XY_VEL_MAX 5 - param set MPC_XY_FF 0.5 - param set MPC_Z_P 1.0 - param set MPC_Z_VEL_P 0.1 - param set MPC_Z_VEL_I 0.02 - param set MPC_Z_VEL_D 0.0 - param set MPC_Z_VEL_MAX 3 - param set MPC_Z_FF 0.5 - param set MPC_TILT_MAX 1.0 - param set MPC_LAND_SPEED 1.0 - param set MPC_LAND_TILT 0.3 -fi +sh /etc/init.d/rc.mc_defaults -set VEHICLE_TYPE mc set MIXER FMU_quad_x -set PWM_OUTPUTS 1234 -set PWM_RATE 400 -set PWM_DISARMED 900 -set PWM_MIN 1100 -set PWM_MAX 2000 +set PWM_OUTPUTS 1234 \ No newline at end of file diff --git a/ROMFS/px4fmu_common/init.d/4010_dji_f330 b/ROMFS/px4fmu_common/init.d/4010_dji_f330 index 7829460339..232af57b4a 100644 --- a/ROMFS/px4fmu_common/init.d/4010_dji_f330 +++ b/ROMFS/px4fmu_common/init.d/4010_dji_f330 @@ -5,49 +5,6 @@ # Anton Babushkin # -if [ $DO_AUTOCONFIG == yes ] -then - # - # Default parameters for this platform - # - param set MC_ROLL_P 7.0 - param set MC_ROLLRATE_P 0.12 - param set MC_ROLLRATE_I 0.0 - param set MC_ROLLRATE_D 0.004 - param set MC_PITCH_P 7.0 - param set MC_PITCHRATE_P 0.12 - param set MC_PITCHRATE_I 0.0 - param set MC_PITCHRATE_D 0.004 - param set MC_YAW_P 2.8 - param set MC_YAWRATE_P 0.2 - param set MC_YAWRATE_I 0.05 - param set MC_YAWRATE_D 0.0 - - param set MPC_THR_MAX 1.0 - param set MPC_THR_MIN 0.1 - param set MPC_XY_P 1.0 - param set MPC_XY_VEL_P 0.1 - param set MPC_XY_VEL_I 0.02 - param set MPC_XY_VEL_D 0.01 - param set MPC_XY_VEL_MAX 5 - param set MPC_XY_FF 0.5 - param set MPC_Z_P 1.0 - param set MPC_Z_VEL_P 0.1 - param set MPC_Z_VEL_I 0.02 - param set MPC_Z_VEL_D 0.0 - param set MPC_Z_VEL_MAX 3 - param set MPC_Z_FF 0.5 - param set MPC_TILT_MAX 1.0 - param set MPC_LAND_SPEED 1.0 - param set MPC_LAND_TILT 0.3 -fi +sh /etc/init.d/4001_quad_x -set VEHICLE_TYPE mc -set MIXER FMU_quad_x - -set PWM_OUTPUTS 1234 -set PWM_RATE 400 -# DJI ESC range -set PWM_DISARMED 900 -set PWM_MIN 1200 -set PWM_MAX 1900 +set PWM_MIN 1175 \ No newline at end of file diff --git a/ROMFS/px4fmu_common/init.d/4011_dji_f450 b/ROMFS/px4fmu_common/init.d/4011_dji_f450 index 8eb53d1720..259636acc2 100644 --- a/ROMFS/px4fmu_common/init.d/4011_dji_f450 +++ b/ROMFS/px4fmu_common/init.d/4011_dji_f450 @@ -5,49 +5,6 @@ # Lorenz Meier # -if [ $DO_AUTOCONFIG == yes ] -then - # - # Default parameters for this platform - # - param set MC_ROLL_P 7.0 - param set MC_ROLLRATE_P 0.12 - param set MC_ROLLRATE_I 0.0 - param set MC_ROLLRATE_D 0.004 - param set MC_PITCH_P 7.0 - param set MC_PITCHRATE_P 0.12 - param set MC_PITCHRATE_I 0.0 - param set MC_PITCHRATE_D 0.004 - param set MC_YAW_P 2.0 - param set MC_YAWRATE_P 0.3 - param set MC_YAWRATE_I 0.2 - param set MC_YAWRATE_D 0.005 - - param set MPC_THR_MAX 1.0 - param set MPC_THR_MIN 0.1 - param set MPC_XY_P 1.0 - param set MPC_XY_VEL_P 0.1 - param set MPC_XY_VEL_I 0.02 - param set MPC_XY_VEL_D 0.01 - param set MPC_XY_VEL_MAX 5 - param set MPC_XY_FF 0.5 - param set MPC_Z_P 1.0 - param set MPC_Z_VEL_P 0.1 - param set MPC_Z_VEL_I 0.02 - param set MPC_Z_VEL_D 0.0 - param set MPC_Z_VEL_MAX 3 - param set MPC_Z_FF 0.5 - param set MPC_TILT_MAX 1.0 - param set MPC_LAND_SPEED 1.0 - param set MPC_LAND_TILT 0.3 -fi +sh /etc/init.d/4001_quad_x -set VEHICLE_TYPE mc -set MIXER FMU_quad_x - -set PWM_OUTPUTS 1234 -set PWM_RATE 400 -# DJI ESC range -set PWM_DISARMED 900 -set PWM_MIN 1200 -set PWM_MAX 1900 +set PWM_MIN 1175 \ No newline at end of file diff --git a/ROMFS/px4fmu_common/init.d/4012_hk_x550 b/ROMFS/px4fmu_common/init.d/4012_hk_x550 index 3d8e9fcf70..a5c4a86902 100644 --- a/ROMFS/px4fmu_common/init.d/4012_hk_x550 +++ b/ROMFS/px4fmu_common/init.d/4012_hk_x550 @@ -6,9 +6,3 @@ # sh /etc/init.d/4001_quad_x - -set PWM_OUTPUTS 1234 -set PWM_RATE 400 -set PWM_DISARMED 900 -set PWM_MIN 1000 -set PWM_MAX 2000 diff --git a/ROMFS/px4fmu_common/init.d/5001_quad_+ b/ROMFS/px4fmu_common/init.d/5001_quad_+ index ff11bccfe6..2f5ab44d77 100644 --- a/ROMFS/px4fmu_common/init.d/5001_quad_+ +++ b/ROMFS/px4fmu_common/init.d/5001_quad_+ @@ -5,6 +5,8 @@ # Anton Babushkin # -sh /etc/init.d/4001_quad_x +sh /etc/init.d/rc.mc_defaults set MIXER FMU_quad_+ + +set PWM_OUTPUTS 1234 \ No newline at end of file diff --git a/ROMFS/px4fmu_common/init.d/6001_hexa_x b/ROMFS/px4fmu_common/init.d/6001_hexa_x index fd57565860..73ef12569b 100644 --- a/ROMFS/px4fmu_common/init.d/6001_hexa_x +++ b/ROMFS/px4fmu_common/init.d/6001_hexa_x @@ -5,10 +5,10 @@ # Anton Babushkin # -sh /etc/init.d/4001_quad_x +sh /etc/init.d/rc.mc_defaults set MIXER FMU_hexa_x # We only can run one channel group with one rate, # so all 8 at 400 Hz -set PWM_OUTPUTS 12345678 +set PWM_OUTPUTS 12345678 \ No newline at end of file diff --git a/ROMFS/px4fmu_common/init.d/7001_hexa_+ b/ROMFS/px4fmu_common/init.d/7001_hexa_+ index 45279ec390..ef4b6297de 100644 --- a/ROMFS/px4fmu_common/init.d/7001_hexa_+ +++ b/ROMFS/px4fmu_common/init.d/7001_hexa_+ @@ -5,6 +5,10 @@ # Anton Babushkin # -sh /etc/init.d/6001_hexa_x +sh /etc/init.d/rc.mc_defaults set MIXER FMU_hexa_+ + +# We only can run one channel group with one rate, +# so all 8 at 400 Hz +set PWM_OUTPUTS 12345678 \ No newline at end of file diff --git a/ROMFS/px4fmu_common/init.d/8001_octo_x b/ROMFS/px4fmu_common/init.d/8001_octo_x index 6fa9626684..bb87f89fe7 100644 --- a/ROMFS/px4fmu_common/init.d/8001_octo_x +++ b/ROMFS/px4fmu_common/init.d/8001_octo_x @@ -5,8 +5,8 @@ # Anton Babushkin # -sh /etc/init.d/4001_quad_x +sh /etc/init.d/rc.mc_defaults set MIXER FMU_octo_x -set PWM_OUTPUTS 12345678 +set PWM_OUTPUTS 12345678 \ No newline at end of file diff --git a/ROMFS/px4fmu_common/init.d/9001_octo_+ b/ROMFS/px4fmu_common/init.d/9001_octo_+ index fa3869f9f9..81132fd3e7 100644 --- a/ROMFS/px4fmu_common/init.d/9001_octo_+ +++ b/ROMFS/px4fmu_common/init.d/9001_octo_+ @@ -5,6 +5,8 @@ # Anton Babushkin # -sh /etc/init.d/8001_octo_x +sh /etc/init.d/rc.mc_defaults set MIXER FMU_octo_+ + +set PWM_OUTPUTS 12345678 \ No newline at end of file diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_defaults b/ROMFS/px4fmu_common/init.d/rc.mc_defaults new file mode 100644 index 0000000000..14b6fe12cb --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/rc.mc_defaults @@ -0,0 +1,45 @@ +#!nsh + +set VEHICLE_TYPE mc + +if [ $DO_AUTOCONFIG == yes ] +then + # + # Default parameters for this platform + # + param set MC_ROLL_P 7.0 + param set MC_ROLLRATE_P 0.12 + param set MC_ROLLRATE_I 0.0 + param set MC_ROLLRATE_D 0.004 + param set MC_PITCH_P 7.0 + param set MC_PITCHRATE_P 0.12 + param set MC_PITCHRATE_I 0.0 + param set MC_PITCHRATE_D 0.004 + param set MC_YAW_P 2.0 + param set MC_YAWRATE_P 0.3 + param set MC_YAWRATE_I 0.2 + param set MC_YAWRATE_D 0.005 + + param set MPC_THR_MAX 1.0 + param set MPC_THR_MIN 0.1 + param set MPC_XY_P 1.0 + param set MPC_XY_VEL_P 0.1 + param set MPC_XY_VEL_I 0.02 + param set MPC_XY_VEL_D 0.01 + param set MPC_XY_VEL_MAX 5 + param set MPC_XY_FF 0.5 + param set MPC_Z_P 1.0 + param set MPC_Z_VEL_P 0.1 + param set MPC_Z_VEL_I 0.02 + param set MPC_Z_VEL_D 0.0 + param set MPC_Z_VEL_MAX 3 + param set MPC_Z_FF 0.5 + param set MPC_TILT_MAX 1.0 + param set MPC_LAND_SPEED 1.0 + param set MPC_LAND_TILT 0.3 +fi + +set PWM_RATE 400 +set PWM_DISARMED 900 +set PWM_MIN 1075 +set PWM_MAX 2000 \ No newline at end of file From fff00318cdeac8b52affb2210dd0975c65826333 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 7 Feb 2014 22:39:06 +0100 Subject: [PATCH 411/447] Startup scripts: get the indentation right --- ROMFS/px4fmu_common/init.d/rc.mc_defaults | 68 +++++++++++------------ 1 file changed, 34 insertions(+), 34 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_defaults b/ROMFS/px4fmu_common/init.d/rc.mc_defaults index 14b6fe12cb..13b2c97174 100644 --- a/ROMFS/px4fmu_common/init.d/rc.mc_defaults +++ b/ROMFS/px4fmu_common/init.d/rc.mc_defaults @@ -1,42 +1,42 @@ #!nsh -set VEHICLE_TYPE mc +set VEHICLE_TYPE fw if [ $DO_AUTOCONFIG == yes ] then - # - # Default parameters for this platform - # - param set MC_ROLL_P 7.0 - param set MC_ROLLRATE_P 0.12 - param set MC_ROLLRATE_I 0.0 - param set MC_ROLLRATE_D 0.004 - param set MC_PITCH_P 7.0 - param set MC_PITCHRATE_P 0.12 - param set MC_PITCHRATE_I 0.0 - param set MC_PITCHRATE_D 0.004 - param set MC_YAW_P 2.0 - param set MC_YAWRATE_P 0.3 - param set MC_YAWRATE_I 0.2 - param set MC_YAWRATE_D 0.005 - - param set MPC_THR_MAX 1.0 - param set MPC_THR_MIN 0.1 - param set MPC_XY_P 1.0 - param set MPC_XY_VEL_P 0.1 - param set MPC_XY_VEL_I 0.02 - param set MPC_XY_VEL_D 0.01 - param set MPC_XY_VEL_MAX 5 - param set MPC_XY_FF 0.5 - param set MPC_Z_P 1.0 - param set MPC_Z_VEL_P 0.1 - param set MPC_Z_VEL_I 0.02 - param set MPC_Z_VEL_D 0.0 - param set MPC_Z_VEL_MAX 3 - param set MPC_Z_FF 0.5 - param set MPC_TILT_MAX 1.0 - param set MPC_LAND_SPEED 1.0 - param set MPC_LAND_TILT 0.3 + # + # Default parameters for this platform + # + param set MC_ROLL_P 7.0 + param set MC_ROLLRATE_P 0.12 + param set MC_ROLLRATE_I 0.0 + param set MC_ROLLRATE_D 0.004 + param set MC_PITCH_P 7.0 + param set MC_PITCHRATE_P 0.12 + param set MC_PITCHRATE_I 0.0 + param set MC_PITCHRATE_D 0.004 + param set MC_YAW_P 2.0 + param set MC_YAWRATE_P 0.3 + param set MC_YAWRATE_I 0.2 + param set MC_YAWRATE_D 0.005 + + param set MPC_THR_MAX 1.0 + param set MPC_THR_MIN 0.1 + param set MPC_XY_P 1.0 + param set MPC_XY_VEL_P 0.1 + param set MPC_XY_VEL_I 0.02 + param set MPC_XY_VEL_D 0.01 + param set MPC_XY_VEL_MAX 5 + param set MPC_XY_FF 0.5 + param set MPC_Z_P 1.0 + param set MPC_Z_VEL_P 0.1 + param set MPC_Z_VEL_I 0.02 + param set MPC_Z_VEL_D 0.0 + param set MPC_Z_VEL_MAX 3 + param set MPC_Z_FF 0.5 + param set MPC_TILT_MAX 1.0 + param set MPC_LAND_SPEED 1.0 + param set MPC_LAND_TILT 0.3 fi set PWM_RATE 400 From df41c04be74c6d4d576c188f2327226c264d371c Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 7 Feb 2014 23:13:12 +0100 Subject: [PATCH 412/447] Startup scripts: use rc.fw_defaults for default FW parameters --- .../init.d/1000_rc_fw_easystar.hil | 30 +---------- .../init.d/1004_rc_fw_Rascal110.hil | 32 +---------- .../init.d/1005_rc_fw_Malolo1.hil | 53 +------------------ ROMFS/px4fmu_common/init.d/2100_mpx_easystar | 33 +----------- ROMFS/px4fmu_common/init.d/2101_hk_bixler | 32 +---------- ROMFS/px4fmu_common/init.d/2102_3dr_skywalker | 30 +---------- ROMFS/px4fmu_common/init.d/3030_io_camflyer | 35 +----------- ROMFS/px4fmu_common/init.d/3031_phantom | 36 ------------- ROMFS/px4fmu_common/init.d/3032_skywalker_x5 | 35 +----------- ROMFS/px4fmu_common/init.d/3033_wingwing | 35 +----------- ROMFS/px4fmu_common/init.d/3034_fx79 | 35 +----------- ROMFS/px4fmu_common/init.d/rc.fw_defaults | 35 ++++++++++++ 12 files changed, 47 insertions(+), 374 deletions(-) create mode 100644 ROMFS/px4fmu_common/init.d/rc.fw_defaults 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 d7e524b414..36194ad682 100644 --- a/ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil +++ b/ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil @@ -5,36 +5,10 @@ # Lorenz Meier # -echo "HIL Rascal 110 starting.." +sh /etc/init.d/rc.fw_defaults -if [ $DO_AUTOCONFIG == yes ] -then - # Set all params here, then disable autoconfig - - param set FW_P_D 0 - param set FW_P_I 0 - param set FW_P_IMAX 15 - param set FW_P_LIM_MAX 50 - param set FW_P_LIM_MIN -50 - param set FW_P_P 60 - param set FW_P_RMAX_NEG 0 - param set FW_P_RMAX_POS 0 - param set FW_P_ROLLFF 1.1 - param set FW_R_D 0 - param set FW_R_I 5 - param set FW_R_IMAX 20 - param set FW_R_P 100 - param set FW_R_RMAX 100 - param set FW_THR_CRUISE 0.65 - param set FW_T_SINK_MAX 5.0 - param set FW_T_SINK_MIN 4.0 - param set FW_Y_ROLLFF 1.1 - param set FW_L1_PERIOD 16 - param set RC_SCALE_ROLL 1.0 - param set RC_SCALE_PITCH 1.0 -fi +echo "HIL Rascal 110 starting.." set HIL yes -set VEHICLE_TYPE fw set MIXER FMU_AERT diff --git a/ROMFS/px4fmu_common/init.d/1004_rc_fw_Rascal110.hil b/ROMFS/px4fmu_common/init.d/1004_rc_fw_Rascal110.hil index c639bfc254..4e3e183262 100644 --- a/ROMFS/px4fmu_common/init.d/1004_rc_fw_Rascal110.hil +++ b/ROMFS/px4fmu_common/init.d/1004_rc_fw_Rascal110.hil @@ -5,38 +5,10 @@ # Thomas Gubler # -echo "HIL Rascal 110 starting.." +sh /etc/init.d/rc.fw_defaults -if [ $DO_AUTOCONFIG == yes ] -then - # Set all params here, then disable autoconfig - - param set FW_P_D 0 - param set FW_P_I 0 - param set FW_P_IMAX 15 - param set FW_P_LIM_MAX 50 - param set FW_P_LIM_MIN -50 - param set FW_P_P 60 - param set FW_P_RMAX_NEG 0 - param set FW_P_RMAX_POS 0 - param set FW_P_ROLLFF 1.1 - param set FW_R_D 0 - param set FW_R_I 5 - param set FW_R_IMAX 20 - param set FW_R_P 100 - param set FW_R_RMAX 100 - param set FW_THR_CRUISE 0.65 - param set FW_THR_MAX 1 - param set FW_THR_MIN 0 - param set FW_T_SINK_MAX 5.0 - param set FW_T_SINK_MIN 4.0 - param set FW_Y_ROLLFF 1.1 - param set FW_L1_PERIOD 16 - param set RC_SCALE_ROLL 1.0 - param set RC_SCALE_PITCH 1.0 -fi +echo "HIL Rascal 110 starting.." set HIL yes -set VEHICLE_TYPE fw set MIXER FMU_AERT diff --git a/ROMFS/px4fmu_common/init.d/1005_rc_fw_Malolo1.hil b/ROMFS/px4fmu_common/init.d/1005_rc_fw_Malolo1.hil index 124bf63abf..abbe626b13 100644 --- a/ROMFS/px4fmu_common/init.d/1005_rc_fw_Malolo1.hil +++ b/ROMFS/px4fmu_common/init.d/1005_rc_fw_Malolo1.hil @@ -5,60 +5,9 @@ # Thomas Gubler # -echo "HIL Malolo 1 starting.." - -if [ $DO_AUTOCONFIG == yes ] -then - # Set all params here, then disable autoconfig - - param set FW_AIRSPD_MIN 12 - param set FW_AIRSPD_TRIM 25 - param set FW_ATT_TC 0.3 - param set FW_L1_DAMPING 0.74 - param set FW_L1_PERIOD 15 - param set FW_PR_FF 0.8 - param set FW_PR_I 0.05 - param set FW_PR_IMAX 0.2 - param set FW_PR_P 0.1 - param set FW_P_LIM_MAX 45 - param set FW_P_LIM_MIN -45 - param set FW_P_RMAX_NEG 0 - param set FW_P_RMAX_POS 0 - param set FW_P_ROLLFF 0 - param set FW_RR_FF 0.6 - param set FW_RR_I 0.02 - param set FW_RR_IMAX 0.2 - param set FW_RR_P 0.1 - param set FW_R_LIM 45 - param set FW_R_RMAX 0 - param set FW_THR_CRUISE 0.6 - param set FW_THR_MAX 1 - param set FW_THR_MIN 0 - param set FW_T_CLMB_MAX 5 - param set FW_T_HRATE_P 0.02 - param set FW_T_PTCH_DAMP 0 - param set FW_T_RLL2THR 15 - param set FW_T_SINK_MAX 5 - param set FW_T_SINK_MIN 2 - param set FW_T_SRATE_P 0.01 - param set FW_T_TIME_CONST 3 - param set FW_T_VERT_ACC 7 - param set FW_YCO_VMIN 1000 - param set FW_YR_FF 0.0 - param set FW_YR_I 0 - param set FW_YR_IMAX 0.2 - param set FW_YR_P 0.0 - param set FW_Y_RMAX 0 - param set NAV_LAND_ALT 90 - param set NAV_RTL_ALT 100 - param set NAV_RTL_LAND_T -1 - - param set SYS_AUTOCONFIG 0 - param save -fi +sh /etc/init.d/rc.fw_defaults set HIL yes -set VEHICLE_TYPE fw # Set the AERT mixer for HIL (even if the malolo is a flying wing) set MIXER FMU_AERT diff --git a/ROMFS/px4fmu_common/init.d/2100_mpx_easystar b/ROMFS/px4fmu_common/init.d/2100_mpx_easystar index 0e5bf60d60..465a22c53a 100644 --- a/ROMFS/px4fmu_common/init.d/2100_mpx_easystar +++ b/ROMFS/px4fmu_common/init.d/2100_mpx_easystar @@ -2,38 +2,7 @@ # # MPX EasyStar Plane # -# Maintainers: ??? -# -if [ $DO_AUTOCONFIG == yes ] -then - # - # Default parameters for this platform - # - param set FW_P_D 0 - param set FW_P_I 0 - param set FW_P_IMAX 15 - param set FW_P_LIM_MAX 50 - param set FW_P_LIM_MIN -50 - param set FW_P_P 60 - param set FW_P_RMAX_NEG 0 - param set FW_P_RMAX_POS 0 - param set FW_P_ROLLFF 1.1 - param set FW_R_D 0 - param set FW_R_I 5 - param set FW_R_IMAX 20 - param set FW_R_P 100 - param set FW_R_RMAX 100 - param set FW_THR_CRUISE 0.65 - param set FW_THR_MAX 1 - param set FW_THR_MIN 0 - param set FW_T_SINK_MAX 5.0 - param set FW_T_SINK_MIN 4.0 - param set FW_Y_ROLLFF 1.1 - param set FW_L1_PERIOD 16 - param set RC_SCALE_ROLL 1.0 - param set RC_SCALE_PITCH 1.0 -fi +sh /etc/init.d/rc.fw_defaults -set VEHICLE_TYPE fw set MIXER FMU_RET diff --git a/ROMFS/px4fmu_common/init.d/2101_hk_bixler b/ROMFS/px4fmu_common/init.d/2101_hk_bixler index de5e5a8d3a..dcc5db824a 100644 --- a/ROMFS/px4fmu_common/init.d/2101_hk_bixler +++ b/ROMFS/px4fmu_common/init.d/2101_hk_bixler @@ -1,35 +1,5 @@ #!nsh -if [ $DO_AUTOCONFIG == yes ] -then - # Set all params here, then disable autoconfig - param set FW_P_D 0 - param set FW_P_I 0 - param set FW_P_IMAX 15 - param set FW_P_LIM_MAX 50 - param set FW_P_LIM_MIN -50 - param set FW_P_P 60 - param set FW_P_RMAX_NEG 0 - param set FW_P_RMAX_POS 0 - param set FW_P_ROLLFF 1.1 - param set FW_R_D 0 - param set FW_R_I 5 - param set FW_R_IMAX 20 - param set FW_R_P 100 - param set FW_R_RMAX 100 - param set FW_THR_CRUISE 0.65 - param set FW_THR_MAX 1 - param set FW_THR_MIN 0 - param set FW_T_SINK_MAX 5.0 - param set FW_T_SINK_MIN 4.0 - param set FW_Y_ROLLFF 1.1 - param set FW_L1_PERIOD 16 - param set RC_SCALE_ROLL 1.0 - param set RC_SCALE_PITCH 1.0 - - param set SYS_AUTOCONFIG 0 - param save -fi +sh /etc/init.d/rc.fw_defaults -set VEHICLE_TYPE fw set MIXER FMU_AERT \ No newline at end of file diff --git a/ROMFS/px4fmu_common/init.d/2102_3dr_skywalker b/ROMFS/px4fmu_common/init.d/2102_3dr_skywalker index 07f215f6cf..dcc5db824a 100644 --- a/ROMFS/px4fmu_common/init.d/2102_3dr_skywalker +++ b/ROMFS/px4fmu_common/init.d/2102_3dr_skywalker @@ -1,33 +1,5 @@ #!nsh -if [ $DO_AUTOCONFIG == yes ] -then - # Set all params here, then disable autoconfig - param set FW_P_D 0 - param set FW_P_I 0 - param set FW_P_IMAX 15 - param set FW_P_LIM_MAX 50 - param set FW_P_LIM_MIN -50 - param set FW_P_P 60 - param set FW_P_RMAX_NEG 0 - param set FW_P_RMAX_POS 0 - param set FW_P_ROLLFF 1.1 - param set FW_R_D 0 - param set FW_R_I 5 - param set FW_R_IMAX 20 - param set FW_R_P 100 - param set FW_R_RMAX 100 - param set FW_THR_CRUISE 0.65 - param set FW_T_SINK_MAX 5.0 - param set FW_T_SINK_MIN 4.0 - param set FW_Y_ROLLFF 1.1 - param set FW_L1_PERIOD 16 - param set RC_SCALE_ROLL 1.0 - param set RC_SCALE_PITCH 1.0 - - param set SYS_AUTOCONFIG 0 - param save -fi +sh /etc/init.d/rc.fw_defaults -set VEHICLE_TYPE fw set MIXER FMU_AERT \ No newline at end of file diff --git a/ROMFS/px4fmu_common/init.d/3030_io_camflyer b/ROMFS/px4fmu_common/init.d/3030_io_camflyer index 3b7323ac4e..83c470234c 100644 --- a/ROMFS/px4fmu_common/init.d/3030_io_camflyer +++ b/ROMFS/px4fmu_common/init.d/3030_io_camflyer @@ -1,38 +1,5 @@ #!nsh -if [ $DO_AUTOCONFIG == yes ] -then - # - # Default parameters for this platform - # - param set FW_AIRSPD_MIN 7 - param set FW_AIRSPD_TRIM 9 - param set FW_AIRSPD_MAX 14 - param set FW_L1_PERIOD 10 - param set FW_P_D 0 - param set FW_P_I 0 - param set FW_P_IMAX 20 - param set FW_P_LIM_MAX 30 - param set FW_P_LIM_MIN -20 - param set FW_P_P 30 - param set FW_P_RMAX_NEG 0 - param set FW_P_RMAX_POS 0 - param set FW_P_ROLLFF 2 - param set FW_R_D 0 - param set FW_R_I 5 - param set FW_R_IMAX 20 - param set FW_R_P 60 - param set FW_R_RMAX 60 - param set FW_THR_CRUISE 0.65 - param set FW_THR_MAX 1.0 - param set FW_THR_MIN 0 - param set FW_T_SINK_MAX 5 - param set FW_T_SINK_MIN 2 - param set FW_T_TIME_CONST 9 - param set FW_Y_ROLLFF 2.0 - param set RC_SCALE_ROLL 1.0 - param set RC_SCALE_PITCH 1.0 -fi +sh /etc/init.d/rc.fw_defaults -set VEHICLE_TYPE fw set MIXER FMU_Q diff --git a/ROMFS/px4fmu_common/init.d/3031_phantom b/ROMFS/px4fmu_common/init.d/3031_phantom index a4ff61d937..2e2434bb85 100644 --- a/ROMFS/px4fmu_common/init.d/3031_phantom +++ b/ROMFS/px4fmu_common/init.d/3031_phantom @@ -5,40 +5,4 @@ # Simon Wilks # -if [ $DO_AUTOCONFIG == yes ] -then - # - # Default parameters for this platform - # - param set FW_AIRSPD_MIN 11.4 - param set FW_AIRSPD_TRIM 14 - param set FW_AIRSPD_MAX 22 - param set FW_L1_PERIOD 15 - param set FW_P_D 0 - param set FW_P_I 0 - param set FW_P_IMAX 15 - param set FW_P_LIM_MAX 45 - param set FW_P_LIM_MIN -45 - param set FW_P_P 60 - param set FW_P_RMAX_NEG 0 - param set FW_P_RMAX_POS 0 - param set FW_P_ROLLFF 2 - param set FW_R_D 0 - param set FW_R_I 5 - param set FW_R_IMAX 15 - param set FW_R_P 80 - param set FW_R_RMAX 60 - param set FW_THR_CRUISE 0.8 - param set FW_THR_LND_MAX 0 - param set FW_THR_MAX 1 - param set FW_THR_MIN 0.5 - param set FW_T_SINK_MAX 5.0 - param set FW_T_SINK_MIN 2.0 - param set FW_Y_ROLLFF 1.0 - param set RC_SCALE_ROLL 0.6 - param set RC_SCALE_PITCH 0.6 - param set TRIM_PITCH 0.1 -fi - -set VEHICLE_TYPE fw set MIXER FMU_Q diff --git a/ROMFS/px4fmu_common/init.d/3032_skywalker_x5 b/ROMFS/px4fmu_common/init.d/3032_skywalker_x5 index c1e78b6f1e..1657e6d368 100644 --- a/ROMFS/px4fmu_common/init.d/3032_skywalker_x5 +++ b/ROMFS/px4fmu_common/init.d/3032_skywalker_x5 @@ -5,39 +5,6 @@ # Thomas Gubler , Julian Oes # -if [ $DO_AUTOCONFIG == yes ] -then - # - # Default parameters for this platform - # - param set FW_AIRSPD_MIN 7 - param set FW_AIRSPD_TRIM 9 - param set FW_AIRSPD_MAX 14 - param set FW_L1_PERIOD 10 - param set FW_P_D 0 - param set FW_P_I 0 - param set FW_P_IMAX 20 - param set FW_P_LIM_MAX 30 - param set FW_P_LIM_MIN -20 - param set FW_P_P 30 - param set FW_P_RMAX_NEG 0 - param set FW_P_RMAX_POS 0 - param set FW_P_ROLLFF 2 - param set FW_R_D 0 - param set FW_R_I 5 - param set FW_R_IMAX 20 - param set FW_R_P 60 - param set FW_R_RMAX 60 - param set FW_THR_CRUISE 0.65 - param set FW_THR_MAX 0.7 - param set FW_THR_MIN 0 - param set FW_T_SINK_MAX 5 - param set FW_T_SINK_MIN 2 - param set FW_T_TIME_CONST 9 - param set FW_Y_ROLLFF 2.0 - param set RC_SCALE_ROLL 1.0 - param set RC_SCALE_PITCH 1.0 -fi +sh /etc/init.d/rc.fw_defaults -set VEHICLE_TYPE fw set MIXER FMU_X5 diff --git a/ROMFS/px4fmu_common/init.d/3033_wingwing b/ROMFS/px4fmu_common/init.d/3033_wingwing index 2f68797999..2af3618d98 100644 --- a/ROMFS/px4fmu_common/init.d/3033_wingwing +++ b/ROMFS/px4fmu_common/init.d/3033_wingwing @@ -5,39 +5,6 @@ # Simon Wilks # -if [ $DO_AUTOCONFIG == yes ] -then - # - # Default parameters for this platform - # - param set FW_AIRSPD_MIN 7 - param set FW_AIRSPD_TRIM 9 - param set FW_AIRSPD_MAX 14 - param set FW_L1_PERIOD 10 - param set FW_P_D 0 - param set FW_P_I 0 - param set FW_P_IMAX 20 - param set FW_P_LIM_MAX 30 - param set FW_P_LIM_MIN -20 - param set FW_P_P 30 - param set FW_P_RMAX_NEG 0 - param set FW_P_RMAX_POS 0 - param set FW_P_ROLLFF 2 - param set FW_R_D 0 - param set FW_R_I 5 - param set FW_R_IMAX 20 - param set FW_R_P 60 - param set FW_R_RMAX 60 - param set FW_THR_CRUISE 0.65 - param set FW_THR_MAX 0.7 - param set FW_THR_MIN 0 - param set FW_T_SINK_MAX 5 - param set FW_T_SINK_MIN 2 - param set FW_T_TIME_CONST 9 - param set FW_Y_ROLLFF 2.0 - param set RC_SCALE_ROLL 1.0 - param set RC_SCALE_PITCH 1.0 -fi +sh /etc/init.d/rc.fw_defaults -set VEHICLE_TYPE fw set MIXER FMU_Q diff --git a/ROMFS/px4fmu_common/init.d/3034_fx79 b/ROMFS/px4fmu_common/init.d/3034_fx79 index bc02d87f3d..f4bd18269d 100644 --- a/ROMFS/px4fmu_common/init.d/3034_fx79 +++ b/ROMFS/px4fmu_common/init.d/3034_fx79 @@ -5,39 +5,6 @@ # Simon Wilks # -if [ $DO_AUTOCONFIG == yes ] -then - # - # Default parameters for this platform - # - param set FW_AIRSPD_MAX 20 - param set FW_AIRSPD_TRIM 12 - param set FW_AIRSPD_MIN 15 - param set FW_L1_PERIOD 12 - param set FW_P_D 0 - param set FW_P_I 0 - param set FW_P_IMAX 15 - param set FW_P_LIM_MAX 50 - param set FW_P_LIM_MIN -50 - param set FW_P_P 60 - param set FW_P_RMAX_NEG 0 - param set FW_P_RMAX_POS 0 - param set FW_P_ROLLFF 1.1 - param set FW_R_D 0 - param set FW_R_I 5 - param set FW_R_IMAX 20 - param set FW_R_P 80 - param set FW_R_RMAX 100 - param set FW_THR_CRUISE 0.75 - param set FW_THR_MAX 1 - param set FW_THR_MIN 0 - param set FW_T_SINK_MAX 5.0 - param set FW_T_SINK_MIN 4.0 - param set FW_T_TIME_CONST 9 - param set FW_Y_ROLLFF 1.1 - param set RC_SCALE_ROLL 1.0 - param set RC_SCALE_PITCH 1.0 -fi +sh /etc/init.d/rc.fw_defaults -set VEHICLE_TYPE fw set MIXER FMU_FX79 diff --git a/ROMFS/px4fmu_common/init.d/rc.fw_defaults b/ROMFS/px4fmu_common/init.d/rc.fw_defaults new file mode 100644 index 0000000000..0de13af070 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/rc.fw_defaults @@ -0,0 +1,35 @@ +#!nsh + +set VEHICLE_TYPE fw + +if [ $DO_AUTOCONFIG == yes ] +then +# +# Default parameters for this platform +# + param set FW_AIRSPD_MIN 15 + param set FW_AIRSPD_TRIM 20 + param set FW_AIRSPD_MAX 80 + param set FW_ATT_TC 0.3 + param set FW_L1_DAMPING 0.74 + param set FW_L1_PERIOD 15 + param set FW_PR_FF 0.8 + param set FW_PR_I 0 + param set FW_PR_IMAX 0.2 + param set FW_PR_P 0.05 + param set FW_P_LIM_MAX 45 + param set FW_P_LIM_MIN -45 + param set FW_P_RMAX_NEG 0 + param set FW_P_RMAX_POS 0 + param set FW_P_ROLLFF 0 + param set FW_RR_FF 0.6 + param set FW_RR_I 0 + param set FW_RR_IMAX 0.2 + param set FW_RR_P 0.05 + param set FW_R_LIM 60 + param set FW_R_RMAX 0 + param set FW_T_HRATE_P 0.01 + param set FW_T_RLL2THR 15 + param set FW_T_SRATE_P 0.01 + param set FW_T_TIME_CONST 5 +fi \ No newline at end of file From 008a973ef7de6433d3215df5cc8f553ce494ba22 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 7 Feb 2014 23:51:57 +0100 Subject: [PATCH 413/447] Startup scripts: get the indentation right, take 2 --- ROMFS/px4fmu_common/init.d/rc.fw_defaults | 50 +++++++++++------------ 1 file changed, 25 insertions(+), 25 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.fw_defaults b/ROMFS/px4fmu_common/init.d/rc.fw_defaults index 0de13af070..cbf0ba4000 100644 --- a/ROMFS/px4fmu_common/init.d/rc.fw_defaults +++ b/ROMFS/px4fmu_common/init.d/rc.fw_defaults @@ -7,29 +7,29 @@ then # # Default parameters for this platform # - param set FW_AIRSPD_MIN 15 - param set FW_AIRSPD_TRIM 20 - param set FW_AIRSPD_MAX 80 - param set FW_ATT_TC 0.3 - param set FW_L1_DAMPING 0.74 - param set FW_L1_PERIOD 15 - param set FW_PR_FF 0.8 - param set FW_PR_I 0 - param set FW_PR_IMAX 0.2 - param set FW_PR_P 0.05 - param set FW_P_LIM_MAX 45 - param set FW_P_LIM_MIN -45 - param set FW_P_RMAX_NEG 0 - param set FW_P_RMAX_POS 0 - param set FW_P_ROLLFF 0 - param set FW_RR_FF 0.6 - param set FW_RR_I 0 - param set FW_RR_IMAX 0.2 - param set FW_RR_P 0.05 - param set FW_R_LIM 60 - param set FW_R_RMAX 0 - param set FW_T_HRATE_P 0.01 - param set FW_T_RLL2THR 15 - param set FW_T_SRATE_P 0.01 - param set FW_T_TIME_CONST 5 + param set FW_AIRSPD_MIN 15 + param set FW_AIRSPD_TRIM 20 + param set FW_AIRSPD_MAX 80 + param set FW_ATT_TC 0.3 + param set FW_L1_DAMPING 0.74 + param set FW_L1_PERIOD 15 + param set FW_PR_FF 0.8 + param set FW_PR_I 0 + param set FW_PR_IMAX 0.2 + param set FW_PR_P 0.05 + param set FW_P_LIM_MAX 45 + param set FW_P_LIM_MIN -45 + param set FW_P_RMAX_NEG 0 + param set FW_P_RMAX_POS 0 + param set FW_P_ROLLFF 0 + param set FW_RR_FF 0.6 + param set FW_RR_I 0 + param set FW_RR_IMAX 0.2 + param set FW_RR_P 0.05 + param set FW_R_LIM 60 + param set FW_R_RMAX 0 + param set FW_T_HRATE_P 0.01 + param set FW_T_RLL2THR 15 + param set FW_T_SRATE_P 0.01 + param set FW_T_TIME_CONST 5 fi \ No newline at end of file From 096735897c70076ee26c1d343538eae8a585f285 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 7 Feb 2014 23:52:56 +0100 Subject: [PATCH 414/447] Startup scripts: added important LAND and RTL parameters for FW --- ROMFS/px4fmu_common/init.d/rc.fw_defaults | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/ROMFS/px4fmu_common/init.d/rc.fw_defaults b/ROMFS/px4fmu_common/init.d/rc.fw_defaults index cbf0ba4000..a28f67b656 100644 --- a/ROMFS/px4fmu_common/init.d/rc.fw_defaults +++ b/ROMFS/px4fmu_common/init.d/rc.fw_defaults @@ -32,4 +32,8 @@ then param set FW_T_RLL2THR 15 param set FW_T_SRATE_P 0.01 param set FW_T_TIME_CONST 5 + + param set NAV_LAND_ALT 90 + param set NAV_RTL_ALT 100 + param set NAV_RTL_LAND_T -1 fi \ No newline at end of file From de8a9268f3c476c2695df84f0e1a429ea10ddeb3 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sat, 8 Feb 2014 00:31:53 +0100 Subject: [PATCH 415/447] Startup scripts: bring back HIL parameters for Malolo --- .../init.d/1005_rc_fw_Malolo1.hil | 34 +++++++++++++++++++ 1 file changed, 34 insertions(+) diff --git a/ROMFS/px4fmu_common/init.d/1005_rc_fw_Malolo1.hil b/ROMFS/px4fmu_common/init.d/1005_rc_fw_Malolo1.hil index abbe626b13..c753ded233 100644 --- a/ROMFS/px4fmu_common/init.d/1005_rc_fw_Malolo1.hil +++ b/ROMFS/px4fmu_common/init.d/1005_rc_fw_Malolo1.hil @@ -7,6 +7,40 @@ sh /etc/init.d/rc.fw_defaults +if [ $DO_AUTOCONFIG == yes ] +then + param set FW_AIRSPD_MIN 12 + param set FW_AIRSPD_TRIM 25 + param set FW_AIRSPD_MAX 40 + param set FW_ATT_TC 0.3 + param set FW_L1_DAMPING 0.74 + param set FW_L1_PERIOD 15 + param set FW_PR_FF 0.8 + param set FW_PR_I 0.05 + param set FW_PR_IMAX 0.2 + param set FW_PR_P 0.1 + param set FW_P_ROLLFF 0 + param set FW_RR_FF 0.6 + param set FW_RR_I 0.02 + param set FW_RR_IMAX 0.2 + param set FW_RR_P 0.1 + param set FW_R_LIM 45 + param set FW_R_RMAX 0 + param set FW_T_CLMB_MAX 5 + param set FW_T_HRATE_P 0.02 + param set FW_T_PTCH_DAMP 0 + param set FW_T_RLL2THR 15 + param set FW_T_SINK_MAX 5 + param set FW_T_SINK_MIN 2 + param set FW_T_SRATE_P 0.01 + param set FW_T_TIME_CONST 3 + param set FW_T_VERT_ACC 7 + param set FW_YR_FF 0.0 + param set FW_YR_I 0 + param set FW_YR_IMAX 0.2 + param set FW_YR_P 0.0 +fi + set HIL yes # Set the AERT mixer for HIL (even if the malolo is a flying wing) From 0a87f1d01ce089e6f6ddba42be6847d5783cbfaf Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sat, 8 Feb 2014 00:32:57 +0100 Subject: [PATCH 416/447] Startup scripts: move X5 attitude parameters back to X5 script and only leave airframe independent params in FW defaults script --- ROMFS/px4fmu_common/init.d/3032_skywalker_x5 | 29 ++++++++++++++++++++ ROMFS/px4fmu_common/init.d/rc.fw_defaults | 28 +------------------ ROMFS/px4fmu_common/init.d/rc.mc_defaults | 2 +- 3 files changed, 31 insertions(+), 28 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/3032_skywalker_x5 b/ROMFS/px4fmu_common/init.d/3032_skywalker_x5 index 1657e6d368..3f5f79857d 100644 --- a/ROMFS/px4fmu_common/init.d/3032_skywalker_x5 +++ b/ROMFS/px4fmu_common/init.d/3032_skywalker_x5 @@ -7,4 +7,33 @@ sh /etc/init.d/rc.fw_defaults +if [ $DO_AUTOCONFIG == yes ] +then + param set FW_AIRSPD_MIN 15 + param set FW_AIRSPD_TRIM 20 + param set FW_AIRSPD_MAX 40 + param set FW_ATT_TC 0.3 + param set FW_L1_DAMPING 0.74 + param set FW_L1_PERIOD 15 + param set FW_PR_FF 0.8 + param set FW_PR_I 0 + param set FW_PR_IMAX 0.2 + param set FW_PR_P 0.05 + param set FW_P_LIM_MAX 45 + param set FW_P_LIM_MIN -45 + param set FW_P_RMAX_NEG 0 + param set FW_P_RMAX_POS 0 + param set FW_P_ROLLFF 0 + param set FW_RR_FF 0.6 + param set FW_RR_I 0 + param set FW_RR_IMAX 0.2 + param set FW_RR_P 0.05 + param set FW_R_LIM 60 + param set FW_R_RMAX 0 + param set FW_T_HRATE_P 0.01 + param set FW_T_RLL2THR 15 + param set FW_T_SRATE_P 0.01 + param set FW_T_TIME_CONST 5 +fi + set MIXER FMU_X5 diff --git a/ROMFS/px4fmu_common/init.d/rc.fw_defaults b/ROMFS/px4fmu_common/init.d/rc.fw_defaults index a28f67b656..3e340699fc 100644 --- a/ROMFS/px4fmu_common/init.d/rc.fw_defaults +++ b/ROMFS/px4fmu_common/init.d/rc.fw_defaults @@ -5,34 +5,8 @@ set VEHICLE_TYPE fw if [ $DO_AUTOCONFIG == yes ] then # -# Default parameters for this platform +# Default parameters for FW # - param set FW_AIRSPD_MIN 15 - param set FW_AIRSPD_TRIM 20 - param set FW_AIRSPD_MAX 80 - param set FW_ATT_TC 0.3 - param set FW_L1_DAMPING 0.74 - param set FW_L1_PERIOD 15 - param set FW_PR_FF 0.8 - param set FW_PR_I 0 - param set FW_PR_IMAX 0.2 - param set FW_PR_P 0.05 - param set FW_P_LIM_MAX 45 - param set FW_P_LIM_MIN -45 - param set FW_P_RMAX_NEG 0 - param set FW_P_RMAX_POS 0 - param set FW_P_ROLLFF 0 - param set FW_RR_FF 0.6 - param set FW_RR_I 0 - param set FW_RR_IMAX 0.2 - param set FW_RR_P 0.05 - param set FW_R_LIM 60 - param set FW_R_RMAX 0 - param set FW_T_HRATE_P 0.01 - param set FW_T_RLL2THR 15 - param set FW_T_SRATE_P 0.01 - param set FW_T_TIME_CONST 5 - param set NAV_LAND_ALT 90 param set NAV_RTL_ALT 100 param set NAV_RTL_LAND_T -1 diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_defaults b/ROMFS/px4fmu_common/init.d/rc.mc_defaults index 13b2c97174..50d02d9e47 100644 --- a/ROMFS/px4fmu_common/init.d/rc.mc_defaults +++ b/ROMFS/px4fmu_common/init.d/rc.mc_defaults @@ -5,7 +5,7 @@ set VEHICLE_TYPE fw if [ $DO_AUTOCONFIG == yes ] then # - # Default parameters for this platform + # Default parameters for MC # param set MC_ROLL_P 7.0 param set MC_ROLLRATE_P 0.12 From a9e5e2e31a73e4ca546bf89e807b71187a41b657 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 10 Feb 2014 08:54:48 +0100 Subject: [PATCH 417/447] position_estimator_inav: default parameters and min/max EPH/EPV updated --- .../position_estimator_inav_main.c | 8 ++++---- .../position_estimator_inav_params.c | 2 +- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index e045ce4cca..bf4f7ae974 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -527,13 +527,13 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (gps.fix_type >= 3) { /* hysteresis for GPS quality */ if (gps_valid) { - if (gps.eph_m > 10.0f || gps.epv_m > 10.0f) { + if (gps.eph_m > 10.0f || gps.epv_m > 20.0f) { gps_valid = false; mavlink_log_info(mavlink_fd, "[inav] GPS signal lost"); } } else { - if (gps.eph_m < 5.0f && gps.epv_m < 5.0f) { + if (gps.eph_m < 5.0f && gps.epv_m < 10.0f) { gps_valid = true; mavlink_log_info(mavlink_fd, "[inav] GPS signal found"); } @@ -589,8 +589,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) corr_gps[2][1] = 0.0f; } - w_gps_xy = 1.0f / fmaxf(1.0f, gps.eph_m); - w_gps_z = 1.0f / fmaxf(1.0f, gps.epv_m); + w_gps_xy = 2.0f / fmaxf(2.0f, gps.eph_m); + w_gps_z = 4.0f / fmaxf(4.0f, gps.epv_m); } } else { diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.c b/src/modules/position_estimator_inav/position_estimator_inav_params.c index e1bbd75a6e..b71f9472f5 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.c @@ -40,7 +40,7 @@ #include "position_estimator_inav_params.h" -PARAM_DEFINE_FLOAT(INAV_W_Z_BARO, 1.0f); +PARAM_DEFINE_FLOAT(INAV_W_Z_BARO, 0.5f); PARAM_DEFINE_FLOAT(INAV_W_Z_GPS_P, 0.005f); PARAM_DEFINE_FLOAT(INAV_W_Z_ACC, 20.0f); PARAM_DEFINE_FLOAT(INAV_W_Z_SONAR, 3.0f); From 0dc7f223ea26a09cd538114f54d99a7ab130efba Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 10 Feb 2014 12:08:39 +1100 Subject: [PATCH 418/447] FMUv2: fixed UART3 flow control pins --- nuttx-configs/px4fmu-v2/include/board.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/nuttx-configs/px4fmu-v2/include/board.h b/nuttx-configs/px4fmu-v2/include/board.h index e56b14ba41..850043ddf5 100755 --- a/nuttx-configs/px4fmu-v2/include/board.h +++ b/nuttx-configs/px4fmu-v2/include/board.h @@ -209,8 +209,8 @@ #define GPIO_USART3_RX GPIO_USART3_RX_3 #define GPIO_USART3_TX GPIO_USART3_TX_3 -#define GPIO_USART2_RTS GPIO_USART2_RTS_2 -#define GPIO_USART2_CTS GPIO_USART2_CTS_2 +#define GPIO_USART3_RTS GPIO_USART3_RTS_2 +#define GPIO_USART3_CTS GPIO_USART3_CTS_2 #define GPIO_UART4_RX GPIO_UART4_RX_1 #define GPIO_UART4_TX GPIO_UART4_TX_1 From 0d4b5d9395e3fd668b217301d0f888fa0238998d Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 10 Feb 2014 13:15:34 +0100 Subject: [PATCH 419/447] X5: adjusted default parameters based on test flight --- ROMFS/px4fmu_common/init.d/3032_skywalker_x5 | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/3032_skywalker_x5 b/ROMFS/px4fmu_common/init.d/3032_skywalker_x5 index 3f5f79857d..bf5a87068f 100644 --- a/ROMFS/px4fmu_common/init.d/3032_skywalker_x5 +++ b/ROMFS/px4fmu_common/init.d/3032_skywalker_x5 @@ -15,19 +15,19 @@ then param set FW_ATT_TC 0.3 param set FW_L1_DAMPING 0.74 param set FW_L1_PERIOD 15 - param set FW_PR_FF 0.8 + param set FW_PR_FF 0.3 param set FW_PR_I 0 param set FW_PR_IMAX 0.2 - param set FW_PR_P 0.05 + param set FW_PR_P 0.03 param set FW_P_LIM_MAX 45 param set FW_P_LIM_MIN -45 param set FW_P_RMAX_NEG 0 param set FW_P_RMAX_POS 0 param set FW_P_ROLLFF 0 - param set FW_RR_FF 0.6 + param set FW_RR_FF 0.3 param set FW_RR_I 0 param set FW_RR_IMAX 0.2 - param set FW_RR_P 0.05 + param set FW_RR_P 0.03 param set FW_R_LIM 60 param set FW_R_RMAX 0 param set FW_T_HRATE_P 0.01 From 75d08826389680e55543eb017683c8cf9434bf7c Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 10 Feb 2014 13:16:02 +0100 Subject: [PATCH 420/447] fw_pos_control: added default for autoland parameters --- 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 ee8721ff92..a0447d90d2 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 @@ -168,9 +168,9 @@ PARAM_DEFINE_FLOAT(FW_T_SINK_MAX, 5.0f); PARAM_DEFINE_FLOAT(FW_T_HRATE_P, 0.05f); PARAM_DEFINE_FLOAT(FW_T_SRATE_P, 0.05f); -PARAM_DEFINE_FLOAT(FW_LND_ANG, 10.0f); +PARAM_DEFINE_FLOAT(FW_LND_ANG, 5.0f); PARAM_DEFINE_FLOAT(FW_LND_SLLR, 0.9f); PARAM_DEFINE_FLOAT(FW_LND_HVIRT, 10.0f); -PARAM_DEFINE_FLOAT(FW_LND_FLALT, 15.0f); +PARAM_DEFINE_FLOAT(FW_LND_FLALT, 10.0f); PARAM_DEFINE_FLOAT(FW_LND_TLALT, 5.0f); PARAM_DEFINE_FLOAT(FW_LND_HHDIST, 15.0f); From 44bde0db912c78f5a9d7d71f974dd2f36873dd24 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 10 Feb 2014 13:16:27 +0100 Subject: [PATCH 421/447] Navgitor: adjusted default loiter radius --- src/modules/navigator/navigator_params.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/navigator/navigator_params.c b/src/modules/navigator/navigator_params.c index d5e00e35db..1ba159a8e6 100644 --- a/src/modules/navigator/navigator_params.c +++ b/src/modules/navigator/navigator_params.c @@ -55,7 +55,7 @@ PARAM_DEFINE_FLOAT(NAV_MIN_ALT, 50.0f); PARAM_DEFINE_FLOAT(NAV_ACCEPT_RAD, 10.0f); -PARAM_DEFINE_FLOAT(NAV_LOITER_RAD, 100.0f); +PARAM_DEFINE_FLOAT(NAV_LOITER_RAD, 50.0f); PARAM_DEFINE_INT32(NAV_ONB_MIS_EN, 0); PARAM_DEFINE_FLOAT(NAV_TAKEOFF_ALT, 10.0f); // default TAKEOFF altitude PARAM_DEFINE_FLOAT(NAV_LAND_ALT, 5.0f); // slow descend from this altitude when landing From d70d84c9a7be7f629542f40255396f6755239963 Mon Sep 17 00:00:00 2001 From: Stefan Rado Date: Mon, 10 Feb 2014 13:35:11 +0100 Subject: [PATCH 422/447] Fixed wrong VEHICLE_TYPE for multicopters. --- ROMFS/px4fmu_common/init.d/rc.mc_defaults | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_defaults b/ROMFS/px4fmu_common/init.d/rc.mc_defaults index 50d02d9e47..52584677b6 100644 --- a/ROMFS/px4fmu_common/init.d/rc.mc_defaults +++ b/ROMFS/px4fmu_common/init.d/rc.mc_defaults @@ -1,6 +1,6 @@ #!nsh -set VEHICLE_TYPE fw +set VEHICLE_TYPE mc if [ $DO_AUTOCONFIG == yes ] then From aea135a9ced3f1c7e1c4e3be0e5f0e310f96c82d Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 10 Feb 2014 13:41:40 +0100 Subject: [PATCH 423/447] fw_pos_control: flare altitude back to 15m --- src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 a0447d90d2..512ca7b8ac 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 @@ -171,6 +171,6 @@ PARAM_DEFINE_FLOAT(FW_T_SRATE_P, 0.05f); PARAM_DEFINE_FLOAT(FW_LND_ANG, 5.0f); PARAM_DEFINE_FLOAT(FW_LND_SLLR, 0.9f); PARAM_DEFINE_FLOAT(FW_LND_HVIRT, 10.0f); -PARAM_DEFINE_FLOAT(FW_LND_FLALT, 10.0f); +PARAM_DEFINE_FLOAT(FW_LND_FLALT, 15.0f); PARAM_DEFINE_FLOAT(FW_LND_TLALT, 5.0f); PARAM_DEFINE_FLOAT(FW_LND_HHDIST, 15.0f); From 36d1ec80ef264beb34604b3e2b9bb076fd78d52f Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 10 Feb 2014 15:12:32 +0100 Subject: [PATCH 424/447] Startup: don't configure anything if definitions are missing --- ROMFS/px4fmu_common/init.d/rcS | 28 ++++++++++++++++++---------- 1 file changed, 18 insertions(+), 10 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 178bcaeba5..ede835ab71 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -464,16 +464,20 @@ then if [ $MIXER == none ] then - # Set default mixer for multicopter if not defined - set MIXER quad_x + echo "Default mixer for multicopter not defined" fi if [ $MAV_TYPE == none ] then - # Use MAV_TYPE = 2 (quadcopter) if not defined - set MAV_TYPE 2 - # Use mixer to detect vehicle type + if [ $MIXER == FMU_quad_x -o $MIXER == FMU_quad_+ ] + then + set MAV_TYPE 2 + fi + if [ $MIXER == FMU_quad_w ] + then + set MAV_TYPE 2 + fi if [ $MIXER == FMU_hexa_x -o $MIXER == FMU_hexa_+ ] then set MAV_TYPE 13 @@ -487,8 +491,14 @@ then set MAV_TYPE 14 fi fi - - param set MAV_TYPE $MAV_TYPE + + # Still no MAV_TYPE found + if [ $MAV_TYPE == none ] + then + echo "Unknown MAV_TYPE" + else + param set MAV_TYPE $MAV_TYPE + fi # Load mixer and configure outputs sh /etc/init.d/rc.interface @@ -502,10 +512,8 @@ then # if [ $VEHICLE_TYPE == none ] then - echo "[init] Vehicle type: GENERIC" + echo "[init] Vehicle type: No autostart ID found" - # Load mixer and configure outputs - sh /etc/init.d/rc.interface fi # Start any custom addons From 3c592e3a06aa6a0357b9540d8cc1f68333b1c256 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 11 Feb 2014 15:35:26 +0100 Subject: [PATCH 425/447] Startup: fix merge mistake --- ROMFS/px4fmu_common/init.d/rcS | 1 + 1 file changed, 1 insertion(+) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 588a44a013..76f021e339 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -479,6 +479,7 @@ then set MAV_TYPE 2 fi if [ $MIXER == FMU_hexa_x -o $MIXER == FMU_hexa_+ ] + then set MAV_TYPE 13 fi if [ $MIXER == hexa_cox ] From 16908f9aff0e7ad0f967613adf2be9a00c1c6cce Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 11 Feb 2014 23:24:49 +0100 Subject: [PATCH 426/447] autostart for multicopters: frame-specific default parameters reverted and cleaned up --- .../px4fmu_common/init.d/10015_tbs_discovery | 21 ++++++++++++-- ROMFS/px4fmu_common/init.d/10016_3dr_iris | 17 +++++++++-- ROMFS/px4fmu_common/init.d/11001_hexa_cox | 28 ++---------------- ROMFS/px4fmu_common/init.d/12001_octo_cox | 29 ++----------------- ROMFS/px4fmu_common/init.d/4001_quad_x | 2 +- ROMFS/px4fmu_common/init.d/4010_dji_f330 | 19 +++++++++++- ROMFS/px4fmu_common/init.d/4011_dji_f450 | 20 ++++++++++++- ROMFS/px4fmu_common/init.d/4012_hk_x550 | 8 ----- ROMFS/px4fmu_common/init.d/5001_quad_+ | 2 +- ROMFS/px4fmu_common/init.d/6001_hexa_x | 5 ++-- ROMFS/px4fmu_common/init.d/7001_hexa_+ | 5 ++-- ROMFS/px4fmu_common/init.d/8001_octo_x | 2 +- ROMFS/px4fmu_common/init.d/rc.autostart | 5 ---- ROMFS/px4fmu_common/init.d/rc.mc_defaults | 24 +++++++-------- 14 files changed, 92 insertions(+), 95 deletions(-) delete mode 100644 ROMFS/px4fmu_common/init.d/4012_hk_x550 diff --git a/ROMFS/px4fmu_common/init.d/10015_tbs_discovery b/ROMFS/px4fmu_common/init.d/10015_tbs_discovery index b09765e8e7..880e4899b1 100644 --- a/ROMFS/px4fmu_common/init.d/10015_tbs_discovery +++ b/ROMFS/px4fmu_common/init.d/10015_tbs_discovery @@ -2,11 +2,28 @@ # # Team Blacksheep Discovery Quadcopter # -# Simon Wilks +# Anton Babushkin , Simon Wilks # sh /etc/init.d/rc.mc_defaults +if [ $DO_AUTOCONFIG == yes ] +then + # TODO review MC_YAWRATE_I + param set MC_ROLL_P 8.0 + param set MC_ROLLRATE_P 0.07 + param set MC_ROLLRATE_I 0.05 + param set MC_ROLLRATE_D 0.0017 + param set MC_PITCH_P 8.0 + param set MC_PITCHRATE_P 0.14 + param set MC_PITCHRATE_I 0.1 + param set MC_PITCHRATE_D 0.0025 + param set MC_YAW_P 2.8 + param set MC_YAWRATE_P 0.28 + param set MC_YAWRATE_I 0.0 + param set MC_YAWRATE_D 0.0 +fi + set MIXER FMU_quad_w -set PWM_OUTPUTS 1234 \ No newline at end of file +set PWM_OUTPUTS 1234 diff --git a/ROMFS/px4fmu_common/init.d/10016_3dr_iris b/ROMFS/px4fmu_common/init.d/10016_3dr_iris index 42d5169935..d691a6f2e6 100644 --- a/ROMFS/px4fmu_common/init.d/10016_3dr_iris +++ b/ROMFS/px4fmu_common/init.d/10016_3dr_iris @@ -9,9 +9,20 @@ sh /etc/init.d/rc.mc_defaults if [ $DO_AUTOCONFIG == yes ] then - # - # Default parameters for this platform - # + # TODO tune roll/pitch separately + param set MC_ROLL_P 9.0 + param set MC_ROLLRATE_P 0.13 + param set MC_ROLLRATE_I 0.0 + param set MC_ROLLRATE_D 0.004 + param set MC_PITCH_P 9.0 + param set MC_PITCHRATE_P 0.13 + param set MC_PITCHRATE_I 0.0 + param set MC_PITCHRATE_D 0.004 + param set MC_YAW_P 0.5 + param set MC_YAWRATE_P 0.2 + param set MC_YAWRATE_I 0.0 + param set MC_YAWRATE_D 0.0 + param set BAT_V_SCALING 0.00989 param set BAT_C_SCALING 0.0124 fi diff --git a/ROMFS/px4fmu_common/init.d/11001_hexa_cox b/ROMFS/px4fmu_common/init.d/11001_hexa_cox index 2dc83a517c..b98ab47748 100644 --- a/ROMFS/px4fmu_common/init.d/11001_hexa_cox +++ b/ROMFS/px4fmu_common/init.d/11001_hexa_cox @@ -7,33 +7,9 @@ # Lorenz Meier # -if [ $DO_AUTOCONFIG == yes ] -then - # - # Default parameters for this platform - # - param set MC_ATT_P 7.0 - param set MC_ATT_I 0.0 - param set MC_ATT_D 0.0 - param set MC_ATTRATE_P 0.12 - param set MC_ATTRATE_I 0.0 - param set MC_ATTRATE_D 0.004 - param set MC_YAWPOS_P 2.0 - param set MC_YAWPOS_I 0.0 - param set MC_YAWPOS_D 0.0 - param set MC_YAWRATE_P 0.3 - param set MC_YAWRATE_I 0.2 - param set MC_YAWRATE_D 0.005 - - # TODO add default MPC parameters -fi +sh /etc/init.d/rc.mc_defaults -set VEHICLE_TYPE mc set MIXER hexa_cox +# We only can run one channel group with one rate, so set all 8 channels set PWM_OUTPUTS 12345678 -set PWM_RATE 400 -# DJI ESC range -set PWM_DISARMED 900 -set PWM_MIN 1200 -set PWM_MAX 1900 diff --git a/ROMFS/px4fmu_common/init.d/12001_octo_cox b/ROMFS/px4fmu_common/init.d/12001_octo_cox index a55fc5a30d..655cb6226e 100644 --- a/ROMFS/px4fmu_common/init.d/12001_octo_cox +++ b/ROMFS/px4fmu_common/init.d/12001_octo_cox @@ -5,33 +5,8 @@ # Lorenz Meier # -if [ $DO_AUTOCONFIG == yes ] -then - # - # Default parameters for this platform - # - param set MC_ATT_P 7.0 - param set MC_ATT_I 0.0 - param set MC_ATT_D 0.0 - param set MC_ATTRATE_P 0.12 - param set MC_ATTRATE_I 0.0 - param set MC_ATTRATE_D 0.004 - param set MC_YAWPOS_P 2.0 - param set MC_YAWPOS_I 0.0 - param set MC_YAWPOS_D 0.0 - param set MC_YAWRATE_P 0.3 - param set MC_YAWRATE_I 0.2 - param set MC_YAWRATE_D 0.005 - - # TODO add default MPC parameters -fi +sh /etc/init.d/rc.mc_defaults -set VEHICLE_TYPE mc -set MIXER FMU_octo_cox +set MIXER octo_cox set PWM_OUTPUTS 12345678 -set PWM_RATE 400 -# DJI ESC range -set PWM_DISARMED 900 -set PWM_MIN 1200 -set PWM_MAX 1900 diff --git a/ROMFS/px4fmu_common/init.d/4001_quad_x b/ROMFS/px4fmu_common/init.d/4001_quad_x index fa751f1e31..345b0e3e4e 100644 --- a/ROMFS/px4fmu_common/init.d/4001_quad_x +++ b/ROMFS/px4fmu_common/init.d/4001_quad_x @@ -9,4 +9,4 @@ sh /etc/init.d/rc.mc_defaults set MIXER FMU_quad_x -set PWM_OUTPUTS 1234 \ No newline at end of file +set PWM_OUTPUTS 1234 diff --git a/ROMFS/px4fmu_common/init.d/4010_dji_f330 b/ROMFS/px4fmu_common/init.d/4010_dji_f330 index 232af57b4a..cd4480c3eb 100644 --- a/ROMFS/px4fmu_common/init.d/4010_dji_f330 +++ b/ROMFS/px4fmu_common/init.d/4010_dji_f330 @@ -7,4 +7,21 @@ sh /etc/init.d/4001_quad_x -set PWM_MIN 1175 \ No newline at end of file +if [ $DO_AUTOCONFIG == yes ] +then + param set MC_ROLL_P 7.0 + param set MC_ROLLRATE_P 0.1 + param set MC_ROLLRATE_I 0.0 + param set MC_ROLLRATE_D 0.003 + param set MC_PITCH_P 7.0 + param set MC_PITCHRATE_P 0.1 + param set MC_PITCHRATE_I 0.0 + param set MC_PITCHRATE_D 0.003 + param set MC_YAW_P 2.8 + param set MC_YAWRATE_P 0.2 + param set MC_YAWRATE_I 0.0 + param set MC_YAWRATE_D 0.0 +fi + +set PWM_MIN 1175 +set PWM_MAX 1900 diff --git a/ROMFS/px4fmu_common/init.d/4011_dji_f450 b/ROMFS/px4fmu_common/init.d/4011_dji_f450 index 259636acc2..ac2ecc70aa 100644 --- a/ROMFS/px4fmu_common/init.d/4011_dji_f450 +++ b/ROMFS/px4fmu_common/init.d/4011_dji_f450 @@ -7,4 +7,22 @@ sh /etc/init.d/4001_quad_x -set PWM_MIN 1175 \ No newline at end of file +if [ $DO_AUTOCONFIG == yes ] +then + # TODO REVIEW + param set MC_ROLL_P 7.0 + param set MC_ROLLRATE_P 0.1 + param set MC_ROLLRATE_I 0.0 + param set MC_ROLLRATE_D 0.003 + param set MC_PITCH_P 7.0 + param set MC_PITCHRATE_P 0.1 + param set MC_PITCHRATE_I 0.0 + param set MC_PITCHRATE_D 0.003 + param set MC_YAW_P 2.8 + param set MC_YAWRATE_P 0.2 + param set MC_YAWRATE_I 0.0 + param set MC_YAWRATE_D 0.0 +fi + +set PWM_MIN 1175 +set PWM_MAX 1900 diff --git a/ROMFS/px4fmu_common/init.d/4012_hk_x550 b/ROMFS/px4fmu_common/init.d/4012_hk_x550 deleted file mode 100644 index a5c4a86902..0000000000 --- a/ROMFS/px4fmu_common/init.d/4012_hk_x550 +++ /dev/null @@ -1,8 +0,0 @@ -#!nsh -# -# HobbyKing X550 Quadcopter -# -# Todd Stellanova -# - -sh /etc/init.d/4001_quad_x diff --git a/ROMFS/px4fmu_common/init.d/5001_quad_+ b/ROMFS/px4fmu_common/init.d/5001_quad_+ index 2f5ab44d77..55b31067d1 100644 --- a/ROMFS/px4fmu_common/init.d/5001_quad_+ +++ b/ROMFS/px4fmu_common/init.d/5001_quad_+ @@ -9,4 +9,4 @@ sh /etc/init.d/rc.mc_defaults set MIXER FMU_quad_+ -set PWM_OUTPUTS 1234 \ No newline at end of file +set PWM_OUTPUTS 1234 diff --git a/ROMFS/px4fmu_common/init.d/6001_hexa_x b/ROMFS/px4fmu_common/init.d/6001_hexa_x index 73ef12569b..7714a508cf 100644 --- a/ROMFS/px4fmu_common/init.d/6001_hexa_x +++ b/ROMFS/px4fmu_common/init.d/6001_hexa_x @@ -9,6 +9,5 @@ sh /etc/init.d/rc.mc_defaults set MIXER FMU_hexa_x -# We only can run one channel group with one rate, -# so all 8 at 400 Hz -set PWM_OUTPUTS 12345678 \ No newline at end of file +# We only can run one channel group with one rate, so set all 8 channels +set PWM_OUTPUTS 12345678 diff --git a/ROMFS/px4fmu_common/init.d/7001_hexa_+ b/ROMFS/px4fmu_common/init.d/7001_hexa_+ index ef4b6297de..60db8c0690 100644 --- a/ROMFS/px4fmu_common/init.d/7001_hexa_+ +++ b/ROMFS/px4fmu_common/init.d/7001_hexa_+ @@ -9,6 +9,5 @@ sh /etc/init.d/rc.mc_defaults set MIXER FMU_hexa_+ -# We only can run one channel group with one rate, -# so all 8 at 400 Hz -set PWM_OUTPUTS 12345678 \ No newline at end of file +# We only can run one channel group with one rate, so set all 8 channels +set PWM_OUTPUTS 12345678 diff --git a/ROMFS/px4fmu_common/init.d/8001_octo_x b/ROMFS/px4fmu_common/init.d/8001_octo_x index bb87f89fe7..411aee1140 100644 --- a/ROMFS/px4fmu_common/init.d/8001_octo_x +++ b/ROMFS/px4fmu_common/init.d/8001_octo_x @@ -9,4 +9,4 @@ sh /etc/init.d/rc.mc_defaults set MIXER FMU_octo_x -set PWM_OUTPUTS 12345678 \ No newline at end of file +set PWM_OUTPUTS 12345678 diff --git a/ROMFS/px4fmu_common/init.d/rc.autostart b/ROMFS/px4fmu_common/init.d/rc.autostart index 48c26aacd1..3968af58ea 100644 --- a/ROMFS/px4fmu_common/init.d/rc.autostart +++ b/ROMFS/px4fmu_common/init.d/rc.autostart @@ -116,11 +116,6 @@ then sh /etc/init.d/4011_dji_f450 fi -if param compare SYS_AUTOSTART 4012 12 -then - sh /etc/init.d/4012_hk_x550 -fi - # # Quad + # diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_defaults b/ROMFS/px4fmu_common/init.d/rc.mc_defaults index 52584677b6..4db62607a9 100644 --- a/ROMFS/px4fmu_common/init.d/rc.mc_defaults +++ b/ROMFS/px4fmu_common/init.d/rc.mc_defaults @@ -4,22 +4,20 @@ set VEHICLE_TYPE mc if [ $DO_AUTOCONFIG == yes ] then - # - # Default parameters for MC - # param set MC_ROLL_P 7.0 - param set MC_ROLLRATE_P 0.12 + param set MC_ROLLRATE_P 0.1 param set MC_ROLLRATE_I 0.0 - param set MC_ROLLRATE_D 0.004 + param set MC_ROLLRATE_D 0.003 param set MC_PITCH_P 7.0 - param set MC_PITCHRATE_P 0.12 + param set MC_PITCHRATE_P 0.1 param set MC_PITCHRATE_I 0.0 - param set MC_PITCHRATE_D 0.004 - param set MC_YAW_P 2.0 - param set MC_YAWRATE_P 0.3 - param set MC_YAWRATE_I 0.2 - param set MC_YAWRATE_D 0.005 - + param set MC_PITCHRATE_D 0.003 + param set MC_YAW_P 2.8 + param set MC_YAWRATE_P 0.2 + param set MC_YAWRATE_I 0.0 + param set MC_YAWRATE_D 0.0 + param set MC_YAW_FF 0.5 + param set MPC_THR_MAX 1.0 param set MPC_THR_MIN 0.1 param set MPC_XY_P 1.0 @@ -42,4 +40,4 @@ fi set PWM_RATE 400 set PWM_DISARMED 900 set PWM_MIN 1075 -set PWM_MAX 2000 \ No newline at end of file +set PWM_MAX 2000 From 179aa17a3842bb68fa3849e890d20cfb9a1a5392 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 12 Feb 2014 12:21:23 +0100 Subject: [PATCH 427/447] sdlog2: TELE (telemetry status) message added, type for 'rssi' and 'remote_rssi' in 'telemetry_status' topic fixed to be consistent with 'noise'/'remote_noise' and mavlink message. --- src/modules/sdlog2/sdlog2.c | 24 ++++++++++++++++++++++ src/modules/sdlog2/sdlog2_messages.h | 14 +++++++++++++ src/modules/uORB/topics/telemetry_status.h | 10 ++++----- 3 files changed, 43 insertions(+), 5 deletions(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 3c218e21fb..68e6a7469f 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -82,6 +82,7 @@ #include #include #include +#include #include #include @@ -758,6 +759,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct esc_status_s esc; struct vehicle_global_velocity_setpoint_s global_vel_sp; struct battery_status_s battery; + struct telemetry_status_s telemetry; } buf; memset(&buf, 0, sizeof(buf)); @@ -783,6 +785,7 @@ int sdlog2_thread_main(int argc, char *argv[]) int esc_sub; int global_vel_sp_sub; int battery_sub; + int telemetry_sub; } subs; /* log message buffer: header + body */ @@ -811,6 +814,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct log_GVSP_s log_GVSP; struct log_BATT_s log_BATT; struct log_DIST_s log_DIST; + struct log_TELE_s log_TELE; } body; } log_msg = { LOG_PACKET_HEADER_INIT(0) @@ -946,6 +950,12 @@ int sdlog2_thread_main(int argc, char *argv[]) fds[fdsc_count].events = POLLIN; fdsc_count++; + /* --- TELEMETRY STATUS --- */ + subs.telemetry_sub = orb_subscribe(ORB_ID(telemetry_status)); + fds[fdsc_count].fd = subs.telemetry_sub; + fds[fdsc_count].events = POLLIN; + fdsc_count++; + /* WARNING: If you get the error message below, * then the number of registered messages (fdsc) * differs from the number of messages in the above list. @@ -1347,6 +1357,20 @@ int sdlog2_thread_main(int argc, char *argv[]) LOGBUFFER_WRITE_AND_COUNT(BATT); } + /* --- TELEMETRY --- */ + if (fds[ifds++].revents & POLLIN) { + orb_copy(ORB_ID(telemetry_status), subs.telemetry_sub, &buf.telemetry); + log_msg.msg_type = LOG_TELE_MSG; + log_msg.body.log_TELE.rssi = buf.telemetry.rssi; + log_msg.body.log_TELE.remote_rssi = buf.telemetry.remote_rssi; + log_msg.body.log_TELE.noise = buf.telemetry.noise; + log_msg.body.log_TELE.remote_noise = buf.telemetry.remote_noise; + log_msg.body.log_TELE.rxerrors = buf.telemetry.rxerrors; + log_msg.body.log_TELE.fixed = buf.telemetry.fixed; + log_msg.body.log_TELE.txbuf = buf.telemetry.txbuf; + LOGBUFFER_WRITE_AND_COUNT(TELE); + } + /* signal the other thread new data, but not yet unlock */ if (logbuffer_count(&lb) > MIN_BYTES_TO_WRITE) { /* only request write if several packets can be written at once */ diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index db87e3a6a2..16bfc355d6 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -264,6 +264,18 @@ struct log_DIST_s { uint8_t flags; }; +/* --- TELE - TELEMETRY STATUS --- */ +#define LOG_TELE_MSG 22 +struct log_TELE_s { + uint8_t rssi; + uint8_t remote_rssi; + uint8_t noise; + uint8_t remote_noise; + uint16_t rxerrors; + uint16_t fixed; + uint8_t txbuf; +}; + /********** SYSTEM MESSAGES, ID > 0x80 **********/ /* --- TIME - TIME STAMP --- */ @@ -311,6 +323,8 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(GVSP, "fff", "VX,VY,VZ"), LOG_FORMAT(BATT, "ffff", "V,VFilt,C,Discharged"), LOG_FORMAT(DIST, "ffB", "Bottom,BottomRate,Flags"), + LOG_FORMAT(TELE, "BBBBHHB", "RSSI,RemRSSI,Noise,RemNoise,RXErr,Fixed,TXBuf"), + /* system-level messages, ID >= 0x80 */ // FMT: don't write format of format message, it's useless LOG_FORMAT(TIME, "Q", "StartTime"), diff --git a/src/modules/uORB/topics/telemetry_status.h b/src/modules/uORB/topics/telemetry_status.h index 828fb31cc7..5192d4d589 100644 --- a/src/modules/uORB/topics/telemetry_status.h +++ b/src/modules/uORB/topics/telemetry_status.h @@ -58,10 +58,10 @@ enum TELEMETRY_STATUS_RADIO_TYPE { struct telemetry_status_s { uint64_t timestamp; enum TELEMETRY_STATUS_RADIO_TYPE type; /**< type of the radio hardware */ - unsigned rssi; /**< local signal strength */ - unsigned remote_rssi; /**< remote signal strength */ - unsigned rxerrors; /**< receive errors */ - unsigned fixed; /**< count of error corrected packets */ + uint8_t rssi; /**< local signal strength */ + uint8_t remote_rssi; /**< remote signal strength */ + uint16_t rxerrors; /**< receive errors */ + uint16_t fixed; /**< count of error corrected packets */ uint8_t noise; /**< background noise level */ uint8_t remote_noise; /**< remote background noise level */ uint8_t txbuf; /**< how full the tx buffer is as a percentage */ @@ -73,4 +73,4 @@ struct telemetry_status_s { ORB_DECLARE(telemetry_status); -#endif /* TOPIC_TELEMETRY_STATUS_H */ \ No newline at end of file +#endif /* TOPIC_TELEMETRY_STATUS_H */ From 3d83c45f7585c71bee3f07ea414d798ab7e2bae5 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 12 Feb 2014 13:20:15 +0100 Subject: [PATCH 428/447] mavlink: bug in telemetry_status publication fixed --- src/modules/mavlink/mavlink_receiver.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index a371a499ef..1dbe564955 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -351,7 +351,7 @@ handle_message(mavlink_message_t *msg) tstatus.rxerrors = rstatus.rxerrors; tstatus.fixed = rstatus.fixed; - if (telemetry_status_pub == 0) { + if (telemetry_status_pub <= 0) { telemetry_status_pub = orb_advertise(ORB_ID(telemetry_status), &tstatus); } else { From f6694c2cef62ee3284598ed1b4d8c6954effab4e Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 13 Feb 2014 00:03:51 +0100 Subject: [PATCH 429/447] rc.fw_defaults: increase acceptance radius which is used by navigator to generate virtual waypoints (RTL etc.) --- ROMFS/px4fmu_common/init.d/rc.fw_defaults | 1 + 1 file changed, 1 insertion(+) diff --git a/ROMFS/px4fmu_common/init.d/rc.fw_defaults b/ROMFS/px4fmu_common/init.d/rc.fw_defaults index 3e340699fc..3a50fcf56f 100644 --- a/ROMFS/px4fmu_common/init.d/rc.fw_defaults +++ b/ROMFS/px4fmu_common/init.d/rc.fw_defaults @@ -10,4 +10,5 @@ then param set NAV_LAND_ALT 90 param set NAV_RTL_ALT 100 param set NAV_RTL_LAND_T -1 + param set NAV_ACCEPT_RAD 50 fi \ No newline at end of file From 4982e819837195aa512fb977639ce1dd0c0cec3a Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 13 Feb 2014 15:30:06 +0100 Subject: [PATCH 430/447] Navigator: set loiter WP correctly --- 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 5139ae6cd2..260356eca9 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -1080,9 +1080,8 @@ Navigator::start_loiter() mavlink_log_info(_mavlink_fd, "[navigator] loiter at current altitude"); } - _pos_sp_triplet.current.type = SETPOINT_TYPE_LOITER; } - + _pos_sp_triplet.current.type = SETPOINT_TYPE_LOITER; _pos_sp_triplet.current.loiter_radius = _parameters.loiter_radius; _pos_sp_triplet.current.loiter_direction = 1; _pos_sp_triplet.previous.valid = false; From 036ebdbe78defdcc8c1cf5955e8df773a16e7e8c Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 13 Feb 2014 16:08:49 +0100 Subject: [PATCH 431/447] Commander: add guard for parachute deployment --- src/modules/commander/commander.cpp | 14 +++++++++----- src/modules/navigator/navigator_params.c | 1 + 2 files changed, 10 insertions(+), 5 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index c039b85737..8129dddb30 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -152,6 +152,7 @@ static uint64_t last_print_mode_reject_time = 0; static bool on_usb_power = false; static float takeoff_alt = 5.0f; +static int parachute_enabled = 0; static struct vehicle_status_s status; static struct actuator_armed_s armed; @@ -563,7 +564,9 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe /* Flight termination */ case VEHICLE_CMD_DO_SET_SERVO: { //xxx: needs its own mavlink command - if (armed->armed && cmd->param3 > 0.5) { //xxx: for safety only for now, param3 is unused by VEHICLE_CMD_DO_SET_SERVO + //XXX: to enable the parachute, a param needs to be set + //xxx: for safety only for now, param3 is unused by VEHICLE_CMD_DO_SET_SERVO + if (armed->armed && cmd->param3 > 0.5 && parachute_enabled) { transition_result_t failsafe_res = failsafe_state_transition(status, FAILSAFE_STATE_TERMINATION); result = VEHICLE_CMD_RESULT_ACCEPTED; ret = true; @@ -615,6 +618,7 @@ int commander_thread_main(int argc, char *argv[]) param_t _param_system_id = param_find("MAV_SYS_ID"); param_t _param_component_id = param_find("MAV_COMP_ID"); param_t _param_takeoff_alt = param_find("NAV_TAKEOFF_ALT"); + param_t _param_enable_parachute = param_find("NAV_PARACHUTE_EN"); /* welcome user */ warnx("starting"); @@ -860,10 +864,10 @@ int commander_thread_main(int argc, char *argv[]) /* re-check RC calibration */ rc_calibration_ok = (OK == rc_calibration_check(mavlink_fd)); - - /* navigation parameters */ - param_get(_param_takeoff_alt, &takeoff_alt); } + /* navigation parameters */ + param_get(_param_takeoff_alt, &takeoff_alt); + param_get(_param_enable_parachute, ¶chute_enabled); } orb_check(sp_man_sub, &updated); @@ -1251,7 +1255,7 @@ int commander_thread_main(int argc, char *argv[]) // TODO remove this hack /* flight termination in manual mode if assisted switch is on easy position */ - if (!status.is_rotary_wing && armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.assisted_switch > STICK_ON_OFF_LIMIT) { + if (!status.is_rotary_wing && parachute_enabled && armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.assisted_switch > STICK_ON_OFF_LIMIT) { if (TRANSITION_CHANGED == failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION)) { tune_positive(); } diff --git a/src/modules/navigator/navigator_params.c b/src/modules/navigator/navigator_params.c index 1ba159a8e6..9e05bbffa8 100644 --- a/src/modules/navigator/navigator_params.c +++ b/src/modules/navigator/navigator_params.c @@ -61,3 +61,4 @@ PARAM_DEFINE_FLOAT(NAV_TAKEOFF_ALT, 10.0f); // default TAKEOFF altitude PARAM_DEFINE_FLOAT(NAV_LAND_ALT, 5.0f); // slow descend from this altitude when landing PARAM_DEFINE_FLOAT(NAV_RTL_ALT, 30.0f); // min altitude for going home in RTL mode PARAM_DEFINE_FLOAT(NAV_RTL_LAND_T, -1.0f); // delay after descend before landing, if set to -1 the system will not land but loiter at NAV_LAND_ALT +PARAM_DEFINE_INT32(NAV_PARACHUTE_EN, 0); // enable parachute deployment From 08f1e6a9dc86c03015cb2a83dc94c3412d6d9eb4 Mon Sep 17 00:00:00 2001 From: px4dev Date: Thu, 13 Feb 2014 09:20:33 -0800 Subject: [PATCH 432/447] Fix base clock frequencies for timers 9/10/11 (not currently used). Thanks to xiazibin@gmail.com for pointing these out. --- nuttx-configs/px4fmu-v2/include/board.h | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/nuttx-configs/px4fmu-v2/include/board.h b/nuttx-configs/px4fmu-v2/include/board.h index 850043ddf5..3bede8a1f7 100755 --- a/nuttx-configs/px4fmu-v2/include/board.h +++ b/nuttx-configs/px4fmu-v2/include/board.h @@ -141,9 +141,9 @@ #define STM32_APB2_TIM1_CLKIN (2*STM32_PCLK2_FREQUENCY) #define STM32_APB2_TIM8_CLKIN (2*STM32_PCLK2_FREQUENCY) -#define STM32_APB2_TIM9_CLKIN (2*STM32_PCLK1_FREQUENCY) -#define STM32_APB2_TIM10_CLKIN (2*STM32_PCLK1_FREQUENCY) -#define STM32_APB2_TIM11_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB2_TIM9_CLKIN (2*STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM10_CLKIN (2*STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM11_CLKIN (2*STM32_PCLK2_FREQUENCY) /* Timer Frequencies, if APBx is set to 1, frequency is same to APBx * otherwise frequency is 2xAPBx. From ccfe476326d8b01e33a3a7ea115054a31fa7a2b9 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 13 Feb 2014 20:53:47 +0100 Subject: [PATCH 433/447] decrease MC_PITCHRATE_P for TBS Discovery --- ROMFS/px4fmu_common/init.d/10015_tbs_discovery | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/10015_tbs_discovery b/ROMFS/px4fmu_common/init.d/10015_tbs_discovery index 880e4899b1..fe85f7d35d 100644 --- a/ROMFS/px4fmu_common/init.d/10015_tbs_discovery +++ b/ROMFS/px4fmu_common/init.d/10015_tbs_discovery @@ -2,7 +2,7 @@ # # Team Blacksheep Discovery Quadcopter # -# Anton Babushkin , Simon Wilks +# Anton Babushkin , Simon Wilks , Thomas Gubler # sh /etc/init.d/rc.mc_defaults @@ -15,7 +15,7 @@ then param set MC_ROLLRATE_I 0.05 param set MC_ROLLRATE_D 0.0017 param set MC_PITCH_P 8.0 - param set MC_PITCHRATE_P 0.14 + param set MC_PITCHRATE_P 0.1 param set MC_PITCHRATE_I 0.1 param set MC_PITCHRATE_D 0.0025 param set MC_YAW_P 2.8 From 7441efde4745c0dddc08a36a0bbf83307f82948a Mon Sep 17 00:00:00 2001 From: Stefan Rado Date: Fri, 14 Feb 2014 01:48:00 +0100 Subject: [PATCH 434/447] Add a lot of MAVLink parameter documentation. --- .../launchdetection/launchdetection_params.c | 48 ++- src/modules/commander/commander_params.c | 32 ++ src/modules/mavlink/mavlink.c | 12 + src/modules/navigator/geofence_params.c | 16 +- src/modules/navigator/navigator_params.c | 63 +++- src/modules/sensors/sensor_params.c | 303 ++++++++++++++++-- src/modules/systemlib/system_params.c | 19 +- 7 files changed, 433 insertions(+), 60 deletions(-) diff --git a/src/lib/launchdetection/launchdetection_params.c b/src/lib/launchdetection/launchdetection_params.c index 63a8981aa8..45d7957f11 100644 --- a/src/lib/launchdetection/launchdetection_params.c +++ b/src/lib/launchdetection/launchdetection_params.c @@ -45,28 +45,46 @@ #include /* - * Launch detection parameters, accessible via MAVLink + * Catapult launch detection parameters, accessible via MAVLink * */ -/* Catapult Launch detection */ - -// @DisplayName Switch to enable launchdetection -// @Description if set to 1 launchdetection is enabled -// @Range 0 or 1 +/** + * Enable launch detection. + * + * @min 0 + * @max 1 + * @group Launch detection + */ PARAM_DEFINE_INT32(LAUN_ALL_ON, 0); -// @DisplayName Catapult Accelerometer Threshold -// @Description LAUN_CAT_A * LAUN_CAT_T serves as threshold to trigger launch detection -// @Range > 0 +/** + * Catapult accelerometer theshold. + * + * LAUN_CAT_A * LAUN_CAT_T serves as threshold to trigger launch detection. + * + * @min 0 + * @group Launch detection + */ PARAM_DEFINE_FLOAT(LAUN_CAT_A, 30.0f); -// @DisplayName Catapult Time Threshold -// @Description LAUN_CAT_A * LAUN_CAT_T serves as threshold to trigger launch detection -// @Range > 0, in seconds +/** + * Catapult time theshold. + * + * LAUN_CAT_A * LAUN_CAT_T serves as threshold to trigger launch detection. + * + * @min 0 + * @group Launch detection + */ PARAM_DEFINE_FLOAT(LAUN_CAT_T, 0.05f); -// @DisplayName Throttle setting while detecting the launch -// @Description The throttle is set to this value while the system is waiting for the takeoff -// @Range 0 to 1 +/** + * Throttle setting while detecting launch. + * + * The throttle is set to this value while the system is waiting for the take-off. + * + * @min 0 + * @max 1 + * @group Launch detection + */ PARAM_DEFINE_FLOAT(LAUN_THR_PRE, 0.0f); diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index d3155f7bf5..80ca68f21f 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -48,7 +48,39 @@ PARAM_DEFINE_FLOAT(TRIM_ROLL, 0.0f); PARAM_DEFINE_FLOAT(TRIM_PITCH, 0.0f); PARAM_DEFINE_FLOAT(TRIM_YAW, 0.0f); + +/** + * Empty cell voltage. + * + * Defines the voltage where a single cell of the battery is considered empty. + * + * @group Battery Calibration + */ PARAM_DEFINE_FLOAT(BAT_V_EMPTY, 3.4f); + +/** + * Full cell voltage. + * + * Defines the voltage where a single cell of the battery is considered full. + * + * @group Battery Calibration + */ PARAM_DEFINE_FLOAT(BAT_V_FULL, 3.9f); + +/** + * Number of cells. + * + * Defines the number of cells the attached battery consists of. + * + * @group Battery Calibration + */ PARAM_DEFINE_INT32(BAT_N_CELLS, 3); + +/** + * Battery capacity. + * + * Defines the capacity of the attached battery. + * + * @group Battery Calibration + */ PARAM_DEFINE_FLOAT(BAT_CAPACITY, -1.0f); diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c index 4d975066f7..ade4469c58 100644 --- a/src/modules/mavlink/mavlink.c +++ b/src/modules/mavlink/mavlink.c @@ -76,8 +76,20 @@ #include /* define MAVLink specific parameters */ +/** + * MAVLink system ID + * @group MAVLink + */ PARAM_DEFINE_INT32(MAV_SYS_ID, 1); +/** + * MAVLink component ID + * @group MAVLink + */ PARAM_DEFINE_INT32(MAV_COMP_ID, 50); +/** + * MAVLink type + * @group MAVLink + */ PARAM_DEFINE_INT32(MAV_TYPE, MAV_TYPE_FIXED_WING); __EXPORT int mavlink_main(int argc, char *argv[]); diff --git a/src/modules/navigator/geofence_params.c b/src/modules/navigator/geofence_params.c index 20dd1fe2f0..5831a0ca94 100644 --- a/src/modules/navigator/geofence_params.c +++ b/src/modules/navigator/geofence_params.c @@ -45,11 +45,17 @@ #include /* - * geofence parameters, accessible via MAVLink - * + * Geofence parameters, accessible via MAVLink */ -// @DisplayName Switch to enable geofence -// @Description if set to 1 geofence is enabled, defaults to 1 because geofence is only enabled when the geofence.txt file is present -// @Range 0 or 1 +/** + * Enable geofence. + * + * Set to 1 to enable geofence. + * Defaults to 1 because geofence is only enabled when the geofence.txt file is present. + * + * @min 0 + * @max 1 + * @group Geofence + */ PARAM_DEFINE_INT32(GF_ON, 1); diff --git a/src/modules/navigator/navigator_params.c b/src/modules/navigator/navigator_params.c index 9e05bbffa8..ec7a4e2296 100644 --- a/src/modules/navigator/navigator_params.c +++ b/src/modules/navigator/navigator_params.c @@ -50,15 +50,72 @@ /* * Navigator parameters, accessible via MAVLink - * */ +/** + * Minimum altitude + * + * @group Navigation + */ PARAM_DEFINE_FLOAT(NAV_MIN_ALT, 50.0f); + +/** + * Waypoint acceptance radius. + * + * @group Navigation + */ PARAM_DEFINE_FLOAT(NAV_ACCEPT_RAD, 10.0f); + +/** + * Loiter radius. + * + * @group Navigation + */ PARAM_DEFINE_FLOAT(NAV_LOITER_RAD, 50.0f); + +/** + * @group Navigation + */ PARAM_DEFINE_INT32(NAV_ONB_MIS_EN, 0); + +/** + * Default take-off altitude. + * + * @group Navigation + */ PARAM_DEFINE_FLOAT(NAV_TAKEOFF_ALT, 10.0f); // default TAKEOFF altitude + +/** + * Landing altitude. + * + * Slowly descend from this altitude when landing. + * + * @group Navigation + */ PARAM_DEFINE_FLOAT(NAV_LAND_ALT, 5.0f); // slow descend from this altitude when landing + +/** + * Return-to-land altitude. + * + * Minimum altitude for going home in RTL mode. + * + * @group Navigation + */ PARAM_DEFINE_FLOAT(NAV_RTL_ALT, 30.0f); // min altitude for going home in RTL mode -PARAM_DEFINE_FLOAT(NAV_RTL_LAND_T, -1.0f); // delay after descend before landing, if set to -1 the system will not land but loiter at NAV_LAND_ALT -PARAM_DEFINE_INT32(NAV_PARACHUTE_EN, 0); // enable parachute deployment + +/** + * Return-to-land delay. + * + * Delay after descend before landing. + * If set to -1 the system will not land but loiter at NAV_LAND_ALT. + * + * @group Navigation + */ +PARAM_DEFINE_FLOAT(NAV_RTL_LAND_T, -1.0f); + +/** + * Enable parachute deployment. + * + * @group Navigation + */ +PARAM_DEFINE_INT32(NAV_PARACHUTE_EN, 0); diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 30659fd3a0..288c6e00a0 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -42,13 +42,10 @@ */ #include - #include /** - * Gyro X offset - * - * This is an X-axis offset for the gyro. Adjust it according to the calibration data. + * Gyro X-axis offset * * @min -10.0 * @max 10.0 @@ -57,7 +54,7 @@ PARAM_DEFINE_FLOAT(SENS_GYRO_XOFF, 0.0f); /** - * Gyro Y offset + * Gyro Y-axis offset * * @min -10.0 * @max 10.0 @@ -66,7 +63,7 @@ PARAM_DEFINE_FLOAT(SENS_GYRO_XOFF, 0.0f); PARAM_DEFINE_FLOAT(SENS_GYRO_YOFF, 0.0f); /** - * Gyro Z offset + * Gyro Z-axis offset * * @min -5.0 * @max 5.0 @@ -75,9 +72,7 @@ PARAM_DEFINE_FLOAT(SENS_GYRO_YOFF, 0.0f); PARAM_DEFINE_FLOAT(SENS_GYRO_ZOFF, 0.0f); /** - * Gyro X scaling - * - * X-axis scaling. + * Gyro X-axis scaling factor * * @min -1.5 * @max 1.5 @@ -86,9 +81,7 @@ PARAM_DEFINE_FLOAT(SENS_GYRO_ZOFF, 0.0f); PARAM_DEFINE_FLOAT(SENS_GYRO_XSCALE, 1.0f); /** - * Gyro Y scaling - * - * Y-axis scaling. + * Gyro Y-axis scaling factor * * @min -1.5 * @max 1.5 @@ -97,9 +90,7 @@ PARAM_DEFINE_FLOAT(SENS_GYRO_XSCALE, 1.0f); PARAM_DEFINE_FLOAT(SENS_GYRO_YSCALE, 1.0f); /** - * Gyro Z scaling - * - * Z-axis scaling. + * Gyro Z-axis scaling factor * * @min -1.5 * @max 1.5 @@ -107,10 +98,9 @@ PARAM_DEFINE_FLOAT(SENS_GYRO_YSCALE, 1.0f); */ PARAM_DEFINE_FLOAT(SENS_GYRO_ZSCALE, 1.0f); + /** - * Magnetometer X offset - * - * This is an X-axis offset for the magnetometer. + * Magnetometer X-axis offset * * @min -500.0 * @max 500.0 @@ -119,9 +109,7 @@ PARAM_DEFINE_FLOAT(SENS_GYRO_ZSCALE, 1.0f); PARAM_DEFINE_FLOAT(SENS_MAG_XOFF, 0.0f); /** - * Magnetometer Y offset - * - * This is an Y-axis offset for the magnetometer. + * Magnetometer Y-axis offset * * @min -500.0 * @max 500.0 @@ -130,9 +118,7 @@ PARAM_DEFINE_FLOAT(SENS_MAG_XOFF, 0.0f); PARAM_DEFINE_FLOAT(SENS_MAG_YOFF, 0.0f); /** - * Magnetometer Z offset - * - * This is an Z-axis offset for the magnetometer. + * Magnetometer Z-axis offset * * @min -500.0 * @max 500.0 @@ -140,24 +126,134 @@ PARAM_DEFINE_FLOAT(SENS_MAG_YOFF, 0.0f); */ PARAM_DEFINE_FLOAT(SENS_MAG_ZOFF, 0.0f); +/** + * Magnetometer X-axis scaling factor + * + * @group Sensor Calibration + */ PARAM_DEFINE_FLOAT(SENS_MAG_XSCALE, 1.0f); + +/** + * Magnetometer Y-axis scaling factor + * + * @group Sensor Calibration + */ PARAM_DEFINE_FLOAT(SENS_MAG_YSCALE, 1.0f); + +/** + * Magnetometer Z-axis scaling factor + * + * @group Sensor Calibration + */ PARAM_DEFINE_FLOAT(SENS_MAG_ZSCALE, 1.0f); + +/** + * Accelerometer X-axis offset + * + * @group Sensor Calibration + */ PARAM_DEFINE_FLOAT(SENS_ACC_XOFF, 0.0f); + +/** + * Accelerometer Y-axis offset + * + * @group Sensor Calibration + */ PARAM_DEFINE_FLOAT(SENS_ACC_YOFF, 0.0f); + +/** + * Accelerometer Z-axis offset + * + * @group Sensor Calibration + */ PARAM_DEFINE_FLOAT(SENS_ACC_ZOFF, 0.0f); +/** + * Accelerometer X-axis scaling factor + * + * @group Sensor Calibration + */ PARAM_DEFINE_FLOAT(SENS_ACC_XSCALE, 1.0f); + +/** + * Accelerometer Y-axis scaling factor + * + * @group Sensor Calibration + */ PARAM_DEFINE_FLOAT(SENS_ACC_YSCALE, 1.0f); + +/** + * Accelerometer Z-axis scaling factor + * + * @group Sensor Calibration + */ PARAM_DEFINE_FLOAT(SENS_ACC_ZSCALE, 1.0f); + +/** + * Differential pressure sensor offset + * + * @group Sensor Calibration + */ PARAM_DEFINE_FLOAT(SENS_DPRES_OFF, 0.0f); + +/** + * Differential pressure sensor analog enabled + * + * @group Sensor Calibration + */ PARAM_DEFINE_INT32(SENS_DPRES_ANA, 0); + +/** + * Board rotation + * + * This parameter defines the rotation of the FMU board relative to the platform. + * Possible values are: + * 0 = No rotation + * 1 = Yaw 45° + * 2 = Yaw 90° + * 3 = Yaw 135° + * 4 = Yaw 180° + * 5 = Yaw 225° + * 6 = Yaw 270° + * 7 = Yaw 315° + * 8 = Roll 180° + * 9 = Roll 180°, Yaw 45° + * 10 = Roll 180°, Yaw 90° + * 11 = Roll 180°, Yaw 135° + * 12 = Pitch 180° + * 13 = Roll 180°, Yaw 225° + * 14 = Roll 180°, Yaw 270° + * 15 = Roll 180°, Yaw 315° + * 16 = Roll 90° + * 17 = Roll 90°, Yaw 45° + * 18 = Roll 90°, Yaw 90° + * 19 = Roll 90°, Yaw 135° + * 20 = Roll 270° + * 21 = Roll 270°, Yaw 45° + * 22 = Roll 270°, Yaw 90° + * 23 = Roll 270°, Yaw 135° + * 24 = Pitch 90° + * 25 = Pitch 270° + * + * @group Sensor Calibration + */ PARAM_DEFINE_INT32(SENS_BOARD_ROT, 0); + +/** + * External magnetometer rotation + * + * This parameter defines the rotation of the external magnetometer relative + * to the platform (not relative to the FMU). + * See SENS_BOARD_ROT for possible values. + * + * @group Sensor Calibration + */ PARAM_DEFINE_INT32(SENS_EXT_MAG_ROT, 0); + /** * RC Channel 1 Minimum * @@ -367,20 +463,52 @@ PARAM_DEFINE_FLOAT(RC18_DZ, 0.0f); #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 PARAM_DEFINE_INT32(RC_RL1_DSM_VCC, 0); /* Relay 1 controls DSM VCC */ #endif -PARAM_DEFINE_INT32(RC_DSM_BIND, -1); /* -1 = Idle, 0 = Start DSM2 bind, 1 = Start DSMX bind */ +/** + * DSM binding trigger. + * + * -1 = Idle, 0 = Start DSM2 bind, 1 = Start DSMX bind + * + * @group Radio Calibration + */ +PARAM_DEFINE_INT32(RC_DSM_BIND, -1); + + +/** + * Scaling factor for battery voltage sensor on PX4IO. + * + * @group Battery Calibration + */ PARAM_DEFINE_INT32(BAT_V_SCALE_IO, 10000); + #ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 +/** + * Scaling factor for battery voltage sensor on FMU v2. + * + * @group Battery Calibration + */ PARAM_DEFINE_FLOAT(BAT_V_SCALING, 0.0082f); #else -/* default is conversion factor for the PX4IO / PX4IOAR board, the factor for PX4FMU standalone is different */ -/* PX4IOAR: 0.00838095238 */ -/* FMU standalone: 1/(10 / (47+10)) * (3.3 / 4095) = 0.00459340659 */ -/* FMU with PX4IOAR: (3.3f * 52.0f / 5.0f / 4095.0f) */ +/** + * Scaling factor for battery voltage sensor on FMU v1. + * + * FMUv1 standalone: 1/(10 / (47+10)) * (3.3 / 4095) = 0.00459340659 + * FMUv1 with PX4IO: 0.00459340659 + * FMUv1 with PX4IOAR: (3.3f * 52.0f / 5.0f / 4095.0f) = 0.00838095238 + * + * @group Battery Calibration + */ PARAM_DEFINE_FLOAT(BAT_V_SCALING, 0.00459340659f); #endif + +/** + * Scaling factor for battery current sensor. + * + * @group Battery Calibration + */ PARAM_DEFINE_FLOAT(BAT_C_SCALING, 0.0124); /* scaling for 3DR power brick */ + /** * Roll control channel mapping. * @@ -446,22 +574,127 @@ PARAM_DEFINE_INT32(RC_MAP_YAW, 4); * @group Radio Calibration */ PARAM_DEFINE_INT32(RC_MAP_MODE_SW, 0); + +/** + * Return switch channel mapping. + * + * @min 0 + * @max 18 + * @group Radio Calibration + */ PARAM_DEFINE_INT32(RC_MAP_RETURN_SW, 0); + +/** + * Assist switch channel mapping. + * + * @min 0 + * @max 18 + * @group Radio Calibration + */ PARAM_DEFINE_INT32(RC_MAP_ASSIST_SW, 0); + +/** + * Mission switch channel mapping. + * + * @min 0 + * @max 18 + * @group Radio Calibration + */ PARAM_DEFINE_INT32(RC_MAP_MISSIO_SW, 0); //PARAM_DEFINE_INT32(RC_MAP_OFFB_SW, 0); +/** + * Flaps channel mapping. + * + * @min 0 + * @max 18 + * @group Radio Calibration + */ PARAM_DEFINE_INT32(RC_MAP_FLAPS, 0); -PARAM_DEFINE_INT32(RC_MAP_AUX1, 0); /**< default function: camera pitch */ -PARAM_DEFINE_INT32(RC_MAP_AUX2, 0); /**< default function: camera roll */ -PARAM_DEFINE_INT32(RC_MAP_AUX3, 0); /**< default function: camera azimuth / yaw */ +/** + * Auxiliary switch 1 channel mapping. + * + * Default function: Camera pitch + * + * @min 0 + * @max 18 + * @group Radio Calibration + */ +PARAM_DEFINE_INT32(RC_MAP_AUX1, 0); +/** + * Auxiliary switch 2 channel mapping. + * + * Default function: Camera roll + * + * @min 0 + * @max 18 + * @group Radio Calibration + */ +PARAM_DEFINE_INT32(RC_MAP_AUX2, 0); /**< default function: camera roll */ + +/** + * Auxiliary switch 3 channel mapping. + * + * Default function: Camera azimuth / yaw + * + * @min 0 + * @max 18 + * @group Radio Calibration + */ +PARAM_DEFINE_INT32(RC_MAP_AUX3, 0); + + +/** + * Roll scaling factor + * + * @group Radio Calibration + */ PARAM_DEFINE_FLOAT(RC_SCALE_ROLL, 0.6f); + +/** + * Pitch scaling factor + * + * @group Radio Calibration + */ PARAM_DEFINE_FLOAT(RC_SCALE_PITCH, 0.6f); + +/** + * Yaw scaling factor + * + * @group Radio Calibration + */ PARAM_DEFINE_FLOAT(RC_SCALE_YAW, 2.0f); -PARAM_DEFINE_INT32(RC_FS_CH, 0); /**< RC failsafe channel, 0 = disable */ -PARAM_DEFINE_INT32(RC_FS_MODE, 0); /**< RC failsafe mode: 0 = too low means signal loss, 1 = too high means signal loss */ -PARAM_DEFINE_FLOAT(RC_FS_THR, 800); /**< RC failsafe PWM threshold */ + +/** + * Failsafe channel mapping. + * + * @min 0 + * @max 18 + * @group Radio Calibration + */ +PARAM_DEFINE_INT32(RC_FS_CH, 0); + +/** + * Failsafe channel mode. + * + * 0 = too low means signal loss, + * 1 = too high means signal loss + * + * @min 0 + * @max 1 + * @group Radio Calibration + */ +PARAM_DEFINE_INT32(RC_FS_MODE, 0); + +/** + * Failsafe channel PWM threshold. + * + * @min 0 + * @max 1 + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC_FS_THR, 800); diff --git a/src/modules/systemlib/system_params.c b/src/modules/systemlib/system_params.c index 75be090f80..cb35a25413 100644 --- a/src/modules/systemlib/system_params.c +++ b/src/modules/systemlib/system_params.c @@ -40,8 +40,23 @@ #include #include -// Auto-start script with index #n +/** + * Auto-start script index. + * + * Defines the auto-start script used to bootstrap the system. + * + * @group System + */ PARAM_DEFINE_INT32(SYS_AUTOSTART, 0); -// Automatically configure default values +/** + * Automatically configure default values. + * + * Set to 1 to set platform-specific parameters to their default + * values on next system startup. + * + * @min 0 + * @max 1 + * @group System + */ PARAM_DEFINE_INT32(SYS_AUTOCONFIG, 0); From ed3ffc26d69f25dc087a23761cbf0655afeae8c0 Mon Sep 17 00:00:00 2001 From: Stefan Rado Date: Fri, 14 Feb 2014 02:21:24 +0100 Subject: [PATCH 435/447] Layout fixes for wiki parameter documentation. - Replace newlines in names and comments with a space. - Right align min/max/default values. --- Tools/px4params/dokuwikiout.py | 46 +++++++++++++++++++--------------- 1 file changed, 26 insertions(+), 20 deletions(-) diff --git a/Tools/px4params/dokuwikiout.py b/Tools/px4params/dokuwikiout.py index c5cf65ea60..e02035423d 100644 --- a/Tools/px4params/dokuwikiout.py +++ b/Tools/px4params/dokuwikiout.py @@ -19,34 +19,40 @@ class DokuWikiOutput(output.Output): for group in groups: result += "==== %s ====\n\n" % group.GetName() result += "|< 100% 20% 20% 10% 10% 10% 30%>|\n" - result += "^ Name ^ Description ^ Min ^ Max ^ Default ^ Comment ^\n" + result += "^ Name ^ Description ^ Min ^ Max ^ Default ^ Comment ^\n" for param in group.GetParams(): code = param.GetFieldValue("code") name = param.GetFieldValue("short_desc") - name = name.replace("\n", "") - result += "| %s | %s " % (code, name) min_val = param.GetFieldValue("min") - if min_val is not None: - result += " | %s " % min_val - else: - result += " | " max_val = param.GetFieldValue("max") - if max_val is not None: - result += " | %s " % max_val - else: - result += " | " def_val = param.GetFieldValue("default") - if def_val is not None: - result += "| %s " % def_val - else: - result += " | " long_desc = param.GetFieldValue("long_desc") - if long_desc is not None: - long_desc = long_desc.replace("\n", "") - result += "| %s " % long_desc + + name = name.replace("\n", " ") + result += "| %s | %s |" % (code, name) + + if min_val is not None: + result += " %s |" % min_val else: - result += " | " - result += " |\n" + result += " |" + + if max_val is not None: + result += " %s |" % max_val + else: + result += " |" + + if def_val is not None: + result += " %s |" % def_val + else: + result += " |" + + if long_desc is not None: + long_desc = long_desc.replace("\n", " ") + result += " %s |" % long_desc + else: + result += " |" + + result += "\n" result += "\n" post_text = """ From 9e56652d3eda75a2738bcb9eab3ff99ac2aa455b Mon Sep 17 00:00:00 2001 From: Highlander-UA <315u448@ukrpost.net> Date: Sat, 15 Feb 2014 13:34:49 +0200 Subject: [PATCH 436/447] Added comments for L1 control parameters --- .../fw_pos_control_l1_params.c | 193 +++++++++++++++++- 1 file changed, 186 insertions(+), 7 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 512ca7b8ac..9f15a93866 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 @@ -120,57 +120,236 @@ PARAM_DEFINE_FLOAT(FW_P_LIM_MIN, -45.0f); PARAM_DEFINE_FLOAT(FW_P_LIM_MAX, 45.0f); +/** + * Controller roll limit + * + * The maximum roll the controller will output. + * + * @unit degrees + * @min 0.0 + * @group L1 Control + */ PARAM_DEFINE_FLOAT(FW_R_LIM, 45.0f); - -PARAM_DEFINE_FLOAT(FW_THR_MIN, 0.0f); - - +/* + * Throttle limit max + * + * This is the maximum throttle % that can be used by the controller. + * For overpowered aircraft, this should be reduced to a value that + * provides sufficient thrust to climb at the maximum pitch angle PTCH_MAX. + * +*/ PARAM_DEFINE_FLOAT(FW_THR_MAX, 1.0f); +/* + * Throttle limit min + * + * This is the minimum throttle % that can be used by the controller. + * For electric aircraft this will normally be set to zero, but can be set + * to a small non-zero value if a folding prop is fitted to prevent the + * prop from folding and unfolding repeatedly in-flight or to provide + * some aerodynamic drag from a turning prop to improve the descent rate. + * + * For aircraft with internal combustion engine this parameter should be set + * for desired idle rpm. +*/ +PARAM_DEFINE_FLOAT(FW_THR_MIN, 0.0f); + +/* + * Throttle limit value before flare + * + * This throttle value will be set as throttle limit at FW_LND_TLALT, + * before arcraft will flare. +*/ PARAM_DEFINE_FLOAT(FW_THR_LND_MAX, 1.0f); + +/* + * Maximum climb rate + * + * This is the best climb rate that the aircraft can achieve with + * the throttle set to THR_MAX and the airspeed set to the + * default value. For electric aircraft make sure this number can be + * achieved towards the end of flight when the battery voltage has reduced. + * The setting of this parameter can be checked by commanding a positive + * altitude change of 100m in loiter, RTL or guided mode. If the throttle + * required to climb is close to THR_MAX and the aircraft is maintaining + * airspeed, then this parameter is set correctly. If the airspeed starts + * to reduce, then the parameter is set to high, and if the throttle + * demand required to climb and maintain speed is noticeably less than + * FW_THR_MAX, then either FW_T_CLMB_MAX should be increased or + * FW_THR_MAX reduced. +*/ PARAM_DEFINE_FLOAT(FW_T_CLMB_MAX, 5.0f); +/* + * Minimum descent rate + * + * This is the sink rate of the aircraft with the throttle + * set to THR_MIN and flown at the same airspeed as used + * to measure FW_T_CLMB_MAX. +*/ PARAM_DEFINE_FLOAT(FW_T_SINK_MIN, 2.0f); +/* + * Maximum descent rate + * + * This sets the maximum descent rate that the controller will use. + * If this value is too large, the aircraft can over-speed on descent. + * This should be set to a value that can be achieved without + * exceeding the lower pitch angle limit and without over-speeding + * the aircraft. +*/ +PARAM_DEFINE_FLOAT(FW_T_SINK_MAX, 5.0f); + + +/* + * TECS time constant + * + * This is the time constant of the TECS control algorithm (in seconds). + * Smaller values make it faster to respond, larger values make it slower + * to respond. +*/ PARAM_DEFINE_FLOAT(FW_T_TIME_CONST, 5.0f); +/* + * Throttle damping factor + * + * This is the damping gain for the throttle demand loop. + * Increase to add damping to correct for oscillations in speed and height. +*/ PARAM_DEFINE_FLOAT(FW_T_THR_DAMP, 0.5f); +/* + * Integrator gain + * + * This is the integrator gain on the control loop. + * Increasing this gain increases the speed at which speed + * and height offsets are trimmed out, but reduces damping and + * increases overshoot. +*/ PARAM_DEFINE_FLOAT(FW_T_INTEG_GAIN, 0.1f); +/* + * Maximum vertical acceleration + * + * This is the maximum vertical acceleration (in metres/second^2) + * either up or down that the controller will use to correct speed + * or height errors. The default value of 7 m/s/s (equivalent to +- 0.7 g) + * allows for reasonably aggressive pitch changes if required to recover + * from under-speed conditions. +*/ PARAM_DEFINE_FLOAT(FW_T_VERT_ACC, 7.0f); +*/ + * Complementary filter "omega" parameter for height + * + * This is the cross-over frequency (in radians/second) of the complementary + * filter used to fuse vertical acceleration and barometric height to obtain + * an estimate of height rate and height. Increasing this frequency weights + * the solution more towards use of the barometer, whilst reducing it weights + * the solution more towards use of the accelerometer data. + +*/ PARAM_DEFINE_FLOAT(FW_T_HGT_OMEGA, 3.0f); +/* + * Complementary filter "omega" parameter for speed + * + * This is the cross-over frequency (in radians/second) of the complementary + * filter used to fuse longitudinal acceleration and airspeed to obtain an + * improved airspeed estimate. Increasing this frequency weights the solution + * more towards use of the arispeed sensor, whilst reducing it weights the + * solution more towards use of the accelerometer data. +*/ PARAM_DEFINE_FLOAT(FW_T_SPD_OMEGA, 2.0f); +/* + * Roll -> Throttle feedforward + * + * Increasing this gain turn increases the amount of throttle that will + * be used to compensate for the additional drag created by turning. + * Ideally this should be set to approximately 10 x the extra sink rate + * in m/s created by a 45 degree bank turn. Increase this gain if + * the aircraft initially loses energy in turns and reduce if the + * aircraft initially gains energy in turns. Efficient high aspect-ratio + * aircraft (eg powered sailplanes) can use a lower value, whereas + * inefficient low aspect-ratio models (eg delta wings) can use a higher value. +*/ PARAM_DEFINE_FLOAT(FW_T_RLL2THR, 10.0f); +/* + * Speed <--> Altitude priority + * + * This parameter adjusts the amount of weighting that the pitch control + * applies to speed vs height errors. Setting it to 0.0 will cause the + * pitch control to control height and ignore speed errors. This will + * normally improve height accuracy but give larger airspeed errors. + * Setting it to 2.0 will cause the pitch control loop to control speed + * and ignore height errors. This will normally reduce airspeed errors, + * but give larger height errors. The default value of 1.0 allows the pitch + * control to simultaneously control height and speed. + * Note to Glider Pilots set this parameter to 2.0 (The glider will + * adjust its pitch angle to maintain airspeed, ignoring changes in height). +*/ PARAM_DEFINE_FLOAT(FW_T_SPDWEIGHT, 1.0f); +/* + * Pitch damping factor + * + * This is the damping gain for the pitch demand loop. Increase to add + * damping to correct for oscillations in height. The default value of 0.0 + * will work well provided the pitch to servo controller has been tuned + * properly. +*/ PARAM_DEFINE_FLOAT(FW_T_PTCH_DAMP, 0.0f); - -PARAM_DEFINE_FLOAT(FW_T_SINK_MAX, 5.0f); - +/* + * Height rate P factor +*/ PARAM_DEFINE_FLOAT(FW_T_HRATE_P, 0.05f); + +/* + * Speed rate P factor +*/ PARAM_DEFINE_FLOAT(FW_T_SRATE_P, 0.05f); +/* + * Landing slope angle +*/ PARAM_DEFINE_FLOAT(FW_LND_ANG, 5.0f); + +/* + * Landing slope length +*/ PARAM_DEFINE_FLOAT(FW_LND_SLLR, 0.9f); + +/* + * +*/ PARAM_DEFINE_FLOAT(FW_LND_HVIRT, 10.0f); + +/* + * Landing flare altitude (relative) +*/ PARAM_DEFINE_FLOAT(FW_LND_FLALT, 15.0f); + +/* + * Landing throttle limit altitude (relative) +*/ PARAM_DEFINE_FLOAT(FW_LND_TLALT, 5.0f); + +/* + * Landing heading hold horizontal distance +*/ PARAM_DEFINE_FLOAT(FW_LND_HHDIST, 15.0f); From 480ba491f35019bfe781b1616bc8db71e71f2740 Mon Sep 17 00:00:00 2001 From: Highlander-UA <315u448@ukrpost.net> Date: Sat, 15 Feb 2014 17:27:38 +0200 Subject: [PATCH 437/447] Missing descriptions added for L1 control parameters --- src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 9f15a93866..e0b8d8771a 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 @@ -247,7 +247,7 @@ PARAM_DEFINE_FLOAT(FW_T_INTEG_GAIN, 0.1f); PARAM_DEFINE_FLOAT(FW_T_VERT_ACC, 7.0f); -*/ +/* * Complementary filter "omega" parameter for height * * This is the cross-over frequency (in radians/second) of the complementary From 28536682aa664b5dae74b96688cc66b919fccffe Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 15 Feb 2014 21:00:48 +0100 Subject: [PATCH 438/447] Fix airspeed sensor --- src/drivers/airspeed/airspeed.cpp | 4 ++-- src/drivers/airspeed/airspeed.h | 2 +- src/drivers/ets_airspeed/ets_airspeed.cpp | 7 ++++--- src/drivers/meas_airspeed/meas_airspeed.cpp | 10 ++++++---- 4 files changed, 13 insertions(+), 10 deletions(-) diff --git a/src/drivers/airspeed/airspeed.cpp b/src/drivers/airspeed/airspeed.cpp index 771f2128f2..524151c90d 100644 --- a/src/drivers/airspeed/airspeed.cpp +++ b/src/drivers/airspeed/airspeed.cpp @@ -76,8 +76,8 @@ #include -Airspeed::Airspeed(int bus, int address, unsigned conversion_interval) : - I2C("Airspeed", AIRSPEED_DEVICE_PATH, bus, address, 100000), +Airspeed::Airspeed(int bus, int address, unsigned conversion_interval, const char* path) : + I2C("Airspeed", path, bus, address, 100000), _reports(nullptr), _buffer_overflows(perf_alloc(PC_COUNT, "airspeed_buffer_overflows")), _max_differential_pressure_pa(0), diff --git a/src/drivers/airspeed/airspeed.h b/src/drivers/airspeed/airspeed.h index c27b1bcd80..186602eda8 100644 --- a/src/drivers/airspeed/airspeed.h +++ b/src/drivers/airspeed/airspeed.h @@ -90,7 +90,7 @@ static const int ERROR = -1; class __EXPORT Airspeed : public device::I2C { public: - Airspeed(int bus, int address, unsigned conversion_interval); + Airspeed(int bus, int address, unsigned conversion_interval, const char* path); virtual ~Airspeed(); virtual int init(); diff --git a/src/drivers/ets_airspeed/ets_airspeed.cpp b/src/drivers/ets_airspeed/ets_airspeed.cpp index 8bbef5cfa0..b6e80ce1d0 100644 --- a/src/drivers/ets_airspeed/ets_airspeed.cpp +++ b/src/drivers/ets_airspeed/ets_airspeed.cpp @@ -77,6 +77,7 @@ /* I2C bus address */ #define I2C_ADDRESS 0x75 /* 7-bit address. 8-bit address is 0xEA */ +#define ETS_PATH "/dev/ets_airspeed" /* Register address */ #define READ_CMD 0x07 /* Read the data */ @@ -93,7 +94,7 @@ class ETSAirspeed : public Airspeed { public: - ETSAirspeed(int bus, int address = I2C_ADDRESS); + ETSAirspeed(int bus, int address = I2C_ADDRESS, const char* path = ETS_PATH); protected: @@ -112,8 +113,8 @@ protected: */ extern "C" __EXPORT int ets_airspeed_main(int argc, char *argv[]); -ETSAirspeed::ETSAirspeed(int bus, int address) : Airspeed(bus, address, - CONVERSION_INTERVAL) +ETSAirspeed::ETSAirspeed(int bus, int address, const char* path) : Airspeed(bus, address, + CONVERSION_INTERVAL, path) { } diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp index 05ae21c1f8..c5394d9b49 100644 --- a/src/drivers/meas_airspeed/meas_airspeed.cpp +++ b/src/drivers/meas_airspeed/meas_airspeed.cpp @@ -89,8 +89,10 @@ /* I2C bus address is 1010001x */ #define I2C_ADDRESS_MS4525DO 0x28 //0x51 /* 7-bit address. */ +#define PATH_MS4525 "/dev/ms4525" /* The MS5525DSO address is 111011Cx, where C is the complementary value of the pin CSB */ #define I2C_ADDRESS_MS5525DSO 0x77 //0x77/* 7-bit address, addr. pin pulled low */ +#define PATH_MS5525 "/dev/ms5525" /* Register address */ #define ADDR_READ_MR 0x00 /* write to this address to start conversion */ @@ -120,8 +122,8 @@ protected: */ extern "C" __EXPORT int meas_airspeed_main(int argc, char *argv[]); -MEASAirspeed::MEASAirspeed(int bus, int address) : Airspeed(bus, address, - CONVERSION_INTERVAL) +MEASAirspeed::MEASAirspeed(int bus, int address, const char* path) : Airspeed(bus, address, + CONVERSION_INTERVAL, path) { } @@ -304,7 +306,7 @@ start(int i2c_bus) errx(1, "already started"); /* create the driver, try the MS4525DO first */ - g_dev = new MEASAirspeed(i2c_bus, I2C_ADDRESS_MS4525DO); + g_dev = new MEASAirspeed(i2c_bus, I2C_ADDRESS_MS4525DO, PATH_MS4525); /* check if the MS4525DO was instantiated */ if (g_dev == nullptr) @@ -313,7 +315,7 @@ start(int i2c_bus) /* try the MS5525DSO next if init fails */ if (OK != g_dev->Airspeed::init()) { delete g_dev; - g_dev = new MEASAirspeed(i2c_bus, I2C_ADDRESS_MS5525DSO); + g_dev = new MEASAirspeed(i2c_bus, I2C_ADDRESS_MS5525DSO, PATH_MS4425); /* check if the MS5525DSO was instantiated */ if (g_dev == nullptr) From 4bd83dcaeb48cb6629d205b28532f49ca9440e1c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 15 Feb 2014 21:20:02 +0100 Subject: [PATCH 439/447] Fix compile errors --- src/drivers/meas_airspeed/meas_airspeed.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp index c5394d9b49..eb65c049be 100644 --- a/src/drivers/meas_airspeed/meas_airspeed.cpp +++ b/src/drivers/meas_airspeed/meas_airspeed.cpp @@ -103,7 +103,7 @@ class MEASAirspeed : public Airspeed { public: - MEASAirspeed(int bus, int address = I2C_ADDRESS_MS4525DO); + MEASAirspeed(int bus, int address = I2C_ADDRESS_MS4525DO, const char* path = PATH_MS4525); protected: @@ -315,7 +315,7 @@ start(int i2c_bus) /* try the MS5525DSO next if init fails */ if (OK != g_dev->Airspeed::init()) { delete g_dev; - g_dev = new MEASAirspeed(i2c_bus, I2C_ADDRESS_MS5525DSO, PATH_MS4425); + g_dev = new MEASAirspeed(i2c_bus, I2C_ADDRESS_MS5525DSO, PATH_MS5525); /* check if the MS5525DSO was instantiated */ if (g_dev == nullptr) From 2e4c26c9577d11a3ff0e838ffa2c25c4f4ec1452 Mon Sep 17 00:00:00 2001 From: Stefan Rado Date: Sat, 15 Feb 2014 21:12:15 +0100 Subject: [PATCH 440/447] Rename parser.py to srcparser.py to prevent naming conflicts with built-in Python parser library. --- Tools/px4params/px_process_params.py | 4 ++-- Tools/px4params/{parser.py => srcparser.py} | 0 2 files changed, 2 insertions(+), 2 deletions(-) rename Tools/px4params/{parser.py => srcparser.py} (100%) diff --git a/Tools/px4params/px_process_params.py b/Tools/px4params/px_process_params.py index cdf5aba7c0..4f498026fa 100755 --- a/Tools/px4params/px_process_params.py +++ b/Tools/px4params/px_process_params.py @@ -40,12 +40,12 @@ # import scanner -import parser +import srcparser import xmlout import dokuwikiout # Initialize parser -prs = parser.Parser() +prs = srcparser.Parser() # Scan directories, and parse the files sc = scanner.Scanner() diff --git a/Tools/px4params/parser.py b/Tools/px4params/srcparser.py similarity index 100% rename from Tools/px4params/parser.py rename to Tools/px4params/srcparser.py From 1a7518a4224082cf707585a93eb6d934127e5d9e Mon Sep 17 00:00:00 2001 From: Stefan Rado Date: Sat, 15 Feb 2014 21:18:26 +0100 Subject: [PATCH 441/447] Fixes for Python 3: Use sorted() with key parameter instead of deprecated (and removed) cmp() function. --- Tools/px4params/srcparser.py | 20 ++++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/Tools/px4params/srcparser.py b/Tools/px4params/srcparser.py index 251be672f2..1b2d301108 100644 --- a/Tools/px4params/srcparser.py +++ b/Tools/px4params/srcparser.py @@ -28,8 +28,7 @@ class ParameterGroup(object): state of the parser. """ return sorted(self.params, - cmp=lambda x, y: cmp(x.GetFieldValue("code"), - y.GetFieldValue("code"))) + key=lambda x: x.GetFieldValue("code")) class Parameter(object): """ @@ -61,9 +60,10 @@ class Parameter(object): """ Return list of existing field codes in convenient order """ - return sorted(self.fields.keys(), - cmp=lambda x, y: cmp(self.priority.get(y, 0), - self.priority.get(x, 0)) or cmp(x, y)) + keys = self.fields.keys() + keys = sorted(keys) + keys = sorted(keys, key=lambda x: self.priority.get(x, 0), reverse=True) + return keys def GetFieldValue(self, code): """ @@ -197,7 +197,7 @@ class Parser(object): if tag == "group": group = tags[tag] elif tag not in self.valid_tags: - sys.stderr.write("Skipping invalid" + sys.stderr.write("Skipping invalid " "documentation tag: '%s'\n" % tag) else: param.SetField(tag, tags[tag]) @@ -214,7 +214,7 @@ class Parser(object): object. Note that returned object is not a copy. Modifications affect state of the parser. """ - return sorted(self.param_groups.values(), - cmp=lambda x, y: cmp(self.priority.get(y.GetName(), 0), - self.priority.get(x.GetName(), 0)) or cmp(x.GetName(), - y.GetName())) + groups = self.param_groups.values() + groups = sorted(groups, key=lambda x: x.GetName()) + groups = sorted(groups, key=lambda x: self.priority.get(x.GetName(), 0), reverse=True) + return groups From d30335cb3f141fe0abf7ea853d5e5b097452a0a3 Mon Sep 17 00:00:00 2001 From: Stefan Rado Date: Sat, 15 Feb 2014 21:24:53 +0100 Subject: [PATCH 442/447] Fixes for Python 3 and refactoring: Merge generic Output class into specialized output classes as some need to write files in binary mode. --- Tools/px4params/.gitignore | 1 + Tools/px4params/output.py | 5 -- ...istings.py => output_dokuwiki_listings.py} | 11 ++-- ...kuwikiout.py => output_dokuwiki_tables.py} | 63 ++++++++++--------- Tools/px4params/{xmlout.py => output_xml.py} | 11 ++-- Tools/px4params/px_process_params.py | 22 ++++--- Tools/px4params/xmlrpc.sh | 2 +- 7 files changed, 65 insertions(+), 50 deletions(-) delete mode 100644 Tools/px4params/output.py rename Tools/px4params/{dokuwikiout_listings.py => output_dokuwiki_listings.py} (84%) rename Tools/px4params/{dokuwikiout.py => output_dokuwiki_tables.py} (62%) rename Tools/px4params/{xmlout.py => output_xml.py} (78%) diff --git a/Tools/px4params/.gitignore b/Tools/px4params/.gitignore index 5d0378b4a2..d78b71a6e8 100644 --- a/Tools/px4params/.gitignore +++ b/Tools/px4params/.gitignore @@ -1,3 +1,4 @@ parameters.wiki parameters.xml +parameters.wikirpc.xml cookies.txt \ No newline at end of file diff --git a/Tools/px4params/output.py b/Tools/px4params/output.py deleted file mode 100644 index c092468713..0000000000 --- a/Tools/px4params/output.py +++ /dev/null @@ -1,5 +0,0 @@ -class Output(object): - def Save(self, groups, fn): - data = self.Generate(groups) - with open(fn, 'w') as f: - f.write(data) diff --git a/Tools/px4params/dokuwikiout_listings.py b/Tools/px4params/output_dokuwiki_listings.py similarity index 84% rename from Tools/px4params/dokuwikiout_listings.py rename to Tools/px4params/output_dokuwiki_listings.py index 33f76b415e..117f4edf43 100644 --- a/Tools/px4params/dokuwikiout_listings.py +++ b/Tools/px4params/output_dokuwiki_listings.py @@ -1,7 +1,6 @@ -import output -class DokuWikiOutput(output.Output): - def Generate(self, groups): +class DokuWikiListingsOutput(): + def __init__(self, groups): result = "" for group in groups: result += "==== %s ====\n\n" % group.GetName() @@ -24,4 +23,8 @@ class DokuWikiOutput(output.Output): if def_val is not None: result += "* Default value: %s\n" % def_val result += "\n" - return result + self.output = result + + def Save(self, filename): + with open(filename, 'w') as f: + f.write(self.output) diff --git a/Tools/px4params/dokuwikiout.py b/Tools/px4params/output_dokuwiki_tables.py similarity index 62% rename from Tools/px4params/dokuwikiout.py rename to Tools/px4params/output_dokuwiki_tables.py index e02035423d..dca3fd92d7 100644 --- a/Tools/px4params/dokuwikiout.py +++ b/Tools/px4params/output_dokuwiki_tables.py @@ -1,21 +1,8 @@ -import output from xml.sax.saxutils import escape -class DokuWikiOutput(output.Output): - def Generate(self, groups): - pre_text = """ - - wiki.putPage - - - - :firmware:parameters - - - - - """ - result = "====== Parameter Reference ======\nThis list is auto-generated every few minutes and contains the most recent parameter names and default values." +class DokuWikiTablesOutput(): + def __init__(self, groups): + result = "====== Parameter Reference ======\nThis list is auto-generated every few minutes and contains the most recent parameter names and default values.\n\n" for group in groups: result += "==== %s ====\n\n" % group.GetName() result += "|< 100% 20% 20% 10% 10% 10% 30%>|\n" @@ -54,15 +41,35 @@ class DokuWikiOutput(output.Output): result += "\n" result += "\n" - post_text = """ - - - - - sum - Updated parameters automagically from code. - - - - """ - return pre_text + escape(result) + post_text + self.output = result; + + def Save(self, filename): + with open(filename, 'w') as f: + f.write(self.output) + + def SaveRpc(self, filename): + with open(filename, 'w') as f: + f.write(""" + + wiki.putPage + + + + :firmware:parameters + + + + + """) + f.write(escape(self.output)) + f.write(""" + + + + + sum + Updated parameters automagically from code. + + + +""") diff --git a/Tools/px4params/xmlout.py b/Tools/px4params/output_xml.py similarity index 78% rename from Tools/px4params/xmlout.py rename to Tools/px4params/output_xml.py index d56802b158..5576954c09 100644 --- a/Tools/px4params/xmlout.py +++ b/Tools/px4params/output_xml.py @@ -1,8 +1,7 @@ -import output from xml.dom.minidom import getDOMImplementation -class XMLOutput(output.Output): - def Generate(self, groups): +class XMLOutput(): + def __init__(self, groups): impl = getDOMImplementation() xml_document = impl.createDocument(None, "parameters", None) xml_parameters = xml_document.documentElement @@ -19,4 +18,8 @@ class XMLOutput(output.Output): xml_param.appendChild(xml_field) xml_value = xml_document.createTextNode(value) xml_field.appendChild(xml_value) - return xml_document.toprettyxml(indent=" ", newl="\n", encoding="utf-8") + self.output = xml_document.toprettyxml(indent=" ", newl="\n", encoding="utf-8") + + def Save(self, filename): + with open(filename, 'wb') as f: + f.write(self.output) diff --git a/Tools/px4params/px_process_params.py b/Tools/px4params/px_process_params.py index 4f498026fa..7799f63484 100755 --- a/Tools/px4params/px_process_params.py +++ b/Tools/px4params/px_process_params.py @@ -41,8 +41,9 @@ import scanner import srcparser -import xmlout -import dokuwikiout +import output_xml +import output_dokuwiki_tables +import output_dokuwiki_listings # Initialize parser prs = srcparser.Parser() @@ -50,12 +51,17 @@ prs = srcparser.Parser() # Scan directories, and parse the files sc = scanner.Scanner() sc.ScanDir("../../src", prs) -output = prs.GetParamGroups() +groups = prs.GetParamGroups() # Output into XML -out = xmlout.XMLOutput() -out.Save(output, "parameters.xml") +out = output_xml.XMLOutput(groups) +out.Save("parameters.xml") -# Output into DokuWiki -out = dokuwikiout.DokuWikiOutput() -out.Save(output, "parameters.wiki") +# Output to DokuWiki listings +#out = output_dokuwiki_listings.DokuWikiListingsOutput(groups) +#out.Save("parameters.wiki") + +# Output to DokuWiki tables +out = output_dokuwiki_tables.DokuWikiTablesOutput(groups) +out.Save("parameters.wiki") +out.SaveRpc("parameters.wikirpc.xml") diff --git a/Tools/px4params/xmlrpc.sh b/Tools/px4params/xmlrpc.sh index 36c52ff714..efd177f46a 100644 --- a/Tools/px4params/xmlrpc.sh +++ b/Tools/px4params/xmlrpc.sh @@ -2,4 +2,4 @@ python px_process_params.py rm cookies.txt curl --cookie cookies.txt --cookie-jar cookies.txt --user-agent Mozilla/4.0 --data "u=$XMLRPCUSER&p=$XMLRPCPASS" https://pixhawk.org/start?do=login -curl -k --cookie cookies.txt -H "Content-Type: application/xml" -X POST --data-binary @parameters.wiki "https://pixhawk.org/lib/exe/xmlrpc.php" +curl -k --cookie cookies.txt -H "Content-Type: application/xml" -X POST --data-binary @parameters.wikirpc.xml "https://pixhawk.org/lib/exe/xmlrpc.php" From d811853d4463279556f596c7fb064ba016a83acd Mon Sep 17 00:00:00 2001 From: Stefan Rado Date: Sat, 15 Feb 2014 21:47:13 +0100 Subject: [PATCH 443/447] Fixed Doxygen comments and added parameter documentation group. --- .../fw_pos_control_l1_params.c | 153 +++++++++++------- 1 file changed, 91 insertions(+), 62 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 e0b8d8771a..416a6fcfce 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 @@ -40,12 +40,10 @@ */ #include - #include /* * Controller parameters, accessible via MAVLink - * */ /** @@ -119,7 +117,6 @@ PARAM_DEFINE_FLOAT(FW_P_LIM_MIN, -45.0f); */ PARAM_DEFINE_FLOAT(FW_P_LIM_MAX, 45.0f); - /** * Controller roll limit * @@ -131,17 +128,18 @@ PARAM_DEFINE_FLOAT(FW_P_LIM_MAX, 45.0f); */ PARAM_DEFINE_FLOAT(FW_R_LIM, 45.0f); -/* +/** * Throttle limit max * * This is the maximum throttle % that can be used by the controller. * For overpowered aircraft, this should be reduced to a value that * provides sufficient thrust to climb at the maximum pitch angle PTCH_MAX. * -*/ + * @group L1 Control + */ PARAM_DEFINE_FLOAT(FW_THR_MAX, 1.0f); -/* +/** * Throttle limit min * * This is the minimum throttle % that can be used by the controller. @@ -152,19 +150,22 @@ PARAM_DEFINE_FLOAT(FW_THR_MAX, 1.0f); * * For aircraft with internal combustion engine this parameter should be set * for desired idle rpm. -*/ + * + * @group L1 Control + */ PARAM_DEFINE_FLOAT(FW_THR_MIN, 0.0f); -/* +/** * Throttle limit value before flare * * This throttle value will be set as throttle limit at FW_LND_TLALT, * before arcraft will flare. -*/ + * + * @group L1 Control + */ PARAM_DEFINE_FLOAT(FW_THR_LND_MAX, 1.0f); - -/* +/** * Maximum climb rate * * This is the best climb rate that the aircraft can achieve with @@ -179,21 +180,23 @@ PARAM_DEFINE_FLOAT(FW_THR_LND_MAX, 1.0f); * demand required to climb and maintain speed is noticeably less than * FW_THR_MAX, then either FW_T_CLMB_MAX should be increased or * FW_THR_MAX reduced. -*/ + * + * @group L1 Control + */ PARAM_DEFINE_FLOAT(FW_T_CLMB_MAX, 5.0f); - -/* +/** * Minimum descent rate * * This is the sink rate of the aircraft with the throttle * set to THR_MIN and flown at the same airspeed as used * to measure FW_T_CLMB_MAX. -*/ + * + * @group L1 Control + */ PARAM_DEFINE_FLOAT(FW_T_SINK_MIN, 2.0f); - -/* +/** * Maximum descent rate * * This sets the maximum descent rate that the controller will use. @@ -201,41 +204,45 @@ PARAM_DEFINE_FLOAT(FW_T_SINK_MIN, 2.0f); * This should be set to a value that can be achieved without * exceeding the lower pitch angle limit and without over-speeding * the aircraft. -*/ + * + * @group L1 Control + */ PARAM_DEFINE_FLOAT(FW_T_SINK_MAX, 5.0f); - -/* +/** * TECS time constant * * This is the time constant of the TECS control algorithm (in seconds). * Smaller values make it faster to respond, larger values make it slower * to respond. -*/ + * + * @group L1 Control + */ PARAM_DEFINE_FLOAT(FW_T_TIME_CONST, 5.0f); - -/* +/** * Throttle damping factor * * This is the damping gain for the throttle demand loop. * Increase to add damping to correct for oscillations in speed and height. -*/ + * + * @group L1 Control + */ PARAM_DEFINE_FLOAT(FW_T_THR_DAMP, 0.5f); - -/* +/** * Integrator gain * * This is the integrator gain on the control loop. * Increasing this gain increases the speed at which speed * and height offsets are trimmed out, but reduces damping and * increases overshoot. -*/ + * + * @group L1 Control + */ PARAM_DEFINE_FLOAT(FW_T_INTEG_GAIN, 0.1f); - -/* +/** * Maximum vertical acceleration * * This is the maximum vertical acceleration (in metres/second^2) @@ -243,11 +250,12 @@ PARAM_DEFINE_FLOAT(FW_T_INTEG_GAIN, 0.1f); * or height errors. The default value of 7 m/s/s (equivalent to +- 0.7 g) * allows for reasonably aggressive pitch changes if required to recover * from under-speed conditions. -*/ + * + * @group L1 Control + */ PARAM_DEFINE_FLOAT(FW_T_VERT_ACC, 7.0f); - -/* +/** * Complementary filter "omega" parameter for height * * This is the cross-over frequency (in radians/second) of the complementary @@ -255,12 +263,12 @@ PARAM_DEFINE_FLOAT(FW_T_VERT_ACC, 7.0f); * an estimate of height rate and height. Increasing this frequency weights * the solution more towards use of the barometer, whilst reducing it weights * the solution more towards use of the accelerometer data. - -*/ + * + * @group L1 Control + */ PARAM_DEFINE_FLOAT(FW_T_HGT_OMEGA, 3.0f); - -/* +/** * Complementary filter "omega" parameter for speed * * This is the cross-over frequency (in radians/second) of the complementary @@ -268,11 +276,12 @@ PARAM_DEFINE_FLOAT(FW_T_HGT_OMEGA, 3.0f); * improved airspeed estimate. Increasing this frequency weights the solution * more towards use of the arispeed sensor, whilst reducing it weights the * solution more towards use of the accelerometer data. -*/ + * + * @group L1 Control + */ PARAM_DEFINE_FLOAT(FW_T_SPD_OMEGA, 2.0f); - -/* +/** * Roll -> Throttle feedforward * * Increasing this gain turn increases the amount of throttle that will @@ -283,11 +292,12 @@ PARAM_DEFINE_FLOAT(FW_T_SPD_OMEGA, 2.0f); * aircraft initially gains energy in turns. Efficient high aspect-ratio * aircraft (eg powered sailplanes) can use a lower value, whereas * inefficient low aspect-ratio models (eg delta wings) can use a higher value. -*/ + * + * @group L1 Control + */ PARAM_DEFINE_FLOAT(FW_T_RLL2THR, 10.0f); - -/* +/** * Speed <--> Altitude priority * * This parameter adjusts the amount of weighting that the pitch control @@ -300,56 +310,75 @@ PARAM_DEFINE_FLOAT(FW_T_RLL2THR, 10.0f); * control to simultaneously control height and speed. * Note to Glider Pilots set this parameter to 2.0 (The glider will * adjust its pitch angle to maintain airspeed, ignoring changes in height). -*/ + * + * @group L1 Control + */ PARAM_DEFINE_FLOAT(FW_T_SPDWEIGHT, 1.0f); - -/* +/** * Pitch damping factor * * This is the damping gain for the pitch demand loop. Increase to add * damping to correct for oscillations in height. The default value of 0.0 * will work well provided the pitch to servo controller has been tuned * properly. -*/ + * + * @group L1 Control + */ PARAM_DEFINE_FLOAT(FW_T_PTCH_DAMP, 0.0f); -/* +/** * Height rate P factor -*/ + * + * @group L1 Control + */ PARAM_DEFINE_FLOAT(FW_T_HRATE_P, 0.05f); -/* +/** * Speed rate P factor -*/ + * + * @group L1 Control + */ PARAM_DEFINE_FLOAT(FW_T_SRATE_P, 0.05f); -/* +/** * Landing slope angle -*/ + * + * @group L1 Control + */ PARAM_DEFINE_FLOAT(FW_LND_ANG, 5.0f); -/* +/** * Landing slope length -*/ + * + * @group L1 Control + */ PARAM_DEFINE_FLOAT(FW_LND_SLLR, 0.9f); -/* +/** * -*/ + * + * @group L1 Control + */ PARAM_DEFINE_FLOAT(FW_LND_HVIRT, 10.0f); -/* +/** * Landing flare altitude (relative) -*/ + * + * @group L1 Control + */ PARAM_DEFINE_FLOAT(FW_LND_FLALT, 15.0f); -/* +/** * Landing throttle limit altitude (relative) -*/ + * + * @group L1 Control + */ PARAM_DEFINE_FLOAT(FW_LND_TLALT, 5.0f); -/* +/** * Landing heading hold horizontal distance -*/ + * + * @group L1 Control + */ PARAM_DEFINE_FLOAT(FW_LND_HHDIST, 15.0f); From 387bfad1cacc2f9929e7590130ca93f27c688655 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 15 Feb 2014 22:19:14 +0100 Subject: [PATCH 444/447] Fixed status printing for airspeed sensor --- src/drivers/meas_airspeed/meas_airspeed.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp index 05ae21c1f8..6fe6c22212 100644 --- a/src/drivers/meas_airspeed/meas_airspeed.cpp +++ b/src/drivers/meas_airspeed/meas_airspeed.cpp @@ -50,6 +50,7 @@ * - Interfacing to MEAS Digital Pressure Modules (http://www.meas-spec.com/downloads/Interfacing_to_MEAS_Digital_Pressure_Modules.pdf) */ + #include #include @@ -386,7 +387,7 @@ test() err(1, "immediate read failed"); warnx("single read"); - warnx("diff pressure: %d pa", (double)report.differential_pressure_pa); + warnx("diff pressure: %8.4f pa", (double)report.differential_pressure_pa); /* start the sensor polling at 2Hz */ if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) @@ -411,7 +412,7 @@ test() err(1, "periodic read failed"); warnx("periodic read %u", i); - warnx("diff pressure: %d pa", report.differential_pressure_pa); + warnx("diff pressure: %8.4f pa", (double)report.differential_pressure_pa); warnx("temperature: %d C (0x%02x)", (int)report.temperature, (unsigned) report.temperature); } From 1c13225b194583945324d247b83a5236e665d919 Mon Sep 17 00:00:00 2001 From: Stefan Rado Date: Sun, 16 Feb 2014 01:45:10 +0100 Subject: [PATCH 445/447] Fixed illegal character 0x96. --- src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 416a6fcfce..62a340e905 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 @@ -308,7 +308,7 @@ PARAM_DEFINE_FLOAT(FW_T_RLL2THR, 10.0f); * and ignore height errors. This will normally reduce airspeed errors, * but give larger height errors. The default value of 1.0 allows the pitch * control to simultaneously control height and speed. - * Note to Glider Pilots set this parameter to 2.0 (The glider will + * Note to Glider Pilots - set this parameter to 2.0 (The glider will * adjust its pitch angle to maintain airspeed, ignoring changes in height). * * @group L1 Control From 49cc0bd162b63b188c341aab4be880776d21c388 Mon Sep 17 00:00:00 2001 From: Stefan Rado Date: Sun, 16 Feb 2014 01:47:11 +0100 Subject: [PATCH 446/447] Explicitly treat all files as UTF-8. --- Tools/px4params/output_dokuwiki_listings.py | 3 ++- Tools/px4params/output_dokuwiki_tables.py | 5 +++-- Tools/px4params/output_xml.py | 7 ++++--- Tools/px4params/scanner.py | 3 ++- 4 files changed, 11 insertions(+), 7 deletions(-) diff --git a/Tools/px4params/output_dokuwiki_listings.py b/Tools/px4params/output_dokuwiki_listings.py index 117f4edf43..83c50ae15d 100644 --- a/Tools/px4params/output_dokuwiki_listings.py +++ b/Tools/px4params/output_dokuwiki_listings.py @@ -1,3 +1,4 @@ +import codecs class DokuWikiListingsOutput(): def __init__(self, groups): @@ -26,5 +27,5 @@ class DokuWikiListingsOutput(): self.output = result def Save(self, filename): - with open(filename, 'w') as f: + with codecs.open(filename, 'w', 'utf-8') as f: f.write(self.output) diff --git a/Tools/px4params/output_dokuwiki_tables.py b/Tools/px4params/output_dokuwiki_tables.py index dca3fd92d7..aa04304df7 100644 --- a/Tools/px4params/output_dokuwiki_tables.py +++ b/Tools/px4params/output_dokuwiki_tables.py @@ -1,4 +1,5 @@ from xml.sax.saxutils import escape +import codecs class DokuWikiTablesOutput(): def __init__(self, groups): @@ -44,11 +45,11 @@ class DokuWikiTablesOutput(): self.output = result; def Save(self, filename): - with open(filename, 'w') as f: + with codecs.open(filename, 'w', 'utf-8') as f: f.write(self.output) def SaveRpc(self, filename): - with open(filename, 'w') as f: + with codecs.open(filename, 'w', 'utf-8') as f: f.write(""" wiki.putPage diff --git a/Tools/px4params/output_xml.py b/Tools/px4params/output_xml.py index 5576954c09..e845cd1b10 100644 --- a/Tools/px4params/output_xml.py +++ b/Tools/px4params/output_xml.py @@ -1,4 +1,5 @@ from xml.dom.minidom import getDOMImplementation +import codecs class XMLOutput(): def __init__(self, groups): @@ -18,8 +19,8 @@ class XMLOutput(): xml_param.appendChild(xml_field) xml_value = xml_document.createTextNode(value) xml_field.appendChild(xml_value) - self.output = xml_document.toprettyxml(indent=" ", newl="\n", encoding="utf-8") + self.xml_document = xml_document def Save(self, filename): - with open(filename, 'wb') as f: - f.write(self.output) + with codecs.open(filename, 'w', 'utf-8') as f: + self.xml_document.writexml(f, indent=" ", addindent=" ", newl="\n") diff --git a/Tools/px4params/scanner.py b/Tools/px4params/scanner.py index b5a1af47c3..8779b7bbf6 100644 --- a/Tools/px4params/scanner.py +++ b/Tools/px4params/scanner.py @@ -1,5 +1,6 @@ import os import re +import codecs class Scanner(object): """ @@ -29,6 +30,6 @@ class Scanner(object): Scans provided file and passes its contents to the parser using parser.Parse method. """ - with open(path, 'r') as f: + with codecs.open(path, 'r', 'utf-8') as f: contents = f.read() parser.Parse(contents) From 1be3ea4f4eac121a627c194fefbf202586f6f66f Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sun, 16 Feb 2014 12:18:22 +0100 Subject: [PATCH 447/447] MPU6000: gyro topic was not initialized --- src/drivers/mpu6000/mpu6000.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index bf80c9cff2..ac75682c47 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -1353,6 +1353,7 @@ MPU6000::print_info() MPU6000_gyro::MPU6000_gyro(MPU6000 *parent) : CDev("MPU6000_gyro", MPU_DEVICE_PATH_GYRO), _parent(parent), + _gyro_topic(-1), _gyro_class_instance(-1) { }