From 23237df84ef1fcf955c739c721a15ca356fddf96 Mon Sep 17 00:00:00 2001 From: Juchli D Date: Tue, 5 Nov 2013 13:24:41 +0100 Subject: [PATCH 001/411] fmu: Also take into account actuator group 1 --- src/drivers/px4fmu/fmu.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 0441566e98..a2c62cf067 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -542,7 +542,8 @@ PX4FMU::task_main() if (fds[0].revents & POLLIN) { /* get controls - must always do this to avoid spinning */ - orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, _t_actuators, &_controls); + orb_copy(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS : + ORB_ID(actuator_controls_1), _t_actuators, &_controls); /* can we mix? */ if (_mixers != nullptr) { From 56f1ea9266eacd689a7b4065231d94a78ea96de6 Mon Sep 17 00:00:00 2001 From: Juchli D Date: Tue, 5 Nov 2013 13:25:49 +0100 Subject: [PATCH 002/411] Added Bottle Drop app --- makefiles/config_px4fmu-v2_default.mk | 5 + src/modules/bottle_drop/bottle_drop.c | 188 ++++++++++++++++++++++++++ src/modules/bottle_drop/module.mk | 40 ++++++ 3 files changed, 233 insertions(+) create mode 100644 src/modules/bottle_drop/bottle_drop.c create mode 100644 src/modules/bottle_drop/module.mk diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index 761fb8d9d6..a2fb63422e 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -114,6 +114,11 @@ MODULES += lib/external_lgpl MODULES += lib/geo MODULES += lib/conversion +# +# OBC challenge +# +MODULES += modules/bottle_drop + # # Demo apps # diff --git a/src/modules/bottle_drop/bottle_drop.c b/src/modules/bottle_drop/bottle_drop.c new file mode 100644 index 0000000000..69720b4393 --- /dev/null +++ b/src/modules/bottle_drop/bottle_drop.c @@ -0,0 +1,188 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +/** + * @file px4_daemon_app.c + * daemon application example for PX4 autopilot + * + * @author Example User + */ + +#include +#include +#include +#include + +#include +#include +#include +#include + +#include + +static bool thread_should_exit = false; /**< daemon exit flag */ +static bool thread_running = false; /**< daemon status flag */ +static int daemon_task; /**< Handle of daemon task / thread */ + +static bool drop = false; + +/** + * daemon management function. + */ +__EXPORT int bottle_drop_main(int argc, char *argv[]); + +/** + * Mainloop of daemon. + */ +int bottle_drop_thread_main(int argc, char *argv[]); + +/** + * Print the correct usage. + */ +static void usage(const char *reason); + +static void +usage(const char *reason) +{ + if (reason) + warnx("%s\n", reason); + errx(1, "usage: daemon {start|stop|status} [-p ]\n\n"); +} + +/** + * The daemon 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_create(). + */ +int bottle_drop_main(int argc, char *argv[]) +{ + if (argc < 1) + usage("missing command"); + + if (!strcmp(argv[1], "start")) { + + if (thread_running) { + warnx("daemon already running\n"); + /* this is not an error */ + exit(0); + } + + thread_should_exit = false; + daemon_task = task_spawn_cmd("daemon", + SCHED_DEFAULT, + SCHED_PRIORITY_DEFAULT, + 4096, + bottle_drop_thread_main, + (argv) ? (const char **)&argv[2] : (const char **)NULL); + exit(0); + } + + if (!strcmp(argv[1], "drop")) { + drop = true; + exit(0); + } + + if (!strcmp(argv[1], "stop")) { + thread_should_exit = true; + exit(0); + } + + if (!strcmp(argv[1], "status")) { + if (thread_running) { + warnx("\trunning\n"); + } else { + warnx("\tnot started\n"); + } + exit(0); + } + + usage("unrecognized command"); + exit(1); +} + +int bottle_drop_thread_main(int argc, char *argv[]) { + + warnx("starting\n"); + + unsigned i = 0; + + thread_running = true; + + int rgbleds = open(RGBLED_DEVICE_PATH, 0); + + struct actuator_controls_s actuators; + memset(&actuators, 0, sizeof(actuators)); + + orb_advert_t actuator_pub = orb_advertise(ORB_ID(actuator_controls_1), &actuators); + + while (!thread_should_exit) { + +// warnx("in while!\n"); + // values from -1 to 1 + + if (drop) { + // drop here + drop = false; + + ioctl(rgbleds, RGBLED_SET_MODE, RGBLED_MODE_ON); + ioctl(rgbleds, RGBLED_SET_COLOR, RGBLED_COLOR_RED); + } + + + actuators.control[0] = 0.5f; + actuators.control[1] = 0.5f; + actuators.control[2] = 0.5f; + actuators.control[3] = 0.5f; + + + + actuators.timestamp = hrt_absolute_time(); + orb_publish(ORB_ID(actuator_controls_1), actuator_pub, &actuators); + + usleep(50); + + i++; + } + + warnx("exiting.\n"); + + thread_running = false; + + ioctl(rgbleds, RGBLED_SET_MODE, RGBLED_MODE_OFF); + close(rgbleds); + + return 0; +} diff --git a/src/modules/bottle_drop/module.mk b/src/modules/bottle_drop/module.mk new file mode 100644 index 0000000000..222858b277 --- /dev/null +++ b/src/modules/bottle_drop/module.mk @@ -0,0 +1,40 @@ +############################################################################ +# +# 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. +# +############################################################################ + +# +# Daemon application +# + +MODULE_COMMAND = bottle_drop + +SRCS = bottle_drop.c From 4c9a4bc9a4686a2806a0119c262a744ca5517c02 Mon Sep 17 00:00:00 2001 From: Juchli D Date: Tue, 5 Nov 2013 13:26:34 +0100 Subject: [PATCH 003/411] Added custom start up of bottle drop --- .../px4fmu_common/init.d/900_bottle_drop_test | 46 +++++++++++++++++++ ROMFS/px4fmu_common/init.d/rcS | 6 +++ 2 files changed, 52 insertions(+) create mode 100644 ROMFS/px4fmu_common/init.d/900_bottle_drop_test diff --git a/ROMFS/px4fmu_common/init.d/900_bottle_drop_test b/ROMFS/px4fmu_common/init.d/900_bottle_drop_test new file mode 100644 index 0000000000..522f782108 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/900_bottle_drop_test @@ -0,0 +1,46 @@ +#!nsh + +echo "[init] bottle drop test + +# +# Load default params for this platform +# +if param compare SYS_AUTOCONFIG 1 +then + # Set all params here, then disable autoconfig + param set SYS_AUTOCONFIG 0 + + param save +fi + +# +# Force some key parameters to sane values +# MAV_TYPE 2 = quadrotor +# +param set MAV_TYPE 2 + +set EXIT_ON_END no + +# +# Start and configure PX4IO and FMU interface +# +# Start MAVLink (depends on orb) +mavlink start +usleep 5000 + + +sh /etc/init.d/rc.io + +fmu mode_pwm + +mixer load /dev/px4fmu /etc/mixers/FMU_pass.mix + +pwm arm -d /dev/px4fmu + +bottle_drop start + + +if [ $EXIT_ON_END == yes ] +then + exit +fi diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index cff8446a6b..6e954f58a3 100755 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -309,6 +309,12 @@ then set MODE custom fi + if param compare SYS_AUTOSTART 900 + then + sh /etc/init.d/900_bottle_drop_test + set MODE custom + fi + # Start any custom extensions that might be missing if [ -f /fs/microsd/etc/rc.local ] then From af3a56f17f6863210b0bcbf9569754a8c4ba3e53 Mon Sep 17 00:00:00 2001 From: Juchli D Date: Tue, 5 Nov 2013 13:27:05 +0100 Subject: [PATCH 004/411] Hack to always arm --- src/drivers/px4fmu/fmu.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index a2c62cf067..38cfce2e9f 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -587,6 +587,9 @@ PX4FMU::task_main() uint16_t pwm_limited[num_outputs]; + // XXX: hack: always armed + _armed = true; + pwm_limit_calc(_armed, num_outputs, _disarmed_pwm, _min_pwm, _max_pwm, outputs.output, pwm_limited, &_pwm_limit); /* output actual limited values */ From adee47978236aa113ee29f071f3498a60a802477 Mon Sep 17 00:00:00 2001 From: Juchli D Date: Fri, 15 Nov 2013 10:30:48 +0100 Subject: [PATCH 005/411] Working bottle Drop test --- .../px4fmu_common/init.d/900_bottle_drop_test | 29 +++++++ src/modules/bottle_drop/bottle_drop.c | 78 ++++++++++++++----- 2 files changed, 86 insertions(+), 21 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/900_bottle_drop_test b/ROMFS/px4fmu_common/init.d/900_bottle_drop_test index 522f782108..c0f50fec0c 100644 --- a/ROMFS/px4fmu_common/init.d/900_bottle_drop_test +++ b/ROMFS/px4fmu_common/init.d/900_bottle_drop_test @@ -28,6 +28,32 @@ set EXIT_ON_END no mavlink start usleep 5000 +# +# Start the sensors and test them. +# +sh /etc/init.d/rc.sensors + +# +# Start logging (depends on sensors) +# +#sh /etc/init.d/rc.logging +sdlog2 start -r 200 -e -b 16 + + +# +# Start GPS interface (depends on orb) +# +gps start + +# +# Start the attitude estimator +# +attitude_estimator_ekf start + +# +# Start position estimator +# +position_estimator_inav start sh /etc/init.d/rc.io @@ -35,6 +61,9 @@ fmu mode_pwm mixer load /dev/px4fmu /etc/mixers/FMU_pass.mix +pwm min -d /dev/px4fmu -c 123 -p 900 +pwm max -d /dev/px4fmu -c 123 -p 2100 + pwm arm -d /dev/px4fmu bottle_drop start diff --git a/src/modules/bottle_drop/bottle_drop.c b/src/modules/bottle_drop/bottle_drop.c index 69720b4393..09bbf46871 100644 --- a/src/modules/bottle_drop/bottle_drop.c +++ b/src/modules/bottle_drop/bottle_drop.c @@ -42,13 +42,16 @@ #include #include #include +#include #include #include #include #include +#include #include +#include static bool thread_should_exit = false; /**< daemon exit flag */ static bool thread_running = false; /**< daemon status flag */ @@ -101,7 +104,7 @@ int bottle_drop_main(int argc, char *argv[]) } thread_should_exit = false; - daemon_task = task_spawn_cmd("daemon", + daemon_task = task_spawn_cmd("bottle_drop", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, 4096, @@ -143,38 +146,71 @@ int bottle_drop_thread_main(int argc, char *argv[]) { int rgbleds = open(RGBLED_DEVICE_PATH, 0); + struct vehicle_attitude_s att; + memset(&att, 0, sizeof(att)); + int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude)); + orb_set_interval(vehicle_attitude_sub, 20); + struct actuator_controls_s actuators; memset(&actuators, 0, sizeof(actuators)); - orb_advert_t actuator_pub = orb_advertise(ORB_ID(actuator_controls_1), &actuators); + struct pollfd fds[1] = { + { .fd = vehicle_attitude_sub, .events = POLLIN }, + }; + + uint64_t drop_start = 0; + bool open_now = false; + while (!thread_should_exit) { // warnx("in while!\n"); // values from -1 to 1 - if (drop) { - // drop here - drop = false; + int ret = poll(fds, 1, 500); - ioctl(rgbleds, RGBLED_SET_MODE, RGBLED_MODE_ON); - ioctl(rgbleds, RGBLED_SET_COLOR, RGBLED_COLOR_RED); + if (ret < 0) { + /* poll error, count it in perf */ + warnx("poll error"); + + } else if (ret > 0) { + /* attitude */ + orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &att); + if (drop) { + // drop here + open_now = true; + drop = false; + drop_start = hrt_absolute_time(); + } + + if (open_now && (drop_start + 2e6 > hrt_absolute_time())) { // open door + + actuators.control[0] = -1.0f; + actuators.control[1] = 1.0f; + + } else if (open_now) { // unlock bottle + + ioctl(rgbleds, RGBLED_SET_MODE, RGBLED_MODE_ON); + ioctl(rgbleds, RGBLED_SET_COLOR, RGBLED_COLOR_RED); + actuators.control[2] = 0.5f; + + } else { + + // leave closed + // warnx( "%4.4f", att.pitch); + actuators.control[0] = 0.5f; + actuators.control[1] = -0.5f; + actuators.control[2] = -0.5f; + } + + + + actuators.timestamp = hrt_absolute_time(); + orb_publish(ORB_ID(actuator_controls_1), actuator_pub, &actuators); + + i++; } - - actuators.control[0] = 0.5f; - actuators.control[1] = 0.5f; - actuators.control[2] = 0.5f; - actuators.control[3] = 0.5f; - - - - actuators.timestamp = hrt_absolute_time(); - orb_publish(ORB_ID(actuator_controls_1), actuator_pub, &actuators); - - usleep(50); - - i++; } warnx("exiting.\n"); From 3c2133b9f1af370638cee82d6cda0b31df15dffe Mon Sep 17 00:00:00 2001 From: Juchli D Date: Fri, 29 Nov 2013 12:36:42 +0100 Subject: [PATCH 006/411] Working with faked imputs --- src/modules/bottle_drop/bottle_drop.c | 309 +++++++++++++++++++++++--- 1 file changed, 274 insertions(+), 35 deletions(-) diff --git a/src/modules/bottle_drop/bottle_drop.c b/src/modules/bottle_drop/bottle_drop.c index 09bbf46871..bb12674af0 100644 --- a/src/modules/bottle_drop/bottle_drop.c +++ b/src/modules/bottle_drop/bottle_drop.c @@ -32,27 +32,47 @@ ****************************************************************************/ /** - * @file px4_daemon_app.c - * daemon application example for PX4 autopilot + * @file bottle_drop.c + * bottle_drop application * - * @author Example User + * @author Dominik Juchli */ #include #include #include #include +#include #include +#include +#include +#include +#include + #include #include +#include + +#include + #include #include #include +#include +#include -#include #include +PARAM_DEFINE_FLOAT(BD_HEIGHT, 60.0f); +PARAM_DEFINE_FLOAT(BD_GPROPERTIES, 0.03f); +PARAM_DEFINE_FLOAT(BD_TURNRADIUS, 70.0f); +PARAM_DEFINE_FLOAT(BD_PRECISION, 1.0f); +PARAM_DEFINE_INT32(BD_APPROVAL, 0); + + + + static bool thread_should_exit = false; /**< daemon exit flag */ static bool thread_running = false; /**< daemon status flag */ static int daemon_task; /**< Handle of daemon task / thread */ @@ -140,27 +160,120 @@ int bottle_drop_thread_main(int argc, char *argv[]) { warnx("starting\n"); - unsigned i = 0; + bool updated = false; + + float height; // height at which the normal should be dropped NED + float z_0; // ground properties + float turn_radius; // turn radius of the UAV + float precision; // Expected precision of the UAV + bool drop_approval; // if approval is given = true, otherwise = false thread_running = true; - int rgbleds = open(RGBLED_DEVICE_PATH, 0); + /* XXX TODO: create, publish and read in wind speed topic */ + struct wind_speed_s { + float vx; // m/s + float vy; // m/s + float altitude; // m + } wind_speed; + + wind_speed.vx = 4.2f; + wind_speed.vy = 0.0f; + wind_speed.altitude = 62.0f; + + + /* XXX TODO: create, publish and read in target position in NED*/ + struct position_s { + double lat; //degrees 1E7 + double lon; //degrees 1E7 + float alt; //m + } target_position, drop_position, flight_vector_s, flight_vector_e; + + target_position.lat = 47.385806; + target_position.lon = 8.589093; + target_position.alt = 0.0f; + + + // constant + float g = 9.81f; // constant of gravity [m/s^2] + float m = 0.5f; // mass of bottle [kg] + float rho = 1.2f; // air density [kg/m^3] + float A = (powf(0.063f, 2.0f)/4.0f*M_PI_F); // Bottle cross section [m^2] + float dt = 0.01f; // step size [s] + float dt2 = 0.05f; // step size 2 [s] + + // Has to be estimated by experiment + float cd = 0.86f; // Drag coefficient for a cylinder with a d/l ratio of 1/3 [] + float t_signal = 0.084f; // Time span between sending the signal and the bottle top reaching level height with the bottom of the plane [s] + float t_door = 0.7f; // The time the system needs to open the door + safety, is also the time the palyload needs to safely escape the shaft [s] + + + // Definition + float h_0; // height over target + float az; // acceleration in z direction[m/s^2] + float vz; // velocity in z direction [m/s] + float z; // fallen distance [m] + float h; // height over target [m] + float ax; // acceleration in x direction [m/s^2] + float vx; // ground speed in x direction [m/s] + float x; // traveled distance in x direction [m] + float vw; // wind speed [m/s] + float vrx; // relative velocity in x direction [m/s] + float v; // relative speed vector [m/s] + float Fd; // Drag force [N] + float Fdx; // Drag force in x direction [N] + float Fdz; // Drag force in z direction [N] + float vr; // absolute wind speed [m/s] + float x_drop, y_drop; // coordinates of the drop point in reference to the target (projection of NED) + float x_t,y_t; // coordinates of the target in reference to the target x_t = 0, y_t = 0 (projection of NED) + float x_l,y_l; // local position in projected coordinates + float x_f,y_f; // to-be position of the UAV after dt2 seconds in projected coordinates + double x_f_NED, y_f_NED; // to-be position of the UAV after dt2 seconds in NED + float distance_open_door; // The distance the UAV travels during its doors open [m] + float distance_real = 0; // The distance between the UAVs position and the drop point [m] + float future_distance = 0; // The distance between the UAVs to-be position and the drop point [m] + + // states + bool state_door = false; // Doors are closed = false, open = true + bool state_drop = false; // Drop occurred = true, Drop din't occur = false + bool state_run = false; // A drop was attempted = true, the drop is still in progress = false + + + param_t param_height = param_find("BD_HEIGHT"); + param_t param_gproperties = param_find("BD_GPROPERTIES"); + param_t param_turn_radius = param_find("BD_TURNRADIUS"); + param_t param_precision = param_find("BD_PRECISION"); + param_t param_approval = param_find("BD_APPROVAL"); + + + param_get(param_approval, &drop_approval); + param_get(param_precision, &precision); + param_get(param_turn_radius, &turn_radius); + param_get(param_height, &height); + param_get(param_gproperties, &z_0); + struct vehicle_attitude_s att; memset(&att, 0, sizeof(att)); int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude)); - orb_set_interval(vehicle_attitude_sub, 20); + orb_set_interval(vehicle_attitude_sub, 100); + + struct vehicle_global_position_s globalpos; + memset(&globalpos, 0, sizeof(globalpos)); + int vehicle_global_position_sub = orb_subscribe(ORB_ID(vehicle_global_position)); + + struct parameter_update_s update; + memset(&update, 0, sizeof(update)); + int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update)); struct actuator_controls_s actuators; memset(&actuators, 0, sizeof(actuators)); orb_advert_t actuator_pub = orb_advertise(ORB_ID(actuator_controls_1), &actuators); - struct pollfd fds[1] = { - { .fd = vehicle_attitude_sub, .events = POLLIN }, + struct pollfd fds[] = { + { .fd = vehicle_attitude_sub, .events = POLLIN } }; - uint64_t drop_start = 0; - bool open_now = false; while (!thread_should_exit) { @@ -174,51 +287,177 @@ int bottle_drop_thread_main(int argc, char *argv[]) { warnx("poll error"); } else if (ret > 0) { - /* attitude */ + orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &att); - if (drop) { - // drop here - open_now = true; - drop = false; - drop_start = hrt_absolute_time(); + + orb_check(vehicle_global_position_sub, &updated); + if (updated){ + /* copy global position */ + orb_copy(ORB_ID(vehicle_global_position), vehicle_global_position_sub, &globalpos); + } + //////////////////////////////////////////////////////////////////// DEBUGGING + globalpos.lat = 47.384486; + globalpos.lon = 8.588239; + globalpos.vx = 18.0f; + globalpos.vy = 0.0f; + globalpos.alt = 60.0f; + globalpos.yaw = M_PI_F/2.0f; + + + orb_check(parameter_update_sub, &updated); + if (updated){ + /* copy global position */ + orb_copy(ORB_ID(parameter_update), parameter_update_sub, &update); + + /* update all parameters */ + param_get(param_height, &height); + param_get(param_gproperties, &z_0); + param_get(param_turn_radius, &turn_radius); + param_get(param_approval, &drop_approval); + param_get(param_precision, &precision); + + } - if (open_now && (drop_start + 2e6 > hrt_absolute_time())) { // open door + // Initialization + az = g; // acceleration in z direction[m/s^2] + vz = 0; // velocity in z direction [m/s] + z = 0; // fallen distance [m] + h_0 = globalpos.alt - target_position.alt; // height over target at start[m] + h = h_0; // height over target [m] + ax = 0; // acceleration in x direction [m/s^2] + vx = globalpos.vx; // ground speed in x direction [m/s] + x = 0; // traveled distance in x direction [m] + vw = 0; // wind speed [m/s] + vrx = 0; // relative velocity in x direction [m/s] + v = globalpos.vx; // relative speed vector [m/s] + Fd = 0; // Drag force [N] + Fdx = 0; // Drag force in x direction [N] + Fdz = 0; // Drag force in z direction [N] + vr = sqrt(pow(wind_speed.vx,2) + pow(wind_speed.vy,2)); // absolute wind speed [m/s] + distance_open_door = t_door * globalpos.vx; - actuators.control[0] = -1.0f; + + //warnx("absolut wind speed = %.4f", vr); //////////////////////////////////////////////////////////////////// DEBUGGING + + + //warnx("Initialization complete\n"); //////////////////////////////////////////////////////////////////// DEBUGGING + + + if (drop_approval && !state_drop) + { + //warnx("approval given\n"); //////////////////////////////////////////////////////////////////// DEBUGGING + // drop here + //open_now = true; + //drop = false; + //drop_start = hrt_absolute_time(); + + unsigned counter = 0; + + // Compute the distance the bottle will travel after it is dropped in body frame coordinates --> x + while( h > 0.05f) + { + // z-direction + vz = vz + az*dt; + z = z + vz*dt; + h = h_0 - z; + + // x-direction + vw = vr*logf(h/z_0)/logf(wind_speed.altitude/z_0); + vx = vx + ax*dt; + x = x + vx*dt; + vrx = vx + vw; + + //Drag force + v = sqrtf(powf(vz,2.0f) + powf(vrx,2.0f)); + Fd = 0.5f*rho*A*cd*powf(v,2.0f); + Fdx = Fd*vrx/v; + Fdz = Fd*vz/v; + + //acceleration + az = g - Fdz/m; + ax = -Fdx/m; + + } + // Compute Drop point + x = globalpos.vx*t_signal + x; + map_projection_init(target_position.lat, target_position.lon); + //warnx("x = %.4f", x); //////////////////////////////////////////////////////////////////// DEBUGGING + + + + map_projection_project(target_position.lat, target_position.lon, &x_t, &y_t); + if( vr < 0.001f) // if there is no wind, an arbitrarily direction is chosen + { + vr = 1; + wind_speed.vx = 1; + wind_speed.vy = 0; + } + x_drop = x_t + x*wind_speed.vx/vr; + y_drop = y_t + x*wind_speed.vy/vr; + map_projection_reproject(x_drop, y_drop, &drop_position.lat, &drop_position.lon); + drop_position.alt = height; + //warnx("drop point: lat = %.7f , lon = %.7f", drop_position.lat, drop_position.lon); //////////////////////////////////////////////////////////////////// DEBUGGING + + + + // Compute flight vector + map_projection_reproject(x_drop + 2*turn_radius*wind_speed.vx/vr, y_drop + 2*turn_radius*wind_speed.vy/vr, &flight_vector_s.lat, &flight_vector_s.lon); + flight_vector_s.alt = height; + map_projection_reproject(x_drop - turn_radius*wind_speed.vx/vr, y_drop - turn_radius*wind_speed.vy/vr, &flight_vector_e.lat, &flight_vector_e.lon); + flight_vector_e.alt = height; + //warnx("Flight vector: starting point = %.7f %.7f , end point = %.7f %.7f", flight_vector_s.lat,flight_vector_s.lon,flight_vector_e.lat,flight_vector_e.lon); //////////////////////////////////////////////////////////////////// DEBUGGING + + + // Drop Cancellation if terms are not met + distance_real = get_distance_to_next_waypoint(globalpos.lat, globalpos.lon, drop_position.lat, drop_position.lon); + map_projection_project(globalpos.lat, globalpos.lon, &x_l, &y_l); + x_f = x_l + globalpos.vx*cosf(globalpos.yaw)*dt2 - globalpos.vy*sinf(globalpos.yaw)*dt2; // Attention to sign, has to be checked + y_f = y_l + globalpos.vy*cosf(globalpos.yaw)*dt2 - globalpos.vx*sinf(globalpos.yaw)*dt2; + map_projection_reproject(x_f, y_f, &x_f_NED, &y_f_NED); + future_distance = get_distance_to_next_waypoint(x_f_NED, y_f_NED, drop_position.lat, drop_position.lon); + //warnx("position to-be: = %.7f %.7f" ,x_f_NED, y_f_NED ); //////////////////////////////////////////////////////////////////// DEBUGGING + + } + + if(distance_real < distance_open_door && drop_approval) + { + actuators.control[0] = -1.0f; // open door actuators.control[1] = 1.0f; - - } else if (open_now) { // unlock bottle - - ioctl(rgbleds, RGBLED_SET_MODE, RGBLED_MODE_ON); - ioctl(rgbleds, RGBLED_SET_COLOR, RGBLED_COLOR_RED); - actuators.control[2] = 0.5f; - - } else { - - // leave closed - // warnx( "%4.4f", att.pitch); + state_door = true; + } + else + { // closed door and locked survival kit actuators.control[0] = 0.5f; actuators.control[1] = -0.5f; actuators.control[2] = -0.5f; + state_door = false; + } + if(distance_real < precision && distance_real < future_distance && state_door) // Drop only if the distance between drop point and actual position is getting larger again + { + if(fabsf(acosf(cosf(globalpos.yaw))+acosf(wind_speed.vx)) < 10.0f/180.0f*M_PI_F) // if flight trajectory deviates more than 10 degrees from calculated path, it will no drop + { + actuators.control[2] = 0.5f; + state_drop = true; + state_run = true; + } + else + { + state_run = true; + } } - actuators.timestamp = hrt_absolute_time(); orb_publish(ORB_ID(actuator_controls_1), actuator_pub, &actuators); - i++; } - } warnx("exiting.\n"); thread_running = false; - ioctl(rgbleds, RGBLED_SET_MODE, RGBLED_MODE_OFF); - close(rgbleds); return 0; } From 76e5a755dfdaec4bfc00493e8f16a5e40ffa5093 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 29 Nov 2013 17:03:18 +0100 Subject: [PATCH 007/411] Bottle_drop: Publish to onboard mission --- src/modules/bottle_drop/bottle_drop.c | 55 ++++++++++++++++++--------- 1 file changed, 37 insertions(+), 18 deletions(-) diff --git a/src/modules/bottle_drop/bottle_drop.c b/src/modules/bottle_drop/bottle_drop.c index bb12674af0..8d3d6d5995 100644 --- a/src/modules/bottle_drop/bottle_drop.c +++ b/src/modules/bottle_drop/bottle_drop.c @@ -61,6 +61,7 @@ #include #include #include +#include #include @@ -187,7 +188,7 @@ int bottle_drop_thread_main(int argc, char *argv[]) { double lat; //degrees 1E7 double lon; //degrees 1E7 float alt; //m - } target_position, drop_position, flight_vector_s, flight_vector_e; + } target_position, drop_position; target_position.lat = 47.385806; target_position.lon = 8.589093; @@ -238,6 +239,7 @@ int bottle_drop_thread_main(int argc, char *argv[]) { bool state_drop = false; // Drop occurred = true, Drop din't occur = false bool state_run = false; // A drop was attempted = true, the drop is still in progress = false + unsigned counter = 0; param_t param_height = param_find("BD_HEIGHT"); param_t param_gproperties = param_find("BD_GPROPERTIES"); @@ -270,6 +272,22 @@ int bottle_drop_thread_main(int argc, char *argv[]) { memset(&actuators, 0, sizeof(actuators)); orb_advert_t actuator_pub = orb_advertise(ORB_ID(actuator_controls_1), &actuators); + struct mission_s onboard_mission; + memset(&onboard_mission, 0, sizeof(onboard_mission)); + onboard_mission.items = (struct mission_item_s*)malloc(sizeof(struct mission_item_s) * 2); + + struct mission_item_s *flight_vector_s = &onboard_mission.items[0]; + struct mission_item_s *flight_vector_e = &onboard_mission.items[1]; + + flight_vector_s->nav_cmd = NAV_CMD_WAYPOINT; + flight_vector_s->radius = 50; // TODO: make parameter + flight_vector_s->autocontinue = true; + flight_vector_e->nav_cmd = NAV_CMD_WAYPOINT; + flight_vector_e->radius = 50; // TODO: make parameter + flight_vector_e->autocontinue = true; + + orb_advert_t onboard_mission_pub = orb_advertise(ORB_ID(onboard_mission), &onboard_mission); + struct pollfd fds[] = { { .fd = vehicle_attitude_sub, .events = POLLIN } }; @@ -295,14 +313,6 @@ int bottle_drop_thread_main(int argc, char *argv[]) { /* copy global position */ orb_copy(ORB_ID(vehicle_global_position), vehicle_global_position_sub, &globalpos); } - //////////////////////////////////////////////////////////////////// DEBUGGING - globalpos.lat = 47.384486; - globalpos.lon = 8.588239; - globalpos.vx = 18.0f; - globalpos.vy = 0.0f; - globalpos.alt = 60.0f; - globalpos.yaw = M_PI_F/2.0f; - orb_check(parameter_update_sub, &updated); if (updated){ @@ -344,7 +354,7 @@ int bottle_drop_thread_main(int argc, char *argv[]) { //warnx("Initialization complete\n"); //////////////////////////////////////////////////////////////////// DEBUGGING - if (drop_approval && !state_drop) + if (drop_approval && !state_drop && counter % 10 == 0) { //warnx("approval given\n"); //////////////////////////////////////////////////////////////////// DEBUGGING // drop here @@ -352,8 +362,6 @@ int bottle_drop_thread_main(int argc, char *argv[]) { //drop = false; //drop_start = hrt_absolute_time(); - unsigned counter = 0; - // Compute the distance the bottle will travel after it is dropped in body frame coordinates --> x while( h > 0.05f) { @@ -402,13 +410,12 @@ int bottle_drop_thread_main(int argc, char *argv[]) { // Compute flight vector - map_projection_reproject(x_drop + 2*turn_radius*wind_speed.vx/vr, y_drop + 2*turn_radius*wind_speed.vy/vr, &flight_vector_s.lat, &flight_vector_s.lon); - flight_vector_s.alt = height; - map_projection_reproject(x_drop - turn_radius*wind_speed.vx/vr, y_drop - turn_radius*wind_speed.vy/vr, &flight_vector_e.lat, &flight_vector_e.lon); - flight_vector_e.alt = height; + map_projection_reproject(x_drop + 2*turn_radius*wind_speed.vx/vr, y_drop + 2*turn_radius*wind_speed.vy/vr, &(flight_vector_s->lat), &(flight_vector_s->lon)); + flight_vector_s->altitude = height; + map_projection_reproject(x_drop - turn_radius*wind_speed.vx/vr, y_drop - turn_radius*wind_speed.vy/vr, &(flight_vector_e)->lat, &(flight_vector_e)->lon); + flight_vector_e->altitude = height; //warnx("Flight vector: starting point = %.7f %.7f , end point = %.7f %.7f", flight_vector_s.lat,flight_vector_s.lon,flight_vector_e.lat,flight_vector_e.lon); //////////////////////////////////////////////////////////////////// DEBUGGING - // Drop Cancellation if terms are not met distance_real = get_distance_to_next_waypoint(globalpos.lat, globalpos.lon, drop_position.lat, drop_position.lon); map_projection_project(globalpos.lat, globalpos.lon, &x_l, &y_l); @@ -418,6 +425,18 @@ int bottle_drop_thread_main(int argc, char *argv[]) { future_distance = get_distance_to_next_waypoint(x_f_NED, y_f_NED, drop_position.lat, drop_position.lon); //warnx("position to-be: = %.7f %.7f" ,x_f_NED, y_f_NED ); //////////////////////////////////////////////////////////////////// DEBUGGING + + onboard_mission.count = 2; + + if (state_run && !state_drop) { + onboard_mission.current_index = 0; + } else { + onboard_mission.current_index = -1; + } + + + orb_publish(ORB_ID(onboard_mission), onboard_mission_pub, &onboard_mission); + } if(distance_real < distance_open_door && drop_approval) @@ -447,10 +466,10 @@ int bottle_drop_thread_main(int argc, char *argv[]) { } } - actuators.timestamp = hrt_absolute_time(); orb_publish(ORB_ID(actuator_controls_1), actuator_pub, &actuators); + counter++; } } From a839a09834a5ab3217e794a0a98af2eeffaf571b Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 29 Nov 2013 17:03:48 +0100 Subject: [PATCH 008/411] Bottle drop: Added HIL startup --- .../init.d/901_bottle_drop_test.hil | 98 +++++++++++++++++++ ROMFS/px4fmu_common/init.d/rcS | 6 ++ 2 files changed, 104 insertions(+) create mode 100644 ROMFS/px4fmu_common/init.d/901_bottle_drop_test.hil diff --git a/ROMFS/px4fmu_common/init.d/901_bottle_drop_test.hil b/ROMFS/px4fmu_common/init.d/901_bottle_drop_test.hil new file mode 100644 index 0000000000..060c7d0b5e --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/901_bottle_drop_test.hil @@ -0,0 +1,98 @@ +#!nsh +# +# USB HIL start +# + +echo "[HIL] HILStar starting.." + +# +# Load default params for this platform +# +if param compare SYS_AUTOCONFIG 1 +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 + +# Allow USB some time to come up +sleep 1 +# Tell MAVLink that this link is "fast" +mavlink start -b 230400 -d /dev/ttyACM0 + +# Create a fake HIL /dev/pwm_output interface +hil mode_pwm + +# +# Force some key parameters to sane values +# MAV_TYPE 1 = fixed wing, 2 = quadrotor, 13 = hexarotor +# see https://pixhawk.ethz.ch/mavlink/ +# +param set MAV_TYPE 1 + +# +# Check if we got an IO +# +if px4io start +then + echo "IO started" +else + fmu mode_serial + echo "FMU started" +fi + +# +# Start the sensors (depends on orb, px4io) +# +sh /etc/init.d/rc.sensors + +# +# Start the attitude estimator (depends on orb) +# +att_pos_estimator_ekf start + +# +# Load mixer and start controllers (depends on px4io) +# +mixer load /dev/pwm_output /etc/mixers/FMU_AET.mix +fw_pos_control_l1 start +fw_att_control start + +#fmu mode_pwm + +mixer load /dev/px4io /etc/mixers/FMU_pass.mix + +pwm min -d /dev/px4io -c 123 -p 900 +pwm max -d /dev/px4io -c 123 -p 2100 + +pwm arm -d /dev/px4io + +bottle_drop start + +echo "[HIL] setup done, running" + diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 46bb6d8669..5327ee7cab 100755 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -324,6 +324,12 @@ then sh /etc/init.d/900_bottle_drop_test set MODE custom fi + + if param compare SYS_AUTOSTART 901 + then + sh /etc/init.d/901_bottle_drop_test.hil + set MODE custom + fi # Start any custom extensions that might be missing if [ -f /fs/microsd/etc/rc.local ] From 0ec609f49167e1e6857e970088b4e8de93bba032 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 2 Dec 2013 23:01:02 +0100 Subject: [PATCH 009/411] Bottle drop: added custom HIL startup script --- .../init.d/901_bottle_drop_test.hil | 17 +++++------------ ROMFS/px4fmu_common/init.d/rcS | 12 ++++++------ 2 files changed, 11 insertions(+), 18 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/901_bottle_drop_test.hil b/ROMFS/px4fmu_common/init.d/901_bottle_drop_test.hil index 060c7d0b5e..472351e9e1 100644 --- a/ROMFS/px4fmu_common/init.d/901_bottle_drop_test.hil +++ b/ROMFS/px4fmu_common/init.d/901_bottle_drop_test.hil @@ -55,17 +55,6 @@ hil mode_pwm # param set MAV_TYPE 1 -# -# Check if we got an IO -# -if px4io start -then - echo "IO started" -else - fmu mode_serial - echo "FMU started" -fi - # # Start the sensors (depends on orb, px4io) # @@ -83,7 +72,11 @@ mixer load /dev/pwm_output /etc/mixers/FMU_AET.mix fw_pos_control_l1 start fw_att_control start -#fmu mode_pwm +# +# Start IO +# +px4io start + mixer load /dev/px4io /etc/mixers/FMU_pass.mix diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 5327ee7cab..6c76f5fe92 100755 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -145,6 +145,12 @@ then sh /etc/init.d/1004_rc_fw_Rascal110.hil set MODE custom fi + + if param compare SYS_AUTOSTART 901 + then + sh /etc/init.d/901_bottle_drop_test.hil + set MODE custom + fi if [ $MODE != custom ] then @@ -324,12 +330,6 @@ then sh /etc/init.d/900_bottle_drop_test set MODE custom fi - - if param compare SYS_AUTOSTART 901 - then - sh /etc/init.d/901_bottle_drop_test.hil - set MODE custom - fi # Start any custom extensions that might be missing if [ -f /fs/microsd/etc/rc.local ] From 6048f9beda68507d74e321ad4816aa137e20fe3a Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 2 Dec 2013 23:03:29 +0100 Subject: [PATCH 010/411] HIL: copy correct actuator group --- src/drivers/hil/hil.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/drivers/hil/hil.cpp b/src/drivers/hil/hil.cpp index c1d73dd87f..b848f6c373 100644 --- a/src/drivers/hil/hil.cpp +++ b/src/drivers/hil/hil.cpp @@ -391,7 +391,8 @@ HIL::task_main() if (fds[0].revents & POLLIN) { /* get controls - must always do this to avoid spinning */ - orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, _t_actuators, &_controls); + orb_copy(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS : + ORB_ID(actuator_controls_1), _t_actuators, &_controls); /* can we mix? */ if (_mixers != nullptr) { From 808294b01a0df96817701526b24c6bb80e97775c Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 2 Dec 2013 23:03:52 +0100 Subject: [PATCH 011/411] HIL: only send attitude actuator group 0 --- src/modules/mavlink/orb_listener.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mavlink/orb_listener.c b/src/modules/mavlink/orb_listener.c index abc91d34fa..634586fdb1 100644 --- a/src/modules/mavlink/orb_listener.c +++ b/src/modules/mavlink/orb_listener.c @@ -507,7 +507,7 @@ l_actuator_outputs(const struct listener *l) act_outputs.output[7]); /* only send in HIL mode */ - if (mavlink_hil_enabled && armed.armed) { + if (mavlink_hil_enabled && armed.armed && l->arg == 0) { /* translate the current syste state to mavlink state and mode */ uint8_t mavlink_state = 0; From f2c303679ea859bf722eaa7289fd10f8b806264a Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 2 Dec 2013 23:05:28 +0100 Subject: [PATCH 012/411] Bottle drop: lots of changes, working in HIL --- src/modules/bottle_drop/bottle_drop.c | 130 +++++++++++++++----------- 1 file changed, 78 insertions(+), 52 deletions(-) diff --git a/src/modules/bottle_drop/bottle_drop.c b/src/modules/bottle_drop/bottle_drop.c index 8d3d6d5995..1c2e008ba7 100644 --- a/src/modules/bottle_drop/bottle_drop.c +++ b/src/modules/bottle_drop/bottle_drop.c @@ -258,7 +258,7 @@ int bottle_drop_thread_main(int argc, char *argv[]) { struct vehicle_attitude_s att; memset(&att, 0, sizeof(att)); int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude)); - orb_set_interval(vehicle_attitude_sub, 100); + orb_set_interval(vehicle_attitude_sub, 20); struct vehicle_global_position_s globalpos; memset(&globalpos, 0, sizeof(globalpos)); @@ -292,6 +292,9 @@ int bottle_drop_thread_main(int argc, char *argv[]) { { .fd = vehicle_attitude_sub, .events = POLLIN } }; + double latitude; + double longitude; + while (!thread_should_exit) { @@ -312,6 +315,9 @@ int bottle_drop_thread_main(int argc, char *argv[]) { if (updated){ /* copy global position */ orb_copy(ORB_ID(vehicle_global_position), vehicle_global_position_sub, &globalpos); + + latitude = (double)globalpos.lat / 1e7; + longitude = (double)globalpos.lon / 1e7; } orb_check(parameter_update_sub, &updated); @@ -330,22 +336,10 @@ int bottle_drop_thread_main(int argc, char *argv[]) { } // Initialization - az = g; // acceleration in z direction[m/s^2] - vz = 0; // velocity in z direction [m/s] - z = 0; // fallen distance [m] - h_0 = globalpos.alt - target_position.alt; // height over target at start[m] - h = h_0; // height over target [m] - ax = 0; // acceleration in x direction [m/s^2] - vx = globalpos.vx; // ground speed in x direction [m/s] - x = 0; // traveled distance in x direction [m] - vw = 0; // wind speed [m/s] - vrx = 0; // relative velocity in x direction [m/s] - v = globalpos.vx; // relative speed vector [m/s] - Fd = 0; // Drag force [N] - Fdx = 0; // Drag force in x direction [N] - Fdz = 0; // Drag force in z direction [N] - vr = sqrt(pow(wind_speed.vx,2) + pow(wind_speed.vy,2)); // absolute wind speed [m/s] - distance_open_door = t_door * globalpos.vx; + + + vr = sqrtf(powf(wind_speed.vx,2) + powf(wind_speed.vy,2)); // absolute wind speed [m/s] + distance_open_door = fabsf(t_door * globalpos.vx); //warnx("absolut wind speed = %.4f", vr); //////////////////////////////////////////////////////////////////// DEBUGGING @@ -354,7 +348,7 @@ int bottle_drop_thread_main(int argc, char *argv[]) { //warnx("Initialization complete\n"); //////////////////////////////////////////////////////////////////// DEBUGGING - if (drop_approval && !state_drop && counter % 10 == 0) + if (drop_approval && !state_drop) { //warnx("approval given\n"); //////////////////////////////////////////////////////////////////// DEBUGGING // drop here @@ -362,37 +356,53 @@ int bottle_drop_thread_main(int argc, char *argv[]) { //drop = false; //drop_start = hrt_absolute_time(); - // Compute the distance the bottle will travel after it is dropped in body frame coordinates --> x - while( h > 0.05f) - { - // z-direction - vz = vz + az*dt; - z = z + vz*dt; - h = h_0 - z; - // x-direction - vw = vr*logf(h/z_0)/logf(wind_speed.altitude/z_0); - vx = vx + ax*dt; - x = x + vx*dt; - vrx = vx + vw; + if (counter % 50 == 0) { - //Drag force - v = sqrtf(powf(vz,2.0f) + powf(vrx,2.0f)); - Fd = 0.5f*rho*A*cd*powf(v,2.0f); - Fdx = Fd*vrx/v; - Fdz = Fd*vz/v; + az = g; // acceleration in z direction[m/s^2] + vz = 0; // velocity in z direction [m/s] + z = 0; // fallen distance [m] + h_0 = globalpos.alt - target_position.alt; // height over target at start[m] + h = h_0; // height over target [m] + ax = 0; // acceleration in x direction [m/s^2] + vx = sqrtf(powf(globalpos.vx,2.0f) + powf(globalpos.vy,2.0f)); // ground speed in x direction [m/s] + x = 0; // traveled distance in x direction [m] + vw = 0; // wind speed [m/s] + vrx = 0; // relative velocity in x direction [m/s] + v = sqrtf(powf(globalpos.vx,2.0f) + powf(globalpos.vy,2.0f)); // relative speed vector [m/s] + Fd = 0; // Drag force [N] + Fdx = 0; // Drag force in x direction [N] + Fdz = 0; // Drag force in z direction [N] - //acceleration - az = g - Fdz/m; - ax = -Fdx/m; + // Compute the distance the bottle will travel after it is dropped in body frame coordinates --> x + while( h > 0.05f) + { + // z-direction + vz = vz + az*dt; + z = z + vz*dt; + h = h_0 - z; + + // x-direction + vw = vr*logf(h/z_0)/logf(wind_speed.altitude/z_0); + vx = vx + ax*dt; + x = x + vx*dt; + vrx = vx + vw; + + //Drag force + v = sqrtf(powf(vz,2.0f) + powf(vrx,2.0f)); + Fd = 0.5f*rho*A*cd*powf(v,2.0f); + Fdx = Fd*vrx/v; + Fdz = Fd*vz/v; + + //acceleration + az = g - Fdz/m; + ax = -Fdx/m; + } + // Compute Drop point + x = sqrtf(powf(globalpos.vx,2.0f) + powf(globalpos.vy,2.0f))*t_signal + x; + map_projection_init(target_position.lat, target_position.lon); } - // Compute Drop point - x = globalpos.vx*t_signal + x; - map_projection_init(target_position.lat, target_position.lon); - //warnx("x = %.4f", x); //////////////////////////////////////////////////////////////////// DEBUGGING - - map_projection_project(target_position.lat, target_position.lon, &x_t, &y_t); if( vr < 0.001f) // if there is no wind, an arbitrarily direction is chosen @@ -417,14 +427,26 @@ int bottle_drop_thread_main(int argc, char *argv[]) { //warnx("Flight vector: starting point = %.7f %.7f , end point = %.7f %.7f", flight_vector_s.lat,flight_vector_s.lon,flight_vector_e.lat,flight_vector_e.lon); //////////////////////////////////////////////////////////////////// DEBUGGING // Drop Cancellation if terms are not met - distance_real = get_distance_to_next_waypoint(globalpos.lat, globalpos.lon, drop_position.lat, drop_position.lon); - map_projection_project(globalpos.lat, globalpos.lon, &x_l, &y_l); - x_f = x_l + globalpos.vx*cosf(globalpos.yaw)*dt2 - globalpos.vy*sinf(globalpos.yaw)*dt2; // Attention to sign, has to be checked - y_f = y_l + globalpos.vy*cosf(globalpos.yaw)*dt2 - globalpos.vx*sinf(globalpos.yaw)*dt2; + + // warnx("latitude:%.2f", latitude); + // warnx("longitude:%.2f", longitude); + // warnx("drop_position.lat:%.2f", drop_position.lat); + // warnx("drop_position.lon:%.2f", drop_position.lon); + + distance_real = fabsf(get_distance_to_next_waypoint(latitude, longitude, drop_position.lat, drop_position.lon)); + map_projection_project(latitude, longitude, &x_l, &y_l); + x_f = x_l + globalpos.vx*dt2; + y_f = y_l + globalpos.vy*dt2; map_projection_reproject(x_f, y_f, &x_f_NED, &y_f_NED); - future_distance = get_distance_to_next_waypoint(x_f_NED, y_f_NED, drop_position.lat, drop_position.lon); + future_distance = fabsf(get_distance_to_next_waypoint(x_f_NED, y_f_NED, drop_position.lat, drop_position.lon)); //warnx("position to-be: = %.7f %.7f" ,x_f_NED, y_f_NED ); //////////////////////////////////////////////////////////////////// DEBUGGING + // if (counter % 10 ==0) { + // warnx("x: %.4f", x); + // warnx("drop_position.lat: %.4f, drop_position.lon: %.4f", drop_position.lat, drop_position.lon); + // warnx("latitude %.4f, longitude: %.4f", latitude, longitude); + // warnx("future_distance: %.2f, precision: %.2f", future_distance, precision); + // } onboard_mission.count = 2; @@ -434,16 +456,19 @@ int bottle_drop_thread_main(int argc, char *argv[]) { onboard_mission.current_index = -1; } + // if (counter % 10 ==0) + // warnx("Distance real: %.2f, distance_open_door: %.2f, angle to wind: %.2f", distance_real, distance_open_door, fabsf(_wrap_pi(globalpos.yaw-atan2f(wind_speed.vy,wind_speed.vx)+M_PI_F))); orb_publish(ORB_ID(onboard_mission), onboard_mission_pub, &onboard_mission); } - if(distance_real < distance_open_door && drop_approval) + if(isfinite(distance_real) && distance_real < distance_open_door && drop_approval) { actuators.control[0] = -1.0f; // open door actuators.control[1] = 1.0f; state_door = true; + warnx("open doors"); } else { // closed door and locked survival kit @@ -452,13 +477,14 @@ int bottle_drop_thread_main(int argc, char *argv[]) { actuators.control[2] = -0.5f; state_door = false; } - if(distance_real < precision && distance_real < future_distance && state_door) // Drop only if the distance between drop point and actual position is getting larger again + if(isfinite(distance_real) && distance_real < precision && distance_real < future_distance && state_door) // Drop only if the distance between drop point and actual position is getting larger again { - if(fabsf(acosf(cosf(globalpos.yaw))+acosf(wind_speed.vx)) < 10.0f/180.0f*M_PI_F) // if flight trajectory deviates more than 10 degrees from calculated path, it will no drop + if(fabsf(_wrap_pi(globalpos.yaw-atan2f(wind_speed.vy,wind_speed.vx)+M_PI_F)) < 10.0f/180.0f*M_PI_F) // if flight trajectory deviates more than 10 degrees from calculated path, it will no drop { actuators.control[2] = 0.5f; state_drop = true; state_run = true; + warnx("dropping now"); } else { From 56834414f1e37dd5f091685d3acd643d767dc0fa Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 4 Dec 2013 10:37:51 +0100 Subject: [PATCH 013/411] 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 0aeada16b9601783da4fa1a7d628b34e1dce1bf4 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 4 Dec 2013 10:38:56 +0100 Subject: [PATCH 014/411] 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 d0444497eddb75beae4bc0001e6aacbc137e0d66 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sat, 14 Dec 2013 20:52:25 +0100 Subject: [PATCH 015/411] Bottle_drop: Store WPs in datamanager --- src/modules/bottle_drop/bottle_drop.c | 63 ++++++++++++++++----------- 1 file changed, 37 insertions(+), 26 deletions(-) diff --git a/src/modules/bottle_drop/bottle_drop.c b/src/modules/bottle_drop/bottle_drop.c index e2194bae26..1911329d0c 100644 --- a/src/modules/bottle_drop/bottle_drop.c +++ b/src/modules/bottle_drop/bottle_drop.c @@ -55,6 +55,7 @@ #include #include +#include #include #include @@ -271,22 +272,21 @@ int bottle_drop_thread_main(int argc, char *argv[]) { struct actuator_controls_s actuators; memset(&actuators, 0, sizeof(actuators)); orb_advert_t actuator_pub = orb_advertise(ORB_ID(actuator_controls_1), &actuators); - - // struct mission_s onboard_mission; - // memset(&onboard_mission, 0, sizeof(onboard_mission)); - // onboard_mission.items = (struct mission_item_s*)malloc(sizeof(struct mission_item_s) * 2); - struct mission_item_s *flight_vector_s = NULL;//&onboard_mission.items[0]; - struct mission_item_s *flight_vector_e = NULL;//&onboard_mission.items[1]; + struct mission_item_s flight_vector_s; + struct mission_item_s flight_vector_e; - // flight_vector_s->nav_cmd = NAV_CMD_WAYPOINT; - // flight_vector_s->radius = 50; // TODO: make parameter - // flight_vector_s->autocontinue = true; - // flight_vector_e->nav_cmd = NAV_CMD_WAYPOINT; - // flight_vector_e->radius = 50; // TODO: make parameter - // flight_vector_e->autocontinue = true; + flight_vector_s.nav_cmd = NAV_CMD_WAYPOINT; + flight_vector_s.radius = 50; // TODO: make parameter + flight_vector_s.autocontinue = true; + flight_vector_e.nav_cmd = NAV_CMD_WAYPOINT; + flight_vector_e.radius = 50; // TODO: make parameter + flight_vector_e.autocontinue = true; - // orb_advert_t onboard_mission_pub = orb_advertise(ORB_ID(onboard_mission), &onboard_mission); + struct mission_s onboard_mission; + memset(&onboard_mission, 0, sizeof(onboard_mission)); + + orb_advert_t onboard_mission_pub = orb_advertise(ORB_ID(onboard_mission), &onboard_mission); struct pollfd fds[] = { { .fd = vehicle_attitude_sub, .events = POLLIN } @@ -420,10 +420,10 @@ int bottle_drop_thread_main(int argc, char *argv[]) { // Compute flight vector - map_projection_reproject(x_drop + 2*turn_radius*wind_speed.vx/vr, y_drop + 2*turn_radius*wind_speed.vy/vr, &(flight_vector_s->lat), &(flight_vector_s->lon)); - flight_vector_s->altitude = height; - map_projection_reproject(x_drop - turn_radius*wind_speed.vx/vr, y_drop - turn_radius*wind_speed.vy/vr, &(flight_vector_e)->lat, &(flight_vector_e)->lon); - flight_vector_e->altitude = height; + map_projection_reproject(x_drop + 2*turn_radius*wind_speed.vx/vr, y_drop + 2*turn_radius*wind_speed.vy/vr, &(flight_vector_s.lat), &(flight_vector_s.lon)); + flight_vector_s.altitude = height; + map_projection_reproject(x_drop - turn_radius*wind_speed.vx/vr, y_drop - turn_radius*wind_speed.vy/vr, &flight_vector_e.lat, &flight_vector_e.lon); + flight_vector_e.altitude = height; //warnx("Flight vector: starting point = %.7f %.7f , end point = %.7f %.7f", flight_vector_s.lat,flight_vector_s.lon,flight_vector_e.lat,flight_vector_e.lon); //////////////////////////////////////////////////////////////////// DEBUGGING // Drop Cancellation if terms are not met @@ -448,18 +448,29 @@ int bottle_drop_thread_main(int argc, char *argv[]) { // warnx("future_distance: %.2f, precision: %.2f", future_distance, precision); // } - // onboard_mission.count = 2; + /* Save WPs in datamanager */ + const size_t len = sizeof(struct mission_item_s); - // if (state_run && !state_drop) { - // onboard_mission.current_index = 0; - // } else { - // onboard_mission.current_index = -1; - // } + if (dm_write(DM_KEY_WAYPOINTS_ONBOARD, 0, DM_PERSIST_IN_FLIGHT_RESET, &flight_vector_s, len) != len) { + warnx("ERROR: could not save onboard WP"); + } + + if (dm_write(DM_KEY_WAYPOINTS_ONBOARD, 1, DM_PERSIST_IN_FLIGHT_RESET, &flight_vector_e, len) != len) { + warnx("ERROR: could not save onboard WP"); + } + + onboard_mission.count = 2; + + if (state_run && !state_drop) { + onboard_mission.current_index = 0; + } else { + onboard_mission.current_index = -1; + } - // if (counter % 10 ==0) - // warnx("Distance real: %.2f, distance_open_door: %.2f, angle to wind: %.2f", distance_real, distance_open_door, fabsf(_wrap_pi(globalpos.yaw-atan2f(wind_speed.vy,wind_speed.vx)+M_PI_F))); + if (counter % 10 ==0) + warnx("Distance real: %.2f, distance_open_door: %.2f, angle to wind: %.2f", distance_real, distance_open_door, fabsf(_wrap_pi(globalpos.yaw-atan2f(wind_speed.vy,wind_speed.vx)+M_PI_F))); - // orb_publish(ORB_ID(onboard_mission), onboard_mission_pub, &onboard_mission); + orb_publish(ORB_ID(onboard_mission), onboard_mission_pub, &onboard_mission); } From 73907d1ac00b59759137d71f18693228cc3e75c9 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sat, 14 Dec 2013 20:54:25 +0100 Subject: [PATCH 016/411] 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 e685c65365511341edb7cb4eded8d645643114a6 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sat, 14 Dec 2013 20:55:03 +0100 Subject: [PATCH 017/411] 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 9d4ba6e4f64d631c67a419518a8398debade4641 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 16 Dec 2013 16:59:24 +0100 Subject: [PATCH 018/411] 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 d422d443eebbba24d785f96de5b74fab143e6652 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sat, 29 Mar 2014 12:14:57 +0100 Subject: [PATCH 019/411] bottle_drop: started rewrite in C++ --- src/modules/bottle_drop/bottle_drop.cpp | 198 ++++++++++++++++++++++++ src/modules/bottle_drop/module.mk | 4 +- 2 files changed, 200 insertions(+), 2 deletions(-) create mode 100644 src/modules/bottle_drop/bottle_drop.cpp diff --git a/src/modules/bottle_drop/bottle_drop.cpp b/src/modules/bottle_drop/bottle_drop.cpp new file mode 100644 index 0000000000..7672ace9ed --- /dev/null +++ b/src/modules/bottle_drop/bottle_drop.cpp @@ -0,0 +1,198 @@ +/**************************************************************************** + * + * Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file bottle_drop.cpp + * + * Bottle drop module for Outback Challenge 2014, Team Swiss Fang + * + * @author Dominik Juchli + * @author Julian Oes + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + + +/** + * bottle_drop app start / stop handling function + * + * @ingroup apps + */ +extern "C" __EXPORT int bottle_drop_main(int argc, char *argv[]); + +class BottleDrop +{ +public: + /** + * Constructor + */ + BottleDrop(); + + /** + * Destructor, also kills task. + */ + ~BottleDrop(); + + /** + * Start the task. + * + * @return OK on success. + */ + int start(); + + /** + * Display status. + */ + void status(); + +private: + bool _task_should_exit; /**< if true, sensor task should exit */ + int _main_task; /**< task handle for sensor task */ + + int _mavlink_fd; +}; + +namespace bottle_drop +{ +BottleDrop *g_bottle_drop; +} + +BottleDrop::BottleDrop() : + + _task_should_exit(false), + _main_task(-1), + _mavlink_fd(-1) +{ +} + +BottleDrop::~BottleDrop() +{ + if (_main_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(_main_task); + break; + } + } while (_main_task != -1); + } + + bottle_drop::g_bottle_drop = nullptr; +} + +int BottleDrop::start() +{ +} + + +void BottleDrop::status() +{ +} + +static void usage() +{ + errx(1, "usage: bottle_drop {start|stop|status}"); +} + +int bottle_drop_main(int argc, char *argv[]) +{ + if (argc < 2) { + usage(); + } + + if (!strcmp(argv[1], "start")) { + + if (bottle_drop::g_bottle_drop != nullptr) { + errx(1, "already running"); + } + + bottle_drop::g_bottle_drop = new BottleDrop; + + if (bottle_drop::g_bottle_drop == nullptr) { + errx(1, "alloc failed"); + } + + if (OK != bottle_drop::g_bottle_drop->start()) { + delete bottle_drop::g_bottle_drop; + bottle_drop::g_bottle_drop = nullptr; + err(1, "start failed"); + } + + return 0; + } + + if (bottle_drop::g_bottle_drop == nullptr) + errx(1, "not running"); + + if (!strcmp(argv[1], "stop")) { + delete bottle_drop::g_bottle_drop; + bottle_drop::g_bottle_drop = nullptr; + + } else if (!strcmp(argv[1], "status")) { + bottle_drop::g_bottle_drop->status(); + } else { + usage(); + } + + return 0; +} diff --git a/src/modules/bottle_drop/module.mk b/src/modules/bottle_drop/module.mk index 222858b277..bb1f6c6b5d 100644 --- a/src/modules/bottle_drop/module.mk +++ b/src/modules/bottle_drop/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 @@ -37,4 +37,4 @@ MODULE_COMMAND = bottle_drop -SRCS = bottle_drop.c +SRCS = bottle_drop.cpp From 9da8e249fd2d1504b01571965c4dd5acdbf77ff1 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sat, 29 Mar 2014 20:36:39 +0100 Subject: [PATCH 020/411] bottle_drop: added simple commands to drop bottle --- src/modules/bottle_drop/bottle_drop.cpp | 209 +++++++++++++++++++++++- 1 file changed, 202 insertions(+), 7 deletions(-) diff --git a/src/modules/bottle_drop/bottle_drop.cpp b/src/modules/bottle_drop/bottle_drop.cpp index 7672ace9ed..1dcad21448 100644 --- a/src/modules/bottle_drop/bottle_drop.cpp +++ b/src/modules/bottle_drop/bottle_drop.cpp @@ -55,7 +55,9 @@ #include #include #include -#include +#include +#include +#include #include #include #include @@ -96,10 +98,30 @@ public: void status(); private: - bool _task_should_exit; /**< if true, sensor task should exit */ - int _main_task; /**< task handle for sensor task */ - + bool _task_should_exit; /**< if true, task should exit */ + int _main_task; /**< handle for task */ int _mavlink_fd; + + int _command_sub; + struct vehicle_command_s _command; + + orb_advert_t _actuator_pub; + struct actuator_controls_s _actuators; + + bool _open_door; + bool _drop; + hrt_abstime _doors_opened; + + void task_main(); + + void handle_command(struct vehicle_command_s *cmd); + + void answer_command(struct vehicle_command_s *cmd, enum VEHICLE_CMD_RESULT result); + + /** + * Shim for calling task_main from task_create. + */ + static void task_main_trampoline(int argc, char *argv[]); }; namespace bottle_drop @@ -111,7 +133,14 @@ BottleDrop::BottleDrop() : _task_should_exit(false), _main_task(-1), - _mavlink_fd(-1) + _mavlink_fd(-1), + _command_sub(-1), + _command({}), + _actuator_pub(-1), + _actuators({}), + _open_door(false), + _drop(false), + _doors_opened(0) { } @@ -140,13 +169,179 @@ BottleDrop::~BottleDrop() bottle_drop::g_bottle_drop = nullptr; } -int BottleDrop::start() +int +BottleDrop::start() { + ASSERT(_main_task == -1); + + /* start the task */ + _main_task = task_spawn_cmd("bottle_drop", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 2048, + (main_t)&BottleDrop::task_main_trampoline, + nullptr); + + if (_main_task < 0) { + warn("task start failed"); + return -errno; + } + + return OK; } -void BottleDrop::status() +void +BottleDrop::status() { + warnx("Doors: %s", _open_door ? "OPEN" : "CLOSED"); + warnx("Dropping: %s", _drop ? "YES" : "NO"); +} + +void +BottleDrop::task_main() +{ + /* inform about start */ + warnx("Initializing.."); + fflush(stdout); + + _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); + mavlink_log_info(_mavlink_fd, "[bottle_drop] started"); + + _command_sub = orb_subscribe(ORB_ID(vehicle_command)); + + /* wakeup source(s) */ + struct pollfd fds[1]; + + /* Setup of loop */ + fds[0].fd = _command_sub; + fds[0].events = POLLIN; + + while (!_task_should_exit) { + + /* wait for up to 100ms for data */ + int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 50); + + /* 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; + } + + /* vehicle commands updated */ + if (fds[0].revents & POLLIN) { + orb_copy(ORB_ID(vehicle_command), _command_sub, &_command); + handle_command(&_command); + } + + /* update door actuators */ + if (_open_door) { + _actuators.control[0] = -1.0f; + _actuators.control[1] = 1.0f; + + if (_doors_opened == 0) { + _doors_opened = hrt_absolute_time(); + } + } else { + _actuators.control[0] = 0.5f; + _actuators.control[1] = -0.5f; + + _doors_opened = 0; + } + + /* update drop actuator, wait 0.5s until the doors are open before dropping */ + if (_drop && _doors_opened != 0 && hrt_elapsed_time(&_doors_opened) > 500000) { + _actuators.control[2] = 0.5f; + } else { + _actuators.control[2] = -0.5f; + } + + /* 2s after drop, reset and close everything again */ + if (_drop && _doors_opened != 0 && hrt_elapsed_time(&_doors_opened) > 2000000) { + _open_door = false; + _drop = false; + } + + /* lazily publish _actuators only once available */ + if (_actuator_pub > 0) { + orb_publish(ORB_ID(actuator_controls_1), _actuator_pub, &_actuators); + + } else { + _actuator_pub = orb_advertise(ORB_ID(actuator_controls_1), &_actuators); + } + } + + warnx("exiting."); + + _main_task = -1; + _exit(0); +} + +void +BottleDrop::handle_command(struct vehicle_command_s *cmd) +{ + switch (cmd->command) { + case VEHICLE_CMD_CUSTOM_0: + + /* + * param1 and param2 set to 1: open and drop + * param1 set to 1: open + * else: close (and don't drop) + */ + if (cmd->param1 > 0.5f && cmd->param2 > 0.5f) { + _open_door = true; + _drop = true; + mavlink_log_info(_mavlink_fd, "#audio: drop bottle"); + } + else if (cmd->param1 > 0.5f) { + _open_door = true; + _drop = false; + mavlink_log_info(_mavlink_fd, "#audio: open doors"); + } else { + _open_door = false; + _drop = false; + mavlink_log_info(_mavlink_fd, "#audio: close doors"); + } + + answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); + break; + default: + break; + } +} + +void +BottleDrop::answer_command(struct vehicle_command_s *cmd, enum VEHICLE_CMD_RESULT result) +{ + switch (result) { + case VEHICLE_CMD_RESULT_ACCEPTED: + break; + + case VEHICLE_CMD_RESULT_DENIED: + mavlink_log_critical(_mavlink_fd, "#audio: command denied: %u", cmd->command); + break; + + case VEHICLE_CMD_RESULT_FAILED: + mavlink_log_critical(_mavlink_fd, "#audio: command failed: %u", cmd->command); + break; + + case VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED: + mavlink_log_critical(_mavlink_fd, "#audio: command temporarily rejected: %u", cmd->command); + break; + + case VEHICLE_CMD_RESULT_UNSUPPORTED: + mavlink_log_critical(_mavlink_fd, "#audio: command unsupported: %u", cmd->command); + break; + + default: + break; + } +} + +void +BottleDrop::task_main_trampoline(int argc, char *argv[]) +{ + bottle_drop::g_bottle_drop->task_main(); } static void usage() From 947b09a1209e5fd0c1e6e6cbe6a5820d15c83809 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sat, 29 Mar 2014 20:37:16 +0100 Subject: [PATCH 021/411] commander: don't report unsupported commands --- src/modules/commander/commander.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index cf7ba757e2..46ca046614 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -588,8 +588,8 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe break; default: - /* warn about unsupported commands */ - answer_command(*cmd, VEHICLE_CMD_RESULT_UNSUPPORTED); + /* don't warn about unsupported commands, maybe another app supports it */ +// answer_command(*cmd, VEHICLE_CMD_RESULT_UNSUPPORTED); break; } From 5e51812c8b1cb9af57145eeb85705e1f914aa77c Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sat, 29 Mar 2014 20:37:58 +0100 Subject: [PATCH 022/411] fw_att_control: workaround, don't publish to actuator_1 --- .../fw_att_control/fw_att_control_main.cpp | 24 +++++++++---------- 1 file changed, 12 insertions(+), 12 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 f139c25f48..111f56a902 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -832,18 +832,18 @@ 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); - } +// 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); +// } } From 1d75f3eb8a1dba5b919c417b27a44e16ddcb0d32 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sat, 29 Mar 2014 20:38:27 +0100 Subject: [PATCH 023/411] vehicle_command topic: added CUSOTM_0 as seen in QGC --- src/modules/uORB/topics/vehicle_command.h | 1 + 1 file changed, 1 insertion(+) diff --git a/src/modules/uORB/topics/vehicle_command.h b/src/modules/uORB/topics/vehicle_command.h index e6e4743c6a..9f1bbb3177 100644 --- a/src/modules/uORB/topics/vehicle_command.h +++ b/src/modules/uORB/topics/vehicle_command.h @@ -53,6 +53,7 @@ */ enum VEHICLE_CMD { + VEHICLE_CMD_CUSTOM_0=0, /* XXX: added because this exists in QGroundControl */ VEHICLE_CMD_NAV_WAYPOINT=16, /* Navigate to MISSION. |Hold time in decimal seconds. (ignored by fixed wing, time to stay at MISSION for rotary wing)| Acceptance radius in meters (if the sphere with this radius is hit, the MISSION counts as reached)| 0 to pass through the WP, if > 0 radius in meters to pass by WP. Positive value for clockwise orbit, negative value for counter-clockwise orbit. Allows trajectory control.| Desired yaw angle at MISSION (rotary wing)| Latitude| Longitude| Altitude| */ VEHICLE_CMD_NAV_LOITER_UNLIM=17, /* Loiter around this MISSION an unlimited amount of time |Empty| Empty| Radius around MISSION, in meters. If positive loiter clockwise, else counter-clockwise| Desired yaw angle.| Latitude| Longitude| Altitude| */ VEHICLE_CMD_NAV_LOITER_TURNS=18, /* Loiter around this MISSION for X turns |Turns| Empty| Radius around MISSION, in meters. If positive loiter clockwise, else counter-clockwise| Desired yaw angle.| Latitude| Longitude| Altitude| */ From eb4e250da88cf519850a53266fffbd06fd4a0872 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sun, 30 Mar 2014 15:00:37 +0200 Subject: [PATCH 024/411] Startup script: added viper script --- ROMFS/px4fmu_common/init.d/3035_viper | 10 ++++++++++ ROMFS/px4fmu_common/init.d/rc.autostart | 5 +++++ 2 files changed, 15 insertions(+) create mode 100644 ROMFS/px4fmu_common/init.d/3035_viper diff --git a/ROMFS/px4fmu_common/init.d/3035_viper b/ROMFS/px4fmu_common/init.d/3035_viper new file mode 100644 index 0000000000..9c8f8a585e --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/3035_viper @@ -0,0 +1,10 @@ +#!nsh +# +# Viper +# +# Simon Wilks +# + +sh /etc/init.d/rc.fw_defaults + +set MIXER FMU_FX79 \ No newline at end of file diff --git a/ROMFS/px4fmu_common/init.d/rc.autostart b/ROMFS/px4fmu_common/init.d/rc.autostart index 5ec735d393..6c1329c109 100644 --- a/ROMFS/px4fmu_common/init.d/rc.autostart +++ b/ROMFS/px4fmu_common/init.d/rc.autostart @@ -103,6 +103,11 @@ then sh /etc/init.d/3034_fx79 fi +if param compare SYS_AUTOSTART 3035 35 +then + sh /etc/init.d/3035_viper +fi + if param compare SYS_AUTOSTART 3100 then sh /etc/init.d/3100_tbs_caipirinha From 64148a9e2af255357df8aeae06a4ad2ef972ffec Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sun, 30 Mar 2014 15:01:07 +0200 Subject: [PATCH 025/411] bottle_drop: changed servo travels to match Simon's viper --- src/modules/bottle_drop/bottle_drop.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/modules/bottle_drop/bottle_drop.cpp b/src/modules/bottle_drop/bottle_drop.cpp index 1dcad21448..ce760adc35 100644 --- a/src/modules/bottle_drop/bottle_drop.cpp +++ b/src/modules/bottle_drop/bottle_drop.cpp @@ -243,21 +243,21 @@ BottleDrop::task_main() _doors_opened = hrt_absolute_time(); } } else { - _actuators.control[0] = 0.5f; - _actuators.control[1] = -0.5f; + _actuators.control[0] = 1.0f; + _actuators.control[1] = -1.0f; _doors_opened = 0; } /* update drop actuator, wait 0.5s until the doors are open before dropping */ if (_drop && _doors_opened != 0 && hrt_elapsed_time(&_doors_opened) > 500000) { - _actuators.control[2] = 0.5f; - } else { _actuators.control[2] = -0.5f; + } else { + _actuators.control[2] = 0.5f; } - /* 2s after drop, reset and close everything again */ - if (_drop && _doors_opened != 0 && hrt_elapsed_time(&_doors_opened) > 2000000) { + /* 20s after drop, reset and close everything again */ + if (_drop && _doors_opened != 0 && hrt_elapsed_time(&_doors_opened) > 20000000) { _open_door = false; _drop = false; } From f24c5184e8c1716795f10980a8c3b383c463facd Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sun, 30 Mar 2014 15:01:31 +0200 Subject: [PATCH 026/411] bottle_drop: hack to start bottle drop --- ROMFS/px4fmu_common/init.d/rc.fw_apps | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/ROMFS/px4fmu_common/init.d/rc.fw_apps b/ROMFS/px4fmu_common/init.d/rc.fw_apps index 429abc5ec8..bb17d44eba 100644 --- a/ROMFS/px4fmu_common/init.d/rc.fw_apps +++ b/ROMFS/px4fmu_common/init.d/rc.fw_apps @@ -13,3 +13,7 @@ fw_att_pos_estimator start # fw_att_control start fw_pos_control_l1 start + +bottle_drop start +fmu mode_pwm +mixer load /dev/px4fmu /etc/mixers/FMU_pass.mix \ No newline at end of file From 839710daf841a9528a58e915f8b04484bf54e7dc Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 3 Jul 2014 14:42:59 +0200 Subject: [PATCH 027/411] Update offboard control uorb topic Work towards supporting the new external setpoint mavlink messages --- src/modules/commander/commander.cpp | 16 ++---- src/modules/mavlink/mavlink_receiver.cpp | 28 +++++++++ src/modules/mavlink/mavlink_receiver.h | 1 + src/modules/navigator/offboard.cpp | 57 ++++++++++++------- .../uORB/topics/offboard_control_setpoint.h | 36 +++++++++--- 5 files changed, 97 insertions(+), 41 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 38297745df..7185eebc41 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -946,7 +946,7 @@ int commander_thread_main(int argc, char *argv[]) } /* Perform system checks (again) once params are loaded and MAVLink is up. */ - if (!system_checked && mavlink_fd && + if (!system_checked && mavlink_fd && (telemetry.heartbeat_time > 0) && (hrt_elapsed_time(&telemetry.heartbeat_time) < 1 * 1000 * 1000)) { @@ -1847,21 +1847,17 @@ set_control_mode() control_mode.flag_control_position_enabled = false; control_mode.flag_control_velocity_enabled = false; break; - case OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY: - control_mode.flag_control_rates_enabled = true; - control_mode.flag_control_attitude_enabled = true; - control_mode.flag_control_altitude_enabled = true; /* XXX: hack for now */ - control_mode.flag_control_climb_rate_enabled = true; - control_mode.flag_control_position_enabled = true; /* XXX: hack for now */ - control_mode.flag_control_velocity_enabled = true; - break; - case OFFBOARD_CONTROL_MODE_DIRECT_POSITION: + case OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED: + case OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_OFFSET_NED: + case OFFBOARD_CONTROL_MODE_DIRECT_BODY_NED: + case OFFBOARD_CONTROL_MODE_DIRECT_BODY_OFFSET_NED: 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; + //XXX: the flags could depend on sp_offboard.ignore break; default: control_mode.flag_control_rates_enabled = false; diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 3747ad3ba2..73222637fe 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -154,6 +154,10 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) handle_message_quad_swarm_roll_pitch_yaw_thrust(msg); break; + case MAVLINK_MSG_ID_LOCAL_NED_POSITION_SETPOINT_EXTERNAL: + handle_message_local_ned_position_setpoint_external(msg); + break; + case MAVLINK_MSG_ID_RADIO_STATUS: handle_message_radio_status(msg); break; @@ -393,6 +397,30 @@ MavlinkReceiver::handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message } } +void +MavlinkReceiver::handle_message_local_ned_position_setpoint_external(mavlink_message_t *msg) +{ + mavlink_local_ned_position_setpoint_external_t local_ned_position_setpoint_external; + mavlink_msg_local_ned_position_setpoint_external_decode(msg, &local_ned_position_setpoint_external); + + struct offboard_control_setpoint_s offboard_control_sp; + memset(&offboard_control_sp, 0, sizeof(offboard_control_sp)); + + //XXX check if target system/component is correct + + /* convert mavlink type (local, NED) to uORB offboard control struct */ + //XXX do the conversion + // + offboard_control_sp.timestamp = hrt_absolute_time(); + + if (_offboard_control_sp_pub < 0) { + _offboard_control_sp_pub = orb_advertise(ORB_ID(offboard_control_setpoint), &offboard_control_sp); + + } else { + orb_publish(ORB_ID(offboard_control_setpoint), _offboard_control_sp_pub, &offboard_control_sp); + } +} + void MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg) { diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 65ef884a24..658f515626 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -115,6 +115,7 @@ private: void handle_message_set_mode(mavlink_message_t *msg); void handle_message_vicon_position_estimate(mavlink_message_t *msg); void handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message_t *msg); + void handle_message_local_ned_position_setpoint_external(mavlink_message_t *msg); void handle_message_radio_status(mavlink_message_t *msg); void handle_message_manual_control(mavlink_message_t *msg); void handle_message_heartbeat(mavlink_message_t *msg); diff --git a/src/modules/navigator/offboard.cpp b/src/modules/navigator/offboard.cpp index ef7d11a039..27ce46a1d6 100644 --- a/src/modules/navigator/offboard.cpp +++ b/src/modules/navigator/offboard.cpp @@ -79,33 +79,46 @@ Offboard::on_active(struct position_setpoint_triplet_s *pos_sp_triplet) bool changed = false; /* copy offboard setpoints to the corresponding topics */ - if (_navigator->get_control_mode()->flag_control_position_enabled - && _offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_POSITION) { - /* position control */ - pos_sp_triplet->current.x = _offboard_control_sp.p1; - pos_sp_triplet->current.y = _offboard_control_sp.p2; - pos_sp_triplet->current.yaw = _offboard_control_sp.p3; - pos_sp_triplet->current.z = -_offboard_control_sp.p4; + if (_offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED) { + /* We accept position control only if none of the directions is ignored (as pos_sp_triplet does not + * support deactivation of individual directions) */ + if (_navigator->get_control_mode()->flag_control_position_enabled && + (!_offboard_control_sp.ignore[0] && + !_offboard_control_sp.ignore[1] && + !_offboard_control_sp.ignore[2])) { + /* position control */ + pos_sp_triplet->current.x = _offboard_control_sp.p1; + pos_sp_triplet->current.y = _offboard_control_sp.p2; + pos_sp_triplet->current.yaw = _offboard_control_sp.p3; + pos_sp_triplet->current.z = -_offboard_control_sp.p4; - pos_sp_triplet->current.type = SETPOINT_TYPE_OFFBOARD; - pos_sp_triplet->current.valid = true; - pos_sp_triplet->current.position_valid = true; + pos_sp_triplet->current.type = SETPOINT_TYPE_OFFBOARD; + pos_sp_triplet->current.valid = true; + pos_sp_triplet->current.position_valid = true; - changed = true; + changed = true; - } else if (_navigator->get_control_mode()->flag_control_velocity_enabled - && _offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY) { - /* velocity control */ - pos_sp_triplet->current.vx = _offboard_control_sp.p2; - pos_sp_triplet->current.vy = _offboard_control_sp.p1; - pos_sp_triplet->current.yawspeed = _offboard_control_sp.p3; - pos_sp_triplet->current.vz = _offboard_control_sp.p4; + } + /* We accept velocity control only if none of the directions is ignored (as pos_sp_triplet does not + * support deactivation of individual directions) */ + if (_navigator->get_control_mode()->flag_control_velocity_enabled && + (!_offboard_control_sp.ignore[3] && + !_offboard_control_sp.ignore[4] && + !_offboard_control_sp.ignore[5])) { + /* velocity control */ + pos_sp_triplet->current.vx = _offboard_control_sp.p2; + pos_sp_triplet->current.vy = _offboard_control_sp.p1; + pos_sp_triplet->current.yawspeed = _offboard_control_sp.p3; + pos_sp_triplet->current.vz = _offboard_control_sp.p4; - pos_sp_triplet->current.type = SETPOINT_TYPE_OFFBOARD; - pos_sp_triplet->current.valid = true; - pos_sp_triplet->current.velocity_valid = true; + pos_sp_triplet->current.type = SETPOINT_TYPE_OFFBOARD; + pos_sp_triplet->current.valid = true; + pos_sp_triplet->current.velocity_valid = true; - changed = true; + changed = true; + } + + //XXX: map acceleration setpoint once supported in setpoint triplet } return changed; diff --git a/src/modules/uORB/topics/offboard_control_setpoint.h b/src/modules/uORB/topics/offboard_control_setpoint.h index d7b131e3c3..427ccc6c8c 100644 --- a/src/modules/uORB/topics/offboard_control_setpoint.h +++ b/src/modules/uORB/topics/offboard_control_setpoint.h @@ -53,11 +53,22 @@ enum OFFBOARD_CONTROL_MODE { OFFBOARD_CONTROL_MODE_DIRECT = 0, OFFBOARD_CONTROL_MODE_DIRECT_RATES = 1, OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE = 2, - OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY = 3, - OFFBOARD_CONTROL_MODE_DIRECT_POSITION = 4, - OFFBOARD_CONTROL_MODE_ATT_YAW_RATE = 5, - OFFBOARD_CONTROL_MODE_ATT_YAW_POS = 6, - OFFBOARD_CONTROL_MODE_MULTIROTOR_SIMPLE = 7, /**< roll / pitch rotated aligned to the takeoff orientation, throttle stabilized, yaw pos */ + OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED = 3, + OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_OFFSET_NED = 4, + OFFBOARD_CONTROL_MODE_DIRECT_BODY_NED = 5, + OFFBOARD_CONTROL_MODE_DIRECT_BODY_OFFSET_NED = 6, + OFFBOARD_CONTROL_MODE_DIRECT_GLOBAL = 7, + OFFBOARD_CONTROL_MODE_ATT_YAW_RATE = 8, + OFFBOARD_CONTROL_MODE_ATT_YAW_POS = 9, + OFFBOARD_CONTROL_MODE_MULTIROTOR_SIMPLE = 10, /**< roll / pitch rotated aligned to the takeoff orientation, throttle stabilized, yaw pos */ +}; + +enum OFFBOARD_CONTROL_FRAME { + OFFBOARD_CONTROL_FRAME_LOCAL_NED = 0, + OFFBOARD_CONTROL_FRAME_LOCAL_OFFSET_NED = 1, + OFFBOARD_CONTROL_FRAME_BODY_NED = 2, + OFFBOARD_CONTROL_FRAME_BODY_OFFSET_NED = 3, + OFFBOARD_CONTROL_FRAME_GLOBAL = 4 }; /** @@ -70,10 +81,17 @@ struct offboard_control_setpoint_s { enum OFFBOARD_CONTROL_MODE mode; /**< The current control inputs mode */ - float p1; /**< ailerons roll / roll rate input */ - float p2; /**< elevator / pitch / pitch rate */ - float p3; /**< rudder / yaw rate / yaw */ - float p4; /**< throttle / collective thrust / altitude */ + double p1; /**< ailerons / roll / x pos / lat */ + double p2; /**< elevator / pitch / y pos / lon */ + float p3; /**< rudder / yaw / z pos / alt */ + float p4; /**< throttle / x vel */ + float p5; /**< roll rate / y vel */ + float p6; /**< pitch rate / z vel */ + float p7; /**< yaw rate / x acc */ + float p8; /**< y acc */ + float p9; /**< z acc */ + + bool ignore[9]; /**< if field i is set to true, pi should be ignored */ float override_mode_switch; From cbd602c27cb081502a2f1f1d910abdd1c2d7be13 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 3 Jul 2014 15:00:32 +0200 Subject: [PATCH 028/411] check sysid and compid for external setpoint --- src/modules/mavlink/mavlink_receiver.cpp | 20 +++++++++++--------- 1 file changed, 11 insertions(+), 9 deletions(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 73222637fe..f2b9a4e1e8 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -406,18 +406,20 @@ MavlinkReceiver::handle_message_local_ned_position_setpoint_external(mavlink_mes struct offboard_control_setpoint_s offboard_control_sp; memset(&offboard_control_sp, 0, sizeof(offboard_control_sp)); - //XXX check if target system/component is correct + if (mavlink_system.sysid == local_ned_position_setpoint_external.target_system && + mavlink_system.compid == local_ned_position_setpoint_external.target_component) { - /* convert mavlink type (local, NED) to uORB offboard control struct */ - //XXX do the conversion - // - offboard_control_sp.timestamp = hrt_absolute_time(); + /* convert mavlink type (local, NED) to uORB offboard control struct */ + //XXX do the conversion + // + offboard_control_sp.timestamp = hrt_absolute_time(); - if (_offboard_control_sp_pub < 0) { - _offboard_control_sp_pub = orb_advertise(ORB_ID(offboard_control_setpoint), &offboard_control_sp); + if (_offboard_control_sp_pub < 0) { + _offboard_control_sp_pub = orb_advertise(ORB_ID(offboard_control_setpoint), &offboard_control_sp); - } else { - orb_publish(ORB_ID(offboard_control_setpoint), _offboard_control_sp_pub, &offboard_control_sp); + } else { + orb_publish(ORB_ID(offboard_control_setpoint), _offboard_control_sp_pub, &offboard_control_sp); + } } } From 822403e34b5e1b4adf15783b5bd701e1f52484fe Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 3 Jul 2014 16:20:58 +0200 Subject: [PATCH 029/411] uorb offboard control topic: add force sp flag The flag has the same meaning as bit 10 of type_mask in MAVLINK_MSG_ID_LOCAL_NED_POSITION_SETPOINT_EXTERNAL --- src/modules/uORB/topics/offboard_control_setpoint.h | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/uORB/topics/offboard_control_setpoint.h b/src/modules/uORB/topics/offboard_control_setpoint.h index 427ccc6c8c..2741f0c073 100644 --- a/src/modules/uORB/topics/offboard_control_setpoint.h +++ b/src/modules/uORB/topics/offboard_control_setpoint.h @@ -91,7 +91,9 @@ struct offboard_control_setpoint_s { float p8; /**< y acc */ float p9; /**< z acc */ + //XXX: use a bitmask with wrapper functions instead bool ignore[9]; /**< if field i is set to true, pi should be ignored */ + bool isForceSetpoint; /**< if set to true: p7 to p9 should be interpreted as force instead of acceleration */ float override_mode_switch; From 31474a75fd1ad6e86909bf4af2b484d1decd932a Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 3 Jul 2014 16:22:58 +0200 Subject: [PATCH 030/411] parsing of MAVLINK_MSG_ID_LOCAL_NED_POSITION_SETPOINT_EXTERNAL --- src/modules/mavlink/mavlink_receiver.cpp | 34 ++++++++++++++++++++++-- 1 file changed, 32 insertions(+), 2 deletions(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index f2b9a4e1e8..c8e8c6d513 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -410,8 +410,38 @@ MavlinkReceiver::handle_message_local_ned_position_setpoint_external(mavlink_mes mavlink_system.compid == local_ned_position_setpoint_external.target_component) { /* convert mavlink type (local, NED) to uORB offboard control struct */ - //XXX do the conversion - // + switch (local_ned_position_setpoint_external.coordinate_frame) { + case MAV_FRAME_LOCAL_NED: + offboard_control_sp.mode = OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED; + break; + case MAV_FRAME_LOCAL_OFFSET_NED: + offboard_control_sp.mode = OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_OFFSET_NED; + break; + case MAV_FRAME_BODY_NED: + offboard_control_sp.mode = OFFBOARD_CONTROL_MODE_DIRECT_BODY_NED; + break; + case MAV_FRAME_BODY_OFFSET_NED: + offboard_control_sp.mode = OFFBOARD_CONTROL_MODE_DIRECT_BODY_OFFSET_NED; + break; + default: + /* invalid setpoint, avoid publishing */ + return; + } + offboard_control_sp.p1 = local_ned_position_setpoint_external.x; + offboard_control_sp.p2 = local_ned_position_setpoint_external.y; + offboard_control_sp.p3 = local_ned_position_setpoint_external.z; + offboard_control_sp.p4 = local_ned_position_setpoint_external.vx; + offboard_control_sp.p5 = local_ned_position_setpoint_external.vy; + offboard_control_sp.p6 = local_ned_position_setpoint_external.vz; + offboard_control_sp.p7 = local_ned_position_setpoint_external.afx; + offboard_control_sp.p8 = local_ned_position_setpoint_external.afy; + offboard_control_sp.p9 = local_ned_position_setpoint_external.afz; + offboard_control_sp.isForceSetpoint = (bool)(local_ned_position_setpoint_external.type_mask & (1 << 9)); + for (int i = 0; i < 9; i++) { + offboard_control_sp.ignore[i] = (bool)(local_ned_position_setpoint_external.type_mask & (1 << i)); + } + + offboard_control_sp.timestamp = hrt_absolute_time(); if (_offboard_control_sp_pub < 0) { From 2d26e913921ce80e509bd79462319d09d12c2171 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 7 Jul 2014 13:00:45 +0200 Subject: [PATCH 031/411] WIP, change uorb offboard control sp topic --- src/modules/mavlink/mavlink_receiver.cpp | 30 ++++++++-------- src/modules/navigator/offboard.cpp | 30 ++++++++-------- .../uORB/topics/offboard_control_setpoint.h | 34 +++++++++++++------ 3 files changed, 55 insertions(+), 39 deletions(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index b72bfa858c..e1a74f599b 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -379,10 +379,11 @@ MavlinkReceiver::handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message memset(&offboard_control_sp, 0, sizeof(offboard_control_sp)); /* Convert values * 1000 back */ - offboard_control_sp.p1 = (float)swarm_offboard_control.roll[mavlink_system.sysid - 1] / 1000.0f; - offboard_control_sp.p2 = (float)swarm_offboard_control.pitch[mavlink_system.sysid - 1] / 1000.0f; - offboard_control_sp.p3 = (float)swarm_offboard_control.yaw[mavlink_system.sysid - 1] / 1000.0f; - offboard_control_sp.p4 = (float)swarm_offboard_control.thrust[mavlink_system.sysid - 1] / 1000.0f; + //XXX: convert to quaternion + //offboard_control_sp.p1 = (float)swarm_offboard_control.roll[mavlink_system.sysid - 1] / 1000.0f; + //offboard_control_sp.p2 = (float)swarm_offboard_control.pitch[mavlink_system.sysid - 1] / 1000.0f; + //offboard_control_sp.p3 = (float)swarm_offboard_control.yaw[mavlink_system.sysid - 1] / 1000.0f; + //offboard_control_sp.p4 = (float)swarm_offboard_control.thrust[mavlink_system.sysid - 1] / 1000.0f; offboard_control_sp.mode = (enum OFFBOARD_CONTROL_MODE)swarm_offboard_control.mode; @@ -427,18 +428,19 @@ MavlinkReceiver::handle_message_local_ned_position_setpoint_external(mavlink_mes /* invalid setpoint, avoid publishing */ return; } - offboard_control_sp.p1 = local_ned_position_setpoint_external.x; - offboard_control_sp.p2 = local_ned_position_setpoint_external.y; - offboard_control_sp.p3 = local_ned_position_setpoint_external.z; - offboard_control_sp.p4 = local_ned_position_setpoint_external.vx; - offboard_control_sp.p5 = local_ned_position_setpoint_external.vy; - offboard_control_sp.p6 = local_ned_position_setpoint_external.vz; - offboard_control_sp.p7 = local_ned_position_setpoint_external.afx; - offboard_control_sp.p8 = local_ned_position_setpoint_external.afy; - offboard_control_sp.p9 = local_ned_position_setpoint_external.afz; + offboard_control_sp.position[0] = local_ned_position_setpoint_external.x; + offboard_control_sp.position[1] = local_ned_position_setpoint_external.y; + offboard_control_sp.position[2] = local_ned_position_setpoint_external.z; + offboard_control_sp.velocity[0] = local_ned_position_setpoint_external.vx; + offboard_control_sp.velocity[1] = local_ned_position_setpoint_external.vy; + offboard_control_sp.velocity[2] = local_ned_position_setpoint_external.vz; + offboard_control_sp.acceleration[0] = local_ned_position_setpoint_external.afx; + offboard_control_sp.acceleration[1] = local_ned_position_setpoint_external.afy; + offboard_control_sp.acceleration[2] = local_ned_position_setpoint_external.afz; offboard_control_sp.isForceSetpoint = (bool)(local_ned_position_setpoint_external.type_mask & (1 << 9)); for (int i = 0; i < 9; i++) { - offboard_control_sp.ignore[i] = (bool)(local_ned_position_setpoint_external.type_mask & (1 << i)); + offboard_control_sp.ignore &= ~(local_ned_position_setpoint_external.type_mask & (1 << i)); + offboard_control_sp.ignore |= (local_ned_position_setpoint_external.type_mask & (1 << i)); } diff --git a/src/modules/navigator/offboard.cpp b/src/modules/navigator/offboard.cpp index 27ce46a1d6..71a14c029d 100644 --- a/src/modules/navigator/offboard.cpp +++ b/src/modules/navigator/offboard.cpp @@ -83,14 +83,15 @@ Offboard::on_active(struct position_setpoint_triplet_s *pos_sp_triplet) /* We accept position control only if none of the directions is ignored (as pos_sp_triplet does not * support deactivation of individual directions) */ if (_navigator->get_control_mode()->flag_control_position_enabled && - (!_offboard_control_sp.ignore[0] && - !_offboard_control_sp.ignore[1] && - !_offboard_control_sp.ignore[2])) { + (!offboard_control_sp_ignore_position(_offboard_control_sp, 0) && + !offboard_control_sp_ignore_position(_offboard_control_sp, 1) && + !offboard_control_sp_ignore_position(_offboard_control_sp, 2))) { /* position control */ - pos_sp_triplet->current.x = _offboard_control_sp.p1; - pos_sp_triplet->current.y = _offboard_control_sp.p2; - pos_sp_triplet->current.yaw = _offboard_control_sp.p3; - pos_sp_triplet->current.z = -_offboard_control_sp.p4; + pos_sp_triplet->current.x = _offboard_control_sp.position[0]; + pos_sp_triplet->current.y = _offboard_control_sp.position[1]; + //pos_sp_triplet->current.yaw = _offboard_control_sp.position[2]; + //XXX: copy yaw + pos_sp_triplet->current.z = -_offboard_control_sp.position[2]; pos_sp_triplet->current.type = SETPOINT_TYPE_OFFBOARD; pos_sp_triplet->current.valid = true; @@ -102,14 +103,15 @@ Offboard::on_active(struct position_setpoint_triplet_s *pos_sp_triplet) /* We accept velocity control only if none of the directions is ignored (as pos_sp_triplet does not * support deactivation of individual directions) */ if (_navigator->get_control_mode()->flag_control_velocity_enabled && - (!_offboard_control_sp.ignore[3] && - !_offboard_control_sp.ignore[4] && - !_offboard_control_sp.ignore[5])) { + (!offboard_control_sp_ignore_velocity(_offboard_control_sp, 0) && + !offboard_control_sp_ignore_velocity(_offboard_control_sp, 1) && + !offboard_control_sp_ignore_velocity(_offboard_control_sp, 2))) { /* velocity control */ - pos_sp_triplet->current.vx = _offboard_control_sp.p2; - pos_sp_triplet->current.vy = _offboard_control_sp.p1; - pos_sp_triplet->current.yawspeed = _offboard_control_sp.p3; - pos_sp_triplet->current.vz = _offboard_control_sp.p4; + pos_sp_triplet->current.vx = _offboard_control_sp.velocity[0]; + pos_sp_triplet->current.vy = _offboard_control_sp.velocity[1]; +// pos_sp_triplet->current.yawspeed = _offboard_control_sp.velocity[; +// //XXX: copy yaw speed + pos_sp_triplet->current.vz = _offboard_control_sp.velocity[2]; pos_sp_triplet->current.type = SETPOINT_TYPE_OFFBOARD; pos_sp_triplet->current.valid = true; diff --git a/src/modules/uORB/topics/offboard_control_setpoint.h b/src/modules/uORB/topics/offboard_control_setpoint.h index 2741f0c073..688135533e 100644 --- a/src/modules/uORB/topics/offboard_control_setpoint.h +++ b/src/modules/uORB/topics/offboard_control_setpoint.h @@ -81,19 +81,15 @@ struct offboard_control_setpoint_s { enum OFFBOARD_CONTROL_MODE mode; /**< The current control inputs mode */ - double p1; /**< ailerons / roll / x pos / lat */ - double p2; /**< elevator / pitch / y pos / lon */ - float p3; /**< rudder / yaw / z pos / alt */ - float p4; /**< throttle / x vel */ - float p5; /**< roll rate / y vel */ - float p6; /**< pitch rate / z vel */ - float p7; /**< yaw rate / x acc */ - float p8; /**< y acc */ - float p9; /**< z acc */ + double position[3]; /**< lat, lon, alt / x, y, z */ + float velocity[3]; /**< x vel, y vel, z vel */ + float acceleration[3]; /**< x acc, y acc, z acc */ + float attitude[4]; /**< attitude of vehicle (quaternion) */ + float attitude_rate[3]; /**< body angular rates (x, y, z) */ //XXX: use a bitmask with wrapper functions instead - bool ignore[9]; /**< if field i is set to true, pi should be ignored */ - bool isForceSetpoint; /**< if set to true: p7 to p9 should be interpreted as force instead of acceleration */ + uint16_t ignore; /**< if field i is set to true, pi should be ignored */ + bool isForceSetpoint; /**< the acceleration vector should be interpreted as force */ float override_mode_switch; @@ -107,6 +103,22 @@ struct offboard_control_setpoint_s { * @} */ +inline bool offboard_control_sp_ignore_position(const struct offboard_control_setpoint_s &offboard_control_sp, int index) { + return (bool)(offboard_control_sp.ignore & (1 << index)); +} + +inline bool offboard_control_sp_ignore_velocity(const struct offboard_control_setpoint_s &offboard_control_sp, int index) { + return (bool)(offboard_control_sp.ignore & (1 << (3 + index))); +} + +inline bool offboard_control_sp_ignore_acceleration(const struct offboard_control_setpoint_s &offboard_control_sp, int index) { + return (bool)(offboard_control_sp.ignore & (1 << (6 + index))); +} + +inline bool offboard_control_sp_ignore_bodyrates(const struct offboard_control_setpoint_s &offboard_control_sp, int index) { + return (bool)(offboard_control_sp.ignore & (1 << (9 + index))); +} + /* register this as object request broker structure */ ORB_DECLARE(offboard_control_setpoint); From 2c74babafcc3eae4bd513185395ac754ada6d86a Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 8 Jul 2014 11:16:19 +0200 Subject: [PATCH 032/411] fix merge of mavlink submodule --- mavlink/include/mavlink/v1.0 | 1 + 1 file changed, 1 insertion(+) create mode 160000 mavlink/include/mavlink/v1.0 diff --git a/mavlink/include/mavlink/v1.0 b/mavlink/include/mavlink/v1.0 new file mode 160000 index 0000000000..3711190d23 --- /dev/null +++ b/mavlink/include/mavlink/v1.0 @@ -0,0 +1 @@ +Subproject commit 3711190d23d9928ea0687e00621c8d9ecf145f50 From 19017f100101dbf638bb91f7a520296f979ebb32 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 8 Jul 2014 13:29:59 +0200 Subject: [PATCH 033/411] remove comment --- src/modules/uORB/topics/offboard_control_setpoint.h | 1 - 1 file changed, 1 deletion(-) diff --git a/src/modules/uORB/topics/offboard_control_setpoint.h b/src/modules/uORB/topics/offboard_control_setpoint.h index 688135533e..809412fc09 100644 --- a/src/modules/uORB/topics/offboard_control_setpoint.h +++ b/src/modules/uORB/topics/offboard_control_setpoint.h @@ -87,7 +87,6 @@ struct offboard_control_setpoint_s { float attitude[4]; /**< attitude of vehicle (quaternion) */ float attitude_rate[3]; /**< body angular rates (x, y, z) */ - //XXX: use a bitmask with wrapper functions instead uint16_t ignore; /**< if field i is set to true, pi should be ignored */ bool isForceSetpoint; /**< the acceleration vector should be interpreted as force */ From cf4a11c7e7cfa524992a96d41d885da38ab95ebd Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 8 Jul 2014 13:47:53 +0200 Subject: [PATCH 034/411] fix merge errors in offboard.cpp --- src/modules/navigator/offboard.cpp | 19 +++++++++++-------- 1 file changed, 11 insertions(+), 8 deletions(-) diff --git a/src/modules/navigator/offboard.cpp b/src/modules/navigator/offboard.cpp index 71a14c029d..fc4d183cdf 100644 --- a/src/modules/navigator/offboard.cpp +++ b/src/modules/navigator/offboard.cpp @@ -67,17 +67,22 @@ Offboard::~Offboard() { } -bool -Offboard::on_active(struct position_setpoint_triplet_s *pos_sp_triplet) +void +Offboard::on_activation() { +} + +void +Offboard::on_active() +{ + struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); + bool updated; orb_check(_navigator->get_offboard_control_sp_sub(), &updated); if (updated) { update_offboard_control_setpoint(); } - bool changed = false; - /* copy offboard setpoints to the corresponding topics */ if (_offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED) { /* We accept position control only if none of the directions is ignored (as pos_sp_triplet does not @@ -97,8 +102,7 @@ Offboard::on_active(struct position_setpoint_triplet_s *pos_sp_triplet) pos_sp_triplet->current.valid = true; pos_sp_triplet->current.position_valid = true; - changed = true; - + _navigator->set_position_setpoint_triplet_updated(); } /* We accept velocity control only if none of the directions is ignored (as pos_sp_triplet does not * support deactivation of individual directions) */ @@ -117,13 +121,12 @@ Offboard::on_active(struct position_setpoint_triplet_s *pos_sp_triplet) pos_sp_triplet->current.valid = true; pos_sp_triplet->current.velocity_valid = true; - changed = true; + _navigator->set_position_setpoint_triplet_updated(); } //XXX: map acceleration setpoint once supported in setpoint triplet } - return changed; } void From af4d5c0a3c985007958e512db977832bc93b2553 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 10 Jul 2014 13:14:39 +0200 Subject: [PATCH 035/411] Added initial bits for ST24 decoding --- src/lib/rc/st24.c | 66 ++++++++++++++++++++++ src/lib/rc/st24.h | 140 ++++++++++++++++++++++++++++++++++++++++++++++ 2 files changed, 206 insertions(+) create mode 100644 src/lib/rc/st24.c create mode 100644 src/lib/rc/st24.h diff --git a/src/lib/rc/st24.c b/src/lib/rc/st24.c new file mode 100644 index 0000000000..8287a0453e --- /dev/null +++ b/src/lib/rc/st24.c @@ -0,0 +1,66 @@ +/**************************************************************************** + * + * Copyright (c) 2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/* + * @file st24.h + * + * RC protocol implementation for Yuneec ST24 transmitter. + * + * @author Lorenz Meier + */ + +uint8_t st24_common_crc8(uint8_t *ptr, uint8_t len) +{ + uint8_t i, crc ; + crc = 0; + + while (len--) { + for (i = 0x80; i != 0; i >>= 1) { + if ((crc & 0x80) != 0) { + crc <<= 1; + crc ^= 0x07; + + } else { + crc <<= 1; + } + + if ((*ptr & i) != 0) { + crc ^= 0x07; + } + } + + ptr++; + } + + return (crc); +} diff --git a/src/lib/rc/st24.h b/src/lib/rc/st24.h new file mode 100644 index 0000000000..1c92bad436 --- /dev/null +++ b/src/lib/rc/st24.h @@ -0,0 +1,140 @@ +/**************************************************************************** + * + * Copyright (c) 2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file st24.h + * + * RC protocol definition for Yuneec ST24 transmitter + * + * @author Lorenz Meier + */ + +#define ST24_DATA_LEN_MAX 64 + +enum { + ST24_PACKET_TYPE_CHANNELDATA12 = 0, + ST24_PACKET_TYPE_CHANNELDATA24, + ST24_PACKET_TYPE_TRANSMITTERGPSDATA +} ST24_PACKET_TYPE; + +#pragma pack(push, 1) +typedef struct { + uint8_t header1; ///< 0x55 for a valid packet + uint8_t header2; ///< 0x55 for a valid packet + uint8_t length; ///< length includes type, data, and crc = sizeof(type)+sizeof(data[payload_len])+sizeof(crc8) + uint8_t type; ///< from enum ST24_PACKET_TYPE + uint8_t st24_data[ST24_DATA_LEN_MAX]; + uint8_t crc8; ///< crc8 checksum, calculated by st24_common_crc8 and including fields length, type and st24_data +} ReceiverFcPacket; + +/** + * RC Channel data (12 channels). + * + * This is incoming from the ST24 + */ +typedef struct { + uint16_t t; ///< packet counter or clock + uint8_t rssi ///< signal strength + uint8_t packet_count; ///< Number of UART packets sent since reception of last RF frame (this tells something about age / rate) + uint8_t channel[18]; ///< channel data, 12 channels (12 bit numbers) +} ChannelData12; + +/** + * RC Channel data (12 channels). + * + */ +typedef struct { + uint16_t t; ///< packet counter or clock + uint8_t rssi ///< signal strength + uint8_t packet_count; ///< Number of UART packets sent since reception of last RF frame (this tells something about age / rate) + uint8_t channel[36]; ///< channel data, 24 channels (12 bit numbers) +} ChannelData24; + +/** + * Telemetry packet + * + * This is outgoing to the ST24 + * + * imuStatus: + * 8 bit total + * bits 0-2 for status + * - value 0 is FAILED + * - value 1 is INITIALIZING + * - value 2 is RUNNING + * - values 3 through 7 are reserved + * bits 3-7 are status for sensors (0 or 1) + * - mpu6050 + * - accelerometer + * - primary gyro x + * - primary gyro y + * - primary gyro z + * + * pressCompassStatus + * 8 bit total + * bits 0-3 for compass status + * - value 0 is FAILED + * - value 1 is INITIALIZING + * - value 2 is RUNNING + * - value 3 - 15 are reserved + * bits 4-7 for pressure status + * - value 0 is FAILED + * - value 1 is INITIALIZING + * - value 2 is RUNNING + * - value 3 - 15 are reserved + * + */ +typedef struct { + uint16_t t; ///< packet counter or clock + int32_t lat; ///< lattitude (degrees) +/- 90 deg + int32_t lon; ///< longitude (degrees) +/- 180 deg + int32_t alt; ///< 0.01m resolution, altitude (meters) + int16_t vx, vy, vz; ///< velocity 0.01m res, +/-320.00 North-East- Down + uint8_t nsat; /// Date: Thu, 10 Jul 2014 13:21:29 +0200 Subject: [PATCH 036/411] More protocol decoding skeleton code --- src/lib/rc/module.mk | 40 ++++++++++++++++++++++++++++++++++++++++ src/lib/rc/st24.c | 3 +++ src/lib/rc/st24.h | 14 +++++++++++++- 3 files changed, 56 insertions(+), 1 deletion(-) create mode 100644 src/lib/rc/module.mk diff --git a/src/lib/rc/module.mk b/src/lib/rc/module.mk new file mode 100644 index 0000000000..e089c69658 --- /dev/null +++ b/src/lib/rc/module.mk @@ -0,0 +1,40 @@ +############################################################################ +# +# Copyright (c) 2014 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +# +# Yuntec ST24 transmitter protocol decoder +# + +SRCS = st24.c + +MAXOPTIMIZATION = -Os diff --git a/src/lib/rc/st24.c b/src/lib/rc/st24.c index 8287a0453e..bae28c85e3 100644 --- a/src/lib/rc/st24.c +++ b/src/lib/rc/st24.c @@ -64,3 +64,6 @@ uint8_t st24_common_crc8(uint8_t *ptr, uint8_t len) return (crc); } + + +uint8_t st24_decode(uint8_t byte, uint8_t *rssi, uint8_t* rx_count, uint16_t *channels, uint16_t max_chan_count) \ No newline at end of file diff --git a/src/lib/rc/st24.h b/src/lib/rc/st24.h index 1c92bad436..3621e8506e 100644 --- a/src/lib/rc/st24.h +++ b/src/lib/rc/st24.h @@ -137,4 +137,16 @@ typedef struct { * @param len number of bytes to accumulate in the checksum * @return the checksum of these bytes over len */ -__EXPORT uint8_t st24_common_crc8(uint8_t *ptr, uint8_t len); +uint8_t st24_common_crc8(uint8_t *ptr, uint8_t len); + +/** + * Decoder for ST24 protocol + * + * @param byte current char to read + * @param rssi pointer to a byte where the RSSI value is written back to + * @param rx_count pointer to a byte where the receive count of packets signce last wireless frame is written back to + * @param channels pointer to a datastructure of size max_chan_count where channel values (12 bit) are written back to + * @param max_chan_count maximum channels to decode - if more channels are decoded, the last n are skipped and success (0) is returned + * @return 0 for success (a decoded packet), 1 for no packet yet (accumulating), 3 for out of sync, 4 for checksum error + */ +__EXPORT uint8_t st24_decode(uint8_t byte, uint8_t *rssi, uint8_t* rx_count, uint16_t *channels, uint16_t max_chan_count); From 25a8f2d8056deca5049a8acc27c77e5e17760167 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 10 Jul 2014 14:30:34 +0200 Subject: [PATCH 037/411] ST24 decoding skeleton --- src/lib/rc/st24.c | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/lib/rc/st24.c b/src/lib/rc/st24.c index bae28c85e3..addbcd899d 100644 --- a/src/lib/rc/st24.c +++ b/src/lib/rc/st24.c @@ -66,4 +66,6 @@ uint8_t st24_common_crc8(uint8_t *ptr, uint8_t len) } -uint8_t st24_decode(uint8_t byte, uint8_t *rssi, uint8_t* rx_count, uint16_t *channels, uint16_t max_chan_count) \ No newline at end of file +uint8_t st24_decode(uint8_t byte, uint8_t *rssi, uint8_t* rx_count, uint16_t *channels, uint16_t max_chan_count) { + +} \ No newline at end of file From 373b1705c19627b97f1c65c1e947802e6b88af83 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 14 Jul 2014 10:57:53 +0200 Subject: [PATCH 038/411] vehicle command: add latest guided commands --- src/modules/uORB/topics/vehicle_command.h | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/modules/uORB/topics/vehicle_command.h b/src/modules/uORB/topics/vehicle_command.h index c21a29b130..faf8cc7db1 100644 --- a/src/modules/uORB/topics/vehicle_command.h +++ b/src/modules/uORB/topics/vehicle_command.h @@ -61,6 +61,9 @@ enum VEHICLE_CMD { VEHICLE_CMD_NAV_TAKEOFF = 22, /* Takeoff from ground / hand |Minimum pitch (if airspeed sensor present), desired pitch without sensor| Empty| Empty| Yaw angle (if magnetometer present), ignored without magnetometer| Latitude| Longitude| Altitude| */ VEHICLE_CMD_NAV_ROI = 80, /* Sets the region of interest (ROI) for a sensor set or the vehicle itself. This can then be used by the vehicles control system to control the vehicle attitude and the attitude of various sensors such as cameras. |Region of intereset mode. (see MAV_ROI enum)| MISSION index/ target ID. (see MAV_ROI enum)| ROI index (allows a vehicle to manage multiple ROI's)| Empty| x the location of the fixed ROI (see MAV_FRAME)| y| z| */ VEHICLE_CMD_NAV_PATHPLANNING = 81, /* Control autonomous path planning on the MAV. |0: Disable local obstacle avoidance / local path planning (without resetting map), 1: Enable local path planning, 2: Enable and reset local path planning| 0: Disable full path planning (without resetting map), 1: Enable, 2: Enable and reset map/occupancy grid, 3: Enable and reset planned route, but not occupancy grid| Empty| Yaw angle at goal, in compass degrees, [0..360]| Latitude/X of goal| Longitude/Y of goal| Altitude/Z of goal| */ + VEHICLE_CMD_NAV_GUIDED_LIMITS=90, /* set limits for external control |timeout - maximum time (in seconds) that external controller will be allowed to control vehicle. 0 means no timeout| absolute altitude min (in meters, WGS84) - if vehicle moves below this alt, the command will be aborted and the mission will continue. 0 means no lower altitude limit| absolute altitude max (in meters)- if vehicle moves above this alt, the command will be aborted and the mission will continue. 0 means no upper altitude limit| horizontal move limit (in meters, WGS84) - if vehicle moves more than this distance from it's location at the moment the command was executed, the command will be aborted and the mission will continue. 0 means no horizontal altitude limit| Empty| Empty| Empty| */ + VEHICLE_CMD_NAV_GUIDED_MASTER=91, /* set id of master controller |System ID| Component ID| Empty| Empty| Empty| Empty| Empty| */ + VEHICLE_CMD_NAV_GUIDED_ENABLE=92, /* hand control over to an external controller |On / Off (> 0.5f on)| Empty| Empty| Empty| Empty| Empty| Empty| */ VEHICLE_CMD_NAV_LAST = 95, /* NOP - This command is only used to mark the upper limit of the NAV/ACTION commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ VEHICLE_CMD_CONDITION_DELAY = 112, /* Delay mission state machine. |Delay in seconds (decimal)| Empty| Empty| Empty| Empty| Empty| Empty| */ VEHICLE_CMD_CONDITION_CHANGE_ALT = 113, /* Ascend/descend at rate. Delay mission state machine until desired altitude reached. |Descent / Ascend rate (m/s)| Empty| Empty| Empty| Empty| Empty| Finish Altitude| */ From 747eda92b9396e38746d505e5d79a7528e117c89 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 14 Jul 2014 11:19:06 +0200 Subject: [PATCH 039/411] commander: handle VEHICLE_CMD_NAV_GUIDED_ENABLE --- src/modules/commander/commander.cpp | 19 ++++++++++++++++++- 1 file changed, 18 insertions(+), 1 deletion(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index c112867897..e9db923e6e 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -611,7 +611,24 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s } } break; - + case VEHICLE_CMD_NAV_GUIDED_ENABLE: { + transition_result_t res = TRANSITION_DENIED; + static main_state_t main_state_pre_offboard = MAIN_STATE_MANUAL; + if (status_local->main_state != MAIN_STATE_OFFBOARD) { + main_state_pre_offboard = status_local->main_state; + } + if (cmd->param1 > 0.5f) { + res = main_state_transition(status_local, MAIN_STATE_OFFBOARD); + if (res == TRANSITION_DENIED) { + print_reject_mode(status_local, "OFFBOARD"); + } + } else { + /* If the mavlink command is used to enable or disable offboard control: + * switch back to previous mode when disabling */ + res = main_state_transition(status_local, main_state_pre_offboard); + } + } + break; case VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN: case VEHICLE_CMD_PREFLIGHT_CALIBRATION: case VEHICLE_CMD_PREFLIGHT_SET_SENSOR_OFFSETS: From 7a064174110d827182820960b245031e0b4d42ab Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 14 Jul 2014 13:34:06 +0200 Subject: [PATCH 040/411] mavlink: external setpoint feed trough functionality --- src/modules/mavlink/mavlink.c | 7 +++++++ src/modules/mavlink/mavlink_main.cpp | 16 ++++++++++++---- src/modules/mavlink/mavlink_main.h | 8 ++++++-- src/modules/mavlink/mavlink_receiver.cpp | 15 +++++++++++++++ 4 files changed, 40 insertions(+), 6 deletions(-) diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c index e49288a746..bec706badd 100644 --- a/src/modules/mavlink/mavlink.c +++ b/src/modules/mavlink/mavlink.c @@ -68,6 +68,13 @@ PARAM_DEFINE_INT32(MAV_TYPE, MAV_TYPE_FIXED_WING); * @group MAVLink */ PARAM_DEFINE_INT32(MAV_USEHILGPS, 0); +/** + * Forward external setpoint messages + * If set to 1 incomming external setpoint messages will be directly forwarded to the controllers if in offboard + * control mode + * @group MAVLink + */ +PARAM_DEFINE_INT32(MAV_FWDEXTSP, 1); mavlink_system_t mavlink_system = { 100, diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index cd0581af43..7707c0bc88 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -220,6 +220,7 @@ Mavlink::Mavlink() : _task_running(false), _hil_enabled(false), _use_hil_gps(false), + _forward_externalsp(false), _is_usb_uart(false), _wait_to_transmit(false), _received_messages(false), @@ -246,6 +247,7 @@ Mavlink::Mavlink() : _param_component_id(0), _param_system_type(0), _param_use_hil_gps(0), + _param_forward_externalsp(0), /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "mavlink_el")), @@ -517,6 +519,7 @@ void Mavlink::mavlink_update_system(void) _param_component_id = param_find("MAV_COMP_ID"); _param_system_type = param_find("MAV_TYPE"); _param_use_hil_gps = param_find("MAV_USEHILGPS"); + _param_forward_externalsp = param_find("MAV_FWDEXTSP"); _param_initialized = true; } @@ -546,6 +549,11 @@ void Mavlink::mavlink_update_system(void) param_get(_param_use_hil_gps, &use_hil_gps); _use_hil_gps = (bool)use_hil_gps; + + int32_t forward_externalsp; + param_get(_param_forward_externalsp, &forward_externalsp); + + _forward_externalsp = (bool)forward_externalsp; } int Mavlink::get_system_id() @@ -1501,14 +1509,14 @@ Mavlink::task_main(int argc, char *argv[]) if (read_count > sizeof(mavlink_message_t)) { read_count = sizeof(mavlink_message_t); } - + memcpy(write_ptr, read_ptr, read_count); - + // We hold the mutex until after we complete the second part of the buffer. If we don't // we may end up breaking the empty slot overflow detection semantics when we mark the // possibly partial read below. pthread_mutex_lock(&_message_buffer_mutex); - + message_buffer_mark_read(read_count); /* write second part of buffer if there is some */ @@ -1519,7 +1527,7 @@ Mavlink::task_main(int argc, char *argv[]) memcpy(write_ptr, read_ptr, read_count); message_buffer_mark_read(available); } - + pthread_mutex_unlock(&_message_buffer_mutex); _mavlink_resend_uart(_channel, &msg); diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index acfc8b90eb..8738a6f181 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -134,6 +134,8 @@ public: bool get_use_hil_gps() { return _use_hil_gps; } + bool get_forward_externalsp() { return _forward_externalsp; } + bool get_flow_control_enabled() { return _flow_control_enabled; } bool get_forwarding_on() { return _forwarding_on; } @@ -205,7 +207,7 @@ public: * Send a status text with loglevel * * @param string the message to send (will be capped by mavlink max string length) - * @param severity the log level, one of + * @param severity the log level, one of */ int send_statustext(unsigned severity, const char *string); MavlinkStream * get_streams() const { return _streams; } @@ -220,7 +222,7 @@ public: bool should_transmit() { return (!_wait_to_transmit || (_wait_to_transmit && _received_messages)); } bool message_buffer_write(const void *ptr, int size); - + void lockMessageBufferMutex(void) { pthread_mutex_lock(&_message_buffer_mutex); } void unlockMessageBufferMutex(void) { pthread_mutex_unlock(&_message_buffer_mutex); } @@ -241,6 +243,7 @@ private: /* states */ bool _hil_enabled; /**< Hardware In the Loop mode */ bool _use_hil_gps; /**< Accept GPS HIL messages (for example from an external motion capturing system to fake indoor gps) */ + bool _forward_externalsp; /**< Forward external setpoint messages to controllers directly if in offboard mode */ bool _is_usb_uart; /**< Port is USB */ bool _wait_to_transmit; /**< Wait to transmit until received messages. */ bool _received_messages; /**< Whether we've received valid mavlink messages. */ @@ -301,6 +304,7 @@ private: param_t _param_component_id; param_t _param_system_type; param_t _param_use_hil_gps; + param_t _param_forward_externalsp; perf_counter_t _loop_perf; /**< loop performance counter */ perf_counter_t _txerr_perf; /**< TX error counter */ diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index bd1296f626..9df843e586 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -451,6 +451,21 @@ MavlinkReceiver::handle_message_local_ned_position_setpoint_external(mavlink_mes } else { orb_publish(ORB_ID(offboard_control_setpoint), _offboard_control_sp_pub, &offboard_control_sp); } + + /* If we are in offboard control mode and offboard control loop through is enabled + * also publish the setpoint topic which is read by the controller */ + if (_mavlink->get_forward_externalsp()) { + bool updated; + orb_check(_control_mode_sub, &updated); + if (updated) { + orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode); + } + + if (_control_mode.flag_control_offboard_enabled) { + //XXX: copy to and publish setpoint triplet here + } + + } } } From d0cca02e97a24cf0b9554cf429bbd5716f947e4e Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 15 Jul 2014 10:34:56 +0200 Subject: [PATCH 041/411] add parsing of external attitude message --- src/modules/mavlink/mavlink_receiver.cpp | 88 ++++++++++++++++++- src/modules/mavlink/mavlink_receiver.h | 1 + .../uORB/topics/offboard_control_setpoint.h | 6 ++ 3 files changed, 93 insertions(+), 2 deletions(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 9df843e586..9e38426145 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -157,6 +157,10 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) handle_message_local_ned_position_setpoint_external(msg); break; + case MAVLINK_MSG_ID_ATTITUDE_SETPOINT_EXTERNAL: + handle_message_attitude_setpoint_external(msg); + break; + case MAVLINK_MSG_ID_RADIO_STATUS: handle_message_radio_status(msg); break; @@ -404,7 +408,7 @@ MavlinkReceiver::handle_message_local_ned_position_setpoint_external(mavlink_mes mavlink_msg_local_ned_position_setpoint_external_decode(msg, &local_ned_position_setpoint_external); struct offboard_control_setpoint_s offboard_control_sp; - memset(&offboard_control_sp, 0, sizeof(offboard_control_sp)); + memset(&offboard_control_sp, 0, sizeof(offboard_control_sp));//XXX breaks compatibility with multiple setpoints if (mavlink_system.sysid == local_ned_position_setpoint_external.target_system && mavlink_system.compid == local_ned_position_setpoint_external.target_component) { @@ -438,7 +442,7 @@ MavlinkReceiver::handle_message_local_ned_position_setpoint_external(mavlink_mes offboard_control_sp.acceleration[2] = local_ned_position_setpoint_external.afz; offboard_control_sp.isForceSetpoint = (bool)(local_ned_position_setpoint_external.type_mask & (1 << 9)); for (int i = 0; i < 9; i++) { - offboard_control_sp.ignore &= ~(local_ned_position_setpoint_external.type_mask & (1 << i)); + offboard_control_sp.ignore &= ~(1 << i); offboard_control_sp.ignore |= (local_ned_position_setpoint_external.type_mask & (1 << i)); } @@ -469,6 +473,86 @@ MavlinkReceiver::handle_message_local_ned_position_setpoint_external(mavlink_mes } } +void +MavlinkReceiver::handle_message_attitude_setpoint_external(mavlink_message_t *msg) +{ + mavlink_attitude_setpoint_external_t attitude_setpoint_external; + mavlink_msg_attitude_setpoint_external_decode(msg, &attitude_setpoint_external); + + struct offboard_control_setpoint_s offboard_control_sp; + memset(&offboard_control_sp, 0, sizeof(offboard_control_sp)); //XXX breaks compatibility with multiple setpoints + + if (mavlink_system.sysid == attitude_setpoint_external.target_system && + mavlink_system.compid == attitude_setpoint_external.target_component) { + for (int i = 0; i < 4; i++) { + offboard_control_sp.attitude[i] = attitude_setpoint_external.q[i]; + } + offboard_control_sp.attitude_rate[0] = attitude_setpoint_external.body_roll_rate; + offboard_control_sp.attitude_rate[1] = attitude_setpoint_external.body_pitch_rate; + offboard_control_sp.attitude_rate[2] = attitude_setpoint_external.body_yaw_rate; + + for (int i = 0; i < 3; i++) { + offboard_control_sp.ignore &= ~(1 << (i + 9)); + offboard_control_sp.ignore |= (attitude_setpoint_external.type_mask & (1 << i)) << 9; + } + offboard_control_sp.ignore &= ~(1 << 10); + offboard_control_sp.ignore |= ((attitude_setpoint_external.type_mask & (1 << 7)) << 3); + + + offboard_control_sp.timestamp = hrt_absolute_time(); + + if (_offboard_control_sp_pub < 0) { + _offboard_control_sp_pub = orb_advertise(ORB_ID(offboard_control_setpoint), &offboard_control_sp); + + } else { + orb_publish(ORB_ID(offboard_control_setpoint), _offboard_control_sp_pub, &offboard_control_sp); + } + + /* If we are in offboard control mode and offboard control loop through is enabled + * also publish the setpoint topic which is read by the controller */ + if (_mavlink->get_forward_externalsp()) { + bool updated; + orb_check(_control_mode_sub, &updated); + if (updated) { + orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode); + } + + if (_control_mode.flag_control_offboard_enabled) { + + /* Publish attitude setpoint if ignore bit is not set */ + if (!(attitude_setpoint_external.type_mask & (1 << 7))) { + struct vehicle_attitude_setpoint_s att_sp; + mavlink_quaternion_to_euler(attitude_setpoint_external.q, + &att_sp.roll_body, &att_sp.pitch_body, &att_sp.yaw_body); + att_sp.thrust = attitude_setpoint_external.thrust; + if (_att_sp_pub < 0) { + _att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp); + } else { + orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &att_sp); + } + } + + /* Publish attitude rate setpoint if ignore bit are not set */ + ///XXX add support for ignoring individual axes + if (!(attitude_setpoint_external.type_mask & (0b111))) { + struct vehicle_rates_setpoint_s rates_sp; + rates_sp.roll = attitude_setpoint_external.body_roll_rate; + rates_sp.pitch = attitude_setpoint_external.body_pitch_rate; + rates_sp.yaw = attitude_setpoint_external.body_yaw_rate; + rates_sp.thrust = attitude_setpoint_external.thrust; + + if (_att_sp_pub < 0) { + _rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &rates_sp); + } else { + orb_publish(ORB_ID(vehicle_rates_setpoint), _rates_sp_pub, &rates_sp); + } + } + } + + } + } +} + void MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg) { diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 4fd80f4acc..0f12cf32c7 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -114,6 +114,7 @@ private: void handle_message_vicon_position_estimate(mavlink_message_t *msg); void handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message_t *msg); void handle_message_local_ned_position_setpoint_external(mavlink_message_t *msg); + void handle_message_attitude_setpoint_external(mavlink_message_t *msg); void handle_message_radio_status(mavlink_message_t *msg); void handle_message_manual_control(mavlink_message_t *msg); void handle_message_heartbeat(mavlink_message_t *msg); diff --git a/src/modules/uORB/topics/offboard_control_setpoint.h b/src/modules/uORB/topics/offboard_control_setpoint.h index 809412fc09..809a2ace4d 100644 --- a/src/modules/uORB/topics/offboard_control_setpoint.h +++ b/src/modules/uORB/topics/offboard_control_setpoint.h @@ -88,6 +88,7 @@ struct offboard_control_setpoint_s { float attitude_rate[3]; /**< body angular rates (x, y, z) */ uint16_t ignore; /**< if field i is set to true, pi should be ignored */ + //XXX define constants for bit offsets bool isForceSetpoint; /**< the acceleration vector should be interpreted as force */ float override_mode_switch; @@ -118,6 +119,11 @@ inline bool offboard_control_sp_ignore_bodyrates(const struct offboard_control_s return (bool)(offboard_control_sp.ignore & (1 << (9 + index))); } +inline bool offboard_control_sp_ignore_attitude(const struct offboard_control_setpoint_s &offboard_control_sp) { + return (bool)(offboard_control_sp.ignore & (1 << 10)); +} + + /* register this as object request broker structure */ ORB_DECLARE(offboard_control_setpoint); From 596850e9f5343193ab4dc75cfa7849e0eb63ceea Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 15 Jul 2014 14:19:13 +0200 Subject: [PATCH 042/411] mavlink external sp: accept 0 sysid and compid --- src/modules/mavlink/mavlink_receiver.cpp | 14 ++++++++++---- 1 file changed, 10 insertions(+), 4 deletions(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 9e38426145..64ba93f9c7 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -410,8 +410,11 @@ MavlinkReceiver::handle_message_local_ned_position_setpoint_external(mavlink_mes struct offboard_control_setpoint_s offboard_control_sp; memset(&offboard_control_sp, 0, sizeof(offboard_control_sp));//XXX breaks compatibility with multiple setpoints - if (mavlink_system.sysid == local_ned_position_setpoint_external.target_system && - mavlink_system.compid == local_ned_position_setpoint_external.target_component) { + /* Only accept messages which are intended for this system */ + if ((mavlink_system.sysid == local_ned_position_setpoint_external.target_system || + local_ned_position_setpoint_external.target_system == 0) && + (mavlink_system.compid == local_ned_position_setpoint_external.target_component || + local_ned_position_setpoint_external.target_component == 0)) { /* convert mavlink type (local, NED) to uORB offboard control struct */ switch (local_ned_position_setpoint_external.coordinate_frame) { @@ -482,8 +485,11 @@ MavlinkReceiver::handle_message_attitude_setpoint_external(mavlink_message_t *ms struct offboard_control_setpoint_s offboard_control_sp; memset(&offboard_control_sp, 0, sizeof(offboard_control_sp)); //XXX breaks compatibility with multiple setpoints - if (mavlink_system.sysid == attitude_setpoint_external.target_system && - mavlink_system.compid == attitude_setpoint_external.target_component) { + /* Only accept messages which are intended for this system */ + if ((mavlink_system.sysid == attitude_setpoint_external.target_system || + local_ned_position_setpoint_external.target_system == 0) && + (mavlink_system.compid == local_ned_position_setpoint_external.target_component || + local_ned_position_setpoint_external.target_component == 0)) { for (int i = 0; i < 4; i++) { offboard_control_sp.attitude[i] = attitude_setpoint_external.q[i]; } From 35a9dad998961c9f8aa5ab5d015cb4b3a642a9ce Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 15 Jul 2014 14:42:53 +0200 Subject: [PATCH 043/411] mavlink receiver: fix copy paste error --- src/modules/mavlink/mavlink_receiver.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 64ba93f9c7..30eb6d0e58 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -487,9 +487,9 @@ MavlinkReceiver::handle_message_attitude_setpoint_external(mavlink_message_t *ms /* Only accept messages which are intended for this system */ if ((mavlink_system.sysid == attitude_setpoint_external.target_system || - local_ned_position_setpoint_external.target_system == 0) && - (mavlink_system.compid == local_ned_position_setpoint_external.target_component || - local_ned_position_setpoint_external.target_component == 0)) { + attitude_setpoint_external.target_system == 0) && + (mavlink_system.compid == attitude_setpoint_external.target_component || + attitude_setpoint_external.target_component == 0)) { for (int i = 0; i < 4; i++) { offboard_control_sp.attitude[i] = attitude_setpoint_external.q[i]; } From 3c8927c42386a6528f120d04ec93f5ab9b453a5b Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 16 Jul 2014 09:38:57 +0200 Subject: [PATCH 044/411] once offboard is set by mavlink command ignore RC mode --- src/modules/commander/commander.cpp | 10 ++++++++++ src/modules/uORB/topics/vehicle_status.h | 2 ++ 2 files changed, 12 insertions(+) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index e9db923e6e..cc68c4e363 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -621,11 +621,16 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s res = main_state_transition(status_local, MAIN_STATE_OFFBOARD); if (res == TRANSITION_DENIED) { print_reject_mode(status_local, "OFFBOARD"); + status_local->offboard_control_set_by_command = false; + } else { + /* Set flag that offboard was set via command, main state is not overridden by rc */ + status_local->offboard_control_set_by_command = true; } } else { /* If the mavlink command is used to enable or disable offboard control: * switch back to previous mode when disabling */ res = main_state_transition(status_local, main_state_pre_offboard); + status_local->offboard_control_set_by_command = false; } } break; @@ -1749,6 +1754,11 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s /* set main state according to RC switches */ transition_result_t res = TRANSITION_DENIED; + /* if offboard is set allready by a mavlink command, abort */ + if (status.offboard_control_set_by_command) { + return main_state_transition(status_local, MAIN_STATE_OFFBOARD); + } + /* offboard switch overrides main switch */ if (sp_man->offboard_switch == SWITCH_POS_ON) { res = main_state_transition(status_local, MAIN_STATE_OFFBOARD); diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index b46c00b758..7a40ac636b 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -206,6 +206,8 @@ struct vehicle_status_s { bool offboard_control_signal_lost; bool offboard_control_signal_weak; uint64_t offboard_control_signal_lost_interval; /**< interval in microseconds without an offboard control message */ + bool offboard_control_set_by_command; /**< true if the offboard mode was set by a mavlink command + and should not be overridden by RC */ /* see SYS_STATUS mavlink message for the following */ uint32_t onboard_control_sensors_present; From a63f3a173174e28fb5df63a1646c62dcbdc52bbb Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 16 Jul 2014 10:36:02 +0200 Subject: [PATCH 045/411] external attitude sp: set timestamp --- 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 30eb6d0e58..952bc8735a 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -528,6 +528,7 @@ MavlinkReceiver::handle_message_attitude_setpoint_external(mavlink_message_t *ms /* Publish attitude setpoint if ignore bit is not set */ if (!(attitude_setpoint_external.type_mask & (1 << 7))) { struct vehicle_attitude_setpoint_s att_sp; + att_sp.timestamp = hrt_absolute_time(); mavlink_quaternion_to_euler(attitude_setpoint_external.q, &att_sp.roll_body, &att_sp.pitch_body, &att_sp.yaw_body); att_sp.thrust = attitude_setpoint_external.thrust; @@ -542,6 +543,7 @@ MavlinkReceiver::handle_message_attitude_setpoint_external(mavlink_message_t *ms ///XXX add support for ignoring individual axes if (!(attitude_setpoint_external.type_mask & (0b111))) { struct vehicle_rates_setpoint_s rates_sp; + rates_sp.timestamp = hrt_absolute_time(); rates_sp.roll = attitude_setpoint_external.body_roll_rate; rates_sp.pitch = attitude_setpoint_external.body_pitch_rate; rates_sp.yaw = attitude_setpoint_external.body_yaw_rate; From 9527cc8293749b9ccdec015f587afef9698be1e6 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 16 Jul 2014 14:03:34 +0200 Subject: [PATCH 046/411] att external sp: also write quaternion --- src/modules/mavlink/mavlink_receiver.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 952bc8735a..e5d380cfb0 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -506,6 +506,7 @@ MavlinkReceiver::handle_message_attitude_setpoint_external(mavlink_message_t *ms offboard_control_sp.timestamp = hrt_absolute_time(); + offboard_control_sp.mode =OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE; //XXX handle rate control mode if (_offboard_control_sp_pub < 0) { _offboard_control_sp_pub = orb_advertise(ORB_ID(offboard_control_setpoint), &offboard_control_sp); @@ -532,6 +533,8 @@ MavlinkReceiver::handle_message_attitude_setpoint_external(mavlink_message_t *ms mavlink_quaternion_to_euler(attitude_setpoint_external.q, &att_sp.roll_body, &att_sp.pitch_body, &att_sp.yaw_body); att_sp.thrust = attitude_setpoint_external.thrust; + att_sp.q_d_valid = true; + memcpy(att_sp.q_d, attitude_setpoint_external.q, sizeof(att_sp.q_d)); if (_att_sp_pub < 0) { _att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp); } else { From f4608707389dbc30eb25db524d6e008c8033d052 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 17 Jul 2014 09:11:57 +0200 Subject: [PATCH 047/411] support force setpoints --- src/modules/commander/commander.cpp | 9 +++++++ src/modules/mavlink/mavlink_receiver.cpp | 24 ++++++++++++++++++- src/modules/mavlink/mavlink_receiver.h | 2 ++ .../uORB/topics/offboard_control_setpoint.h | 7 +++--- .../uORB/topics/vehicle_control_mode.h | 1 + 5 files changed, 39 insertions(+), 4 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index cc68c4e363..fc9560e18f 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1951,6 +1951,15 @@ set_control_mode() control_mode.flag_control_position_enabled = false; control_mode.flag_control_velocity_enabled = false; break; + case OFFBOARD_CONTROL_MODE_DIRECT_FORCE: + control_mode.flag_control_rates_enabled = true; + control_mode.flag_control_attitude_enabled = false; + control_mode.flag_control_force_enabled = true; + 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 OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED: case OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_OFFSET_NED: case OFFBOARD_CONTROL_MODE_DIRECT_BODY_NED: diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index e5d380cfb0..2cc2d61626 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -105,6 +105,7 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) : _global_vel_sp_pub(-1), _att_sp_pub(-1), _rates_sp_pub(-1), + _force_sp_pub(-1), _vicon_position_pub(-1), _telemetry_status_pub(-1), _rc_pub(-1), @@ -444,6 +445,13 @@ MavlinkReceiver::handle_message_local_ned_position_setpoint_external(mavlink_mes offboard_control_sp.acceleration[1] = local_ned_position_setpoint_external.afy; offboard_control_sp.acceleration[2] = local_ned_position_setpoint_external.afz; offboard_control_sp.isForceSetpoint = (bool)(local_ned_position_setpoint_external.type_mask & (1 << 9)); + + /* If we are in force control mode, for now set offboard mode to force control */ + if (offboard_control_sp.isForceSetpoint) { + offboard_control_sp.mode = OFFBOARD_CONTROL_MODE_DIRECT_FORCE; + } + + /* set ignore flags */ for (int i = 0; i < 9; i++) { offboard_control_sp.ignore &= ~(1 << i); offboard_control_sp.ignore |= (local_ned_position_setpoint_external.type_mask & (1 << i)); @@ -467,9 +475,23 @@ MavlinkReceiver::handle_message_local_ned_position_setpoint_external(mavlink_mes if (updated) { orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode); } - if (_control_mode.flag_control_offboard_enabled) { + if (offboard_control_sp.isForceSetpoint) { + struct vehicle_force_setpoint_s force_sp; + force_sp.x = offboard_control_sp.acceleration[0]; + force_sp.y = offboard_control_sp.acceleration[1]; + force_sp.z = offboard_control_sp.acceleration[2]; + //XXX: yaw + if (_force_sp_pub < 0) { + _force_sp_pub = orb_advertise(ORB_ID(vehicle_force_setpoint), &force_sp); + } else { + orb_publish(ORB_ID(vehicle_force_setpoint), _force_sp_pub, &force_sp); + } + } else { + //XXX: copy to and publish setpoint triplet here + } + } } diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 0f12cf32c7..0044b42cba 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -70,6 +70,7 @@ #include #include #include +#include #include "mavlink_ftp.h" @@ -146,6 +147,7 @@ private: orb_advert_t _global_vel_sp_pub; orb_advert_t _att_sp_pub; orb_advert_t _rates_sp_pub; + orb_advert_t _force_sp_pub; orb_advert_t _vicon_position_pub; orb_advert_t _telemetry_status_pub; orb_advert_t _rc_pub; diff --git a/src/modules/uORB/topics/offboard_control_setpoint.h b/src/modules/uORB/topics/offboard_control_setpoint.h index 809a2ace4d..19f11ba924 100644 --- a/src/modules/uORB/topics/offboard_control_setpoint.h +++ b/src/modules/uORB/topics/offboard_control_setpoint.h @@ -58,9 +58,10 @@ enum OFFBOARD_CONTROL_MODE { OFFBOARD_CONTROL_MODE_DIRECT_BODY_NED = 5, OFFBOARD_CONTROL_MODE_DIRECT_BODY_OFFSET_NED = 6, OFFBOARD_CONTROL_MODE_DIRECT_GLOBAL = 7, - OFFBOARD_CONTROL_MODE_ATT_YAW_RATE = 8, - OFFBOARD_CONTROL_MODE_ATT_YAW_POS = 9, - OFFBOARD_CONTROL_MODE_MULTIROTOR_SIMPLE = 10, /**< roll / pitch rotated aligned to the takeoff orientation, throttle stabilized, yaw pos */ + OFFBOARD_CONTROL_MODE_DIRECT_FORCE = 8, + OFFBOARD_CONTROL_MODE_ATT_YAW_RATE = 9, + OFFBOARD_CONTROL_MODE_ATT_YAW_POS = 10, + OFFBOARD_CONTROL_MODE_MULTIROTOR_SIMPLE = 11, /**< roll / pitch rotated aligned to the takeoff orientation, throttle stabilized, yaw pos */ }; enum OFFBOARD_CONTROL_FRAME { diff --git a/src/modules/uORB/topics/vehicle_control_mode.h b/src/modules/uORB/topics/vehicle_control_mode.h index 49e2ba4b56..ca77054569 100644 --- a/src/modules/uORB/topics/vehicle_control_mode.h +++ b/src/modules/uORB/topics/vehicle_control_mode.h @@ -77,6 +77,7 @@ struct vehicle_control_mode_s { bool flag_control_offboard_enabled; /**< true if offboard control should be used */ 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_force_enabled; /**< true if force control 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 */ From 7baa337d9bcf7077a5e5080e951899cb595f6ff6 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 19 Jul 2014 19:25:53 +0200 Subject: [PATCH 048/411] flight termination on geofence violation --- src/modules/commander/commander.cpp | 8 ++++++++ src/modules/navigator/navigator_main.cpp | 10 ++++++++++ src/modules/uORB/topics/position_setpoint_triplet.h | 1 + 3 files changed, 19 insertions(+) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 04450a44fb..522c6e8860 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1244,6 +1244,14 @@ int commander_thread_main(int argc, char *argv[]) if (updated) { orb_copy(ORB_ID(position_setpoint_triplet), pos_sp_triplet_sub, &pos_sp_triplet); + + /* Check for geofence violation */ + if (pos_sp_triplet.geofence_violated) { + //XXX: make this configurable to select different actions (e.g. navigation modes) + /* this will only trigger if geofence is activated via param and a geofence file is present */ + armed.force_failsafe = true; + status_changed = true; + } // no reset is done here on purpose, on geofence violation we want to stay in flighttermination } if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) { diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 331a9a728f..ba46bd5681 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -335,6 +335,11 @@ Navigator::task_main() /* Check geofence violation */ if (!_geofence.inside(&_global_pos)) { + /* inform other apps via the sp triplet */ + _pos_sp_triplet.geofence_violated = true; + if (_pos_sp_triplet.geofence_violated != true) { + _pos_sp_triplet_updated = true; + } /* Issue a warning about the geofence violation once */ if (!_geofence_violation_warning_sent) { @@ -342,6 +347,11 @@ Navigator::task_main() _geofence_violation_warning_sent = true; } } else { + /* inform other apps via the sp triplet */ + _pos_sp_triplet.geofence_violated = false; + if (_pos_sp_triplet.geofence_violated != false) { + _pos_sp_triplet_updated = true; + } /* Reset the _geofence_violation_warning_sent field */ _geofence_violation_warning_sent = false; } diff --git a/src/modules/uORB/topics/position_setpoint_triplet.h b/src/modules/uORB/topics/position_setpoint_triplet.h index 4a19321809..4e8c6c53ef 100644 --- a/src/modules/uORB/topics/position_setpoint_triplet.h +++ b/src/modules/uORB/topics/position_setpoint_triplet.h @@ -97,6 +97,7 @@ struct position_setpoint_triplet_s struct position_setpoint_s next; unsigned nav_state; /**< report the navigation state */ + bool geofence_violated; /**< true if the geofence is violated */ }; /** From b5ef8fd2cd54d180b5debe362a4c1f07f64394af Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 19 Jul 2014 20:03:37 +0200 Subject: [PATCH 049/411] create empty datalinkloss class for OBC Currently the class does the same as the RTL class. It is now possible whichclass is sued in the navigator to handle datalink loss via a parameter --- src/modules/navigator/datalinkloss.cpp | 317 ++++++++++++++++++++ src/modules/navigator/datalinkloss.h | 95 ++++++ src/modules/navigator/datalinkloss_params.c | 98 ++++++ src/modules/navigator/module.mk | 4 +- src/modules/navigator/navigator.h | 4 + src/modules/navigator/navigator_main.cpp | 13 +- src/modules/navigator/navigator_params.c | 11 + 7 files changed, 539 insertions(+), 3 deletions(-) create mode 100644 src/modules/navigator/datalinkloss.cpp create mode 100644 src/modules/navigator/datalinkloss.h create mode 100644 src/modules/navigator/datalinkloss_params.c diff --git a/src/modules/navigator/datalinkloss.cpp b/src/modules/navigator/datalinkloss.cpp new file mode 100644 index 0000000000..17e85b2841 --- /dev/null +++ b/src/modules/navigator/datalinkloss.cpp @@ -0,0 +1,317 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ +/** + * @file datalinkloss.cpp + * Helper class for Data Link Loss Mode acording to the OBC rules + * + * @author Thomas Gubler + */ + +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include + +#include "navigator.h" +#include "datalinkloss.h" + +#define DELAY_SIGMA 0.01f + +DataLinkLoss::DataLinkLoss(Navigator *navigator, const char *name) : + MissionBlock(navigator, name), + _rtl_state(RTL_STATE_NONE), + _param_return_alt(this, "RETURN_ALT"), + _param_descend_alt(this, "DESCEND_ALT"), + _param_land_delay(this, "LAND_DELAY") +{ + /* load initial params */ + updateParams(); + /* initial reset */ + on_inactive(); +} + +DataLinkLoss::~DataLinkLoss() +{ +} + +void +DataLinkLoss::on_inactive() +{ + /* reset RTL state only if setpoint moved */ + if (!_navigator->get_can_loiter_at_sp()) { + _rtl_state = RTL_STATE_NONE; + } +} + +void +DataLinkLoss::on_activation() +{ + /* decide where to enter the RTL procedure when we switch into it */ + if (_rtl_state == RTL_STATE_NONE) { + /* for safety reasons don't go into RTL if landed */ + if (_navigator->get_vstatus()->condition_landed) { + _rtl_state = RTL_STATE_LANDED; + mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: no RTL when landed"); + + /* if lower than return altitude, climb up first */ + } else if (_navigator->get_global_position()->alt < _navigator->get_home_position()->alt + + _param_return_alt.get()) { + _rtl_state = RTL_STATE_CLIMB; + + /* otherwise go straight to return */ + } else { + /* set altitude setpoint to current altitude */ + _rtl_state = RTL_STATE_RETURN; + _mission_item.altitude_is_relative = false; + _mission_item.altitude = _navigator->get_global_position()->alt; + } + } + + set_rtl_item(); +} + +void +DataLinkLoss::on_active() +{ + if (_rtl_state != RTL_STATE_LANDED && is_mission_item_reached()) { + advance_rtl(); + set_rtl_item(); + } +} + +void +DataLinkLoss::set_rtl_item() +{ + struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); + + /* make sure we have the latest params */ + updateParams(); + + set_previous_pos_setpoint(); + _navigator->set_can_loiter_at_sp(false); + + switch (_rtl_state) { + case RTL_STATE_CLIMB: { + float climb_alt = _navigator->get_home_position()->alt + _param_return_alt.get(); + + _mission_item.lat = _navigator->get_global_position()->lat; + _mission_item.lon = _navigator->get_global_position()->lon; + _mission_item.altitude_is_relative = false; + _mission_item.altitude = climb_alt; + _mission_item.yaw = NAN; + _mission_item.loiter_radius = _navigator->get_loiter_radius(); + _mission_item.loiter_direction = 1; + _mission_item.nav_cmd = NAV_CMD_WAYPOINT; + _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); + _mission_item.time_inside = 0.0f; + _mission_item.pitch_min = 0.0f; + _mission_item.autocontinue = true; + _mission_item.origin = ORIGIN_ONBOARD; + + mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: climb to %d meters above home", + (int)(climb_alt - _navigator->get_home_position()->alt)); + break; + } + + case RTL_STATE_RETURN: { + _mission_item.lat = _navigator->get_home_position()->lat; + _mission_item.lon = _navigator->get_home_position()->lon; + // don't change altitude + + if (pos_sp_triplet->previous.valid) { + /* if previous setpoint is valid then use it to calculate heading to home */ + _mission_item.yaw = get_bearing_to_next_waypoint( + pos_sp_triplet->previous.lat, pos_sp_triplet->previous.lon, + _mission_item.lat, _mission_item.lon); + + } else { + /* else use current position */ + _mission_item.yaw = get_bearing_to_next_waypoint( + _navigator->get_global_position()->lat, _navigator->get_global_position()->lon, + _mission_item.lat, _mission_item.lon); + } + _mission_item.loiter_radius = _navigator->get_loiter_radius(); + _mission_item.loiter_direction = 1; + _mission_item.nav_cmd = NAV_CMD_WAYPOINT; + _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); + _mission_item.time_inside = 0.0f; + _mission_item.pitch_min = 0.0f; + _mission_item.autocontinue = true; + _mission_item.origin = ORIGIN_ONBOARD; + + mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: return at %d meters above home", + (int)(_mission_item.altitude - _navigator->get_home_position()->alt)); + break; + } + + case RTL_STATE_DESCEND: { + _mission_item.lat = _navigator->get_home_position()->lat; + _mission_item.lon = _navigator->get_home_position()->lon; + _mission_item.altitude_is_relative = false; + _mission_item.altitude = _navigator->get_home_position()->alt + _param_descend_alt.get(); + _mission_item.yaw = NAN; + _mission_item.loiter_radius = _navigator->get_loiter_radius(); + _mission_item.loiter_direction = 1; + _mission_item.nav_cmd = NAV_CMD_LOITER_TIME_LIMIT; + _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); + _mission_item.time_inside = 0.0f; + _mission_item.pitch_min = 0.0f; + _mission_item.autocontinue = false; + _mission_item.origin = ORIGIN_ONBOARD; + + mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: descend to %d meters above home", + (int)(_mission_item.altitude - _navigator->get_home_position()->alt)); + break; + } + + case RTL_STATE_LOITER: { + bool autoland = _param_land_delay.get() > -DELAY_SIGMA; + + _mission_item.lat = _navigator->get_home_position()->lat; + _mission_item.lon = _navigator->get_home_position()->lon; + _mission_item.altitude_is_relative = false; + _mission_item.altitude = _navigator->get_home_position()->alt + _param_descend_alt.get(); + _mission_item.yaw = NAN; + _mission_item.loiter_radius = _navigator->get_loiter_radius(); + _mission_item.loiter_direction = 1; + _mission_item.nav_cmd = autoland ? NAV_CMD_LOITER_TIME_LIMIT : NAV_CMD_LOITER_UNLIMITED; + _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); + _mission_item.time_inside = _param_land_delay.get() < 0.0f ? 0.0f : _param_land_delay.get(); + _mission_item.pitch_min = 0.0f; + _mission_item.autocontinue = autoland; + _mission_item.origin = ORIGIN_ONBOARD; + + _navigator->set_can_loiter_at_sp(true); + + if (autoland) { + mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: loiter %.1fs", (double)_mission_item.time_inside); + + } else { + mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: completed, loiter"); + } + break; + } + + case RTL_STATE_LAND: { + _mission_item.lat = _navigator->get_home_position()->lat; + _mission_item.lon = _navigator->get_home_position()->lon; + _mission_item.altitude_is_relative = false; + _mission_item.altitude = _navigator->get_home_position()->alt; + _mission_item.yaw = NAN; + _mission_item.loiter_radius = _navigator->get_loiter_radius(); + _mission_item.loiter_direction = 1; + _mission_item.nav_cmd = NAV_CMD_LAND; + _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); + _mission_item.time_inside = 0.0f; + _mission_item.pitch_min = 0.0f; + _mission_item.autocontinue = true; + _mission_item.origin = ORIGIN_ONBOARD; + + mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: land at home"); + break; + } + + case RTL_STATE_LANDED: { + _mission_item.lat = _navigator->get_home_position()->lat; + _mission_item.lon = _navigator->get_home_position()->lon; + _mission_item.altitude_is_relative = false; + _mission_item.altitude = _navigator->get_home_position()->alt; + _mission_item.yaw = NAN; + _mission_item.loiter_radius = _navigator->get_loiter_radius(); + _mission_item.loiter_direction = 1; + _mission_item.nav_cmd = NAV_CMD_IDLE; + _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); + _mission_item.time_inside = 0.0f; + _mission_item.pitch_min = 0.0f; + _mission_item.autocontinue = true; + _mission_item.origin = ORIGIN_ONBOARD; + + mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: completed, landed"); + break; + } + + default: + break; + } + + reset_mission_item_reached(); + + /* convert mission item to current position setpoint and make it valid */ + mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current); + pos_sp_triplet->next.valid = false; + + _navigator->set_position_setpoint_triplet_updated(); +} + +void +DataLinkLoss::advance_rtl() +{ + switch (_rtl_state) { + case RTL_STATE_CLIMB: + _rtl_state = RTL_STATE_RETURN; + break; + + case RTL_STATE_RETURN: + _rtl_state = RTL_STATE_DESCEND; + break; + + case RTL_STATE_DESCEND: + /* only go to land if autoland is enabled */ + if (_param_land_delay.get() < -DELAY_SIGMA || _param_land_delay.get() > DELAY_SIGMA) { + _rtl_state = RTL_STATE_LOITER; + + } else { + _rtl_state = RTL_STATE_LAND; + } + break; + + case RTL_STATE_LOITER: + _rtl_state = RTL_STATE_LAND; + break; + + case RTL_STATE_LAND: + _rtl_state = RTL_STATE_LANDED; + break; + + default: + break; + } +} diff --git a/src/modules/navigator/datalinkloss.h b/src/modules/navigator/datalinkloss.h new file mode 100644 index 0000000000..242cfac8dc --- /dev/null +++ b/src/modules/navigator/datalinkloss.h @@ -0,0 +1,95 @@ +/*************************************************************************** + * + * Copyright (c) 2013-2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ +/** + * @file datalinkloss.cpp + * Helper class for Data Link Loss Mode acording to the OBC rules + * + * @author Thomas Gubler + */ + +#ifndef NAVIGATOR_DATALINKLOSS_H +#define NAVIGATOR_DATALINKLOSS_H + +#include +#include + +#include +#include +#include +#include + +#include "navigator_mode.h" +#include "mission_block.h" + +class Navigator; + +class DataLinkLoss : public MissionBlock +{ +public: + DataLinkLoss(Navigator *navigator, const char *name); + + ~DataLinkLoss(); + + virtual void on_inactive(); + + virtual void on_activation(); + + virtual void on_active(); + +private: + /** + * Set the RTL item + */ + void set_rtl_item(); + + /** + * Move to next RTL item + */ + void advance_rtl(); + + enum RTLState { + RTL_STATE_NONE = 0, + RTL_STATE_CLIMB, + RTL_STATE_RETURN, + RTL_STATE_DESCEND, + RTL_STATE_LOITER, + RTL_STATE_LAND, + RTL_STATE_LANDED, + } _rtl_state; + + control::BlockParamFloat _param_return_alt; + control::BlockParamFloat _param_descend_alt; + control::BlockParamFloat _param_land_delay; +}; + +#endif diff --git a/src/modules/navigator/datalinkloss_params.c b/src/modules/navigator/datalinkloss_params.c new file mode 100644 index 0000000000..bfe6ce7e18 --- /dev/null +++ b/src/modules/navigator/datalinkloss_params.c @@ -0,0 +1,98 @@ +/**************************************************************************** + * + * Copyright (c) 2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * AS IS AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file rtl_params.c + * + * Parameters for RTL + * + * @author Julian Oes + */ + +#include + +#include + +/* + * RTL parameters, accessible via MAVLink + */ + +/** + * Loiter radius after RTL (FW only) + * + * Default value of loiter radius after RTL (fixedwing only). + * + * @unit meters + * @min 0.0 + * @group RTL + */ +PARAM_DEFINE_FLOAT(RTL_LOITER_RAD, 50.0f); + +/** + * RTL altitude + * + * Altitude to fly back in RTL in meters + * + * @unit meters + * @min 0 + * @max 1 + * @group RTL + */ +PARAM_DEFINE_FLOAT(RTL_RETURN_ALT, 100); + + +/** + * RTL loiter altitude + * + * Stay at this altitude above home position after RTL descending. + * Land (i.e. slowly descend) from this altitude if autolanding allowed. + * + * @unit meters + * @min 0 + * @max 100 + * @group RTL + */ +PARAM_DEFINE_FLOAT(RTL_DESCEND_ALT, 20); + +/** + * RTL delay + * + * Delay after descend before landing in RTL mode. + * If set to -1 the system will not land but loiter at NAV_LAND_ALT. + * + * @unit seconds + * @min -1 + * @max + * @group RTL + */ +PARAM_DEFINE_FLOAT(RTL_LAND_DELAY, -1.0f); diff --git a/src/modules/navigator/module.mk b/src/modules/navigator/module.mk index b501989961..a1e42ec383 100644 --- a/src/modules/navigator/module.mk +++ b/src/modules/navigator/module.mk @@ -49,7 +49,9 @@ SRCS = navigator_main.cpp \ offboard.cpp \ mission_feasibility_checker.cpp \ geofence.cpp \ - geofence_params.c + geofence_params.c \ + datalinkloss.cpp \ + datalinkloss_params.c INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index 8edbb63b35..d0b2ed841f 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -35,6 +35,7 @@ * Helper class to access missions * @author Julian Oes * @author Anton Babushkin + * @author Thomas Gubler */ #ifndef NAVIGATOR_H @@ -57,6 +58,7 @@ #include "loiter.h" #include "rtl.h" #include "offboard.h" +#include "datalinkloss.h" #include "geofence.h" /** @@ -165,6 +167,7 @@ private: Loiter _loiter; /**< class that handles loiter */ RTL _rtl; /**< class that handles RTL */ Offboard _offboard; /**< class that handles offboard */ + DataLinkLoss _dataLinkLoss; /**< class that handles offboard */ NavigatorMode *_navigation_mode_array[NAVIGATOR_MODE_ARRAY_SIZE]; /**< array of navigation modes */ @@ -173,6 +176,7 @@ private: control::BlockParamFloat _param_loiter_radius; /**< loiter radius for fixedwing */ control::BlockParamFloat _param_acceptance_radius; /**< acceptance for takeoff */ + control::BlockParamInt _param_datalinkloss_obc; /**< if true: obc mode on data link loss enabled */ /** * Retrieve global position */ diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index ba46bd5681..1ce6770c98 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -40,6 +40,7 @@ * @author Jean Cyr * @author Julian Oes * @author Anton Babushkin + * @author Thomas Gubler */ #include @@ -125,10 +126,12 @@ Navigator::Navigator() : _loiter(this, "LOI"), _rtl(this, "RTL"), _offboard(this, "OFF"), + _dataLinkLoss(this, "DLL"), _can_loiter_at_sp(false), _pos_sp_triplet_updated(false), _param_loiter_radius(this, "LOITER_RAD"), - _param_acceptance_radius(this, "ACC_RAD") + _param_acceptance_radius(this, "ACC_RAD"), + _param_datalinkloss_obc(this, "DLL_OBC") { /* Create a list of our possible navigation types */ _navigation_mode_array[0] = &_mission; @@ -376,7 +379,13 @@ Navigator::task_main() _navigation_mode = &_rtl; break; case NAVIGATION_STATE_AUTO_RTGS: - _navigation_mode = &_rtl; /* TODO: change this to something else */ + /* Use complex data link loss mode only when enabled via param + * otherwise use rtl */ + if (_param_datalinkloss_obc.get() != 0) { + _navigation_mode = &_dataLinkLoss; + } else { + _navigation_mode = &_rtl; /* TODO: change this to something else */ + } break; case NAVIGATION_STATE_LAND: case NAVIGATION_STATE_TERMINATION: diff --git a/src/modules/navigator/navigator_params.c b/src/modules/navigator/navigator_params.c index 084afe3401..afaf1c3c31 100644 --- a/src/modules/navigator/navigator_params.c +++ b/src/modules/navigator/navigator_params.c @@ -37,6 +37,7 @@ * Parameters for navigator in general * * @author Julian Oes + * @author Thomas Gubler */ #include @@ -64,3 +65,13 @@ PARAM_DEFINE_FLOAT(NAV_LOITER_RAD, 50.0f); * @group Mission */ PARAM_DEFINE_FLOAT(NAV_ACC_RAD, 25.0f); + +/** + * Set OBC mode for data link loss + * + * If set to 1 the behaviour on data link loss is set to a mode according to the OBC rules + * + * @min 0 + * @group Mission + */ +PARAM_DEFINE_INT32(NAV_DLL_OBC, 0); From 8739308999b410ac8e2a92cf3e5fa63c5e18f5ba Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 20 Jul 2014 17:40:26 +0200 Subject: [PATCH 050/411] WIP, datalinkloss: implementing basic behavior --- src/modules/navigator/datalinkloss.cpp | 224 +++++--------------- src/modules/navigator/datalinkloss.h | 33 ++- src/modules/navigator/datalinkloss_params.c | 123 ++++++----- 3 files changed, 144 insertions(+), 236 deletions(-) diff --git a/src/modules/navigator/datalinkloss.cpp b/src/modules/navigator/datalinkloss.cpp index 17e85b2841..2bd80165d5 100644 --- a/src/modules/navigator/datalinkloss.cpp +++ b/src/modules/navigator/datalinkloss.cpp @@ -57,10 +57,14 @@ DataLinkLoss::DataLinkLoss(Navigator *navigator, const char *name) : MissionBlock(navigator, name), - _rtl_state(RTL_STATE_NONE), - _param_return_alt(this, "RETURN_ALT"), - _param_descend_alt(this, "DESCEND_ALT"), - _param_land_delay(this, "LAND_DELAY") + _dll_state(DLL_STATE_NONE), + _param_commsholdwaittime(this, "CH_T"), + _param_commsholdlat(this, "CH_LAT"), + _param_commsholdlon(this, "CH_LON"), + _param_commsholdalt(this, "CH_ALT"), + _param_airfieldhomelat(this, "AH_LAT"), + _param_airfieldhomelon(this, "AH_LON"), + _param_airfieldhomealt(this, "AH_ALT") { /* load initial params */ updateParams(); @@ -77,7 +81,7 @@ DataLinkLoss::on_inactive() { /* reset RTL state only if setpoint moved */ if (!_navigator->get_can_loiter_at_sp()) { - _rtl_state = RTL_STATE_NONE; + _dll_state = DLL_STATE_NONE; } } @@ -85,40 +89,40 @@ void DataLinkLoss::on_activation() { /* decide where to enter the RTL procedure when we switch into it */ - if (_rtl_state == RTL_STATE_NONE) { - /* for safety reasons don't go into RTL if landed */ - if (_navigator->get_vstatus()->condition_landed) { - _rtl_state = RTL_STATE_LANDED; - mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: no RTL when landed"); + //if (_rtl_state == RTL_STATE_NONE) { + //[> for safety reasons don't go into RTL if landed <] + //if (_navigator->get_vstatus()->condition_landed) { + //_rtl_state = RTL_STATE_LANDED; + //mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: no RTL when landed"); - /* if lower than return altitude, climb up first */ - } else if (_navigator->get_global_position()->alt < _navigator->get_home_position()->alt - + _param_return_alt.get()) { - _rtl_state = RTL_STATE_CLIMB; + //[> if lower than return altitude, climb up first <] + //} else if (_navigator->get_global_position()->alt < _navigator->get_home_position()->alt + //+ _param_return_alt.get()) { + //_rtl_state = RTL_STATE_CLIMB; - /* otherwise go straight to return */ - } else { - /* set altitude setpoint to current altitude */ - _rtl_state = RTL_STATE_RETURN; - _mission_item.altitude_is_relative = false; - _mission_item.altitude = _navigator->get_global_position()->alt; - } - } - - set_rtl_item(); + //[> otherwise go straight to return <] + //} else { + //[> set altitude setpoint to current altitude <] + //_rtl_state = RTL_STATE_RETURN; + //_mission_item.altitude_is_relative = false; + //_mission_item.altitude = _navigator->get_global_position()->alt; + //} + //} + _dll_state = DLL_STATE_FLYTOCOMMSHOLDWP; + set_dll_item(); } void DataLinkLoss::on_active() { - if (_rtl_state != RTL_STATE_LANDED && is_mission_item_reached()) { - advance_rtl(); - set_rtl_item(); + if (is_mission_item_reached()) { + advance_dll(); + set_dll_item(); } } void -DataLinkLoss::set_rtl_item() +DataLinkLoss::set_dll_item() { struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); @@ -128,146 +132,43 @@ DataLinkLoss::set_rtl_item() set_previous_pos_setpoint(); _navigator->set_can_loiter_at_sp(false); - switch (_rtl_state) { - case RTL_STATE_CLIMB: { - float climb_alt = _navigator->get_home_position()->alt + _param_return_alt.get(); - - _mission_item.lat = _navigator->get_global_position()->lat; - _mission_item.lon = _navigator->get_global_position()->lon; + switch (_dll_state) { + case DLL_STATE_FLYTOCOMMSHOLDWP: { + _mission_item.lat = (double)(_param_commsholdlat.get()) * 1.0e-7; + _mission_item.lon = (double)(_param_commsholdlon.get()) * 1.0e-7; _mission_item.altitude_is_relative = false; - _mission_item.altitude = climb_alt; - _mission_item.yaw = NAN; - _mission_item.loiter_radius = _navigator->get_loiter_radius(); - _mission_item.loiter_direction = 1; - _mission_item.nav_cmd = NAV_CMD_WAYPOINT; - _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); - _mission_item.time_inside = 0.0f; - _mission_item.pitch_min = 0.0f; - _mission_item.autocontinue = true; - _mission_item.origin = ORIGIN_ONBOARD; - - mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: climb to %d meters above home", - (int)(climb_alt - _navigator->get_home_position()->alt)); - break; - } - - case RTL_STATE_RETURN: { - _mission_item.lat = _navigator->get_home_position()->lat; - _mission_item.lon = _navigator->get_home_position()->lon; - // don't change altitude - - if (pos_sp_triplet->previous.valid) { - /* if previous setpoint is valid then use it to calculate heading to home */ - _mission_item.yaw = get_bearing_to_next_waypoint( - pos_sp_triplet->previous.lat, pos_sp_triplet->previous.lon, - _mission_item.lat, _mission_item.lon); - - } else { - /* else use current position */ - _mission_item.yaw = get_bearing_to_next_waypoint( - _navigator->get_global_position()->lat, _navigator->get_global_position()->lon, - _mission_item.lat, _mission_item.lon); - } - _mission_item.loiter_radius = _navigator->get_loiter_radius(); - _mission_item.loiter_direction = 1; - _mission_item.nav_cmd = NAV_CMD_WAYPOINT; - _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); - _mission_item.time_inside = 0.0f; - _mission_item.pitch_min = 0.0f; - _mission_item.autocontinue = true; - _mission_item.origin = ORIGIN_ONBOARD; - - mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: return at %d meters above home", - (int)(_mission_item.altitude - _navigator->get_home_position()->alt)); - break; - } - - case RTL_STATE_DESCEND: { - _mission_item.lat = _navigator->get_home_position()->lat; - _mission_item.lon = _navigator->get_home_position()->lon; - _mission_item.altitude_is_relative = false; - _mission_item.altitude = _navigator->get_home_position()->alt + _param_descend_alt.get(); + _mission_item.altitude = (double)(_param_commsholdalt.get()); _mission_item.yaw = NAN; _mission_item.loiter_radius = _navigator->get_loiter_radius(); _mission_item.loiter_direction = 1; _mission_item.nav_cmd = NAV_CMD_LOITER_TIME_LIMIT; _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); - _mission_item.time_inside = 0.0f; + _mission_item.time_inside = _param_commsholdwaittime.get() < 0.0f ? 0.0f : _param_commsholdwaittime.get(); _mission_item.pitch_min = 0.0f; - _mission_item.autocontinue = false; - _mission_item.origin = ORIGIN_ONBOARD; - - mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: descend to %d meters above home", - (int)(_mission_item.altitude - _navigator->get_home_position()->alt)); - break; - } - - case RTL_STATE_LOITER: { - bool autoland = _param_land_delay.get() > -DELAY_SIGMA; - - _mission_item.lat = _navigator->get_home_position()->lat; - _mission_item.lon = _navigator->get_home_position()->lon; - _mission_item.altitude_is_relative = false; - _mission_item.altitude = _navigator->get_home_position()->alt + _param_descend_alt.get(); - _mission_item.yaw = NAN; - _mission_item.loiter_radius = _navigator->get_loiter_radius(); - _mission_item.loiter_direction = 1; - _mission_item.nav_cmd = autoland ? NAV_CMD_LOITER_TIME_LIMIT : NAV_CMD_LOITER_UNLIMITED; - _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); - _mission_item.time_inside = _param_land_delay.get() < 0.0f ? 0.0f : _param_land_delay.get(); - _mission_item.pitch_min = 0.0f; - _mission_item.autocontinue = autoland; + _mission_item.autocontinue = true; _mission_item.origin = ORIGIN_ONBOARD; _navigator->set_can_loiter_at_sp(true); - - if (autoland) { - mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: loiter %.1fs", (double)_mission_item.time_inside); - - } else { - mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: completed, loiter"); - } break; } - - case RTL_STATE_LAND: { - _mission_item.lat = _navigator->get_home_position()->lat; - _mission_item.lon = _navigator->get_home_position()->lon; + case DLL_STATE_FLYTOAIRFIELDHOMEWP: { + _mission_item.lat = (double)(_param_airfieldhomelat.get()) * 1.0e-7; + _mission_item.lon = (double)(_param_airfieldhomelon.get()) * 1.0e-7; _mission_item.altitude_is_relative = false; - _mission_item.altitude = _navigator->get_home_position()->alt; + _mission_item.altitude = (double)(_param_airfieldhomealt.get()); _mission_item.yaw = NAN; _mission_item.loiter_radius = _navigator->get_loiter_radius(); _mission_item.loiter_direction = 1; - _mission_item.nav_cmd = NAV_CMD_LAND; + _mission_item.nav_cmd = NAV_CMD_LOITER_UNLIMITED; _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); - _mission_item.time_inside = 0.0f; + _mission_item.time_inside = _param_commsholdwaittime.get() < 0.0f ? 0.0f : _param_commsholdwaittime.get(); _mission_item.pitch_min = 0.0f; _mission_item.autocontinue = true; _mission_item.origin = ORIGIN_ONBOARD; - mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: land at home"); + _navigator->set_can_loiter_at_sp(true); break; } - - case RTL_STATE_LANDED: { - _mission_item.lat = _navigator->get_home_position()->lat; - _mission_item.lon = _navigator->get_home_position()->lon; - _mission_item.altitude_is_relative = false; - _mission_item.altitude = _navigator->get_home_position()->alt; - _mission_item.yaw = NAN; - _mission_item.loiter_radius = _navigator->get_loiter_radius(); - _mission_item.loiter_direction = 1; - _mission_item.nav_cmd = NAV_CMD_IDLE; - _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); - _mission_item.time_inside = 0.0f; - _mission_item.pitch_min = 0.0f; - _mission_item.autocontinue = true; - _mission_item.origin = ORIGIN_ONBOARD; - - mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: completed, landed"); - break; - } - default: break; } @@ -282,35 +183,16 @@ DataLinkLoss::set_rtl_item() } void -DataLinkLoss::advance_rtl() +DataLinkLoss::advance_dll() { - switch (_rtl_state) { - case RTL_STATE_CLIMB: - _rtl_state = RTL_STATE_RETURN; + switch (_dll_state) { + case DLL_STATE_NONE: + _dll_state = DLL_STATE_FLYTOCOMMSHOLDWP; break; - - case RTL_STATE_RETURN: - _rtl_state = RTL_STATE_DESCEND; + case DLL_STATE_FLYTOCOMMSHOLDWP: + //XXX check here if time is over are over + _dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP; break; - - case RTL_STATE_DESCEND: - /* only go to land if autoland is enabled */ - if (_param_land_delay.get() < -DELAY_SIGMA || _param_land_delay.get() > DELAY_SIGMA) { - _rtl_state = RTL_STATE_LOITER; - - } else { - _rtl_state = RTL_STATE_LAND; - } - break; - - case RTL_STATE_LOITER: - _rtl_state = RTL_STATE_LAND; - break; - - case RTL_STATE_LAND: - _rtl_state = RTL_STATE_LANDED; - break; - default: break; } diff --git a/src/modules/navigator/datalinkloss.h b/src/modules/navigator/datalinkloss.h index 242cfac8dc..101c88a254 100644 --- a/src/modules/navigator/datalinkloss.h +++ b/src/modules/navigator/datalinkloss.h @@ -68,28 +68,27 @@ public: private: /** - * Set the RTL item + * Set the DLL item */ - void set_rtl_item(); + void set_dll_item(); /** - * Move to next RTL item + * Move to next DLL item */ - void advance_rtl(); + void advance_dll(); - enum RTLState { - RTL_STATE_NONE = 0, - RTL_STATE_CLIMB, - RTL_STATE_RETURN, - RTL_STATE_DESCEND, - RTL_STATE_LOITER, - RTL_STATE_LAND, - RTL_STATE_LANDED, - } _rtl_state; + enum DLLState { + DLL_STATE_NONE = 0, + DLL_STATE_FLYTOCOMMSHOLDWP = 1, + DLL_STATE_FLYTOAIRFIELDHOMEWP = 2, + } _dll_state; - control::BlockParamFloat _param_return_alt; - control::BlockParamFloat _param_descend_alt; - control::BlockParamFloat _param_land_delay; + control::BlockParamFloat _param_commsholdwaittime; + control::BlockParamInt _param_commsholdlat; // * 1e7 + control::BlockParamInt _param_commsholdlon; // * 1e7 + control::BlockParamFloat _param_commsholdalt; + control::BlockParamInt _param_airfieldhomelat; // * 1e7 + control::BlockParamInt _param_airfieldhomelon; // * 1e7 + control::BlockParamFloat _param_airfieldhomealt; }; - #endif diff --git a/src/modules/navigator/datalinkloss_params.c b/src/modules/navigator/datalinkloss_params.c index bfe6ce7e18..a836fc8caf 100644 --- a/src/modules/navigator/datalinkloss_params.c +++ b/src/modules/navigator/datalinkloss_params.c @@ -32,11 +32,11 @@ ****************************************************************************/ /** - * @file rtl_params.c + * @file datalinkloss_params.c * - * Parameters for RTL + * Parameters for DLL * - * @author Julian Oes + * @author Thomas Gubler */ #include @@ -44,55 +44,82 @@ #include /* - * RTL parameters, accessible via MAVLink + * Data Link Loss parameters, accessible via MAVLink */ /** - * Loiter radius after RTL (FW only) + * Comms hold wait time * - * Default value of loiter radius after RTL (fixedwing only). - * - * @unit meters - * @min 0.0 - * @group RTL - */ -PARAM_DEFINE_FLOAT(RTL_LOITER_RAD, 50.0f); - -/** - * RTL altitude - * - * Altitude to fly back in RTL in meters - * - * @unit meters - * @min 0 - * @max 1 - * @group RTL - */ -PARAM_DEFINE_FLOAT(RTL_RETURN_ALT, 100); - - -/** - * RTL loiter altitude - * - * Stay at this altitude above home position after RTL descending. - * Land (i.e. slowly descend) from this altitude if autolanding allowed. - * - * @unit meters - * @min 0 - * @max 100 - * @group RTL - */ -PARAM_DEFINE_FLOAT(RTL_DESCEND_ALT, 20); - -/** - * RTL delay - * - * Delay after descend before landing in RTL mode. - * If set to -1 the system will not land but loiter at NAV_LAND_ALT. + * The amount of time in seconds the system should wait at the comms hold waypoint * * @unit seconds - * @min -1 - * @max - * @group RTL + * @min 0.0 + * @group DLL */ -PARAM_DEFINE_FLOAT(RTL_LAND_DELAY, -1.0f); +PARAM_DEFINE_FLOAT(NAV_DLL_CH_T, 120.0f); + +/** + * Comms hold Lat + * + * Latitude of comms hold waypoint + * + * @unit degrees * 1e7 + * @min 0.0 + * @group DLL + */ +PARAM_DEFINE_INT32(NAV_DLL_CH_LAT, 266072120); + +/** + * Comms hold Lon + * + * Longitude of comms hold waypoint + * + * @unit degrees * 1e7 + * @min 0.0 + * @group DLL + */ +PARAM_DEFINE_INT32(NAV_DLL_CH_LON, 1518453890); + +/** + * Comms hold alt + * + * Altitude of comms hold waypoint + * + * @unit m + * @min 0.0 + * @group DLL + */ +PARAM_DEFINE_FLOAT(NAV_DLL_CH_ALT, 600.0f); + +/** + * Airfield home Lat + * + * Latitude of airfield home waypoint + * + * @unit degrees * 1e7 + * @min 0.0 + * @group DLL + */ +PARAM_DEFINE_INT32(NAV_DLL_AH_LAT, 265847810); + +/** + * Airfield home Lon + * + * Longitude of airfield home waypoint + * + * @unit degrees * 1e7 + * @min 0.0 + * @group DLL + */ +PARAM_DEFINE_INT32(NAV_DLL_AH_LON, 1518423250); + +/** + * Airfield home alt + * + * Altitude of airfield home waypoint + * + * @unit m + * @min 0.0 + * @group DLL + */ +PARAM_DEFINE_FLOAT(NAV_DLL_AH_ALT, 600.0f); From dcf114aa65273d5d5ce522565fc364fc347ba3fe Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 20 Jul 2014 17:53:04 +0200 Subject: [PATCH 051/411] data link loss timeout as param --- src/modules/commander/commander.cpp | 8 +++++--- src/modules/commander/commander_params.c | 12 ++++++++++++ 2 files changed, 17 insertions(+), 3 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 7d4d677d09..8eba64c1d2 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -127,7 +127,6 @@ extern struct system_load_s system_load; #define POSITION_TIMEOUT (600 * 1000) /**< consider the local or global position estimate invalid after 600ms */ #define FAILSAFE_DEFAULT_TIMEOUT (3 * 1000 * 1000) /**< hysteresis time - the failsafe will trigger after 3 seconds in this state */ #define RC_TIMEOUT 500000 -#define DL_TIMEOUT 5 * 1000* 1000 #define OFFBOARD_TIMEOUT 500000 #define DIFFPRESS_TIMEOUT 2000000 @@ -650,6 +649,7 @@ int commander_thread_main(int argc, char *argv[]) param_t _param_takeoff_alt = param_find("NAV_TAKEOFF_ALT"); param_t _param_enable_parachute = param_find("NAV_PARACHUTE_EN"); param_t _param_enable_datalink_loss = param_find("COM_DL_LOSS_EN"); + param_t _param_datalink_loss_timeout = param_find("COM_DL_LOSS_T"); /* welcome user */ warnx("starting"); @@ -924,6 +924,7 @@ int commander_thread_main(int argc, char *argv[]) transition_result_t arming_ret; int32_t datalink_loss_enabled = false; + int32_t datalink_loss_timeout = 10; /* check which state machines for changes, clear "changed" flag */ bool arming_state_changed = false; @@ -983,6 +984,7 @@ int commander_thread_main(int argc, char *argv[]) param_get(_param_takeoff_alt, &takeoff_alt); param_get(_param_enable_parachute, ¶chute_enabled); param_get(_param_enable_datalink_loss, &datalink_loss_enabled); + param_get(_param_datalink_loss_timeout, &datalink_loss_timeout); } orb_check(sp_man_sub, &updated); @@ -1023,7 +1025,7 @@ int commander_thread_main(int argc, char *argv[]) if (mavlink_fd && telemetry_last_heartbeat[i] == 0 && telemetry.heartbeat_time > 0 && - hrt_elapsed_time(&telemetry.heartbeat_time) < DL_TIMEOUT) { + hrt_elapsed_time(&telemetry.heartbeat_time) < datalink_loss_timeout) { (void)rc_calibration_check(mavlink_fd); } @@ -1448,7 +1450,7 @@ int commander_thread_main(int argc, char *argv[]) /* data links check */ bool have_link = false; for (int i = 0; i < TELEMETRY_STATUS_ORB_ID_NUM; i++) { - if (telemetry_last_heartbeat[i] != 0 && hrt_elapsed_time(&telemetry_last_heartbeat[i]) < DL_TIMEOUT) { + if (telemetry_last_heartbeat[i] != 0 && hrt_elapsed_time(&telemetry_last_heartbeat[i]) < datalink_loss_timeout) { /* handle the case where data link was regained */ if (telemetry_lost[i]) { mavlink_log_critical(mavlink_fd, "data link %i regained", i); diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index 4750f9d5cb..25effbd217 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -95,3 +95,15 @@ PARAM_DEFINE_FLOAT(BAT_CAPACITY, -1.0f); * @max 1 */ PARAM_DEFINE_INT32(COM_DL_LOSS_EN, 0); + +/** + * Datalink timeout threshold + * + * After this amount of seconds the data link lost mode triggers + * + * @group commander + * @unit second + * @min 0 + * @max 1000 + */ +PARAM_DEFINE_INT32(COM_DL_LOSS_T, 10); From 86b9e367a6cc5791f83df3223190a470798c00ff Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 20 Jul 2014 18:23:41 +0200 Subject: [PATCH 052/411] introduce data link lost counter --- src/modules/commander/commander.cpp | 1 + src/modules/uORB/topics/vehicle_status.h | 1 + 2 files changed, 2 insertions(+) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 8eba64c1d2..46065fef10 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1477,6 +1477,7 @@ int commander_thread_main(int argc, char *argv[]) if (!status.data_link_lost) { mavlink_log_critical(mavlink_fd, "ALL DATA LINKS LOST"); status.data_link_lost = true; + status.data_link_lost_counter++; status_changed = true; } } diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index b46c00b758..eda1dfaec9 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 rc_input_blocked; /**< set if RC input should be ignored */ bool data_link_lost; /**< datalink to GCS lost */ + uint8_t data_link_lost_counter; /**< counts unique data link lost events */ bool offboard_control_signal_found_once; bool offboard_control_signal_lost; From a9a8f1435fa798b289aa4e4af9312041abdbcf94 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 21 Jul 2014 23:56:32 +0200 Subject: [PATCH 053/411] abort comm loss mode if counter above param and return home directly --- src/modules/commander/commander_params.c | 5 ++- src/modules/navigator/datalinkloss.cpp | 14 ++++++-- src/modules/navigator/datalinkloss.h | 37 +++++++++++---------- src/modules/navigator/datalinkloss_params.c | 11 ++++++ 4 files changed, 44 insertions(+), 23 deletions(-) diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index 25effbd217..980a7a2bbc 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -94,10 +94,9 @@ PARAM_DEFINE_FLOAT(BAT_CAPACITY, -1.0f); * @min 0 * @max 1 */ -PARAM_DEFINE_INT32(COM_DL_LOSS_EN, 0); +PARAM_DEFINE_INT32(DL_LOSS_EN, 0); -/** - * Datalink timeout threshold + /** Datalink timeout threshold * * After this amount of seconds the data link lost mode triggers * diff --git a/src/modules/navigator/datalinkloss.cpp b/src/modules/navigator/datalinkloss.cpp index 2bd80165d5..a98e21139e 100644 --- a/src/modules/navigator/datalinkloss.cpp +++ b/src/modules/navigator/datalinkloss.cpp @@ -57,14 +57,16 @@ DataLinkLoss::DataLinkLoss(Navigator *navigator, const char *name) : MissionBlock(navigator, name), - _dll_state(DLL_STATE_NONE), + _vehicleStatus(&getSubscriptions(), ORB_ID(vehicle_status), 100), _param_commsholdwaittime(this, "CH_T"), _param_commsholdlat(this, "CH_LAT"), _param_commsholdlon(this, "CH_LON"), _param_commsholdalt(this, "CH_ALT"), _param_airfieldhomelat(this, "AH_LAT"), _param_airfieldhomelon(this, "AH_LON"), - _param_airfieldhomealt(this, "AH_ALT") + _param_airfieldhomealt(this, "AH_ALT"), + _param_numberdatalinklosses(this, "DLL_N"), + _dll_state(DLL_STATE_NONE) { /* load initial params */ updateParams(); @@ -187,7 +189,13 @@ DataLinkLoss::advance_dll() { switch (_dll_state) { case DLL_STATE_NONE: - _dll_state = DLL_STATE_FLYTOCOMMSHOLDWP; + /* Check the number of data link losses. If above home fly home directly */ + updateSubscriptions(); + if (_vehicleStatus.data_link_lost_counter > _param_numberdatalinklosses.get()) { + _dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP; + } else { + _dll_state = DLL_STATE_FLYTOCOMMSHOLDWP; + } break; case DLL_STATE_FLYTOCOMMSHOLDWP: //XXX check here if time is over are over diff --git a/src/modules/navigator/datalinkloss.h b/src/modules/navigator/datalinkloss.h index 101c88a254..650cc7bc54 100644 --- a/src/modules/navigator/datalinkloss.h +++ b/src/modules/navigator/datalinkloss.h @@ -43,10 +43,7 @@ #include #include -#include -#include -#include -#include +#include #include "navigator_mode.h" #include "mission_block.h" @@ -67,6 +64,25 @@ public: virtual void on_active(); private: + /* Subscriptions */ + uORB::Subscription _vehicleStatus; + + /* Params */ + control::BlockParamFloat _param_commsholdwaittime; + control::BlockParamInt _param_commsholdlat; // * 1e7 + control::BlockParamInt _param_commsholdlon; // * 1e7 + control::BlockParamFloat _param_commsholdalt; + control::BlockParamInt _param_airfieldhomelat; // * 1e7 + control::BlockParamInt _param_airfieldhomelon; // * 1e7 + control::BlockParamFloat _param_airfieldhomealt; + control::BlockParamInt _param_numberdatalinklosses; + + enum DLLState { + DLL_STATE_NONE = 0, + DLL_STATE_FLYTOCOMMSHOLDWP = 1, + DLL_STATE_FLYTOAIRFIELDHOMEWP = 2, + } _dll_state; + /** * Set the DLL item */ @@ -77,18 +93,5 @@ private: */ void advance_dll(); - enum DLLState { - DLL_STATE_NONE = 0, - DLL_STATE_FLYTOCOMMSHOLDWP = 1, - DLL_STATE_FLYTOAIRFIELDHOMEWP = 2, - } _dll_state; - - control::BlockParamFloat _param_commsholdwaittime; - control::BlockParamInt _param_commsholdlat; // * 1e7 - control::BlockParamInt _param_commsholdlon; // * 1e7 - control::BlockParamFloat _param_commsholdalt; - control::BlockParamInt _param_airfieldhomelat; // * 1e7 - control::BlockParamInt _param_airfieldhomelon; // * 1e7 - control::BlockParamFloat _param_airfieldhomealt; }; #endif diff --git a/src/modules/navigator/datalinkloss_params.c b/src/modules/navigator/datalinkloss_params.c index a836fc8caf..038c80a1a1 100644 --- a/src/modules/navigator/datalinkloss_params.c +++ b/src/modules/navigator/datalinkloss_params.c @@ -123,3 +123,14 @@ PARAM_DEFINE_INT32(NAV_DLL_AH_LON, 1518423250); * @group DLL */ PARAM_DEFINE_FLOAT(NAV_DLL_AH_ALT, 600.0f); + +/** + * Number of allowed Datalink timeouts + * + * After more than this number of data link timeouts the aircraft returns home directly + * + * @group commander + * @min 0 + * @max 1000 + */ +PARAM_DEFINE_INT32(NAV_DLL_N, 2); From 7bc0e26734a0319295e488e413db8f618b9b621c Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 23 Jul 2014 11:37:27 +0200 Subject: [PATCH 054/411] mavlink external sp: support for local pos and vel --- src/modules/mavlink/mavlink_receiver.cpp | 46 ++++++++++++++++++++++-- src/modules/mavlink/mavlink_receiver.h | 2 +- 2 files changed, 45 insertions(+), 3 deletions(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index b39aebf9e2..b7cddf5231 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -37,6 +37,7 @@ * * @author Lorenz Meier * @author Anton Babushkin + * @author Thomas Gubler */ /* XXX trim includes */ @@ -103,11 +104,11 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) : _cmd_pub(-1), _flow_pub(-1), _offboard_control_sp_pub(-1), - _local_pos_sp_pub(-1), _global_vel_sp_pub(-1), _att_sp_pub(-1), _rates_sp_pub(-1), _force_sp_pub(-1), + _pos_sp_triplet_pub(-1), _vicon_position_pub(-1), _telemetry_status_pub(-1), _rc_pub(-1), @@ -487,8 +488,49 @@ MavlinkReceiver::handle_message_local_ned_position_setpoint_external(mavlink_mes orb_publish(ORB_ID(vehicle_force_setpoint), _force_sp_pub, &force_sp); } } else { + /* It's not a force setpoint: publish to setpoint triplet topic */ + struct position_setpoint_triplet_s pos_sp_triplet; + pos_sp_triplet.previous.valid = false; + pos_sp_triplet.next.valid = false; + pos_sp_triplet.current.valid = true; + + /* set the local pos values if the setpoint type is 'local pos' and none + * of the local pos fields is set to 'ignore' */ + if (offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED && + !offboard_control_sp_ignore_position(offboard_control_sp, 0) && + !offboard_control_sp_ignore_position(offboard_control_sp, 1) && + !offboard_control_sp_ignore_position(offboard_control_sp, 2)) { + pos_sp_triplet.current.type = SETPOINT_TYPE_POSITION; //XXX support others + pos_sp_triplet.current.position_valid = true; + pos_sp_triplet.current.x = offboard_control_sp.position[0]; + pos_sp_triplet.current.y = offboard_control_sp.position[1]; + pos_sp_triplet.current.z = offboard_control_sp.position[2]; + } + + /* set the local vel values if the setpoint type is 'local pos' and none + * of the local vel fields is set to 'ignore' */ + if (offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED && + !offboard_control_sp_ignore_velocity(offboard_control_sp, 0) && + !offboard_control_sp_ignore_velocity(offboard_control_sp, 1) && + !offboard_control_sp_ignore_velocity(offboard_control_sp, 2)) { + pos_sp_triplet.current.type = SETPOINT_TYPE_POSITION; //XXX support others + pos_sp_triplet.current.velocity_valid = true; + pos_sp_triplet.current.vx = offboard_control_sp.velocity[0]; + pos_sp_triplet.current.vy = offboard_control_sp.velocity[1]; + pos_sp_triplet.current.vz = offboard_control_sp.velocity[2]; + } + + //XXX handle global pos setpoints (different MAV frames) + + + if (_pos_sp_triplet_pub < 0) { + _pos_sp_triplet_pub = orb_advertise(ORB_ID(position_setpoint_triplet), + &pos_sp_triplet); + } else { + orb_publish(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_pub, + &pos_sp_triplet); + } - //XXX: copy to and publish setpoint triplet here } } diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index a6553cb0ae..a2ed4264f5 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -143,11 +143,11 @@ private: orb_advert_t _cmd_pub; orb_advert_t _flow_pub; orb_advert_t _offboard_control_sp_pub; - orb_advert_t _local_pos_sp_pub; orb_advert_t _global_vel_sp_pub; orb_advert_t _att_sp_pub; orb_advert_t _rates_sp_pub; orb_advert_t _force_sp_pub; + orb_advert_t _pos_sp_triplet_pub; orb_advert_t _vicon_position_pub; orb_advert_t _telemetry_status_pub; orb_advert_t _rc_pub; From 24f380137ecb91fb9647e22e1d29c13da5fc0357 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 23 Jul 2014 22:58:19 +0200 Subject: [PATCH 055/411] add method to block fallback to mission failsafe navigation modes can use a flag in mission_result to tell the commander to not switch back to mission --- src/modules/commander/commander.cpp | 5 +-- .../commander/state_machine_helper.cpp | 3 +- src/modules/commander/state_machine_helper.h | 2 +- src/modules/navigator/datalinkloss.cpp | 2 ++ src/modules/navigator/mission.cpp | 32 ++++--------------- src/modules/navigator/mission.h | 8 ----- src/modules/navigator/navigator.h | 13 +++++++- src/modules/navigator/navigator_main.cpp | 22 +++++++++++++ src/modules/navigator/navigator_mode.cpp | 3 ++ src/modules/uORB/topics/mission_result.h | 1 + 10 files changed, 53 insertions(+), 38 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 5c89e01238..cb09a68e31 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1461,7 +1461,7 @@ int commander_thread_main(int argc, char *argv[]) for (int i = 0; i < TELEMETRY_STATUS_ORB_ID_NUM; i++) { if (telemetry_last_heartbeat[i] != 0 && hrt_elapsed_time(&telemetry_last_heartbeat[i]) < datalink_loss_timeout) { /* handle the case where data link was regained */ - if (telemetry_lost[i]) { + if (telemetry_lost[i]) {//XXX also add hysteresis here mavlink_log_critical(mavlink_fd, "data link %i regained", i); telemetry_lost[i] = false; } @@ -1545,7 +1545,8 @@ int commander_thread_main(int argc, char *argv[]) /* now set navigation state according to failsafe and main state */ bool nav_state_changed = set_nav_state(&status, (bool)datalink_loss_enabled, - mission_result.finished); + mission_result.finished, + mission_result.stay_in_failsafe); // TODO handle mode changes by commands if (main_state_changed) { diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 7b26e3e8cb..4e1cfb9873 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -435,7 +435,8 @@ transition_result_t hil_state_transition(hil_state_t new_state, int status_pub, /** * Check failsafe and main status and set navigation status for navigator accordingly */ -bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_enabled, const bool mission_finished) +bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_enabled, const bool mission_finished, + const bool stay_in_failsafe) { navigation_state_t nav_state_old = status->nav_state; diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h index bb1b87e712..4285d29777 100644 --- a/src/modules/commander/state_machine_helper.h +++ b/src/modules/commander/state_machine_helper.h @@ -63,7 +63,7 @@ transition_result_t main_state_transition(struct vehicle_status_s *current_state transition_result_t hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_state, const int mavlink_fd); -bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_enabled, const bool mission_finished); +bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_enabled, const bool mission_finished, const bool stay_in_failsafe); int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd); diff --git a/src/modules/navigator/datalinkloss.cpp b/src/modules/navigator/datalinkloss.cpp index a98e21139e..52f263aff0 100644 --- a/src/modules/navigator/datalinkloss.cpp +++ b/src/modules/navigator/datalinkloss.cpp @@ -200,6 +200,8 @@ DataLinkLoss::advance_dll() case DLL_STATE_FLYTOCOMMSHOLDWP: //XXX check here if time is over are over _dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP; + _navigator->get_mission_result()->stay_in_failsafe = true; + _navigator->publish_mission_result(); break; default: break; diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index ba766cd10f..7ce0e2f891 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -68,8 +68,6 @@ Mission::Mission(Navigator *navigator, const char *name) : _current_offboard_mission_index(-1), _need_takeoff(true), _takeoff(false), - _mission_result_pub(-1), - _mission_result({0}), _mission_type(MISSION_TYPE_NONE), _inited(false), _dist_1wp_ok(false) @@ -577,18 +575,18 @@ void Mission::report_mission_item_reached() { if (_mission_type == MISSION_TYPE_OFFBOARD) { - _mission_result.reached = true; - _mission_result.seq_reached = _current_offboard_mission_index; + _navigator->get_mission_result()->reached = true; + _navigator->get_mission_result()->seq_reached = _current_offboard_mission_index; } - publish_mission_result(); + _navigator->publish_mission_result(); } void Mission::report_current_offboard_mission_item() { warnx("current offboard mission index: %d", _current_offboard_mission_index); - _mission_result.seq_current = _current_offboard_mission_index; - publish_mission_result(); + _navigator->get_mission_result()->seq_current = _current_offboard_mission_index; + _navigator->publish_mission_result(); save_offboard_mission_state(); } @@ -596,23 +594,7 @@ Mission::report_current_offboard_mission_item() void Mission::report_mission_finished() { - _mission_result.finished = true; - publish_mission_result(); + _navigator->get_mission_result()->finished = true; + _navigator->publish_mission_result(); } -void -Mission::publish_mission_result() -{ - /* lazily publish the mission result only once available */ - if (_mission_result_pub > 0) { - /* publish 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); - } - /* reset reached bool */ - _mission_result.reached = false; - _mission_result.finished = false; -} diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h index 4da6a11553..1b8c8c8740 100644 --- a/src/modules/navigator/mission.h +++ b/src/modules/navigator/mission.h @@ -128,11 +128,6 @@ private: */ void report_mission_finished(); - /** - * Publish the mission result so commander and mavlink know what is going on - */ - void publish_mission_result(); - control::BlockParamInt _param_onboard_enabled; control::BlockParamFloat _param_takeoff_alt; control::BlockParamFloat _param_dist_1wp; @@ -145,9 +140,6 @@ private: bool _need_takeoff; bool _takeoff; - orb_advert_t _mission_result_pub; - struct mission_result_s _mission_result; - enum { MISSION_TYPE_NONE, MISSION_TYPE_ONBOARD, diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index d0b2ed841f..363877bb83 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -52,6 +52,7 @@ #include #include #include +#include #include "navigator_mode.h" #include "mission.h" @@ -102,6 +103,11 @@ public: */ void load_fence_from_file(const char *filename); + /** + * Publish the mission result so commander and mavlink know what is going on + */ + void publish_mission_result(); + /** * Setters */ @@ -115,7 +121,9 @@ public: struct vehicle_control_mode_s* get_control_mode() { return &_control_mode; } struct vehicle_global_position_s* get_global_position() { return &_global_pos; } struct home_position_s* get_home_position() { return &_home_pos; } - struct position_setpoint_triplet_s* get_position_setpoint_triplet() { return &_pos_sp_triplet; } + struct position_setpoint_triplet_s* get_position_setpoint_triplet() { return &_pos_sp_triplet; } + struct mission_result_s* get_mission_result() { return &_mission_result; } + int get_onboard_mission_sub() { return _onboard_mission_sub; } int get_offboard_mission_sub() { return _offboard_mission_sub; } int get_offboard_control_sp_sub() { return _offboard_control_sp_sub; } @@ -143,6 +151,7 @@ private: int _param_update_sub; /**< param update subscription */ orb_advert_t _pos_sp_triplet_pub; /**< publish position setpoint triplet */ + orb_advert_t _mission_result_pub; vehicle_status_s _vstatus; /**< vehicle status */ vehicle_control_mode_s _control_mode; /**< vehicle control mode */ @@ -152,6 +161,8 @@ private: navigation_capabilities_s _nav_caps; /**< navigation capabilities */ position_setpoint_triplet_s _pos_sp_triplet; /**< triplet of position setpoints */ + mission_result_s _mission_result; + bool _mission_item_valid; /**< flags if the current mission item is valid */ perf_counter_t _loop_perf; /**< loop performance counter */ diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 1ce6770c98..77124e8f65 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -108,6 +108,7 @@ Navigator::Navigator() : _offboard_mission_sub(-1), _param_update_sub(-1), _pos_sp_triplet_pub(-1), + _mission_result_pub(-1), _vstatus{}, _control_mode{}, _global_pos{}, @@ -115,6 +116,7 @@ Navigator::Navigator() : _mission_item{}, _nav_caps{}, _pos_sp_triplet{}, + _mission_result{}, _mission_item_valid(false), _loop_perf(perf_alloc(PC_ELAPSED, "navigator")), _geofence{}, @@ -370,6 +372,9 @@ Navigator::task_main() _can_loiter_at_sp = false; break; case NAVIGATION_STATE_AUTO_MISSION: + /* Some failsafe modes prohibit the fallback to mission + * usually this is done after some time to make sure + * that the full failsafe operation is performed */ _navigation_mode = &_mission; break; case NAVIGATION_STATE_AUTO_LOITER: @@ -553,3 +558,20 @@ int navigator_main(int argc, char *argv[]) return 0; } + +void +Navigator::publish_mission_result() +{ + /* lazily publish the mission result only once available */ + if (_mission_result_pub > 0) { + /* publish 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); + } + /* reset reached bool */ + _mission_result.reached = false; + _mission_result.finished = false; +} diff --git a/src/modules/navigator/navigator_mode.cpp b/src/modules/navigator/navigator_mode.cpp index f432156652..3c6754c556 100644 --- a/src/modules/navigator/navigator_mode.cpp +++ b/src/modules/navigator/navigator_mode.cpp @@ -63,6 +63,9 @@ NavigatorMode::run(bool active) { if (_first_run) { /* first run */ _first_run = false; + /* Reset stay in failsafe flag */ + _navigator->get_mission_result()->stay_in_failsafe = true; + _navigator->publish_mission_result(); on_activation(); } else { diff --git a/src/modules/uORB/topics/mission_result.h b/src/modules/uORB/topics/mission_result.h index beb797e624..65ddfb4ad4 100644 --- a/src/modules/uORB/topics/mission_result.h +++ b/src/modules/uORB/topics/mission_result.h @@ -57,6 +57,7 @@ struct mission_result_s unsigned seq_current; /**< Sequence of the current mission item */ bool reached; /**< true if mission has been reached */ bool finished; /**< true if mission has been completed */ + bool stay_in_failsafe; /**< true if the commander should not switch out of the failsafe mode*/ }; /** From 756ea3019ec3a50623d846c019c6474488273a34 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 23 Jul 2014 23:00:34 +0200 Subject: [PATCH 056/411] datalink loss: fix type error --- src/modules/navigator/datalinkloss.cpp | 24 ++---------------------- 1 file changed, 2 insertions(+), 22 deletions(-) diff --git a/src/modules/navigator/datalinkloss.cpp b/src/modules/navigator/datalinkloss.cpp index 52f263aff0..91bb5b1104 100644 --- a/src/modules/navigator/datalinkloss.cpp +++ b/src/modules/navigator/datalinkloss.cpp @@ -90,26 +90,6 @@ DataLinkLoss::on_inactive() void DataLinkLoss::on_activation() { - /* decide where to enter the RTL procedure when we switch into it */ - //if (_rtl_state == RTL_STATE_NONE) { - //[> for safety reasons don't go into RTL if landed <] - //if (_navigator->get_vstatus()->condition_landed) { - //_rtl_state = RTL_STATE_LANDED; - //mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: no RTL when landed"); - - //[> if lower than return altitude, climb up first <] - //} else if (_navigator->get_global_position()->alt < _navigator->get_home_position()->alt - //+ _param_return_alt.get()) { - //_rtl_state = RTL_STATE_CLIMB; - - //[> otherwise go straight to return <] - //} else { - //[> set altitude setpoint to current altitude <] - //_rtl_state = RTL_STATE_RETURN; - //_mission_item.altitude_is_relative = false; - //_mission_item.altitude = _navigator->get_global_position()->alt; - //} - //} _dll_state = DLL_STATE_FLYTOCOMMSHOLDWP; set_dll_item(); } @@ -139,7 +119,7 @@ DataLinkLoss::set_dll_item() _mission_item.lat = (double)(_param_commsholdlat.get()) * 1.0e-7; _mission_item.lon = (double)(_param_commsholdlon.get()) * 1.0e-7; _mission_item.altitude_is_relative = false; - _mission_item.altitude = (double)(_param_commsholdalt.get()); + _mission_item.altitude = _param_commsholdalt.get(); _mission_item.yaw = NAN; _mission_item.loiter_radius = _navigator->get_loiter_radius(); _mission_item.loiter_direction = 1; @@ -157,7 +137,7 @@ DataLinkLoss::set_dll_item() _mission_item.lat = (double)(_param_airfieldhomelat.get()) * 1.0e-7; _mission_item.lon = (double)(_param_airfieldhomelon.get()) * 1.0e-7; _mission_item.altitude_is_relative = false; - _mission_item.altitude = (double)(_param_airfieldhomealt.get()); + _mission_item.altitude = _param_airfieldhomealt.get(); _mission_item.yaw = NAN; _mission_item.loiter_radius = _navigator->get_loiter_radius(); _mission_item.loiter_direction = 1; From f10d1277a5e795256a247cb6f130cefba12c5ffc Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 23 Jul 2014 23:07:11 +0200 Subject: [PATCH 057/411] navigator: fix comment --- src/modules/navigator/navigator.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index 363877bb83..fc0d47b388 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -178,7 +178,7 @@ private: Loiter _loiter; /**< class that handles loiter */ RTL _rtl; /**< class that handles RTL */ Offboard _offboard; /**< class that handles offboard */ - DataLinkLoss _dataLinkLoss; /**< class that handles offboard */ + DataLinkLoss _dataLinkLoss; /**< class that handles the OBC datalink loss mode */ NavigatorMode *_navigation_mode_array[NAVIGATOR_MODE_ARRAY_SIZE]; /**< array of navigation modes */ From a35814d15b1317f73f325e98f0500f5fd1233583 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 24 Jul 2014 08:57:58 +0200 Subject: [PATCH 058/411] dl loss: correct timeout, add hysteresis also for regain --- src/modules/commander/commander.cpp | 21 ++++++++++++++++----- src/modules/commander/commander_params.c | 18 +++++++++++++++--- 2 files changed, 31 insertions(+), 8 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index cb09a68e31..fe1974c889 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -650,6 +650,7 @@ int commander_thread_main(int argc, char *argv[]) param_t _param_enable_parachute = param_find("NAV_PARACHUTE_EN"); param_t _param_enable_datalink_loss = param_find("COM_DL_LOSS_EN"); param_t _param_datalink_loss_timeout = param_find("COM_DL_LOSS_T"); + param_t _param_datalink_regain_timeout = param_find("COM_DL_REG_T"); /* welcome user */ warnx("starting"); @@ -841,11 +842,13 @@ int commander_thread_main(int argc, char *argv[]) /* Subscribe to telemetry status topics */ int telemetry_subs[TELEMETRY_STATUS_ORB_ID_NUM]; uint64_t telemetry_last_heartbeat[TELEMETRY_STATUS_ORB_ID_NUM]; + uint64_t telemetry_last_dl_loss[TELEMETRY_STATUS_ORB_ID_NUM]; bool telemetry_lost[TELEMETRY_STATUS_ORB_ID_NUM]; for (int i = 0; i < TELEMETRY_STATUS_ORB_ID_NUM; i++) { telemetry_subs[i] = orb_subscribe(telemetry_status_orb_id[i]); telemetry_last_heartbeat[i] = 0; + telemetry_last_dl_loss[i] = 0; telemetry_lost[i] = true; } @@ -930,6 +933,7 @@ int commander_thread_main(int argc, char *argv[]) int32_t datalink_loss_enabled = false; int32_t datalink_loss_timeout = 10; + int32_t datalink_regain_timeout = 0; /* check which state machines for changes, clear "changed" flag */ bool arming_state_changed = false; @@ -990,6 +994,7 @@ int commander_thread_main(int argc, char *argv[]) param_get(_param_enable_parachute, ¶chute_enabled); param_get(_param_enable_datalink_loss, &datalink_loss_enabled); param_get(_param_datalink_loss_timeout, &datalink_loss_timeout); + param_get(_param_datalink_regain_timeout, &datalink_regain_timeout); } orb_check(sp_man_sub, &updated); @@ -1030,7 +1035,7 @@ int commander_thread_main(int argc, char *argv[]) if (mavlink_fd && telemetry_last_heartbeat[i] == 0 && telemetry.heartbeat_time > 0 && - hrt_elapsed_time(&telemetry.heartbeat_time) < datalink_loss_timeout) { + hrt_elapsed_time(&telemetry.heartbeat_time) < datalink_loss_timeout * 1e6) { (void)rc_calibration_check(mavlink_fd); } @@ -1459,15 +1464,21 @@ int commander_thread_main(int argc, char *argv[]) /* data links check */ bool have_link = false; for (int i = 0; i < TELEMETRY_STATUS_ORB_ID_NUM; i++) { - if (telemetry_last_heartbeat[i] != 0 && hrt_elapsed_time(&telemetry_last_heartbeat[i]) < datalink_loss_timeout) { - /* handle the case where data link was regained */ - if (telemetry_lost[i]) {//XXX also add hysteresis here + if (telemetry_last_heartbeat[i] != 0 && + hrt_elapsed_time(&telemetry_last_heartbeat[i]) < datalink_loss_timeout * 1e6) { + /* handle the case where data link was regained, + * accept datalink as healthy only after datalink_regain_timeout seconds + * */ + if (telemetry_lost[i] && + hrt_elapsed_time(&telemetry_last_dl_loss[i]) > datalink_regain_timeout * 1e6) { + mavlink_log_critical(mavlink_fd, "data link %i regained", i); telemetry_lost[i] = false; + have_link = true; } - have_link = true; } else { + telemetry_last_dl_loss[i] = hrt_absolute_time(); if (!telemetry_lost[i]) { mavlink_log_critical(mavlink_fd, "data link %i lost", i); telemetry_lost[i] = true; diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index f73ae71f30..3d1e231c6a 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -106,13 +106,25 @@ PARAM_DEFINE_FLOAT(BAT_CAPACITY, -1.0f); */ PARAM_DEFINE_INT32(DL_LOSS_EN, 0); - /** Datalink timeout threshold + /** Datalink loss time threshold * - * After this amount of seconds the data link lost mode triggers + * After this amount of seconds without datalink the data link lost mode triggers * * @group commander * @unit second * @min 0 - * @max 1000 + * @max 30 */ PARAM_DEFINE_INT32(COM_DL_LOSS_T, 10); + +/** Datalink regain time threshold + * + * After a data link loss: after this this amount of seconds with a healthy datalink the 'datalink loss' + * flag is set back to false + * + * @group commander + * @unit second + * @min 0 + * @max 30 + */ +PARAM_DEFINE_INT32(COM_DL_REG_T, 0); From bffa9e3fa83b84b51b2446291682519433fd3fff Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 24 Jul 2014 19:27:40 +0200 Subject: [PATCH 059/411] navigator: add skeleton of FW engine failure mode --- src/modules/navigator/datalinkloss.h | 2 +- src/modules/navigator/enginefailure.cpp | 147 +++++++++++++++++++++++ src/modules/navigator/enginefailure.h | 83 +++++++++++++ src/modules/navigator/module.mk | 1 + src/modules/navigator/navigator.h | 3 + src/modules/navigator/navigator_main.cpp | 1 + 6 files changed, 236 insertions(+), 1 deletion(-) create mode 100644 src/modules/navigator/enginefailure.cpp create mode 100644 src/modules/navigator/enginefailure.h diff --git a/src/modules/navigator/datalinkloss.h b/src/modules/navigator/datalinkloss.h index 650cc7bc54..5a46b5700e 100644 --- a/src/modules/navigator/datalinkloss.h +++ b/src/modules/navigator/datalinkloss.h @@ -31,7 +31,7 @@ * ****************************************************************************/ /** - * @file datalinkloss.cpp + * @file datalinkloss.h * Helper class for Data Link Loss Mode acording to the OBC rules * * @author Thomas Gubler diff --git a/src/modules/navigator/enginefailure.cpp b/src/modules/navigator/enginefailure.cpp new file mode 100644 index 0000000000..ea654c5fd4 --- /dev/null +++ b/src/modules/navigator/enginefailure.cpp @@ -0,0 +1,147 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + + /* @file enginefailure.cpp + * Helper class for a fixedwing engine failure mode + * + * @author Thomas Gubler + */ +#include +#include +#include +#include + +#include +#include +#include + +#include +#include + +#include "navigator.h" +#include "enginefailure.h" + +#define DELAY_SIGMA 0.01f + +EngineFailure::EngineFailure(Navigator *navigator, const char *name) : + MissionBlock(navigator, name), + _ef_state(EF_STATE_NONE) +{ + /* load initial params */ + updateParams(); + /* initial reset */ + on_inactive(); +} + +EngineFailure::~EngineFailure() +{ +} + +void +EngineFailure::on_inactive() +{ + _ef_state = EF_STATE_NONE; +} + +void +EngineFailure::on_activation() +{ + _ef_state = EF_STATE_LOITERDOWN; + set_ef_item(); +} + +void +EngineFailure::on_active() +{ + if (is_mission_item_reached()) { + advance_ef(); + set_ef_item(); + } +} + +void +EngineFailure::set_ef_item() +{ + struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); + + /* make sure we have the latest params */ + updateParams(); + + set_previous_pos_setpoint(); + _navigator->set_can_loiter_at_sp(false); + + switch (_ef_state) { + case EF_STATE_LOITERDOWN: { + //XXX create mission item at ground (below?) here + + //_mission_item.lat = (double)(_param_airfieldhomelat.get()) * 1.0e-7; + //_mission_item.lon = (double)(_param_airfieldhomelon.get()) * 1.0e-7; + //_mission_item.altitude_is_relative = false; + //_mission_item.altitude = _param_airfieldhomealt.get(); + //_mission_item.yaw = NAN; + //_mission_item.loiter_radius = _navigator->get_loiter_radius(); + //_mission_item.loiter_direction = 1; + //_mission_item.nav_cmd = NAV_CMD_LOITER_UNLIMITED; + //_mission_item.acceptance_radius = _navigator->get_acceptance_radius(); + //_mission_item.time_inside = _param_commsholdwaittime.get() < 0.0f ? 0.0f : _param_commsholdwaittime.get(); + //_mission_item.pitch_min = 0.0f; + //_mission_item.autocontinue = true; + //_mission_item.origin = ORIGIN_ONBOARD; + + //_navigator->set_can_loiter_at_sp(true); + break; + } + default: + break; + } + + reset_mission_item_reached(); + + /* convert mission item to current position setpoint and make it valid */ + mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current); + pos_sp_triplet->next.valid = false; + + _navigator->set_position_setpoint_triplet_updated(); +} + +void +EngineFailure::advance_ef() +{ + switch (_ef_state) { + case EF_STATE_NONE: + _ef_state = EF_STATE_LOITERDOWN; + break; + default: + break; + } +} diff --git a/src/modules/navigator/enginefailure.h b/src/modules/navigator/enginefailure.h new file mode 100644 index 0000000000..2c48c2fce2 --- /dev/null +++ b/src/modules/navigator/enginefailure.h @@ -0,0 +1,83 @@ +/*************************************************************************** + * + * Copyright (c) 2013-2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ +/** + * @file enginefailure.h + * Helper class for a fixedwing engine failure mode + * + * @author Thomas Gubler + */ + +#ifndef NAVIGATOR_ENGINEFAILURE_H +#define NAVIGATOR_ENGINEFAILURE_H + +#include +#include + +#include + +#include "navigator_mode.h" +#include "mission_block.h" + +class Navigator; + +class EngineFailure : public MissionBlock +{ +public: + EngineFailure(Navigator *navigator, const char *name); + + ~EngineFailure(); + + virtual void on_inactive(); + + virtual void on_activation(); + + virtual void on_active(); + +private: + enum EFState { + EF_STATE_NONE = 0, + EF_STATE_LOITERDOWN = 1, + } _ef_state; + + /** + * Set the DLL item + */ + void set_ef_item(); + + /** + * Move to next EF item + */ + void advance_ef(); + +}; +#endif diff --git a/src/modules/navigator/module.mk b/src/modules/navigator/module.mk index a1e42ec383..0539087dfd 100644 --- a/src/modules/navigator/module.mk +++ b/src/modules/navigator/module.mk @@ -51,6 +51,7 @@ SRCS = navigator_main.cpp \ geofence.cpp \ geofence_params.c \ datalinkloss.cpp \ + enginefailure.cpp \ datalinkloss_params.c INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index fc0d47b388..fe6639dfe7 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -60,6 +60,7 @@ #include "rtl.h" #include "offboard.h" #include "datalinkloss.h" +#include "enginefailure.h" #include "geofence.h" /** @@ -179,6 +180,8 @@ private: RTL _rtl; /**< class that handles RTL */ Offboard _offboard; /**< class that handles offboard */ DataLinkLoss _dataLinkLoss; /**< class that handles the OBC datalink loss mode */ + EngineFailure _engineFailure; /**< class that handles the engine failure mode + (FW only!) */ NavigatorMode *_navigation_mode_array[NAVIGATOR_MODE_ARRAY_SIZE]; /**< array of navigation modes */ diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 77124e8f65..21ab691ff6 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -129,6 +129,7 @@ Navigator::Navigator() : _rtl(this, "RTL"), _offboard(this, "OFF"), _dataLinkLoss(this, "DLL"), + _engineFailure(this, "EF"), _can_loiter_at_sp(false), _pos_sp_triplet_updated(false), _param_loiter_radius(this, "LOITER_RAD"), From bc4d7952f384fc079cd30327a5b4e32da90bbcb6 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 24 Jul 2014 21:22:20 +0200 Subject: [PATCH 060/411] dl loss: don't set unnecessary value --- src/modules/navigator/datalinkloss.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/src/modules/navigator/datalinkloss.cpp b/src/modules/navigator/datalinkloss.cpp index 91bb5b1104..19f3356331 100644 --- a/src/modules/navigator/datalinkloss.cpp +++ b/src/modules/navigator/datalinkloss.cpp @@ -143,7 +143,6 @@ DataLinkLoss::set_dll_item() _mission_item.loiter_direction = 1; _mission_item.nav_cmd = NAV_CMD_LOITER_UNLIMITED; _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); - _mission_item.time_inside = _param_commsholdwaittime.get() < 0.0f ? 0.0f : _param_commsholdwaittime.get(); _mission_item.pitch_min = 0.0f; _mission_item.autocontinue = true; _mission_item.origin = ORIGIN_ONBOARD; From a08b7a9f359cc0cf9c6d0631bb844b54f50adcab Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 24 Jul 2014 21:22:40 +0200 Subject: [PATCH 061/411] enginefailure: set mission item --- src/modules/navigator/enginefailure.cpp | 28 ++++++++++++------------- 1 file changed, 14 insertions(+), 14 deletions(-) diff --git a/src/modules/navigator/enginefailure.cpp b/src/modules/navigator/enginefailure.cpp index ea654c5fd4..de567f0dc6 100644 --- a/src/modules/navigator/enginefailure.cpp +++ b/src/modules/navigator/enginefailure.cpp @@ -104,21 +104,21 @@ EngineFailure::set_ef_item() case EF_STATE_LOITERDOWN: { //XXX create mission item at ground (below?) here - //_mission_item.lat = (double)(_param_airfieldhomelat.get()) * 1.0e-7; - //_mission_item.lon = (double)(_param_airfieldhomelon.get()) * 1.0e-7; - //_mission_item.altitude_is_relative = false; - //_mission_item.altitude = _param_airfieldhomealt.get(); - //_mission_item.yaw = NAN; - //_mission_item.loiter_radius = _navigator->get_loiter_radius(); - //_mission_item.loiter_direction = 1; - //_mission_item.nav_cmd = NAV_CMD_LOITER_UNLIMITED; - //_mission_item.acceptance_radius = _navigator->get_acceptance_radius(); - //_mission_item.time_inside = _param_commsholdwaittime.get() < 0.0f ? 0.0f : _param_commsholdwaittime.get(); - //_mission_item.pitch_min = 0.0f; - //_mission_item.autocontinue = true; - //_mission_item.origin = ORIGIN_ONBOARD; + _mission_item.lat = _navigator->get_global_position()->lat; + _mission_item.lon = _navigator->get_global_position()->lon; + _mission_item.altitude_is_relative = false; + //XXX setting altitude to a very low value, evaluate other options + _mission_item.altitude = _navigator->get_home_position()->alt - 1000.0f; + _mission_item.yaw = NAN; + _mission_item.loiter_radius = _navigator->get_loiter_radius(); + _mission_item.loiter_direction = 1; + _mission_item.nav_cmd = NAV_CMD_LOITER_UNLIMITED; + _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); + _mission_item.pitch_min = 0.0f; + _mission_item.autocontinue = true; + _mission_item.origin = ORIGIN_ONBOARD; - //_navigator->set_can_loiter_at_sp(true); + _navigator->set_can_loiter_at_sp(true); break; } default: From 303664d94af4949bcc7f1c3393f82eb242a60c5e Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 24 Jul 2014 21:49:22 +0200 Subject: [PATCH 062/411] add landengfail nav state --- src/modules/navigator/navigator_main.cpp | 3 +++ src/modules/uORB/topics/vehicle_status.h | 1 + 2 files changed, 4 insertions(+) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 21ab691ff6..043d883b2d 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -393,6 +393,9 @@ Navigator::task_main() _navigation_mode = &_rtl; /* TODO: change this to something else */ } break; + case NAVIGATION_STATE_AUTO_LANDENGFAIL: + _navigation_mode = &_engineFailure; + break; case NAVIGATION_STATE_LAND: case NAVIGATION_STATE_TERMINATION: case NAVIGATION_STATE_OFFBOARD: diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index eda1dfaec9..c2926dce8f 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -103,6 +103,7 @@ typedef enum { NAVIGATION_STATE_AUTO_LOITER, /**< Auto loiter mode */ NAVIGATION_STATE_AUTO_RTL, /**< Auto return to launch mode */ NAVIGATION_STATE_AUTO_RTGS, /**< Auto return to groundstation on data link loss */ + NAVIGATION_STATE_AUTO_LANDENGFAIL, /**< Auto land on engine failure */ NAVIGATION_STATE_ACRO, /**< Acro mode */ NAVIGATION_STATE_LAND, /**< Land mode */ NAVIGATION_STATE_DESCEND, /**< Descend mode (no position control) */ From 1b2f9070ea063255aca4a882e4adda7f89e082e7 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 24 Jul 2014 22:29:04 +0200 Subject: [PATCH 063/411] engine_failure flag Added engine_failure flag to behicle status, alsoset_nav_state in the state machine helper takes this flag into account --- .../commander/state_machine_helper.cpp | 19 ++++++++++++++----- src/modules/uORB/topics/vehicle_status.h | 6 ++++-- 2 files changed, 18 insertions(+), 7 deletions(-) diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 4e1cfb9873..bc68035966 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -490,9 +490,12 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en case MAIN_STATE_AUTO_MISSION: /* go into failsafe + * - if we have an engine failure * - if either the datalink is enabled and lost as well as RC is lost * - if there is no datalink and the mission is finished */ - if (((status->data_link_lost && data_link_loss_enabled) && status->rc_signal_lost) || + if (status->engine_failure) { + status->nav_state = NAVIGATION_STATE_AUTO_LANDENGFAIL; + } else if (((status->data_link_lost && data_link_loss_enabled) && status->rc_signal_lost) || (!data_link_loss_enabled && status->rc_signal_lost && mission_finished)) { status->failsafe = true; @@ -532,8 +535,10 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en break; case MAIN_STATE_AUTO_LOITER: - /* go into failsafe if datalink and RC is lost */ - if ((status->data_link_lost && data_link_loss_enabled) && status->rc_signal_lost) { + /* go into failsafe on a engine failure or if datalink and RC is lost */ + if (status->engine_failure) { + status->nav_state = NAVIGATION_STATE_AUTO_LANDENGFAIL; + } else if ((status->data_link_lost && data_link_loss_enabled) && status->rc_signal_lost) { status->failsafe = true; if (status->condition_global_position_valid && status->condition_home_position_valid) { @@ -586,8 +591,12 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en break; case MAIN_STATE_AUTO_RTL: - /* require global position and home */ - if ((!status->condition_global_position_valid || !status->condition_home_position_valid)) { + /* require global position and home, also go into failsafe on an engine failure */ + + if (status->engine_failure) { + status->nav_state = NAVIGATION_STATE_AUTO_LANDENGFAIL; + } else if ((!status->condition_global_position_valid || + !status->condition_home_position_valid)) { status->failsafe = true; if (status->condition_local_position_valid) { diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index c2926dce8f..a474886219 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -201,8 +201,10 @@ struct vehicle_status_s { bool rc_signal_lost; /**< true if RC reception lost */ bool rc_input_blocked; /**< set if RC input should be ignored */ - bool data_link_lost; /**< datalink to GCS lost */ - uint8_t data_link_lost_counter; /**< counts unique data link lost events */ + bool data_link_lost; /**< datalink to GCS lost */ + uint8_t data_link_lost_counter; /**< counts unique data link lost events */ + + bool engine_failure; /** Set to true if an engine failure is detected */ bool offboard_control_signal_found_once; bool offboard_control_signal_lost; From 351598626054e5d853e8768d2f5d04226510c12a Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 24 Jul 2014 23:44:38 +0200 Subject: [PATCH 064/411] define gps loss failsafe nav state --- src/modules/uORB/topics/vehicle_status.h | 1 + 1 file changed, 1 insertion(+) diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index a474886219..e0072b52d5 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -104,6 +104,7 @@ typedef enum { NAVIGATION_STATE_AUTO_RTL, /**< Auto return to launch mode */ NAVIGATION_STATE_AUTO_RTGS, /**< Auto return to groundstation on data link loss */ NAVIGATION_STATE_AUTO_LANDENGFAIL, /**< Auto land on engine failure */ + NAVIGATION_STATE_AUTO_LANDGPSFAIL, /**< Auto land on gps failure (e.g. open loop loiter down) */ NAVIGATION_STATE_ACRO, /**< Acro mode */ NAVIGATION_STATE_LAND, /**< Land mode */ NAVIGATION_STATE_DESCEND, /**< Descend mode (no position control) */ From db5d668439be63e4c8fd7dab49b81c5e162ee095 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 27 Jul 2014 00:55:43 +0200 Subject: [PATCH 065/411] add simplistic gps failure detection --- src/modules/commander/commander.cpp | 16 ++++++++++++++++ src/modules/uORB/topics/vehicle_status.h | 1 + 2 files changed, 17 insertions(+) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index fe1974c889..50beb5da85 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1135,6 +1135,22 @@ int commander_thread_main(int argc, char *argv[]) check_valid(global_position.timestamp, POSITION_TIMEOUT, eph_epv_good, &(status.condition_global_position_valid), &status_changed); /* check if GPS fix is ok */ + if (gps_position.fix_type >= 3 && //XXX check eph and epv ? + hrt_elapsed_time(&gps_position.timestamp_position) < FAILSAFE_DEFAULT_TIMEOUT) { + /* handle the case where gps was regained */ + if (status.gps_failure) { + status.gps_failure = false; + status_changed = true; + mavlink_log_critical(mavlink_fd, "gps regained"); + } + } else { + if (!status.gps_failure) { + status.gps_failure = true; + status_changed = true; + mavlink_log_critical(mavlink_fd, "gps fix lost"); + } + } + /* update home position */ if (!status.condition_home_position_valid && status.condition_global_position_valid && !armed.armed && diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index 707abb5458..0751b57fe0 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -206,6 +206,7 @@ struct vehicle_status_s { uint8_t data_link_lost_counter; /**< counts unique data link lost events */ bool engine_failure; /** Set to true if an engine failure is detected */ + bool gps_failure; /** Set to true if a gps failure is detected */ bool offboard_control_signal_found_once; bool offboard_control_signal_lost; From ab80459b326aaef5ce9e2d7cae04613af6653c66 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 11 Aug 2014 08:56:06 +0200 Subject: [PATCH 066/411] Fixed whitespace in rcS --- ROMFS/px4fmu_common/init.d/rcS | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 19d42dfa57..24b2a299af 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -235,7 +235,7 @@ then tone_alarm $TUNE_OUT_ERROR fi fi - + # # Set default output if not set # From b0fe5ae1ba1f6bbdb0fff1b450c0bf0a032095ec Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 11 Aug 2014 08:58:39 +0200 Subject: [PATCH 067/411] Bring back nominal state of fw attitude controller --- .../fw_att_control/fw_att_control_main.cpp | 24 +++++++++---------- 1 file changed, 12 insertions(+), 12 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 e9480336a2..0cea13cc4d 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -910,18 +910,18 @@ 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); -// } + 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); + } } From c7819dbb690680e3fb57a31f04d155ac8be7410d Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sat, 26 Jul 2014 22:08:51 +0200 Subject: [PATCH 068/411] mavlink: use pixhawk dialect --- src/modules/mavlink/mavlink_bridge_header.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_bridge_header.h b/src/modules/mavlink/mavlink_bridge_header.h index 0cd08769e1..00d427636a 100644 --- a/src/modules/mavlink/mavlink_bridge_header.h +++ b/src/modules/mavlink/mavlink_bridge_header.h @@ -84,7 +84,7 @@ void mavlink_send_uart_bytes(mavlink_channel_t chan, const uint8_t *ch, int leng extern mavlink_status_t *mavlink_get_channel_status(uint8_t chan); extern mavlink_message_t *mavlink_get_channel_buffer(uint8_t chan); -#include +#include __END_DECLS From 01a25547717736ef1692ffac4364dac1e75fb848 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 11 Aug 2014 14:51:49 +0200 Subject: [PATCH 069/411] Viper mixer: Set failsafe values so we are not starting to command actuators --- ROMFS/px4fmu_common/init.d/3035_viper | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/ROMFS/px4fmu_common/init.d/3035_viper b/ROMFS/px4fmu_common/init.d/3035_viper index a4c1e832dc..cb2b26c3c7 100644 --- a/ROMFS/px4fmu_common/init.d/3035_viper +++ b/ROMFS/px4fmu_common/init.d/3035_viper @@ -8,3 +8,8 @@ sh /etc/init.d/rc.fw_defaults set MIXER Viper + +pwm failsafe -c 5 -p 1000 +pwm failsafe -c 6 -p 1000 +pwm failsafe -c 7 -p 1000 +pwm failsafe -c 8 -p 1000 From c81c94d74f049fac409d97e5dcafb3b3afa72ab6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 11 Aug 2014 14:52:07 +0200 Subject: [PATCH 070/411] Fix control group in Viper mixer --- ROMFS/px4fmu_common/mixers/Viper.mix | 13 +++++-------- 1 file changed, 5 insertions(+), 8 deletions(-) diff --git a/ROMFS/px4fmu_common/mixers/Viper.mix b/ROMFS/px4fmu_common/mixers/Viper.mix index 79c4447bee..b05a8c8b04 100755 --- a/ROMFS/px4fmu_common/mixers/Viper.mix +++ b/ROMFS/px4fmu_common/mixers/Viper.mix @@ -52,21 +52,18 @@ M: 1 O: 10000 10000 0 -10000 10000 S: 0 3 0 20000 -10000 -10000 10000 -Gimbal / flaps / payload mixer for last four channels +Inputs to the mixer come from channel group 2 (payload), channels 0 +(bay servo 1), 1 (bay servo 2) and 3 (drop release). ----------------------------------------------------- M: 1 O: 10000 10000 0 -10000 10000 -S: 0 4 10000 10000 0 -10000 10000 +S: 2 4 10000 10000 0 -10000 10000 M: 1 O: 10000 10000 0 -10000 10000 -S: 0 5 10000 10000 0 -10000 10000 +S: 2 5 10000 10000 0 -10000 10000 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 +S: 2 6 10000 10000 0 -10000 10000 From f47cf9a002cc2fbe27e922adfdedf111f4b28093 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 11 Aug 2014 14:52:45 +0200 Subject: [PATCH 071/411] Bottle drop: C++ify full code base, fix transformation assumptions and other things, ready for integration testing --- src/modules/bottle_drop/bottle_drop.c | 477 +---------------------- src/modules/bottle_drop/bottle_drop.cpp | 492 +++++++++++++++++++++--- src/modules/bottle_drop/module.mk | 3 +- 3 files changed, 451 insertions(+), 521 deletions(-) diff --git a/src/modules/bottle_drop/bottle_drop.c b/src/modules/bottle_drop/bottle_drop.c index 1911329d0c..b867820755 100644 --- a/src/modules/bottle_drop/bottle_drop.c +++ b/src/modules/bottle_drop/bottle_drop.c @@ -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 @@ -33,487 +33,16 @@ /** * @file bottle_drop.c - * bottle_drop application - * + * Bottle drop parameters + * * @author Dominik Juchli */ #include -#include -#include -#include -#include -#include -#include -#include -#include -#include - - -#include -#include #include -#include -#include - -#include -#include -#include -#include -#include -#include - -#include - PARAM_DEFINE_FLOAT(BD_HEIGHT, 60.0f); PARAM_DEFINE_FLOAT(BD_GPROPERTIES, 0.03f); PARAM_DEFINE_FLOAT(BD_TURNRADIUS, 70.0f); PARAM_DEFINE_FLOAT(BD_PRECISION, 1.0f); PARAM_DEFINE_INT32(BD_APPROVAL, 0); - - - - -static bool thread_should_exit = false; /**< daemon exit flag */ -static bool thread_running = false; /**< daemon status flag */ -static int daemon_task; /**< Handle of daemon task / thread */ - -static bool drop = false; - -/** - * daemon management function. - */ -__EXPORT int bottle_drop_main(int argc, char *argv[]); - -/** - * Mainloop of daemon. - */ -int bottle_drop_thread_main(int argc, char *argv[]); - -/** - * Print the correct usage. - */ -static void usage(const char *reason); - -static void -usage(const char *reason) -{ - if (reason) - warnx("%s\n", reason); - errx(1, "usage: daemon {start|stop|status} [-p ]\n\n"); -} - -/** - * The daemon 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_create(). - */ -int bottle_drop_main(int argc, char *argv[]) -{ - if (argc < 1) - usage("missing command"); - - if (!strcmp(argv[1], "start")) { - - if (thread_running) { - warnx("daemon already running\n"); - /* this is not an error */ - exit(0); - } - - thread_should_exit = false; - daemon_task = task_spawn_cmd("bottle_drop", - SCHED_DEFAULT, - SCHED_PRIORITY_DEFAULT, - 4096, - bottle_drop_thread_main, - (argv) ? (const char **)&argv[2] : (const char **)NULL); - exit(0); - } - - if (!strcmp(argv[1], "drop")) { - drop = true; - exit(0); - } - - if (!strcmp(argv[1], "stop")) { - thread_should_exit = true; - exit(0); - } - - if (!strcmp(argv[1], "status")) { - if (thread_running) { - warnx("\trunning\n"); - } else { - warnx("\tnot started\n"); - } - exit(0); - } - - usage("unrecognized command"); - exit(1); -} - -int bottle_drop_thread_main(int argc, char *argv[]) { - - warnx("starting\n"); - - bool updated = false; - - float height; // height at which the normal should be dropped NED - float z_0; // ground properties - float turn_radius; // turn radius of the UAV - float precision; // Expected precision of the UAV - bool drop_approval; // if approval is given = true, otherwise = false - - thread_running = true; - - /* XXX TODO: create, publish and read in wind speed topic */ - struct wind_speed_s { - float vx; // m/s - float vy; // m/s - float altitude; // m - } wind_speed; - - wind_speed.vx = 4.2f; - wind_speed.vy = 0.0f; - wind_speed.altitude = 62.0f; - - - /* XXX TODO: create, publish and read in target position in NED*/ - struct position_s { - double lat; //degrees 1E7 - double lon; //degrees 1E7 - float alt; //m - } target_position, drop_position; - - target_position.lat = 47.385806; - target_position.lon = 8.589093; - target_position.alt = 0.0f; - - - // constant - float g = 9.81f; // constant of gravity [m/s^2] - float m = 0.5f; // mass of bottle [kg] - float rho = 1.2f; // air density [kg/m^3] - float A = (powf(0.063f, 2.0f)/4.0f*M_PI_F); // Bottle cross section [m^2] - float dt = 0.01f; // step size [s] - float dt2 = 0.05f; // step size 2 [s] - - // Has to be estimated by experiment - float cd = 0.86f; // Drag coefficient for a cylinder with a d/l ratio of 1/3 [] - float t_signal = 0.084f; // Time span between sending the signal and the bottle top reaching level height with the bottom of the plane [s] - float t_door = 0.7f; // The time the system needs to open the door + safety, is also the time the palyload needs to safely escape the shaft [s] - - - // Definition - float h_0; // height over target - float az; // acceleration in z direction[m/s^2] - float vz; // velocity in z direction [m/s] - float z; // fallen distance [m] - float h; // height over target [m] - float ax; // acceleration in x direction [m/s^2] - float vx; // ground speed in x direction [m/s] - float x; // traveled distance in x direction [m] - float vw; // wind speed [m/s] - float vrx; // relative velocity in x direction [m/s] - float v; // relative speed vector [m/s] - float Fd; // Drag force [N] - float Fdx; // Drag force in x direction [N] - float Fdz; // Drag force in z direction [N] - float vr; // absolute wind speed [m/s] - float x_drop, y_drop; // coordinates of the drop point in reference to the target (projection of NED) - float x_t,y_t; // coordinates of the target in reference to the target x_t = 0, y_t = 0 (projection of NED) - float x_l,y_l; // local position in projected coordinates - float x_f,y_f; // to-be position of the UAV after dt2 seconds in projected coordinates - double x_f_NED, y_f_NED; // to-be position of the UAV after dt2 seconds in NED - float distance_open_door; // The distance the UAV travels during its doors open [m] - float distance_real = 0; // The distance between the UAVs position and the drop point [m] - float future_distance = 0; // The distance between the UAVs to-be position and the drop point [m] - - // states - bool state_door = false; // Doors are closed = false, open = true - bool state_drop = false; // Drop occurred = true, Drop din't occur = false - bool state_run = false; // A drop was attempted = true, the drop is still in progress = false - - unsigned counter = 0; - - param_t param_height = param_find("BD_HEIGHT"); - param_t param_gproperties = param_find("BD_GPROPERTIES"); - param_t param_turn_radius = param_find("BD_TURNRADIUS"); - param_t param_precision = param_find("BD_PRECISION"); - param_t param_approval = param_find("BD_APPROVAL"); - - - param_get(param_approval, &drop_approval); - param_get(param_precision, &precision); - param_get(param_turn_radius, &turn_radius); - param_get(param_height, &height); - param_get(param_gproperties, &z_0); - - - struct vehicle_attitude_s att; - memset(&att, 0, sizeof(att)); - int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude)); - orb_set_interval(vehicle_attitude_sub, 20); - - struct vehicle_global_position_s globalpos; - memset(&globalpos, 0, sizeof(globalpos)); - int vehicle_global_position_sub = orb_subscribe(ORB_ID(vehicle_global_position)); - - struct parameter_update_s update; - memset(&update, 0, sizeof(update)); - int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update)); - - struct actuator_controls_s actuators; - memset(&actuators, 0, sizeof(actuators)); - orb_advert_t actuator_pub = orb_advertise(ORB_ID(actuator_controls_1), &actuators); - - struct mission_item_s flight_vector_s; - struct mission_item_s flight_vector_e; - - flight_vector_s.nav_cmd = NAV_CMD_WAYPOINT; - flight_vector_s.radius = 50; // TODO: make parameter - flight_vector_s.autocontinue = true; - flight_vector_e.nav_cmd = NAV_CMD_WAYPOINT; - flight_vector_e.radius = 50; // TODO: make parameter - flight_vector_e.autocontinue = true; - - struct mission_s onboard_mission; - memset(&onboard_mission, 0, sizeof(onboard_mission)); - - orb_advert_t onboard_mission_pub = orb_advertise(ORB_ID(onboard_mission), &onboard_mission); - - struct pollfd fds[] = { - { .fd = vehicle_attitude_sub, .events = POLLIN } - }; - - double latitude; - double longitude; - - - while (!thread_should_exit) { - -// warnx("in while!\n"); - // values from -1 to 1 - - int ret = poll(fds, 1, 500); - - if (ret < 0) { - /* poll error, count it in perf */ - warnx("poll error"); - - } else if (ret > 0) { - - orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &att); - - orb_check(vehicle_global_position_sub, &updated); - if (updated){ - /* copy global position */ - orb_copy(ORB_ID(vehicle_global_position), vehicle_global_position_sub, &globalpos); - - latitude = (double)globalpos.lat / 1e7; - longitude = (double)globalpos.lon / 1e7; - } - - orb_check(parameter_update_sub, &updated); - if (updated){ - /* copy global position */ - orb_copy(ORB_ID(parameter_update), parameter_update_sub, &update); - - /* update all parameters */ - param_get(param_height, &height); - param_get(param_gproperties, &z_0); - param_get(param_turn_radius, &turn_radius); - param_get(param_approval, &drop_approval); - param_get(param_precision, &precision); - - - } - - // Initialization - - - vr = sqrtf(powf(wind_speed.vx,2) + powf(wind_speed.vy,2)); // absolute wind speed [m/s] - distance_open_door = fabsf(t_door * globalpos.vx); - - - //warnx("absolut wind speed = %.4f", vr); //////////////////////////////////////////////////////////////////// DEBUGGING - - - //warnx("Initialization complete\n"); //////////////////////////////////////////////////////////////////// DEBUGGING - - - if (drop_approval && !state_drop) - { - //warnx("approval given\n"); //////////////////////////////////////////////////////////////////// DEBUGGING - // drop here - //open_now = true; - //drop = false; - //drop_start = hrt_absolute_time(); - - - if (counter % 50 == 0) { - - az = g; // acceleration in z direction[m/s^2] - vz = 0; // velocity in z direction [m/s] - z = 0; // fallen distance [m] - h_0 = globalpos.alt - target_position.alt; // height over target at start[m] - h = h_0; // height over target [m] - ax = 0; // acceleration in x direction [m/s^2] - vx = sqrtf(powf(globalpos.vx,2.0f) + powf(globalpos.vy,2.0f)); // ground speed in x direction [m/s] - x = 0; // traveled distance in x direction [m] - vw = 0; // wind speed [m/s] - vrx = 0; // relative velocity in x direction [m/s] - v = sqrtf(powf(globalpos.vx,2.0f) + powf(globalpos.vy,2.0f)); // relative speed vector [m/s] - Fd = 0; // Drag force [N] - Fdx = 0; // Drag force in x direction [N] - Fdz = 0; // Drag force in z direction [N] - - - // Compute the distance the bottle will travel after it is dropped in body frame coordinates --> x - while( h > 0.05f) - { - // z-direction - vz = vz + az*dt; - z = z + vz*dt; - h = h_0 - z; - - // x-direction - vw = vr*logf(h/z_0)/logf(wind_speed.altitude/z_0); - vx = vx + ax*dt; - x = x + vx*dt; - vrx = vx + vw; - - //Drag force - v = sqrtf(powf(vz,2.0f) + powf(vrx,2.0f)); - Fd = 0.5f*rho*A*cd*powf(v,2.0f); - Fdx = Fd*vrx/v; - Fdz = Fd*vz/v; - - //acceleration - az = g - Fdz/m; - ax = -Fdx/m; - } - // Compute Drop point - x = sqrtf(powf(globalpos.vx,2.0f) + powf(globalpos.vy,2.0f))*t_signal + x; - map_projection_init(target_position.lat, target_position.lon); - } - - map_projection_project(target_position.lat, target_position.lon, &x_t, &y_t); - if( vr < 0.001f) // if there is no wind, an arbitrarily direction is chosen - { - vr = 1; - wind_speed.vx = 1; - wind_speed.vy = 0; - } - x_drop = x_t + x*wind_speed.vx/vr; - y_drop = y_t + x*wind_speed.vy/vr; - map_projection_reproject(x_drop, y_drop, &drop_position.lat, &drop_position.lon); - drop_position.alt = height; - //warnx("drop point: lat = %.7f , lon = %.7f", drop_position.lat, drop_position.lon); //////////////////////////////////////////////////////////////////// DEBUGGING - - - - // Compute flight vector - map_projection_reproject(x_drop + 2*turn_radius*wind_speed.vx/vr, y_drop + 2*turn_radius*wind_speed.vy/vr, &(flight_vector_s.lat), &(flight_vector_s.lon)); - flight_vector_s.altitude = height; - map_projection_reproject(x_drop - turn_radius*wind_speed.vx/vr, y_drop - turn_radius*wind_speed.vy/vr, &flight_vector_e.lat, &flight_vector_e.lon); - flight_vector_e.altitude = height; - //warnx("Flight vector: starting point = %.7f %.7f , end point = %.7f %.7f", flight_vector_s.lat,flight_vector_s.lon,flight_vector_e.lat,flight_vector_e.lon); //////////////////////////////////////////////////////////////////// DEBUGGING - - // Drop Cancellation if terms are not met - - // warnx("latitude:%.2f", latitude); - // warnx("longitude:%.2f", longitude); - // warnx("drop_position.lat:%.2f", drop_position.lat); - // warnx("drop_position.lon:%.2f", drop_position.lon); - - distance_real = fabsf(get_distance_to_next_waypoint(latitude, longitude, drop_position.lat, drop_position.lon)); - map_projection_project(latitude, longitude, &x_l, &y_l); - x_f = x_l + globalpos.vx*dt2; - y_f = y_l + globalpos.vy*dt2; - map_projection_reproject(x_f, y_f, &x_f_NED, &y_f_NED); - future_distance = fabsf(get_distance_to_next_waypoint(x_f_NED, y_f_NED, drop_position.lat, drop_position.lon)); - //warnx("position to-be: = %.7f %.7f" ,x_f_NED, y_f_NED ); //////////////////////////////////////////////////////////////////// DEBUGGING - - // if (counter % 10 ==0) { - // warnx("x: %.4f", x); - // warnx("drop_position.lat: %.4f, drop_position.lon: %.4f", drop_position.lat, drop_position.lon); - // warnx("latitude %.4f, longitude: %.4f", latitude, longitude); - // warnx("future_distance: %.2f, precision: %.2f", future_distance, precision); - // } - - /* Save WPs in datamanager */ - const size_t len = sizeof(struct mission_item_s); - - if (dm_write(DM_KEY_WAYPOINTS_ONBOARD, 0, DM_PERSIST_IN_FLIGHT_RESET, &flight_vector_s, len) != len) { - warnx("ERROR: could not save onboard WP"); - } - - if (dm_write(DM_KEY_WAYPOINTS_ONBOARD, 1, DM_PERSIST_IN_FLIGHT_RESET, &flight_vector_e, len) != len) { - warnx("ERROR: could not save onboard WP"); - } - - onboard_mission.count = 2; - - if (state_run && !state_drop) { - onboard_mission.current_index = 0; - } else { - onboard_mission.current_index = -1; - } - - if (counter % 10 ==0) - warnx("Distance real: %.2f, distance_open_door: %.2f, angle to wind: %.2f", distance_real, distance_open_door, fabsf(_wrap_pi(globalpos.yaw-atan2f(wind_speed.vy,wind_speed.vx)+M_PI_F))); - - orb_publish(ORB_ID(onboard_mission), onboard_mission_pub, &onboard_mission); - - } - - if(isfinite(distance_real) && distance_real < distance_open_door && drop_approval) - { - actuators.control[0] = -1.0f; // open door - actuators.control[1] = 1.0f; - state_door = true; - warnx("open doors"); - } - else - { // closed door and locked survival kit - actuators.control[0] = 0.5f; - actuators.control[1] = -0.5f; - actuators.control[2] = -0.5f; - state_door = false; - } - if(isfinite(distance_real) && distance_real < precision && distance_real < future_distance && state_door) // Drop only if the distance between drop point and actual position is getting larger again - { - if(fabsf(_wrap_pi(globalpos.yaw-atan2f(wind_speed.vy,wind_speed.vx)+M_PI_F)) < 10.0f/180.0f*M_PI_F) // if flight trajectory deviates more than 10 degrees from calculated path, it will no drop - { - actuators.control[2] = 0.5f; - state_drop = true; - state_run = true; - warnx("dropping now"); - } - else - { - state_run = true; - } - } - - actuators.timestamp = hrt_absolute_time(); - orb_publish(ORB_ID(actuator_controls_1), actuator_pub, &actuators); - - counter++; - } - } - - warnx("exiting.\n"); - - thread_running = false; - - - return 0; -} diff --git a/src/modules/bottle_drop/bottle_drop.cpp b/src/modules/bottle_drop/bottle_drop.cpp index ce760adc35..84c7900ce0 100644 --- a/src/modules/bottle_drop/bottle_drop.cpp +++ b/src/modules/bottle_drop/bottle_drop.cpp @@ -57,10 +57,13 @@ #include #include #include +#include +#include #include #include #include #include +#include #include #include @@ -72,7 +75,7 @@ */ extern "C" __EXPORT int bottle_drop_main(int argc, char *argv[]); -class BottleDrop +class BottleDrop { public: /** @@ -101,17 +104,26 @@ private: bool _task_should_exit; /**< if true, task should exit */ int _main_task; /**< handle for task */ int _mavlink_fd; - + int _command_sub; + int _wind_estimate_sub; struct vehicle_command_s _command; orb_advert_t _actuator_pub; struct actuator_controls_s _actuators; + bool drop_approval; bool _open_door; bool _drop; hrt_abstime _doors_opened; + struct position_s { + double lat; //degrees + double lon; //degrees + float alt; //m + bool initialized; + } _target_position, _drop_position; + void task_main(); void handle_command(struct vehicle_command_s *cmd); @@ -135,12 +147,13 @@ BottleDrop::BottleDrop() : _main_task(-1), _mavlink_fd(-1), _command_sub(-1), - _command({}), - _actuator_pub(-1), - _actuators({}), - _open_door(false), - _drop(false), - _doors_opened(0) + _wind_estimate_sub(-1), + _command {}, + _actuator_pub(-1), + _actuators {}, + _open_door(false), + _drop(false), + _doors_opened(0) { } @@ -176,11 +189,11 @@ BottleDrop::start() /* start the task */ _main_task = task_spawn_cmd("bottle_drop", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 5, - 2048, - (main_t)&BottleDrop::task_main_trampoline, - nullptr); + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 2048, + (main_t)&BottleDrop::task_main_trampoline, + nullptr); if (_main_task < 0) { warn("task start failed"); @@ -201,14 +214,112 @@ BottleDrop::status() void BottleDrop::task_main() { - /* inform about start */ - warnx("Initializing.."); - fflush(stdout); _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); mavlink_log_info(_mavlink_fd, "[bottle_drop] started"); _command_sub = orb_subscribe(ORB_ID(vehicle_command)); + _wind_estimate_sub = orb_subscribe(ORB_ID(wind_estimate)); + + bool updated = false; + + float height; // height at which the normal should be dropped NED + float z_0; // ground properties + float turn_radius; // turn radius of the UAV + float precision; // Expected precision of the UAV + + // XXX we do not measure the exact ground altitude yet, but eventually we have to + float ground_distance = 70.0f; + + // constant + float g = 9.81f; // constant of gravity [m/s^2] + float m = 0.5f; // mass of bottle [kg] + float rho = 1.2f; // air density [kg/m^3] + float A = (powf(0.063f, 2.0f) / 4.0f * M_PI_F); // Bottle cross section [m^2] + float dt = 0.01f; // step size [s] + float dt2 = 0.05f; // step size 2 [s] + + // Has to be estimated by experiment + float cd = 0.86f; // Drag coefficient for a cylinder with a d/l ratio of 1/3 [] + float t_signal = + 0.084f; // Time span between sending the signal and the bottle top reaching level height with the bottom of the plane [s] + float t_door = + 0.7f; // The time the system needs to open the door + safety, is also the time the palyload needs to safely escape the shaft [s] + + + // Definition + float h_0; // height over target + float az; // acceleration in z direction[m/s^2] + float vz; // velocity in z direction [m/s] + float z; // fallen distance [m] + float h; // height over target [m] + float ax; // acceleration in x direction [m/s^2] + float vx; // ground speed in x direction [m/s] + float x; // traveled distance in x direction [m] + float vw; // wind speed [m/s] + float vrx; // relative velocity in x direction [m/s] + float v; // relative speed vector [m/s] + float Fd; // Drag force [N] + float Fdx; // Drag force in x direction [N] + float Fdz; // Drag force in z direction [N] + float x_drop, y_drop; // coordinates of the drop point in reference to the target (projection of NED) + float x_t, y_t; // coordinates of the target in reference to the target x_t = 0, y_t = 0 (projection of NED) + float x_l, y_l; // local position in projected coordinates + float x_f, y_f; // to-be position of the UAV after dt2 seconds in projected coordinates + double x_f_NED, y_f_NED; // to-be position of the UAV after dt2 seconds in NED + float distance_open_door; // The distance the UAV travels during its doors open [m] + float distance_real = 0; // The distance between the UAVs position and the drop point [m] + float future_distance = 0; // The distance between the UAVs to-be position and the drop point [m] + + // states + bool state_door = false; // Doors are closed = false, open = true + bool state_drop = false; // Drop occurred = true, Drop din't occur = false + bool state_run = false; // A drop was attempted = true, the drop is still in progress = false + + unsigned counter = 0; + + param_t param_height = param_find("BD_HEIGHT"); + param_t param_gproperties = param_find("BD_GPROPERTIES"); + param_t param_turn_radius = param_find("BD_TURNRADIUS"); + param_t param_precision = param_find("BD_PRECISION"); + + + param_get(param_precision, &precision); + param_get(param_turn_radius, &turn_radius); + param_get(param_height, &height); + param_get(param_gproperties, &z_0); + + struct vehicle_global_position_s globalpos; + memset(&globalpos, 0, sizeof(globalpos)); + int vehicle_global_position_sub = orb_subscribe(ORB_ID(vehicle_global_position)); + + struct parameter_update_s update; + memset(&update, 0, sizeof(update)); + int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update)); + + struct actuator_controls_s actuators; + memset(&actuators, 0, sizeof(actuators)); + + struct mission_item_s flight_vector_s; + struct mission_item_s flight_vector_e; + + flight_vector_s.nav_cmd = NAV_CMD_WAYPOINT; + flight_vector_s.acceptance_radius = 50; // TODO: make parameter + flight_vector_s.autocontinue = true; + flight_vector_e.nav_cmd = NAV_CMD_WAYPOINT; + flight_vector_e.acceptance_radius = 50; // TODO: make parameter + flight_vector_e.autocontinue = true; + + struct mission_s onboard_mission; + memset(&onboard_mission, 0, sizeof(onboard_mission)); + orb_advert_t onboard_mission_pub = -1; + + double latitude; + double longitude; + + struct wind_estimate_s wind; + + map_projection_reference_s ref; /* wakeup source(s) */ struct pollfd fds[1]; @@ -234,40 +345,246 @@ BottleDrop::task_main() handle_command(&_command); } - /* update door actuators */ - if (_open_door) { - _actuators.control[0] = -1.0f; - _actuators.control[1] = 1.0f; + /* switch to faster updates during the drop */ + while (_drop) { - if (_doors_opened == 0) { - _doors_opened = hrt_absolute_time(); + /* 20s after drop, reset and close everything again */ + if (_drop && _doors_opened != 0 && hrt_elapsed_time(&_doors_opened) > 20000000) { + _open_door = false; + _drop = false; } - } else { - _actuators.control[0] = 1.0f; - _actuators.control[1] = -1.0f; - _doors_opened = 0; - } + orb_check(_wind_estimate_sub, &updated); - /* update drop actuator, wait 0.5s until the doors are open before dropping */ - if (_drop && _doors_opened != 0 && hrt_elapsed_time(&_doors_opened) > 500000) { - _actuators.control[2] = -0.5f; - } else { - _actuators.control[2] = 0.5f; - } + if (updated) { + orb_copy(ORB_ID(wind_estimate), _wind_estimate_sub, &wind); + } - /* 20s after drop, reset and close everything again */ - if (_drop && _doors_opened != 0 && hrt_elapsed_time(&_doors_opened) > 20000000) { - _open_door = false; - _drop = false; - } + orb_check(vehicle_global_position_sub, &updated); - /* lazily publish _actuators only once available */ - if (_actuator_pub > 0) { - orb_publish(ORB_ID(actuator_controls_1), _actuator_pub, &_actuators); + if (updated) { + /* copy global position */ + orb_copy(ORB_ID(vehicle_global_position), vehicle_global_position_sub, &globalpos); - } else { - _actuator_pub = orb_advertise(ORB_ID(actuator_controls_1), &_actuators); + latitude = (double)globalpos.lat / 1e7; + longitude = (double)globalpos.lon / 1e7; + } + + orb_check(parameter_update_sub, &updated); + + if (updated) { + /* copy global position */ + orb_copy(ORB_ID(parameter_update), parameter_update_sub, &update); + + /* update all parameters */ + param_get(param_height, &height); + param_get(param_gproperties, &z_0); + param_get(param_turn_radius, &turn_radius); + param_get(param_precision, &precision); + } + + // Initialization + + float windspeed_norm = sqrtf(wind.windspeed_north * wind.windspeed_north + wind.windspeed_east * wind.windspeed_east); + float groundspeed_body = sqrtf(globalpos.vel_n * globalpos.vel_n + globalpos.vel_e * globalpos.vel_e); + + distance_open_door = fabsf(t_door * groundspeed_body); + + + //warnx("absolut wind speed = %.4f", vr); //////////////////////////////////////////////////////////////////// DEBUGGING + + + //warnx("Initialization complete\n"); //////////////////////////////////////////////////////////////////// DEBUGGING + + + if (drop_approval && !state_drop) { + //warnx("approval given\n"); //////////////////////////////////////////////////////////////////// DEBUGGING + // drop here + //open_now = true; + //drop = false; + //drop_start = hrt_absolute_time(); + + // Update drop point at 10 Hz + if (counter % 10 == 0) { + + az = g; // acceleration in z direction[m/s^2] + vz = 0; // velocity in z direction [m/s] + z = 0; // fallen distance [m] + h_0 = globalpos.alt - _target_position.alt; // height over target at start[m] + h = h_0; // height over target [m] + ax = 0; // acceleration in x direction [m/s^2] + vx = groundspeed_body;// XXX project // ground speed in x direction [m/s] + x = 0; // traveled distance in x direction [m] + vw = 0; // wind speed [m/s] + vrx = 0; // relative velocity in x direction [m/s] + v = groundspeed_body; // relative speed vector [m/s] + Fd = 0; // Drag force [N] + Fdx = 0; // Drag force in x direction [N] + Fdz = 0; // Drag force in z direction [N] + + + // Compute the distance the bottle will travel after it is dropped in body frame coordinates --> x + while (h > 0.05f) { + // z-direction + vz = vz + az * dt; + z = z + vz * dt; + h = h_0 - z; + + // x-direction + vw = windspeed_norm * logf(h / z_0) / logf(ground_distance / z_0); + vx = vx + ax * dt; + x = x + vx * dt; + vrx = vx + vw; + + //Drag force + v = sqrtf(vz * vz + vrx * vrx); + Fd = 0.5f * rho * A * cd * powf(v, 2.0f); + Fdx = Fd * vrx / v; + Fdz = Fd * vz / v; + + //acceleration + az = g - Fdz / m; + ax = -Fdx / m; + } + + // Compute Drop point + x = groundspeed_body * t_signal + x; + map_projection_init(&ref, _target_position.lat, _target_position.lon); + } + + map_projection_project(&ref, _target_position.lat, _target_position.lon, &x_t, &y_t); + + float wind_direction_n, wind_direction_e; + + if (windspeed_norm < 0.5f) { // If there is no wind, an arbitrarily direction is chosen + wind_direction_n = 1.0f; + wind_direction_e = 0.0f; + + } else { + wind_direction_n = wind.windspeed_north / windspeed_norm; + wind_direction_e = wind.windspeed_east / windspeed_norm; + } + + x_drop = x_t + x * wind_direction_n; + y_drop = y_t + x * wind_direction_e; + map_projection_reproject(&ref, x_drop, y_drop, &_drop_position.lat, &_drop_position.lon); + _drop_position.alt = height; + //warnx("drop point: lat = %.7f , lon = %.7f", drop_position.lat, drop_position.lon); //////////////////////////////////////////////////////////////////// DEBUGGING + + + + // Compute flight vector + map_projection_reproject(&ref, x_drop + 2 * turn_radius * wind_direction_n, y_drop + 2 * turn_radius * wind_direction_n, + &(flight_vector_s.lat), &(flight_vector_s.lon)); + flight_vector_s.altitude = height; + map_projection_reproject(&ref, x_drop - turn_radius * wind_direction_e, y_drop - turn_radius * wind_direction_e, + &flight_vector_e.lat, &flight_vector_e.lon); + flight_vector_e.altitude = height; + //warnx("Flight vector: starting point = %.7f %.7f , end point = %.7f %.7f", flight_vector_s.lat,flight_vector_s.lon,flight_vector_e.lat,flight_vector_e.lon); //////////////////////////////////////////////////////////////////// DEBUGGING + + // Drop Cancellation if terms are not met + + // warnx("latitude:%.2f", latitude); + // warnx("longitude:%.2f", longitude); + // warnx("drop_position.lat:%.2f", drop_position.lat); + // warnx("drop_position.lon:%.2f", drop_position.lon); + + distance_real = fabsf(get_distance_to_next_waypoint(latitude, longitude, _drop_position.lat, _drop_position.lon)); + map_projection_project(&ref, latitude, longitude, &x_l, &y_l); + x_f = x_l + globalpos.vel_n * dt2; + y_f = y_l + globalpos.vel_e * dt2; + map_projection_reproject(&ref, x_f, y_f, &x_f_NED, &y_f_NED); + future_distance = fabsf(get_distance_to_next_waypoint(x_f_NED, y_f_NED, _drop_position.lat, _drop_position.lon)); + //warnx("position to-be: = %.7f %.7f" ,x_f_NED, y_f_NED ); //////////////////////////////////////////////////////////////////// DEBUGGING + + // if (counter % 10 ==0) { + // warnx("x: %.4f", x); + // warnx("drop_position.lat: %.4f, drop_position.lon: %.4f", drop_position.lat, drop_position.lon); + // warnx("latitude %.4f, longitude: %.4f", latitude, longitude); + // warnx("future_distance: %.2f, precision: %.2f", future_distance, precision); + // } + + /* Save WPs in datamanager */ + const size_t len = sizeof(struct mission_item_s); + + if (dm_write(DM_KEY_WAYPOINTS_ONBOARD, 0, DM_PERSIST_IN_FLIGHT_RESET, &flight_vector_s, len) != len) { + warnx("ERROR: could not save onboard WP"); + } + + if (dm_write(DM_KEY_WAYPOINTS_ONBOARD, 1, DM_PERSIST_IN_FLIGHT_RESET, &flight_vector_e, len) != len) { + warnx("ERROR: could not save onboard WP"); + } + + onboard_mission.count = 2; + + if (state_run && !state_drop) { + onboard_mission.current_seq = 0; + + } else { + onboard_mission.current_seq = -1; + } + + if (counter % 10 == 0) + warnx("Distance real: %.2f, distance_open_door: %.2f, angle to wind: %.2f", (double)distance_real, + (double)distance_open_door, + (double)(_wrap_pi(globalpos.yaw - atan2f(wind.windspeed_north, wind.windspeed_east)))); + + if (onboard_mission_pub > 0) { + orb_publish(ORB_ID(onboard_mission), onboard_mission_pub, &onboard_mission); + + } else { + onboard_mission_pub = orb_advertise(ORB_ID(onboard_mission), &onboard_mission); + } + + } + + if (isfinite(distance_real) && distance_real < distance_open_door && drop_approval) { + actuators.control[0] = -1.0f; // open door + actuators.control[1] = 1.0f; + state_door = true; + warnx("open doors"); + + } else { + // closed door and locked survival kit + actuators.control[0] = 0.5f; + actuators.control[1] = -0.5f; + actuators.control[2] = -0.5f; + state_door = false; + } + + if (isfinite(distance_real) && distance_real < precision && distance_real < future_distance + && state_door) { // Drop only if the distance between drop point and actual position is getting larger again + // XXX this check needs to be carefully validated - right now we prefer to drop if we're close to the goal + + // if(fabsf(_wrap_pi(globalpos.yaw-atan2f(wind.windspeed_north,wind_speed.windspeed_east)+M_PI_F)) < 80.0f/180.0f*M_PI_F) // if flight trajectory deviates more than 80 degrees from calculated path, it will no drop + // { + actuators.control[2] = 0.5f; + state_drop = true; + state_run = true; + warnx("dropping now"); + // } + // else + // { + // state_run = true; + // } + } + + actuators.timestamp = hrt_absolute_time(); + + // lazily publish _actuators only once available + if (_actuator_pub > 0) { + orb_publish(ORB_ID(actuator_controls_2), _actuator_pub, &actuators); + + } else { + _actuator_pub = orb_advertise(ORB_ID(actuator_controls_2), &actuators); + } + + counter++; + + // update_actuators(); + + // run at roughly 100 Hz + usleep(1000); } } @@ -277,6 +594,43 @@ BottleDrop::task_main() _exit(0); } +// XXX this is out of sync with the C port +// void +// BottleDrop::update_actuators() +// { +// _actuators.timestamp = hrt_absolute_time(); + +// // update door actuators +// if (_open_door) { +// _actuators.control[0] = -1.0f; +// _actuators.control[1] = 1.0f; + +// if (_doors_opened == 0) { +// _doors_opened = hrt_absolute_time(); +// } +// } else { +// _actuators.control[0] = 1.0f; +// _actuators.control[1] = -1.0f; + +// _doors_opened = 0; +// } + +// // update drop actuator, wait 0.5s until the doors are open before dropping +// if (_drop && _doors_opened != 0 && hrt_elapsed_time(&_doors_opened) > 500000) { +// _actuators.control[2] = -0.5f; +// } else { +// _actuators.control[2] = 0.5f; +// } + +// // lazily publish _actuators only once available +// if (_actuator_pub > 0) { +// orb_publish(ORB_ID(actuator_controls_2), _actuator_pub, &_actuators); + +// } else { +// _actuator_pub = orb_advertise(ORB_ID(actuator_controls_2), &_actuators); +// } +// } + void BottleDrop::handle_command(struct vehicle_command_s *cmd) { @@ -292,11 +646,12 @@ BottleDrop::handle_command(struct vehicle_command_s *cmd) _open_door = true; _drop = true; mavlink_log_info(_mavlink_fd, "#audio: drop bottle"); - } - else if (cmd->param1 > 0.5f) { + + } else if (cmd->param1 > 0.5f) { _open_door = true; _drop = false; mavlink_log_info(_mavlink_fd, "#audio: open doors"); + } else { _open_door = false; _drop = false; @@ -305,6 +660,49 @@ BottleDrop::handle_command(struct vehicle_command_s *cmd) answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); break; + + case VEHICLE_CMD_PAYLOAD_PREPARE_DEPLOY: + + switch ((int)(cmd->param1 + 0.5f)) { + case 0: + drop_approval = false; + break; + + case 1: + drop_approval = true; + break; + + default: + drop_approval = false; + break; + } + + // XXX check all fields + _target_position.lat = cmd->param5; + _target_position.lon = cmd->param6; + _target_position.alt = cmd->param7; + answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); + break; + + case VEHICLE_CMD_PAYLOAD_CONTROL_DEPLOY: + switch ((int)(cmd->param1 + 0.5f)) { + case 0: + drop_approval = false; + break; + + case 1: + drop_approval = true; + break; + + default: + drop_approval = false; + break; + // XXX handle other values + } + + answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); + break; + default: break; } @@ -376,8 +774,9 @@ int bottle_drop_main(int argc, char *argv[]) return 0; } - if (bottle_drop::g_bottle_drop == nullptr) + if (bottle_drop::g_bottle_drop == nullptr) { errx(1, "not running"); + } if (!strcmp(argv[1], "stop")) { delete bottle_drop::g_bottle_drop; @@ -385,6 +784,7 @@ int bottle_drop_main(int argc, char *argv[]) } else if (!strcmp(argv[1], "status")) { bottle_drop::g_bottle_drop->status(); + } else { usage(); } diff --git a/src/modules/bottle_drop/module.mk b/src/modules/bottle_drop/module.mk index bb1f6c6b5d..61ac89e38b 100644 --- a/src/modules/bottle_drop/module.mk +++ b/src/modules/bottle_drop/module.mk @@ -37,4 +37,5 @@ MODULE_COMMAND = bottle_drop -SRCS = bottle_drop.cpp +SRCS = bottle_drop.cpp \ + bottle_drop.c From f0458af28b3414cd43276c08c029be7900d0d9cf Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 11 Aug 2014 14:53:04 +0200 Subject: [PATCH 072/411] Extend vehicle commands as needed --- src/modules/uORB/topics/vehicle_command.h | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/src/modules/uORB/topics/vehicle_command.h b/src/modules/uORB/topics/vehicle_command.h index 15cdc517fc..44aa505721 100644 --- a/src/modules/uORB/topics/vehicle_command.h +++ b/src/modules/uORB/topics/vehicle_command.h @@ -53,6 +53,9 @@ * but can contain additional ones. */ enum VEHICLE_CMD { + VEHICLE_CMD_CUSTOM_0 = 0, /* test command */ + VEHICLE_CMD_CUSTOM_1 = 1, /* test command */ + VEHICLE_CMD_CUSTOM_2 = 2, /* test command */ VEHICLE_CMD_NAV_WAYPOINT = 16, /* Navigate to MISSION. |Hold time in decimal seconds. (ignored by fixed wing, time to stay at MISSION for rotary wing)| Acceptance radius in meters (if the sphere with this radius is hit, the MISSION counts as reached)| 0 to pass through the WP, if > 0 radius in meters to pass by WP. Positive value for clockwise orbit, negative value for counter-clockwise orbit. Allows trajectory control.| Desired yaw angle at MISSION (rotary wing)| Latitude| Longitude| Altitude| */ VEHICLE_CMD_NAV_LOITER_UNLIM = 17, /* Loiter around this MISSION an unlimited amount of time |Empty| Empty| Radius around MISSION, in meters. If positive loiter clockwise, else counter-clockwise| Desired yaw angle.| Latitude| Longitude| Altitude| */ VEHICLE_CMD_NAV_LOITER_TURNS = 18, /* Loiter around this MISSION for X turns |Turns| Empty| Radius around MISSION, in meters. If positive loiter clockwise, else counter-clockwise| Desired yaw angle.| Latitude| Longitude| Altitude| */ @@ -117,8 +120,8 @@ struct vehicle_command_s { float param2; /**< Parameter 2, as defined by MAVLink VEHICLE_CMD enum. */ float param3; /**< Parameter 3, as defined by MAVLink VEHICLE_CMD enum. */ float param4; /**< Parameter 4, as defined by MAVLink VEHICLE_CMD enum. */ - float param5; /**< Parameter 5, as defined by MAVLink VEHICLE_CMD enum. */ - float param6; /**< Parameter 6, as defined by MAVLink VEHICLE_CMD enum. */ + double param5; /**< Parameter 5, as defined by MAVLink VEHICLE_CMD enum. */ + double param6; /**< Parameter 6, as defined by MAVLink VEHICLE_CMD enum. */ float param7; /**< Parameter 7, as defined by MAVLink VEHICLE_CMD enum. */ enum VEHICLE_CMD command; /**< Command ID, as defined MAVLink by VEHICLE_CMD enum. */ uint8_t target_system; /**< System which should execute the command */ From f4cf94b08427bfda10919de1cee6b58171ed5e50 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 12 Aug 2014 08:21:06 +0200 Subject: [PATCH 073/411] Improved rcS handling, added failsafe flag. Needs further testing for USB stability --- ROMFS/px4fmu_common/init.d/3035_viper | 2 ++ ROMFS/px4fmu_common/init.d/rc.interface | 5 +++++ ROMFS/px4fmu_common/init.d/rc.sensors | 5 +++++ ROMFS/px4fmu_common/init.d/rcS | 9 ++++----- 4 files changed, 16 insertions(+), 5 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/3035_viper b/ROMFS/px4fmu_common/init.d/3035_viper index a4c1e832dc..3714b612fc 100644 --- a/ROMFS/px4fmu_common/init.d/3035_viper +++ b/ROMFS/px4fmu_common/init.d/3035_viper @@ -8,3 +8,5 @@ sh /etc/init.d/rc.fw_defaults set MIXER Viper + +set FAILSAFE "-c567 -p 1000" diff --git a/ROMFS/px4fmu_common/init.d/rc.interface b/ROMFS/px4fmu_common/init.d/rc.interface index 1de0abb58d..e44cd0953d 100644 --- a/ROMFS/px4fmu_common/init.d/rc.interface +++ b/ROMFS/px4fmu_common/init.d/rc.interface @@ -77,4 +77,9 @@ then pwm max -c $PWM_OUTPUTS -p $PWM_MAX fi fi + + if [ $FAILSAFE != none ] + then + pwm failsafe -d $OUTPUT_DEV $FAILSAFE + fi fi diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors index ecb408a543..739df7ac0b 100644 --- a/ROMFS/px4fmu_common/init.d/rc.sensors +++ b/ROMFS/px4fmu_common/init.d/rc.sensors @@ -68,6 +68,11 @@ else fi fi +# Check for flow sensor +if px4flow start +then +fi + # # Start the sensor collection task. # IMPORTANT: this also loads param offsets diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index c9e6a27cac..c1948460dd 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -66,6 +66,9 @@ then # sercon + # Try to get an USB console + nshterm /dev/ttyACM0 & + # # Start the ORB (first app to start) # @@ -96,11 +99,9 @@ then # if rgbled start then - echo "[init] RGB Led" else if blinkm start then - echo "[init] BlinkM" blinkm systemstate fi fi @@ -129,6 +130,7 @@ then set LOAD_DEFAULT_APPS yes set GPS yes set GPS_FAKE no + set FAILSAFE none # # Set DO_AUTOCONFIG flag to use it in AUTOSTART scripts @@ -279,9 +281,6 @@ then fi fi - # Try to get an USB console - nshterm /dev/ttyACM0 & - # # Start the datamanager (and do not abort boot if it fails) # From a04d70ed5a800ff6d74afc6800f7bb94ed472ad4 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 12 Aug 2014 14:54:35 +0200 Subject: [PATCH 074/411] offboard setpoints: correctly check for force setpoint --- src/modules/mavlink/mavlink_receiver.cpp | 6 +++- .../uORB/topics/offboard_control_setpoint.h | 30 +++++++++++++++++++ 2 files changed, 35 insertions(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index dfc5ddc915..418b81ee17 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -441,7 +441,11 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode); } if (_control_mode.flag_control_offboard_enabled) { - if (offboard_control_sp.isForceSetpoint) { + if (offboard_control_sp.isForceSetpoint && + offboard_control_sp_ignore_position_all(offboard_control_sp) && + offboard_control_sp_ignore_velocity_all(offboard_control_sp)) { + /* The offboard setpoint is a force setpoint only, directly writing to the force + * setpoint topic and not publishing the setpoint triplet topic */ struct vehicle_force_setpoint_s force_sp; force_sp.x = offboard_control_sp.acceleration[0]; force_sp.y = offboard_control_sp.acceleration[1]; diff --git a/src/modules/uORB/topics/offboard_control_setpoint.h b/src/modules/uORB/topics/offboard_control_setpoint.h index 19f11ba924..4f45ac14ff 100644 --- a/src/modules/uORB/topics/offboard_control_setpoint.h +++ b/src/modules/uORB/topics/offboard_control_setpoint.h @@ -104,14 +104,44 @@ struct offboard_control_setpoint_s { * @} */ +/** + * Returns true if the position setpoint at index should be ignored + */ inline bool offboard_control_sp_ignore_position(const struct offboard_control_setpoint_s &offboard_control_sp, int index) { return (bool)(offboard_control_sp.ignore & (1 << index)); } +/** + * Returns true if all position setpoints should be ignored + */ +inline bool offboard_control_sp_ignore_position_all(const struct offboard_control_setpoint_s &offboard_control_sp) { + for (int i = 0; i < 3; i++) { + if (offboard_control_sp_ignore_position(offboard_control_sp, i)) { + return true; + } + } + return false; +} + +/** + * Returns true if the velocity setpoint at index should be ignored + */ inline bool offboard_control_sp_ignore_velocity(const struct offboard_control_setpoint_s &offboard_control_sp, int index) { return (bool)(offboard_control_sp.ignore & (1 << (3 + index))); } +/** + * Returns true if all velocity setpoints should be ignored + */ +inline bool offboard_control_sp_ignore_velocity_all(const struct offboard_control_setpoint_s &offboard_control_sp) { + for (int i = 0; i < 3; i++) { + if (offboard_control_sp_ignore_velocity(offboard_control_sp, i)) { + return true; + } + } + return false; +} + inline bool offboard_control_sp_ignore_acceleration(const struct offboard_control_setpoint_s &offboard_control_sp, int index) { return (bool)(offboard_control_sp.ignore & (1 << (6 + index))); } From 85eed22150e4a24098554992a6d77bce6f1ddf31 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 12 Aug 2014 16:27:02 +0200 Subject: [PATCH 075/411] offboard sp: correctly map acceleration/force in combined setpoint to setpoint triple Conflicts: src/modules/uORB/topics/position_setpoint_triplet.h --- src/modules/mavlink/mavlink_receiver.cpp | 39 +++++++++++++------ .../uORB/topics/offboard_control_setpoint.h | 21 ++++++++++ .../uORB/topics/position_setpoint_triplet.h | 5 +++ 3 files changed, 53 insertions(+), 12 deletions(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 418b81ee17..da651e4e76 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -457,7 +457,7 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t orb_publish(ORB_ID(vehicle_force_setpoint), _force_sp_pub, &force_sp); } } else { - /* It's not a force setpoint: publish to setpoint triplet topic */ + /* It's not a pure force setpoint: publish to setpoint triplet topic */ struct position_setpoint_triplet_s pos_sp_triplet; pos_sp_triplet.previous.valid = false; pos_sp_triplet.next.valid = false; @@ -466,29 +466,44 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t /* set the local pos values if the setpoint type is 'local pos' and none * of the local pos fields is set to 'ignore' */ if (offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED && - !offboard_control_sp_ignore_position(offboard_control_sp, 0) && - !offboard_control_sp_ignore_position(offboard_control_sp, 1) && - !offboard_control_sp_ignore_position(offboard_control_sp, 2)) { + !offboard_control_sp_ignore_position_all(offboard_control_sp)) { pos_sp_triplet.current.type = SETPOINT_TYPE_POSITION; //XXX support others pos_sp_triplet.current.position_valid = true; pos_sp_triplet.current.x = offboard_control_sp.position[0]; pos_sp_triplet.current.y = offboard_control_sp.position[1]; pos_sp_triplet.current.z = offboard_control_sp.position[2]; + } else { + pos_sp_triplet.current.position_valid = false; } /* set the local vel values if the setpoint type is 'local pos' and none * of the local vel fields is set to 'ignore' */ if (offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED && - !offboard_control_sp_ignore_velocity(offboard_control_sp, 0) && - !offboard_control_sp_ignore_velocity(offboard_control_sp, 1) && - !offboard_control_sp_ignore_velocity(offboard_control_sp, 2)) { - pos_sp_triplet.current.type = SETPOINT_TYPE_POSITION; //XXX support others - pos_sp_triplet.current.velocity_valid = true; - pos_sp_triplet.current.vx = offboard_control_sp.velocity[0]; - pos_sp_triplet.current.vy = offboard_control_sp.velocity[1]; - pos_sp_triplet.current.vz = offboard_control_sp.velocity[2]; + !offboard_control_sp_ignore_velocity_all(offboard_control_sp)) { + pos_sp_triplet.current.type = SETPOINT_TYPE_POSITION; //XXX support others + pos_sp_triplet.current.velocity_valid = true; + pos_sp_triplet.current.vx = offboard_control_sp.velocity[0]; + pos_sp_triplet.current.vy = offboard_control_sp.velocity[1]; + pos_sp_triplet.current.vz = offboard_control_sp.velocity[2]; + } else { + pos_sp_triplet.current.velocity_valid = false; } + /* set the local acceleration values if the setpoint type is 'local pos' and none + * of the accelerations fields is set to 'ignore' */ + if (offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED && + !offboard_control_sp_ignore_acceleration_all(offboard_control_sp)) { + pos_sp_triplet.current.type = SETPOINT_TYPE_POSITION; //XXX support others + pos_sp_triplet.current.acceleration_valid = true; + pos_sp_triplet.current.a_x = offboard_control_sp.acceleration[0]; + pos_sp_triplet.current.a_y = offboard_control_sp.acceleration[1]; + pos_sp_triplet.current.a_z = offboard_control_sp.acceleration[2]; + pos_sp_triplet.current.acceleration_is_force = + offboard_control_sp.isForceSetpoint; + + } else { + pos_sp_triplet.current.acceleration_valid = false; + } //XXX handle global pos setpoints (different MAV frames) diff --git a/src/modules/uORB/topics/offboard_control_setpoint.h b/src/modules/uORB/topics/offboard_control_setpoint.h index 4f45ac14ff..47c89f7083 100644 --- a/src/modules/uORB/topics/offboard_control_setpoint.h +++ b/src/modules/uORB/topics/offboard_control_setpoint.h @@ -142,14 +142,35 @@ inline bool offboard_control_sp_ignore_velocity_all(const struct offboard_contro return false; } +/** + * Returns true if the acceleration setpoint at index should be ignored + */ inline bool offboard_control_sp_ignore_acceleration(const struct offboard_control_setpoint_s &offboard_control_sp, int index) { return (bool)(offboard_control_sp.ignore & (1 << (6 + index))); } +/** + * Returns true if all acceleration setpoints should be ignored + */ +inline bool offboard_control_sp_ignore_acceleration_all(const struct offboard_control_setpoint_s &offboard_control_sp) { + for (int i = 0; i < 3; i++) { + if (offboard_control_sp_ignore_acceleration(offboard_control_sp, i)) { + return true; + } + } + return false; +} + +/** + * Returns true if the bodyrate setpoint at index should be ignored + */ inline bool offboard_control_sp_ignore_bodyrates(const struct offboard_control_setpoint_s &offboard_control_sp, int index) { return (bool)(offboard_control_sp.ignore & (1 << (9 + index))); } +/** + * Returns true if the attitude setpoint should be ignored + */ inline bool offboard_control_sp_ignore_attitude(const struct offboard_control_setpoint_s &offboard_control_sp) { return (bool)(offboard_control_sp.ignore & (1 << 10)); } diff --git a/src/modules/uORB/topics/position_setpoint_triplet.h b/src/modules/uORB/topics/position_setpoint_triplet.h index ec2131abd8..3ea3f671e0 100644 --- a/src/modules/uORB/topics/position_setpoint_triplet.h +++ b/src/modules/uORB/topics/position_setpoint_triplet.h @@ -83,6 +83,11 @@ struct position_setpoint_s 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 */ + float a_x; //**< acceleration x setpoint */ + float a_y; //**< acceleration y setpoint */ + float a_z; //**< acceleration z setpoint */ + bool acceleration_valid; //*< true if acceleration setpoint is valid/should be used */ + bool acceleration_is_force; //*< interprete acceleration as force */ }; /** From 309a718db41ea683502792199d8dc9fa6fbdeabc Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 12 Aug 2014 17:57:14 +0200 Subject: [PATCH 076/411] Put payload outputs for AERT mixer onto right actuator group --- ROMFS/px4fmu_common/mixers/FMU_AERT.mix | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/ROMFS/px4fmu_common/mixers/FMU_AERT.mix b/ROMFS/px4fmu_common/mixers/FMU_AERT.mix index 0ec663e35e..7fed488af1 100644 --- a/ROMFS/px4fmu_common/mixers/FMU_AERT.mix +++ b/ROMFS/px4fmu_common/mixers/FMU_AERT.mix @@ -64,21 +64,22 @@ O: 10000 10000 0 -10000 10000 S: 0 3 0 20000 -10000 -10000 10000 -Gimbal / flaps / payload mixer for last four channels +Gimbal / flaps / payload mixer for last four channels, +using the payload control group ----------------------------------------------------- M: 1 O: 10000 10000 0 -10000 10000 -S: 0 4 10000 10000 0 -10000 10000 +S: 2 0 10000 10000 0 -10000 10000 M: 1 O: 10000 10000 0 -10000 10000 -S: 0 5 10000 10000 0 -10000 10000 +S: 2 1 10000 10000 0 -10000 10000 M: 1 O: 10000 10000 0 -10000 10000 -S: 0 6 10000 10000 0 -10000 10000 +S: 2 2 10000 10000 0 -10000 10000 M: 1 O: 10000 10000 0 -10000 10000 -S: 0 7 10000 10000 0 -10000 10000 +S: 2 3 10000 10000 0 -10000 10000 From d6c018d14b283f67cc5d1f96d8bc6de3ccd73326 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 12 Aug 2014 17:57:47 +0200 Subject: [PATCH 077/411] Bottle drop iteration - commandline tool for component testing --- src/modules/bottle_drop/bottle_drop.cpp | 188 +++++++++++------- .../{bottle_drop.c => bottle_drop_params.c} | 2 +- src/modules/bottle_drop/module.mk | 2 +- 3 files changed, 121 insertions(+), 71 deletions(-) rename src/modules/bottle_drop/{bottle_drop.c => bottle_drop_params.c} (98%) diff --git a/src/modules/bottle_drop/bottle_drop.cpp b/src/modules/bottle_drop/bottle_drop.cpp index 84c7900ce0..e73f36163b 100644 --- a/src/modules/bottle_drop/bottle_drop.cpp +++ b/src/modules/bottle_drop/bottle_drop.cpp @@ -100,6 +100,10 @@ public: */ void status(); + void open_bay(); + void close_bay(); + void drop(); + private: bool _task_should_exit; /**< if true, task should exit */ int _main_task; /**< handle for task */ @@ -116,6 +120,7 @@ private: bool _open_door; bool _drop; hrt_abstime _doors_opened; + hrt_abstime _drop_time; struct position_s { double lat; //degrees @@ -130,6 +135,11 @@ private: void answer_command(struct vehicle_command_s *cmd, enum VEHICLE_CMD_RESULT result); + /** + * Set the actuators + */ + int actuators_publish(); + /** * Shim for calling task_main from task_create. */ @@ -153,7 +163,8 @@ BottleDrop::BottleDrop() : _actuators {}, _open_door(false), _drop(false), - _doors_opened(0) + _doors_opened(0), + _drop_time(0) { } @@ -211,6 +222,82 @@ BottleDrop::status() warnx("Dropping: %s", _drop ? "YES" : "NO"); } +void +BottleDrop::open_bay() +{ + _actuators.control[0] = -1.0f; + _actuators.control[1] = 1.0f; + + if (_doors_opened == 0) { + _doors_opened = hrt_absolute_time(); + } + warnx("open doors"); + + actuators_publish(); +} + +void +BottleDrop::close_bay() +{ + // closed door and locked survival kit + _actuators.control[0] = 1.0f; + _actuators.control[1] = -1.0f; + + _doors_opened = 0; + + actuators_publish(); +} + +void +BottleDrop::drop() +{ + + // update drop actuator, wait 0.5s until the doors are open before dropping + hrt_abstime starttime = hrt_absolute_time(); + + // force the door open if we have to + if (_doors_opened == 0) { + open_bay(); + warnx("bay not ready, forced open"); + } + + while (hrt_elapsed_time(&_doors_opened) < 400000 && hrt_elapsed_time(&starttime) < 2000000) { + usleep(50000); + warnx("delayed by door!"); + } + + if (_drop && _doors_opened != 0 && hrt_elapsed_time(&_doors_opened) < 500000) { + _actuators.control[2] = -0.5f; + } else { + _actuators.control[2] = 0.5f; + } + + _drop_time = hrt_absolute_time(); + + warnx("dropping now"); + + actuators_publish(); +} + +int +BottleDrop::actuators_publish() +{ + _actuators.timestamp = hrt_absolute_time(); + + // lazily publish _actuators only once available + if (_actuator_pub > 0) { + return orb_publish(ORB_ID(actuator_controls_2), _actuator_pub, &_actuators); + + } else { + _actuator_pub = orb_advertise(ORB_ID(actuator_controls_2), &_actuators); + if (_actuator_pub > 0) { + return OK; + } else { + return -1; + } + } +} + void BottleDrop::task_main() { @@ -272,7 +359,6 @@ BottleDrop::task_main() float future_distance = 0; // The distance between the UAVs to-be position and the drop point [m] // states - bool state_door = false; // Doors are closed = false, open = true bool state_drop = false; // Drop occurred = true, Drop din't occur = false bool state_run = false; // A drop was attempted = true, the drop is still in progress = false @@ -297,9 +383,6 @@ BottleDrop::task_main() memset(&update, 0, sizeof(update)); int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update)); - struct actuator_controls_s actuators; - memset(&actuators, 0, sizeof(actuators)); - struct mission_item_s flight_vector_s; struct mission_item_s flight_vector_e; @@ -314,6 +397,7 @@ BottleDrop::task_main() memset(&onboard_mission, 0, sizeof(onboard_mission)); orb_advert_t onboard_mission_pub = -1; + bool position_initialized = false; double latitude; double longitude; @@ -328,6 +412,9 @@ BottleDrop::task_main() fds[0].fd = _command_sub; fds[0].events = POLLIN; + // Whatever state the bay is in, we want it closed on startup + close_bay(); + while (!_task_should_exit) { /* wait for up to 100ms for data */ @@ -345,6 +432,19 @@ BottleDrop::task_main() handle_command(&_command); } + orb_check(vehicle_global_position_sub, &updated); + if (updated) { + /* copy global position */ + orb_copy(ORB_ID(vehicle_global_position), vehicle_global_position_sub, &globalpos); + + latitude = (double)globalpos.lat / 1e7; + longitude = (double)globalpos.lon / 1e7; + } + + if (!position_initialized) { + continue; + } + /* switch to faster updates during the drop */ while (_drop) { @@ -505,7 +605,7 @@ BottleDrop::task_main() // } /* Save WPs in datamanager */ - const size_t len = sizeof(struct mission_item_s); + const ssize_t len = sizeof(struct mission_item_s); if (dm_write(DM_KEY_WAYPOINTS_ONBOARD, 0, DM_PERSIST_IN_FLIGHT_RESET, &flight_vector_s, len) != len) { warnx("ERROR: could not save onboard WP"); @@ -539,29 +639,17 @@ BottleDrop::task_main() } if (isfinite(distance_real) && distance_real < distance_open_door && drop_approval) { - actuators.control[0] = -1.0f; // open door - actuators.control[1] = 1.0f; - state_door = true; - warnx("open doors"); - + open_bay(); } else { - // closed door and locked survival kit - actuators.control[0] = 0.5f; - actuators.control[1] = -0.5f; - actuators.control[2] = -0.5f; - state_door = false; + close_bay(); } - if (isfinite(distance_real) && distance_real < precision && distance_real < future_distance - && state_door) { // Drop only if the distance between drop point and actual position is getting larger again + if (isfinite(distance_real) && distance_real < precision && distance_real < future_distance) { // Drop only if the distance between drop point and actual position is getting larger again // XXX this check needs to be carefully validated - right now we prefer to drop if we're close to the goal // if(fabsf(_wrap_pi(globalpos.yaw-atan2f(wind.windspeed_north,wind_speed.windspeed_east)+M_PI_F)) < 80.0f/180.0f*M_PI_F) // if flight trajectory deviates more than 80 degrees from calculated path, it will no drop // { - actuators.control[2] = 0.5f; - state_drop = true; - state_run = true; - warnx("dropping now"); + drop(); // } // else // { @@ -569,16 +657,6 @@ BottleDrop::task_main() // } } - actuators.timestamp = hrt_absolute_time(); - - // lazily publish _actuators only once available - if (_actuator_pub > 0) { - orb_publish(ORB_ID(actuator_controls_2), _actuator_pub, &actuators); - - } else { - _actuator_pub = orb_advertise(ORB_ID(actuator_controls_2), &actuators); - } - counter++; // update_actuators(); @@ -594,43 +672,6 @@ BottleDrop::task_main() _exit(0); } -// XXX this is out of sync with the C port -// void -// BottleDrop::update_actuators() -// { -// _actuators.timestamp = hrt_absolute_time(); - -// // update door actuators -// if (_open_door) { -// _actuators.control[0] = -1.0f; -// _actuators.control[1] = 1.0f; - -// if (_doors_opened == 0) { -// _doors_opened = hrt_absolute_time(); -// } -// } else { -// _actuators.control[0] = 1.0f; -// _actuators.control[1] = -1.0f; - -// _doors_opened = 0; -// } - -// // update drop actuator, wait 0.5s until the doors are open before dropping -// if (_drop && _doors_opened != 0 && hrt_elapsed_time(&_doors_opened) > 500000) { -// _actuators.control[2] = -0.5f; -// } else { -// _actuators.control[2] = 0.5f; -// } - -// // lazily publish _actuators only once available -// if (_actuator_pub > 0) { -// orb_publish(ORB_ID(actuator_controls_2), _actuator_pub, &_actuators); - -// } else { -// _actuator_pub = orb_advertise(ORB_ID(actuator_controls_2), &_actuators); -// } -// } - void BottleDrop::handle_command(struct vehicle_command_s *cmd) { @@ -785,6 +826,15 @@ int bottle_drop_main(int argc, char *argv[]) } else if (!strcmp(argv[1], "status")) { bottle_drop::g_bottle_drop->status(); + } else if (!strcmp(argv[1], "drop")) { + bottle_drop::g_bottle_drop->drop(); + + } else if (!strcmp(argv[1], "open")) { + bottle_drop::g_bottle_drop->open_bay(); + + } else if (!strcmp(argv[1], "close")) { + bottle_drop::g_bottle_drop->close_bay(); + } else { usage(); } diff --git a/src/modules/bottle_drop/bottle_drop.c b/src/modules/bottle_drop/bottle_drop_params.c similarity index 98% rename from src/modules/bottle_drop/bottle_drop.c rename to src/modules/bottle_drop/bottle_drop_params.c index b867820755..0f60b58e49 100644 --- a/src/modules/bottle_drop/bottle_drop.c +++ b/src/modules/bottle_drop/bottle_drop_params.c @@ -32,7 +32,7 @@ ****************************************************************************/ /** - * @file bottle_drop.c + * @file bottle_drop_params.c * Bottle drop parameters * * @author Dominik Juchli diff --git a/src/modules/bottle_drop/module.mk b/src/modules/bottle_drop/module.mk index 61ac89e38b..6b18fff556 100644 --- a/src/modules/bottle_drop/module.mk +++ b/src/modules/bottle_drop/module.mk @@ -38,4 +38,4 @@ MODULE_COMMAND = bottle_drop SRCS = bottle_drop.cpp \ - bottle_drop.c + bottle_drop_params.c From 0c78794e5729d17a4649b567899cf9492a498c2b Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Thu, 14 Aug 2014 16:43:49 +0200 Subject: [PATCH 078/411] Fix the index numbers and command max travel in the app for the drop servo. --- ROMFS/px4fmu_common/mixers/Viper.mix | 8 +++++--- src/modules/bottle_drop/bottle_drop.cpp | 4 ++-- 2 files changed, 7 insertions(+), 5 deletions(-) diff --git a/ROMFS/px4fmu_common/mixers/Viper.mix b/ROMFS/px4fmu_common/mixers/Viper.mix index b05a8c8b04..5a0381bd87 100755 --- a/ROMFS/px4fmu_common/mixers/Viper.mix +++ b/ROMFS/px4fmu_common/mixers/Viper.mix @@ -58,12 +58,14 @@ Inputs to the mixer come from channel group 2 (payload), channels 0 M: 1 O: 10000 10000 0 -10000 10000 -S: 2 4 10000 10000 0 -10000 10000 +S: 2 0 10000 10000 0 -10000 10000 M: 1 O: 10000 10000 0 -10000 10000 -S: 2 5 10000 10000 0 -10000 10000 +S: 2 1 10000 10000 0 -10000 10000 M: 1 O: 10000 10000 0 -10000 10000 -S: 2 6 10000 10000 0 -10000 10000 +S: 2 2 -10000 -10000 0 -10000 10000 + + diff --git a/src/modules/bottle_drop/bottle_drop.cpp b/src/modules/bottle_drop/bottle_drop.cpp index e73f36163b..e5acc3c7e6 100644 --- a/src/modules/bottle_drop/bottle_drop.cpp +++ b/src/modules/bottle_drop/bottle_drop.cpp @@ -267,9 +267,9 @@ BottleDrop::drop() } if (_drop && _doors_opened != 0 && hrt_elapsed_time(&_doors_opened) < 500000) { - _actuators.control[2] = -0.5f; + _actuators.control[2] = -1.0f; } else { - _actuators.control[2] = 0.5f; + _actuators.control[2] = 1.0f; } _drop_time = hrt_absolute_time(); From 9ee6ab366d2a7c6f24e2f08021da9cd861663bdc Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 14 Aug 2014 17:30:05 +0200 Subject: [PATCH 079/411] add flighttermination circuit breaker --- src/modules/systemlib/circuit_breaker.c | 13 +++++++++++++ src/modules/systemlib/circuit_breaker.h | 1 + 2 files changed, 14 insertions(+) diff --git a/src/modules/systemlib/circuit_breaker.c b/src/modules/systemlib/circuit_breaker.c index 64317a18a8..8a0b51b71b 100644 --- a/src/modules/systemlib/circuit_breaker.c +++ b/src/modules/systemlib/circuit_breaker.c @@ -95,6 +95,19 @@ PARAM_DEFINE_INT32(CBRK_IO_SAFETY, 0); */ PARAM_DEFINE_INT32(CBRK_AIRSPD_CHK, 0); +/** + * Circuit breaker for flight termination + * + * Setting this parameter to 121212 will disable the flight termination action. + * --> The IO driver will not do flight termination if requested by the FMU + * WARNING: ENABLING THIS CIRCUIT BREAKER IS AT OWN RISK + * + * @min 0 + * @max 121212 + * @group Circuit Breaker + */ +PARAM_DEFINE_INT32(CBRK_FLIGHTTERMINATION, 0); + bool circuit_breaker_enabled(const char* breaker, int32_t magic) { int32_t val; diff --git a/src/modules/systemlib/circuit_breaker.h b/src/modules/systemlib/circuit_breaker.h index b605846082..54c4fced62 100644 --- a/src/modules/systemlib/circuit_breaker.h +++ b/src/modules/systemlib/circuit_breaker.h @@ -53,6 +53,7 @@ #define CBRK_RATE_CTRL_KEY 140253 #define CBRK_IO_SAFETY_KEY 22027 #define CBRK_AIRSPD_CHK_KEY 162128 +#define CBRK_FLIGHTTERMINATION_KEY 121212 #include From 0f2b66fa8b3782c965966e9c0b0b6f16b80d7f38 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 14 Aug 2014 22:38:48 +0200 Subject: [PATCH 080/411] failsafe: enable support for commands --- src/modules/commander/commander.cpp | 25 +++++++++++++++++++ .../commander/state_machine_helper.cpp | 11 +++++++- src/modules/navigator/navigator_main.cpp | 2 +- src/modules/uORB/topics/vehicle_status.h | 5 +++- 4 files changed, 40 insertions(+), 3 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index c6f56d5e3a..b635a22cb6 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -557,6 +557,31 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s armed_local->force_failsafe = false; warnx("disabling failsafe"); } + /* param2 is currently used for other failsafe modes */ + status_local->engine_failure_cmd = false; + status_local->data_link_lost_cmd = false; + status_local->gps_failure_cmd = false; + status_local->rc_signal_lost_cmd = false; + if ((int)cmd->param2 <= 0) { + /* reset all commanded failure modes */ + warnx("revert to normal mode"); + } else if ((int)cmd->param2 == 1) { + /* trigger engine failure mode */ + status_local->engine_failure_cmd = true; + warnx("engine failure mode commanded"); + } else if ((int)cmd->param2 == 2) { + /* trigger data link loss mode */ + status_local->data_link_lost_cmd = true; + warnx("data link loss mode commanded"); + } else if ((int)cmd->param2 == 3) { + /* trigger gps loss mode */ + status_local->gps_failure_cmd = true; + warnx("gps loss mode commanded"); + } else if ((int)cmd->param2 == 4) { + /* trigger rc loss mode */ + status_local->rc_signal_lost_cmd = true; + warnx("rc loss mode commanded"); + } cmd_result = VEHICLE_CMD_RESULT_ACCEPTED; } break; diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index ecfe62e03e..157e35ef8d 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -491,10 +491,19 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en case MAIN_STATE_AUTO_MISSION: /* go into failsafe + * - if commanded to do so * - if we have an engine failure * - if either the datalink is enabled and lost as well as RC is lost * - if there is no datalink and the mission is finished */ - if (status->engine_failure) { + if (status->engine_failure_cmd) { + status->nav_state = NAVIGATION_STATE_AUTO_LANDENGFAIL; + } else if (status->data_link_lost_cmd) { + status->nav_state = NAVIGATION_STATE_AUTO_RTGS; + //} else if (status->gps_failure_cmd) { + //status->nav_state = NAVIGATION_STATE_AUTO_***; + } else if (status->rc_signal_lost_cmd) { + status->nav_state = NAVIGATION_STATE_AUTO_RTGS; //XXX + } else if (status->engine_failure) { status->nav_state = NAVIGATION_STATE_AUTO_LANDENGFAIL; } else if (((status->data_link_lost && data_link_loss_enabled) && status->rc_signal_lost) || (!data_link_loss_enabled && status->rc_signal_lost && mission_finished)) { diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 043d883b2d..4af37fa3eb 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -384,7 +384,7 @@ Navigator::task_main() case NAVIGATION_STATE_AUTO_RTL: _navigation_mode = &_rtl; break; - case NAVIGATION_STATE_AUTO_RTGS: + case NAVIGATION_STATE_AUTO_RTGS: //XXX OBC: differentiate between rc loss and dl loss here /* Use complex data link loss mode only when enabled via param * otherwise use rtl */ if (_param_datalinkloss_obc.get() != 0) { diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index 301503b82e..b465f8407d 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -200,13 +200,16 @@ struct vehicle_status_s { bool rc_signal_found_once; bool rc_signal_lost; /**< true if RC reception lost */ + bool rc_signal_lost_cmd; /**< true if RC lost mode is commanded */ bool rc_input_blocked; /**< set if RC input should be ignored */ bool data_link_lost; /**< datalink to GCS lost */ + bool data_link_lost_cmd; /**< datalink to GCS lost mode commanded */ uint8_t data_link_lost_counter; /**< counts unique data link lost events */ - bool engine_failure; /** Set to true if an engine failure is detected */ + bool engine_failure_cmd; /** Set to true if an engine failure mode is commanded */ bool gps_failure; /** Set to true if a gps failure is detected */ + bool gps_failure_cmd; /** Set to true if a gps failure mode is commanded */ bool offboard_control_signal_found_once; bool offboard_control_signal_lost; From 6fe0482b277cd7120878787171c735e2d424c1c6 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 15 Aug 2014 00:07:38 +0200 Subject: [PATCH 081/411] failsafe: make warnx more clear --- src/modules/commander/commander.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index b635a22cb6..4f996b5261 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -552,10 +552,10 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s if (cmd->param1 > 0.5f) { //XXX update state machine? armed_local->force_failsafe = true; - warnx("forcing failsafe"); + warnx("forcing failsafe (termination)"); } else { armed_local->force_failsafe = false; - warnx("disabling failsafe"); + warnx("disabling failsafe (termination)"); } /* param2 is currently used for other failsafe modes */ status_local->engine_failure_cmd = false; From 85d8781bc32fc25b30350aab518a8b9823cce661 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 15 Aug 2014 01:26:04 +0200 Subject: [PATCH 082/411] datalinkloss: improve logic --- src/modules/navigator/datalinkloss.cpp | 34 +++++++++++++++++--------- 1 file changed, 23 insertions(+), 11 deletions(-) diff --git a/src/modules/navigator/datalinkloss.cpp b/src/modules/navigator/datalinkloss.cpp index 19f3356331..6c5012bfdd 100644 --- a/src/modules/navigator/datalinkloss.cpp +++ b/src/modules/navigator/datalinkloss.cpp @@ -58,14 +58,14 @@ DataLinkLoss::DataLinkLoss(Navigator *navigator, const char *name) : MissionBlock(navigator, name), _vehicleStatus(&getSubscriptions(), ORB_ID(vehicle_status), 100), - _param_commsholdwaittime(this, "CH_T"), - _param_commsholdlat(this, "CH_LAT"), - _param_commsholdlon(this, "CH_LON"), - _param_commsholdalt(this, "CH_ALT"), - _param_airfieldhomelat(this, "AH_LAT"), - _param_airfieldhomelon(this, "AH_LON"), - _param_airfieldhomealt(this, "AH_ALT"), - _param_numberdatalinklosses(this, "DLL_N"), + _param_commsholdwaittime(this, "NAV_DLL_CH_T", false), + _param_commsholdlat(this, "NAV_DLL_CH_LAT", false), + _param_commsholdlon(this, "NAV_DLL_CH_LON", false), + _param_commsholdalt(this, "NAV_DLL_CH_ALT", false), + _param_airfieldhomelat(this, "NAV_DLL_AH_LAT", false), + _param_airfieldhomelon(this, "NAV_DLL_AH_LON", false), + _param_airfieldhomealt(this, "NAV_DLL_AH_ALT", false), + _param_numberdatalinklosses(this, "NAV_DLL_N", false), _dll_state(DLL_STATE_NONE) { /* load initial params */ @@ -81,7 +81,7 @@ DataLinkLoss::~DataLinkLoss() void DataLinkLoss::on_inactive() { - /* reset RTL state only if setpoint moved */ + /* reset DLL state only if setpoint moved */ if (!_navigator->get_can_loiter_at_sp()) { _dll_state = DLL_STATE_NONE; } @@ -90,7 +90,8 @@ DataLinkLoss::on_inactive() void DataLinkLoss::on_activation() { - _dll_state = DLL_STATE_FLYTOCOMMSHOLDWP; + _dll_state = DLL_STATE_NONE; + advance_dll(); set_dll_item(); } @@ -131,6 +132,10 @@ DataLinkLoss::set_dll_item() _mission_item.origin = ORIGIN_ONBOARD; _navigator->set_can_loiter_at_sp(true); + warnx("mission item: lat %.7f lon %.7f alt %.3f", + _mission_item.lat, + _mission_item.lon, + (double)_mission_item.altitude); break; } case DLL_STATE_FLYTOAIRFIELDHOMEWP: { @@ -159,6 +164,10 @@ DataLinkLoss::set_dll_item() /* convert mission item to current position setpoint and make it valid */ mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current); pos_sp_triplet->next.valid = false; + warnx("triplet current: lat %.7f lon %.7f alt %.3f", + pos_sp_triplet->current.lat, + pos_sp_triplet->current.lon, + (double)pos_sp_triplet->current.alt); _navigator->set_position_setpoint_triplet_updated(); } @@ -166,18 +175,21 @@ DataLinkLoss::set_dll_item() void DataLinkLoss::advance_dll() { + warnx("dll_state %u", _dll_state); switch (_dll_state) { case DLL_STATE_NONE: /* Check the number of data link losses. If above home fly home directly */ updateSubscriptions(); if (_vehicleStatus.data_link_lost_counter > _param_numberdatalinklosses.get()) { + warnx("too many data link losses, fly to airfield home"); _dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP; } else { + warnx("fly to comms hold"); _dll_state = DLL_STATE_FLYTOCOMMSHOLDWP; } break; case DLL_STATE_FLYTOCOMMSHOLDWP: - //XXX check here if time is over are over + warnx("fly to airfield home"); _dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP; _navigator->get_mission_result()->stay_in_failsafe = true; _navigator->publish_mission_result(); From 8bc7d7f43dd350c19993cd12b514a6a0b5366b3c Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 15 Aug 2014 01:26:43 +0200 Subject: [PATCH 083/411] navigator: correctly use all navigator modes --- src/modules/navigator/navigator.h | 3 +-- src/modules/navigator/navigator_main.cpp | 2 ++ 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index fe6639dfe7..5369779723 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -65,9 +65,8 @@ /** * Number of navigation modes that need on_active/on_inactive calls - * Currently: mission, loiter, and rtl */ -#define NAVIGATOR_MODE_ARRAY_SIZE 4 +#define NAVIGATOR_MODE_ARRAY_SIZE 6 class Navigator : public control::SuperBlock { diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 4af37fa3eb..18bfc2cece 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -141,6 +141,8 @@ Navigator::Navigator() : _navigation_mode_array[1] = &_loiter; _navigation_mode_array[2] = &_rtl; _navigation_mode_array[3] = &_offboard; + _navigation_mode_array[4] = &_dataLinkLoss; + _navigation_mode_array[5] = &_engineFailure; updateParams(); } From b71778d7e1d03adf8b1366982f0e86753ac072be Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 15 Aug 2014 01:41:48 +0200 Subject: [PATCH 084/411] datalinkloss: change default param value --- src/modules/navigator/datalinkloss_params.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/navigator/datalinkloss_params.c b/src/modules/navigator/datalinkloss_params.c index 038c80a1a1..77a8763cb1 100644 --- a/src/modules/navigator/datalinkloss_params.c +++ b/src/modules/navigator/datalinkloss_params.c @@ -67,7 +67,7 @@ PARAM_DEFINE_FLOAT(NAV_DLL_CH_T, 120.0f); * @min 0.0 * @group DLL */ -PARAM_DEFINE_INT32(NAV_DLL_CH_LAT, 266072120); +PARAM_DEFINE_INT32(NAV_DLL_CH_LAT, -266072120); /** * Comms hold Lon @@ -100,7 +100,7 @@ PARAM_DEFINE_FLOAT(NAV_DLL_CH_ALT, 600.0f); * @min 0.0 * @group DLL */ -PARAM_DEFINE_INT32(NAV_DLL_AH_LAT, 265847810); +PARAM_DEFINE_INT32(NAV_DLL_AH_LAT, -265847810); /** * Airfield home Lon From 1e9d77f1d2dfd3f500c8486edf8940bc4a15162e Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 15 Aug 2014 12:58:06 +0200 Subject: [PATCH 085/411] offboard: ignore mask cleanup add handling of thrust ignore bit, add defines for the bitnumbers, improve and fix interface to ignore bitmask --- src/modules/mavlink/mavlink_receiver.cpp | 29 +++--- .../uORB/topics/offboard_control_setpoint.h | 90 +++++++++++++++++-- 2 files changed, 100 insertions(+), 19 deletions(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 7dd043bbec..6b3c3accba 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -472,7 +472,7 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t /* set the local pos values if the setpoint type is 'local pos' and none * of the local pos fields is set to 'ignore' */ if (offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED && - !offboard_control_sp_ignore_position_all(offboard_control_sp)) { + !offboard_control_sp_ignore_position_some(offboard_control_sp)) { pos_sp_triplet.current.type = SETPOINT_TYPE_POSITION; //XXX support others pos_sp_triplet.current.position_valid = true; pos_sp_triplet.current.x = offboard_control_sp.position[0]; @@ -485,7 +485,7 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t /* set the local vel values if the setpoint type is 'local pos' and none * of the local vel fields is set to 'ignore' */ if (offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED && - !offboard_control_sp_ignore_velocity_all(offboard_control_sp)) { + !offboard_control_sp_ignore_velocity_some(offboard_control_sp)) { pos_sp_triplet.current.type = SETPOINT_TYPE_POSITION; //XXX support others pos_sp_triplet.current.velocity_valid = true; pos_sp_triplet.current.vx = offboard_control_sp.velocity[0]; @@ -498,7 +498,7 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t /* set the local acceleration values if the setpoint type is 'local pos' and none * of the accelerations fields is set to 'ignore' */ if (offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED && - !offboard_control_sp_ignore_acceleration_all(offboard_control_sp)) { + !offboard_control_sp_ignore_acceleration_some(offboard_control_sp)) { pos_sp_triplet.current.type = SETPOINT_TYPE_POSITION; //XXX support others pos_sp_triplet.current.acceleration_valid = true; pos_sp_triplet.current.a_x = offboard_control_sp.acceleration[0]; @@ -589,12 +589,17 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg) offboard_control_sp.attitude_rate[1] = set_attitude_target.body_pitch_rate; offboard_control_sp.attitude_rate[2] = set_attitude_target.body_yaw_rate; + /* set correct ignore flags for body rate fields: copy from mavlink message */ for (int i = 0; i < 3; i++) { - offboard_control_sp.ignore &= ~(1 << (i + 9)); - offboard_control_sp.ignore |= (set_attitude_target.type_mask & (1 << i)) << 9; + offboard_control_sp.ignore &= ~(1 << (i + OFB_IGN_BIT_BODYRATE_X)); + offboard_control_sp.ignore |= (set_attitude_target.type_mask & (1 << i)) << OFB_IGN_BIT_BODYRATE_X; } - offboard_control_sp.ignore &= ~(1 << 10); - offboard_control_sp.ignore |= ((set_attitude_target.type_mask & (1 << 7)) << 3); + /* set correct ignore flags for thrust field: copy from mavlink message */ + offboard_control_sp.ignore &= ~(1 << OFB_IGN_BIT_THRUST); + offboard_control_sp.ignore |= ((set_attitude_target.type_mask & (1 << 6)) << OFB_IGN_BIT_THRUST); + /* set correct ignore flags for attitude field: copy from mavlink message */ + offboard_control_sp.ignore &= ~(1 << OFB_IGN_BIT_ATT); + offboard_control_sp.ignore |= ((set_attitude_target.type_mask & (1 << 7)) << OFB_IGN_BIT_ATT); offboard_control_sp.timestamp = hrt_absolute_time(); @@ -618,8 +623,9 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg) if (_control_mode.flag_control_offboard_enabled) { - /* Publish attitude setpoint if ignore bit is not set */ - if (!(set_attitude_target.type_mask & (1 << 7))) { + /* Publish attitude setpoint if attitude and thrust ignore bits are not set */ + if (!(offboard_control_sp_ignore_attitude(offboard_control_sp) || + offboard_control_sp_ignore_thrust(offboard_control_sp))) { struct vehicle_attitude_setpoint_s att_sp; att_sp.timestamp = hrt_absolute_time(); mavlink_quaternion_to_euler(set_attitude_target.q, @@ -634,9 +640,10 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg) } } - /* Publish attitude rate setpoint if ignore bit are not set */ + /* Publish attitude rate setpoint if bodyrate and thrust ignore bits are not set */ ///XXX add support for ignoring individual axes - if (!(set_attitude_target.type_mask & (0b111))) { + if (!(offboard_control_sp_ignore_bodyrates_some(offboard_control_sp) || + offboard_control_sp_ignore_thrust(offboard_control_sp))) { struct vehicle_rates_setpoint_s rates_sp; rates_sp.timestamp = hrt_absolute_time(); rates_sp.roll = set_attitude_target.body_roll_rate; diff --git a/src/modules/uORB/topics/offboard_control_setpoint.h b/src/modules/uORB/topics/offboard_control_setpoint.h index 47c89f7083..6e37896afe 100644 --- a/src/modules/uORB/topics/offboard_control_setpoint.h +++ b/src/modules/uORB/topics/offboard_control_setpoint.h @@ -61,7 +61,7 @@ enum OFFBOARD_CONTROL_MODE { OFFBOARD_CONTROL_MODE_DIRECT_FORCE = 8, OFFBOARD_CONTROL_MODE_ATT_YAW_RATE = 9, OFFBOARD_CONTROL_MODE_ATT_YAW_POS = 10, - OFFBOARD_CONTROL_MODE_MULTIROTOR_SIMPLE = 11, /**< roll / pitch rotated aligned to the takeoff orientation, throttle stabilized, yaw pos */ + OFFBOARD_CONTROL_MODE_MULTIROTOR_SIMPLE = 11 /**< roll / pitch rotated aligned to the takeoff orientation, throttle stabilized, yaw pos */ }; enum OFFBOARD_CONTROL_FRAME { @@ -72,6 +72,23 @@ enum OFFBOARD_CONTROL_FRAME { OFFBOARD_CONTROL_FRAME_GLOBAL = 4 }; +/* mappings for the ignore bitmask */ +enum {OFB_IGN_BIT_POS_X, + OFB_IGN_BIT_POS_Y, + OFB_IGN_BIT_POS_Z, + OFB_IGN_BIT_VEL_X, + OFB_IGN_BIT_VEL_Y, + OFB_IGN_BIT_VEL_Z, + OFB_IGN_BIT_ACC_X, + OFB_IGN_BIT_ACC_Y, + OFB_IGN_BIT_ACC_Z, + OFB_IGN_BIT_BODYRATE_X, + OFB_IGN_BIT_BODYRATE_Y, + OFB_IGN_BIT_BODYRATE_Z, + OFB_IGN_BIT_ATT, + OFB_IGN_BIT_THRUST +}; + /** * @addtogroup topics * @{ @@ -87,9 +104,11 @@ struct offboard_control_setpoint_s { float acceleration[3]; /**< x acc, y acc, z acc */ float attitude[4]; /**< attitude of vehicle (quaternion) */ float attitude_rate[3]; /**< body angular rates (x, y, z) */ + float thrust; /**< thrust */ + + uint16_t ignore; /**< if field i is set to true, the value should be ignored, see definition at top of file + for mapping */ - uint16_t ignore; /**< if field i is set to true, pi should be ignored */ - //XXX define constants for bit offsets bool isForceSetpoint; /**< the acceleration vector should be interpreted as force */ float override_mode_switch; @@ -108,13 +127,25 @@ struct offboard_control_setpoint_s { * Returns true if the position setpoint at index should be ignored */ inline bool offboard_control_sp_ignore_position(const struct offboard_control_setpoint_s &offboard_control_sp, int index) { - return (bool)(offboard_control_sp.ignore & (1 << index)); + return (bool)(offboard_control_sp.ignore & (1 << (OFB_IGN_BIT_POS_X + index))); } /** * Returns true if all position setpoints should be ignored */ inline bool offboard_control_sp_ignore_position_all(const struct offboard_control_setpoint_s &offboard_control_sp) { + for (int i = 0; i < 3; i++) { + if (!offboard_control_sp_ignore_position(offboard_control_sp, i)) { + return false; + } + } + return true; +} + +/** + * Returns true if some position setpoints should be ignored + */ +inline bool offboard_control_sp_ignore_position_some(const struct offboard_control_setpoint_s &offboard_control_sp) { for (int i = 0; i < 3; i++) { if (offboard_control_sp_ignore_position(offboard_control_sp, i)) { return true; @@ -127,13 +158,25 @@ inline bool offboard_control_sp_ignore_position_all(const struct offboard_contro * Returns true if the velocity setpoint at index should be ignored */ inline bool offboard_control_sp_ignore_velocity(const struct offboard_control_setpoint_s &offboard_control_sp, int index) { - return (bool)(offboard_control_sp.ignore & (1 << (3 + index))); + return (bool)(offboard_control_sp.ignore & (1 << (OFB_IGN_BIT_VEL_X + index))); } /** * Returns true if all velocity setpoints should be ignored */ inline bool offboard_control_sp_ignore_velocity_all(const struct offboard_control_setpoint_s &offboard_control_sp) { + for (int i = 0; i < 3; i++) { + if (!offboard_control_sp_ignore_velocity(offboard_control_sp, i)) { + return false; + } + } + return true; +} + +/** + * Returns true if some velocity setpoints should be ignored + */ +inline bool offboard_control_sp_ignore_velocity_some(const struct offboard_control_setpoint_s &offboard_control_sp) { for (int i = 0; i < 3; i++) { if (offboard_control_sp_ignore_velocity(offboard_control_sp, i)) { return true; @@ -146,13 +189,25 @@ inline bool offboard_control_sp_ignore_velocity_all(const struct offboard_contro * Returns true if the acceleration setpoint at index should be ignored */ inline bool offboard_control_sp_ignore_acceleration(const struct offboard_control_setpoint_s &offboard_control_sp, int index) { - return (bool)(offboard_control_sp.ignore & (1 << (6 + index))); + return (bool)(offboard_control_sp.ignore & (1 << (OFB_IGN_BIT_ACC_X + index))); } /** * Returns true if all acceleration setpoints should be ignored */ inline bool offboard_control_sp_ignore_acceleration_all(const struct offboard_control_setpoint_s &offboard_control_sp) { + for (int i = 0; i < 3; i++) { + if (!offboard_control_sp_ignore_acceleration(offboard_control_sp, i)) { + return false; + } + } + return true; +} + +/** + * Returns true if some acceleration setpoints should be ignored + */ +inline bool offboard_control_sp_ignore_acceleration_some(const struct offboard_control_setpoint_s &offboard_control_sp) { for (int i = 0; i < 3; i++) { if (offboard_control_sp_ignore_acceleration(offboard_control_sp, i)) { return true; @@ -165,14 +220,33 @@ inline bool offboard_control_sp_ignore_acceleration_all(const struct offboard_co * Returns true if the bodyrate setpoint at index should be ignored */ inline bool offboard_control_sp_ignore_bodyrates(const struct offboard_control_setpoint_s &offboard_control_sp, int index) { - return (bool)(offboard_control_sp.ignore & (1 << (9 + index))); + return (bool)(offboard_control_sp.ignore & (1 << (OFB_IGN_BIT_BODYRATE_X + index))); +} + +/** + * Returns true if some of the bodyrate setpoints should be ignored + */ +inline bool offboard_control_sp_ignore_bodyrates_some(const struct offboard_control_setpoint_s &offboard_control_sp) { + for (int i = 0; i < 3; i++) { + if (offboard_control_sp_ignore_bodyrates(offboard_control_sp, i)) { + return true; + } + } + return false; } /** * Returns true if the attitude setpoint should be ignored */ inline bool offboard_control_sp_ignore_attitude(const struct offboard_control_setpoint_s &offboard_control_sp) { - return (bool)(offboard_control_sp.ignore & (1 << 10)); + return (bool)(offboard_control_sp.ignore & (1 << OFB_IGN_BIT_ATT)); +} + +/** + * Returns true if the thrust setpoint should be ignored + */ +inline bool offboard_control_sp_ignore_thrust(const struct offboard_control_setpoint_s &offboard_control_sp) { + return (bool)(offboard_control_sp.ignore & (1 << OFB_IGN_BIT_THRUST)); } From 8dbe6a6dc0d753a0f092fc2d4be508e48429c008 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 15 Aug 2014 14:35:45 +0200 Subject: [PATCH 086/411] px4io driver: use flighttermination circuit breaker --- src/drivers/px4io/px4io.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 32069cf09b..afb03789ff 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1169,7 +1169,8 @@ PX4IO::io_set_arming_state() clear |= PX4IO_P_SETUP_ARMING_LOCKDOWN; } - if (armed.force_failsafe) { + /* Do not set failsafe if circuit breaker is enabled */ + if (armed.force_failsafe && !circuit_breaker_enabled("CBRK_FLIGHTTERMINATION", CBRK_FLIGHTTERMINATION_KEY)) { set |= PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE; } else { clear |= PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE; From 20ceba48cfc4e306f2dc4f5f98d0730730cd3233 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 15 Aug 2014 20:34:04 +0200 Subject: [PATCH 087/411] Do not modify startup where not absolutely required --- ROMFS/px4fmu_common/init.d/rc.fw_apps | 2 -- 1 file changed, 2 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.fw_apps b/ROMFS/px4fmu_common/init.d/rc.fw_apps index 767c546290..c97b3477f8 100644 --- a/ROMFS/px4fmu_common/init.d/rc.fw_apps +++ b/ROMFS/px4fmu_common/init.d/rc.fw_apps @@ -15,5 +15,3 @@ fw_att_control start fw_pos_control_l1 start bottle_drop start -fmu mode_pwm -mixer load /dev/px4fmu /etc/mixers/FMU_pass.mix From 5406ba78a8de82c49aa9ea8e033c82670950259d Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 15 Aug 2014 20:53:03 +0200 Subject: [PATCH 088/411] make navigator mode a child of navigator --- src/modules/navigator/datalinkloss.cpp | 19 +++++++++++-------- src/modules/navigator/mission.cpp | 6 +++--- src/modules/navigator/navigator_mode.cpp | 2 +- src/modules/navigator/rtl.cpp | 6 +++--- 4 files changed, 18 insertions(+), 15 deletions(-) diff --git a/src/modules/navigator/datalinkloss.cpp b/src/modules/navigator/datalinkloss.cpp index 6c5012bfdd..2efca0a949 100644 --- a/src/modules/navigator/datalinkloss.cpp +++ b/src/modules/navigator/datalinkloss.cpp @@ -58,14 +58,14 @@ DataLinkLoss::DataLinkLoss(Navigator *navigator, const char *name) : MissionBlock(navigator, name), _vehicleStatus(&getSubscriptions(), ORB_ID(vehicle_status), 100), - _param_commsholdwaittime(this, "NAV_DLL_CH_T", false), - _param_commsholdlat(this, "NAV_DLL_CH_LAT", false), - _param_commsholdlon(this, "NAV_DLL_CH_LON", false), - _param_commsholdalt(this, "NAV_DLL_CH_ALT", false), - _param_airfieldhomelat(this, "NAV_DLL_AH_LAT", false), - _param_airfieldhomelon(this, "NAV_DLL_AH_LON", false), - _param_airfieldhomealt(this, "NAV_DLL_AH_ALT", false), - _param_numberdatalinklosses(this, "NAV_DLL_N", false), + _param_commsholdwaittime(this, "CH_T"), + _param_commsholdlat(this, "CH_LAT"), + _param_commsholdlon(this, "CH_LON"), + _param_commsholdalt(this, "CH_ALT"), + _param_airfieldhomelat(this, "AH_LAT"), + _param_airfieldhomelon(this, "AH_LON"), + _param_airfieldhomealt(this, "AH_ALT"), + _param_numberdatalinklosses(this, "N"), _dll_state(DLL_STATE_NONE) { /* load initial params */ @@ -182,14 +182,17 @@ DataLinkLoss::advance_dll() updateSubscriptions(); if (_vehicleStatus.data_link_lost_counter > _param_numberdatalinklosses.get()) { warnx("too many data link losses, fly to airfield home"); + mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: too many DL losses, fly to home"); _dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP; } else { warnx("fly to comms hold"); + mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: fly to comms hold"); _dll_state = DLL_STATE_FLYTOCOMMSHOLDWP; } break; case DLL_STATE_FLYTOCOMMSHOLDWP: warnx("fly to airfield home"); + mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: fly to home"); _dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP; _navigator->get_mission_result()->stay_in_failsafe = true; _navigator->publish_mission_result(); diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index c76192f667..79865e2ae0 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -59,9 +59,9 @@ Mission::Mission(Navigator *navigator, const char *name) : MissionBlock(navigator, name), - _param_onboard_enabled(this, "ONBOARD_EN"), - _param_takeoff_alt(this, "TAKEOFF_ALT"), - _param_dist_1wp(this, "DIST_1WP"), + _param_onboard_enabled(this, "MIS_ONBOARD_EN", false), + _param_takeoff_alt(this, "MIS_TAKEOFF_ALT", false), + _param_dist_1wp(this, "MIS_DIST_1WP", false), _onboard_mission({0}), _offboard_mission({0}), _current_onboard_mission_index(-1), diff --git a/src/modules/navigator/navigator_mode.cpp b/src/modules/navigator/navigator_mode.cpp index 3c6754c556..d91f7ab181 100644 --- a/src/modules/navigator/navigator_mode.cpp +++ b/src/modules/navigator/navigator_mode.cpp @@ -43,7 +43,7 @@ #include "navigator.h" NavigatorMode::NavigatorMode(Navigator *navigator, const char *name) : - SuperBlock(NULL, name), + SuperBlock(navigator, name), _navigator(navigator), _first_run(true) { diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index 142a734098..b6c4b8fdff 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -58,9 +58,9 @@ RTL::RTL(Navigator *navigator, const char *name) : MissionBlock(navigator, name), _rtl_state(RTL_STATE_NONE), - _param_return_alt(this, "RETURN_ALT"), - _param_descend_alt(this, "DESCEND_ALT"), - _param_land_delay(this, "LAND_DELAY") + _param_return_alt(this, "RTL_RETURN_ALT", false), + _param_descend_alt(this, "RTL_DESCEND_ALT", false), + _param_land_delay(this, "RTL_LAND_DELAY", false) { /* load initial params */ updateParams(); From 2791a7097686d327e91ac31c20716bb602011d65 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 15 Aug 2014 20:57:21 +0200 Subject: [PATCH 089/411] remove warnx --- src/modules/navigator/datalinkloss.cpp | 8 -------- 1 file changed, 8 deletions(-) diff --git a/src/modules/navigator/datalinkloss.cpp b/src/modules/navigator/datalinkloss.cpp index 2efca0a949..7b9a7b151c 100644 --- a/src/modules/navigator/datalinkloss.cpp +++ b/src/modules/navigator/datalinkloss.cpp @@ -132,10 +132,6 @@ DataLinkLoss::set_dll_item() _mission_item.origin = ORIGIN_ONBOARD; _navigator->set_can_loiter_at_sp(true); - warnx("mission item: lat %.7f lon %.7f alt %.3f", - _mission_item.lat, - _mission_item.lon, - (double)_mission_item.altitude); break; } case DLL_STATE_FLYTOAIRFIELDHOMEWP: { @@ -164,10 +160,6 @@ DataLinkLoss::set_dll_item() /* convert mission item to current position setpoint and make it valid */ mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current); pos_sp_triplet->next.valid = false; - warnx("triplet current: lat %.7f lon %.7f alt %.3f", - pos_sp_triplet->current.lat, - pos_sp_triplet->current.lon, - (double)pos_sp_triplet->current.alt); _navigator->set_position_setpoint_triplet_updated(); } From f8f72d665d2a0f07d14bb9c27293423e715486c4 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 15 Aug 2014 23:40:53 +0200 Subject: [PATCH 090/411] Last drop fixes and adjustments --- src/modules/bottle_drop/bottle_drop.cpp | 121 ++++++++++--------- src/modules/bottle_drop/bottle_drop_params.c | 4 +- 2 files changed, 67 insertions(+), 58 deletions(-) diff --git a/src/modules/bottle_drop/bottle_drop.cpp b/src/modules/bottle_drop/bottle_drop.cpp index e5acc3c7e6..4367d13432 100644 --- a/src/modules/bottle_drop/bottle_drop.cpp +++ b/src/modules/bottle_drop/bottle_drop.cpp @@ -112,16 +112,20 @@ private: int _command_sub; int _wind_estimate_sub; struct vehicle_command_s _command; + struct vehicle_global_position_s _global_pos; + map_projection_reference_s ref; orb_advert_t _actuator_pub; struct actuator_controls_s _actuators; - bool drop_approval; + bool _drop_approval; bool _open_door; bool _drop; hrt_abstime _doors_opened; hrt_abstime _drop_time; + float _alt_clearance; + struct position_s { double lat; //degrees double lon; //degrees @@ -159,12 +163,16 @@ BottleDrop::BottleDrop() : _command_sub(-1), _wind_estimate_sub(-1), _command {}, - _actuator_pub(-1), - _actuators {}, - _open_door(false), - _drop(false), - _doors_opened(0), - _drop_time(0) + _global_pos {}, + ref {}, + _actuator_pub(-1), + _actuators {}, + _drop_approval(false), + _open_door(false), + _drop(false), + _doors_opened(0), + _drop_time(0), + _alt_clearance(0) { } @@ -310,7 +318,6 @@ BottleDrop::task_main() bool updated = false; - float height; // height at which the normal should be dropped NED float z_0; // ground properties float turn_radius; // turn radius of the UAV float precision; // Expected precision of the UAV @@ -319,12 +326,12 @@ BottleDrop::task_main() float ground_distance = 70.0f; // constant - float g = 9.81f; // constant of gravity [m/s^2] + float g = CONSTANTS_ONE_G; // constant of gravity [m/s^2] float m = 0.5f; // mass of bottle [kg] float rho = 1.2f; // air density [kg/m^3] - float A = (powf(0.063f, 2.0f) / 4.0f * M_PI_F); // Bottle cross section [m^2] - float dt = 0.01f; // step size [s] - float dt2 = 0.05f; // step size 2 [s] + float A = ((0.063f * 0.063f) / 4.0f * M_PI_F); // Bottle cross section [m^2] + float dt_freefall_prediction = 0.01f; // step size of the free fall prediction [s] + float dt_runs = 0.05f; // step size 2 [s] // Has to be estimated by experiment float cd = 0.86f; // Drag coefficient for a cylinder with a d/l ratio of 1/3 [] @@ -352,8 +359,8 @@ BottleDrop::task_main() float x_drop, y_drop; // coordinates of the drop point in reference to the target (projection of NED) float x_t, y_t; // coordinates of the target in reference to the target x_t = 0, y_t = 0 (projection of NED) float x_l, y_l; // local position in projected coordinates - float x_f, y_f; // to-be position of the UAV after dt2 seconds in projected coordinates - double x_f_NED, y_f_NED; // to-be position of the UAV after dt2 seconds in NED + float x_f, y_f; // to-be position of the UAV after dt_runs seconds in projected coordinates + double x_f_NED, y_f_NED; // to-be position of the UAV after dt_runs seconds in NED float distance_open_door; // The distance the UAV travels during its doors open [m] float distance_real = 0; // The distance between the UAVs position and the drop point [m] float future_distance = 0; // The distance between the UAVs to-be position and the drop point [m] @@ -364,7 +371,6 @@ BottleDrop::task_main() unsigned counter = 0; - param_t param_height = param_find("BD_HEIGHT"); param_t param_gproperties = param_find("BD_GPROPERTIES"); param_t param_turn_radius = param_find("BD_TURNRADIUS"); param_t param_precision = param_find("BD_PRECISION"); @@ -372,11 +378,8 @@ BottleDrop::task_main() param_get(param_precision, &precision); param_get(param_turn_radius, &turn_radius); - param_get(param_height, &height); param_get(param_gproperties, &z_0); - struct vehicle_global_position_s globalpos; - memset(&globalpos, 0, sizeof(globalpos)); int vehicle_global_position_sub = orb_subscribe(ORB_ID(vehicle_global_position)); struct parameter_update_s update; @@ -389,6 +392,7 @@ BottleDrop::task_main() flight_vector_s.nav_cmd = NAV_CMD_WAYPOINT; flight_vector_s.acceptance_radius = 50; // TODO: make parameter flight_vector_s.autocontinue = true; + flight_vector_e.nav_cmd = NAV_CMD_WAYPOINT; flight_vector_e.acceptance_radius = 50; // TODO: make parameter flight_vector_e.autocontinue = true; @@ -397,14 +401,11 @@ BottleDrop::task_main() memset(&onboard_mission, 0, sizeof(onboard_mission)); orb_advert_t onboard_mission_pub = -1; - bool position_initialized = false; double latitude; double longitude; struct wind_estimate_s wind; - map_projection_reference_s ref; - /* wakeup source(s) */ struct pollfd fds[1]; @@ -435,16 +436,21 @@ BottleDrop::task_main() orb_check(vehicle_global_position_sub, &updated); if (updated) { /* copy global position */ - orb_copy(ORB_ID(vehicle_global_position), vehicle_global_position_sub, &globalpos); + orb_copy(ORB_ID(vehicle_global_position), vehicle_global_position_sub, &_global_pos); - latitude = (double)globalpos.lat / 1e7; - longitude = (double)globalpos.lon / 1e7; + latitude = (double)_global_pos.lat / 1e7; + longitude = (double)_global_pos.lon / 1e7; } - if (!position_initialized) { + if (_global_pos.timestamp == 0) { continue; } + const unsigned sleeptime_us = 10000; + + hrt_abstime last_run = hrt_absolute_time(); + dt_runs = 1e6f / sleeptime_us; + /* switch to faster updates during the drop */ while (_drop) { @@ -464,10 +470,10 @@ BottleDrop::task_main() if (updated) { /* copy global position */ - orb_copy(ORB_ID(vehicle_global_position), vehicle_global_position_sub, &globalpos); + orb_copy(ORB_ID(vehicle_global_position), vehicle_global_position_sub, &_global_pos); - latitude = (double)globalpos.lat / 1e7; - longitude = (double)globalpos.lon / 1e7; + latitude = (double)_global_pos.lat / 1e7; + longitude = (double)_global_pos.lon / 1e7; } orb_check(parameter_update_sub, &updated); @@ -477,7 +483,6 @@ BottleDrop::task_main() orb_copy(ORB_ID(parameter_update), parameter_update_sub, &update); /* update all parameters */ - param_get(param_height, &height); param_get(param_gproperties, &z_0); param_get(param_turn_radius, &turn_radius); param_get(param_precision, &precision); @@ -486,7 +491,7 @@ BottleDrop::task_main() // Initialization float windspeed_norm = sqrtf(wind.windspeed_north * wind.windspeed_north + wind.windspeed_east * wind.windspeed_east); - float groundspeed_body = sqrtf(globalpos.vel_n * globalpos.vel_n + globalpos.vel_e * globalpos.vel_e); + float groundspeed_body = sqrtf(_global_pos.vel_n * _global_pos.vel_n + _global_pos.vel_e * _global_pos.vel_e); distance_open_door = fabsf(t_door * groundspeed_body); @@ -497,7 +502,7 @@ BottleDrop::task_main() //warnx("Initialization complete\n"); //////////////////////////////////////////////////////////////////// DEBUGGING - if (drop_approval && !state_drop) { + if (_drop_approval && !state_drop) { //warnx("approval given\n"); //////////////////////////////////////////////////////////////////// DEBUGGING // drop here //open_now = true; @@ -510,7 +515,7 @@ BottleDrop::task_main() az = g; // acceleration in z direction[m/s^2] vz = 0; // velocity in z direction [m/s] z = 0; // fallen distance [m] - h_0 = globalpos.alt - _target_position.alt; // height over target at start[m] + h_0 = _global_pos.alt - _target_position.alt; // height over target at start[m] h = h_0; // height over target [m] ax = 0; // acceleration in x direction [m/s^2] vx = groundspeed_body;// XXX project // ground speed in x direction [m/s] @@ -526,14 +531,14 @@ BottleDrop::task_main() // Compute the distance the bottle will travel after it is dropped in body frame coordinates --> x while (h > 0.05f) { // z-direction - vz = vz + az * dt; - z = z + vz * dt; + vz = vz + az * dt_freefall_prediction; + z = z + vz * dt_freefall_prediction; h = h_0 - z; // x-direction vw = windspeed_norm * logf(h / z_0) / logf(ground_distance / z_0); - vx = vx + ax * dt; - x = x + vx * dt; + vx = vx + ax * dt_freefall_prediction; + x = x + vx * dt_freefall_prediction; vrx = vx + vw; //Drag force @@ -547,12 +552,12 @@ BottleDrop::task_main() ax = -Fdx / m; } - // Compute Drop point + // Compute drop vector x = groundspeed_body * t_signal + x; - map_projection_init(&ref, _target_position.lat, _target_position.lon); } - map_projection_project(&ref, _target_position.lat, _target_position.lon, &x_t, &y_t); + x_t = 0.0f; + y_t = 0.0f; float wind_direction_n, wind_direction_e; @@ -568,7 +573,6 @@ BottleDrop::task_main() x_drop = x_t + x * wind_direction_n; y_drop = y_t + x * wind_direction_e; map_projection_reproject(&ref, x_drop, y_drop, &_drop_position.lat, &_drop_position.lon); - _drop_position.alt = height; //warnx("drop point: lat = %.7f , lon = %.7f", drop_position.lat, drop_position.lon); //////////////////////////////////////////////////////////////////// DEBUGGING @@ -576,10 +580,10 @@ BottleDrop::task_main() // Compute flight vector map_projection_reproject(&ref, x_drop + 2 * turn_radius * wind_direction_n, y_drop + 2 * turn_radius * wind_direction_n, &(flight_vector_s.lat), &(flight_vector_s.lon)); - flight_vector_s.altitude = height; + flight_vector_s.altitude = _drop_position.alt + _alt_clearance; map_projection_reproject(&ref, x_drop - turn_radius * wind_direction_e, y_drop - turn_radius * wind_direction_e, &flight_vector_e.lat, &flight_vector_e.lon); - flight_vector_e.altitude = height; + flight_vector_e.altitude = _drop_position.alt + _alt_clearance; //warnx("Flight vector: starting point = %.7f %.7f , end point = %.7f %.7f", flight_vector_s.lat,flight_vector_s.lon,flight_vector_e.lat,flight_vector_e.lon); //////////////////////////////////////////////////////////////////// DEBUGGING // Drop Cancellation if terms are not met @@ -591,8 +595,8 @@ BottleDrop::task_main() distance_real = fabsf(get_distance_to_next_waypoint(latitude, longitude, _drop_position.lat, _drop_position.lon)); map_projection_project(&ref, latitude, longitude, &x_l, &y_l); - x_f = x_l + globalpos.vel_n * dt2; - y_f = y_l + globalpos.vel_e * dt2; + x_f = x_l + _global_pos.vel_n * dt_runs; + y_f = y_l + _global_pos.vel_e * dt_runs; map_projection_reproject(&ref, x_f, y_f, &x_f_NED, &y_f_NED); future_distance = fabsf(get_distance_to_next_waypoint(x_f_NED, y_f_NED, _drop_position.lat, _drop_position.lon)); //warnx("position to-be: = %.7f %.7f" ,x_f_NED, y_f_NED ); //////////////////////////////////////////////////////////////////// DEBUGGING @@ -627,7 +631,7 @@ BottleDrop::task_main() if (counter % 10 == 0) warnx("Distance real: %.2f, distance_open_door: %.2f, angle to wind: %.2f", (double)distance_real, (double)distance_open_door, - (double)(_wrap_pi(globalpos.yaw - atan2f(wind.windspeed_north, wind.windspeed_east)))); + (double)(_wrap_pi(_global_pos.yaw - atan2f(wind.windspeed_north, wind.windspeed_east)))); if (onboard_mission_pub > 0) { orb_publish(ORB_ID(onboard_mission), onboard_mission_pub, &onboard_mission); @@ -638,7 +642,7 @@ BottleDrop::task_main() } - if (isfinite(distance_real) && distance_real < distance_open_door && drop_approval) { + if (isfinite(distance_real) && distance_real < distance_open_door && _drop_approval) { open_bay(); } else { close_bay(); @@ -647,7 +651,7 @@ BottleDrop::task_main() if (isfinite(distance_real) && distance_real < precision && distance_real < future_distance) { // Drop only if the distance between drop point and actual position is getting larger again // XXX this check needs to be carefully validated - right now we prefer to drop if we're close to the goal - // if(fabsf(_wrap_pi(globalpos.yaw-atan2f(wind.windspeed_north,wind_speed.windspeed_east)+M_PI_F)) < 80.0f/180.0f*M_PI_F) // if flight trajectory deviates more than 80 degrees from calculated path, it will no drop + // if(fabsf(_wrap_pi(_global_pos.yaw-atan2f(wind.windspeed_north,wind_speed.windspeed_east)+M_PI_F)) < 80.0f/180.0f*M_PI_F) // if flight trajectory deviates more than 80 degrees from calculated path, it will no drop // { drop(); // } @@ -662,7 +666,10 @@ BottleDrop::task_main() // update_actuators(); // run at roughly 100 Hz - usleep(1000); + usleep(sleeptime_us); + + dt_runs = 1e6f / hrt_elapsed_time(&last_run); + last_run = hrt_absolute_time(); } } @@ -706,37 +713,41 @@ BottleDrop::handle_command(struct vehicle_command_s *cmd) switch ((int)(cmd->param1 + 0.5f)) { case 0: - drop_approval = false; + _drop_approval = false; break; case 1: - drop_approval = true; + _drop_approval = true; break; default: - drop_approval = false; + _drop_approval = false; + warnx("param1 val unknown"); break; } - // XXX check all fields + // XXX check all fields (2-3) + _alt_clearance = cmd->param4; _target_position.lat = cmd->param5; _target_position.lon = cmd->param6; _target_position.alt = cmd->param7; + _target_position.initialized; + map_projection_init(&ref, _target_position.lat, _target_position.lon); answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); break; case VEHICLE_CMD_PAYLOAD_CONTROL_DEPLOY: switch ((int)(cmd->param1 + 0.5f)) { case 0: - drop_approval = false; + _drop_approval = false; break; case 1: - drop_approval = true; + _drop_approval = true; break; default: - drop_approval = false; + _drop_approval = false; break; // XXX handle other values } diff --git a/src/modules/bottle_drop/bottle_drop_params.c b/src/modules/bottle_drop/bottle_drop_params.c index 0f60b58e49..239f25884a 100644 --- a/src/modules/bottle_drop/bottle_drop_params.c +++ b/src/modules/bottle_drop/bottle_drop_params.c @@ -41,8 +41,6 @@ #include #include -PARAM_DEFINE_FLOAT(BD_HEIGHT, 60.0f); PARAM_DEFINE_FLOAT(BD_GPROPERTIES, 0.03f); -PARAM_DEFINE_FLOAT(BD_TURNRADIUS, 70.0f); +PARAM_DEFINE_FLOAT(BD_TURNRADIUS, 90.0f); PARAM_DEFINE_FLOAT(BD_PRECISION, 1.0f); -PARAM_DEFINE_INT32(BD_APPROVAL, 0); From 61d052ede751274969869e53e98b1d8b2004a128 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 15 Aug 2014 23:41:57 +0200 Subject: [PATCH 091/411] Fix param value --- src/modules/bottle_drop/bottle_drop_params.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/bottle_drop/bottle_drop_params.c b/src/modules/bottle_drop/bottle_drop_params.c index 239f25884a..22e9baf8aa 100644 --- a/src/modules/bottle_drop/bottle_drop_params.c +++ b/src/modules/bottle_drop/bottle_drop_params.c @@ -43,4 +43,4 @@ PARAM_DEFINE_FLOAT(BD_GPROPERTIES, 0.03f); PARAM_DEFINE_FLOAT(BD_TURNRADIUS, 90.0f); -PARAM_DEFINE_FLOAT(BD_PRECISION, 1.0f); +PARAM_DEFINE_FLOAT(BD_PRECISION, 10.0f); From 35daef948bb6dac06900d7bc74aa09fe35aceabd Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 16 Aug 2014 10:52:01 +0200 Subject: [PATCH 092/411] fix datalink loss detection logic --- src/modules/commander/commander.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 28aba759fd..8a12e16ca6 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1534,6 +1534,10 @@ int commander_thread_main(int argc, char *argv[]) mavlink_log_critical(mavlink_fd, "data link %i regained", i); telemetry_lost[i] = false; have_link = true; + } else if (!telemetry_lost[i]) { + /* telemetry was healthy also in last iteration + * we don't have to check a timeout */ + have_link = true; } } else { From ee254be845f16531990f4574ca7505ed359c4a61 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 16 Aug 2014 11:16:27 +0200 Subject: [PATCH 093/411] navigator: skip takeoff if already above takeoff altitude --- src/modules/navigator/mission.cpp | 75 ++++++++++++++++++------------- src/modules/navigator/mission.h | 4 +- 2 files changed, 45 insertions(+), 34 deletions(-) diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index c0e37a3eda..98a4340e1c 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -107,6 +107,7 @@ Mission::on_inactive() update_offboard_mission(); } + /* require takeoff after non-loiter or landing */ if (!_navigator->get_can_loiter_at_sp() || _navigator->get_vstatus()->condition_landed) { _need_takeoff = true; } @@ -399,7 +400,7 @@ Mission::set_mission_items() takeoff_alt += _navigator->get_home_position()->alt; } - /* perform takeoff at least to NAV_TAKEOFF_ALT above home/ground, even if first waypoint is lower */ + /* takeoff to at least NAV_TAKEOFF_ALT above home/ground, even if first waypoint is lower */ if (_navigator->get_vstatus()->condition_landed) { takeoff_alt = fmaxf(takeoff_alt, _navigator->get_global_position()->alt + _param_takeoff_alt.get()); @@ -407,45 +408,55 @@ Mission::set_mission_items() takeoff_alt = fmaxf(takeoff_alt, _navigator->get_home_position()->alt + _param_takeoff_alt.get()); } - mavlink_log_critical(_navigator->get_mavlink_fd(), "takeoff to %.1f meters above home", (double)(takeoff_alt - _navigator->get_home_position()->alt)); + /* check if we already above takeoff altitude */ + if (_navigator->get_global_position()->alt < takeoff_alt - _navigator->get_acceptance_radius()) { + mavlink_log_critical(_navigator->get_mavlink_fd(), "takeoff to %.1f meters above home", (double)(takeoff_alt - _navigator->get_home_position()->alt)); - _mission_item.lat = _navigator->get_global_position()->lat; - _mission_item.lon = _navigator->get_global_position()->lon; - _mission_item.altitude = takeoff_alt; - _mission_item.altitude_is_relative = false; + _mission_item.nav_cmd = NAV_CMD_TAKEOFF; + _mission_item.lat = _navigator->get_global_position()->lat; + _mission_item.lon = _navigator->get_global_position()->lon; + _mission_item.altitude = takeoff_alt; + _mission_item.altitude_is_relative = false; - mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current); + mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current); - } else { - /* set current position setpoint from mission item */ - mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current); - - /* require takeoff after landing or idle */ - if (pos_sp_triplet->current.type == SETPOINT_TYPE_LAND || pos_sp_triplet->current.type == SETPOINT_TYPE_IDLE) { - _need_takeoff = true; - } - - _navigator->set_can_loiter_at_sp(false); - reset_mission_item_reached(); - - if (_mission_type == MISSION_TYPE_OFFBOARD) { - report_current_offboard_mission_item(); - } - // TODO: report onboard mission item somehow - - /* try to read next mission item */ - struct mission_item_s mission_item_next; - - if (read_mission_item(_mission_type == MISSION_TYPE_ONBOARD, false, &mission_item_next)) { - /* got next mission item, update setpoint triplet */ - mission_item_to_position_setpoint(&mission_item_next, &pos_sp_triplet->next); + _navigator->set_position_setpoint_triplet_updated(); + return; } else { - /* next mission item is not available */ - pos_sp_triplet->next.valid = false; + /* skip takeoff */ + _takeoff = false; } } + /* set current position setpoint from mission item */ + mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current); + + /* require takeoff after landing or idle */ + if (pos_sp_triplet->current.type == SETPOINT_TYPE_LAND || pos_sp_triplet->current.type == SETPOINT_TYPE_IDLE) { + _need_takeoff = true; + } + + _navigator->set_can_loiter_at_sp(false); + reset_mission_item_reached(); + + if (_mission_type == MISSION_TYPE_OFFBOARD) { + report_current_offboard_mission_item(); + } + // TODO: report onboard mission item somehow + + /* try to read next mission item */ + struct mission_item_s mission_item_next; + + if (read_mission_item(_mission_type == MISSION_TYPE_ONBOARD, false, &mission_item_next)) { + /* got next mission item, update setpoint triplet */ + mission_item_to_position_setpoint(&mission_item_next, &pos_sp_triplet->next); + + } else { + /* next mission item is not available */ + pos_sp_triplet->next.valid = false; + } + _navigator->set_position_setpoint_triplet_updated(); } diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h index 4da6a11553..1edaa5c8ac 100644 --- a/src/modules/navigator/mission.h +++ b/src/modules/navigator/mission.h @@ -142,8 +142,8 @@ private: int _current_onboard_mission_index; int _current_offboard_mission_index; - bool _need_takeoff; - bool _takeoff; + bool _need_takeoff; /**< if true, then takeoff must be performed before going to the first waypoint (if needed) */ + bool _takeoff; /**< takeoff state flag */ orb_advert_t _mission_result_pub; struct mission_result_s _mission_result; From a972a2857197706ed414b674dcaf291d48ac0f04 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 16 Aug 2014 11:41:31 +0200 Subject: [PATCH 094/411] fix param name --- src/modules/commander/commander_params.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index 3d1e231c6a..30159dad9c 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -104,7 +104,7 @@ PARAM_DEFINE_FLOAT(BAT_CAPACITY, -1.0f); * @min 0 * @max 1 */ -PARAM_DEFINE_INT32(DL_LOSS_EN, 0); +PARAM_DEFINE_INT32(COM_DL_LOSS_EN, 0); /** Datalink loss time threshold * From 72beef90c9a9bb901355483e275a47f166f654a8 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 16 Aug 2014 13:09:10 +0200 Subject: [PATCH 095/411] set correct nav state for data link loss --- 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 157e35ef8d..f82eec6956 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -510,7 +510,7 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en status->failsafe = true; if (status->condition_global_position_valid && status->condition_home_position_valid) { - status->nav_state = NAVIGATION_STATE_AUTO_RTL; + status->nav_state = NAVIGATION_STATE_AUTO_RTGS; } else if (status->condition_local_position_valid) { status->nav_state = NAVIGATION_STATE_LAND; } else if (status->condition_local_altitude_valid) { From 94765f1fe01a3ae1ba72a330f77cfb4cacbd8c0e Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 16 Aug 2014 13:17:26 +0200 Subject: [PATCH 096/411] datalinkloss: use vstatus from navigator For some reason the own subscription did not work (the task launch pattern used for the navigator may be the reason again) --- src/modules/navigator/datalinkloss.cpp | 14 +++++--------- src/modules/navigator/datalinkloss.h | 3 --- 2 files changed, 5 insertions(+), 12 deletions(-) diff --git a/src/modules/navigator/datalinkloss.cpp b/src/modules/navigator/datalinkloss.cpp index 7b9a7b151c..ab9e67a336 100644 --- a/src/modules/navigator/datalinkloss.cpp +++ b/src/modules/navigator/datalinkloss.cpp @@ -57,7 +57,6 @@ DataLinkLoss::DataLinkLoss(Navigator *navigator, const char *name) : MissionBlock(navigator, name), - _vehicleStatus(&getSubscriptions(), ORB_ID(vehicle_status), 100), _param_commsholdwaittime(this, "CH_T"), _param_commsholdlat(this, "CH_LAT"), _param_commsholdlon(this, "CH_LON"), @@ -91,6 +90,7 @@ void DataLinkLoss::on_activation() { _dll_state = DLL_STATE_NONE; + updateParams(); advance_dll(); set_dll_item(); } @@ -99,6 +99,7 @@ void DataLinkLoss::on_active() { if (is_mission_item_reached()) { + updateParams(); advance_dll(); set_dll_item(); } @@ -109,9 +110,6 @@ DataLinkLoss::set_dll_item() { struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); - /* make sure we have the latest params */ - updateParams(); - set_previous_pos_setpoint(); _navigator->set_can_loiter_at_sp(false); @@ -167,17 +165,15 @@ DataLinkLoss::set_dll_item() void DataLinkLoss::advance_dll() { - warnx("dll_state %u", _dll_state); switch (_dll_state) { case DLL_STATE_NONE: /* Check the number of data link losses. If above home fly home directly */ - updateSubscriptions(); - if (_vehicleStatus.data_link_lost_counter > _param_numberdatalinklosses.get()) { - warnx("too many data link losses, fly to airfield home"); + if (_navigator->get_vstatus()->data_link_lost_counter > _param_numberdatalinklosses.get()) { + warnx("%d data link losses, limit is %d, fly to airfield home", _navigator->get_vstatus()->data_link_lost_counter, _param_numberdatalinklosses.get()); mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: too many DL losses, fly to home"); _dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP; } else { - warnx("fly to comms hold"); + warnx("fly to comms hold, datalink loss counter: %d", _navigator->get_vstatus()->data_link_lost_counter); mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: fly to comms hold"); _dll_state = DLL_STATE_FLYTOCOMMSHOLDWP; } diff --git a/src/modules/navigator/datalinkloss.h b/src/modules/navigator/datalinkloss.h index 5a46b5700e..96b4ce010d 100644 --- a/src/modules/navigator/datalinkloss.h +++ b/src/modules/navigator/datalinkloss.h @@ -64,9 +64,6 @@ public: virtual void on_active(); private: - /* Subscriptions */ - uORB::Subscription _vehicleStatus; - /* Params */ control::BlockParamFloat _param_commsholdwaittime; control::BlockParamInt _param_commsholdlat; // * 1e7 From 14189816f7e751b14725939fd5a200c39818f5df Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 16 Aug 2014 14:28:39 +0200 Subject: [PATCH 097/411] dlloss: better output --- src/modules/navigator/datalinkloss.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/navigator/datalinkloss.cpp b/src/modules/navigator/datalinkloss.cpp index ab9e67a336..b914fdb347 100644 --- a/src/modules/navigator/datalinkloss.cpp +++ b/src/modules/navigator/datalinkloss.cpp @@ -180,7 +180,7 @@ DataLinkLoss::advance_dll() break; case DLL_STATE_FLYTOCOMMSHOLDWP: warnx("fly to airfield home"); - mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: fly to home"); + mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: fly to airfield home"); _dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP; _navigator->get_mission_result()->stay_in_failsafe = true; _navigator->publish_mission_result(); From f480c10282efcb56b643b2c676303f1f57498fda Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 16 Aug 2014 14:37:46 +0200 Subject: [PATCH 098/411] state machine helper: use stay in failsafe flag --- src/modules/commander/state_machine_helper.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index f82eec6956..3f4bfaa1ca 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -534,11 +534,11 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en } /* don't bother if RC is lost and mission is not yet finished */ - } else if (status->rc_signal_lost) { + } else if (status->rc_signal_lost && !stay_in_failsafe) { /* this mode is ok, we don't need RC for missions */ status->nav_state = NAVIGATION_STATE_AUTO_MISSION; - } else { + } else if (!stay_in_failsafe){ /* everything is perfect */ status->nav_state = NAVIGATION_STATE_AUTO_MISSION; } From 26b2f73c6fce19982136e12a73faf96eee4895c6 Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Sat, 16 Aug 2014 14:42:18 +0200 Subject: [PATCH 099/411] Call the appropriate functions directly. --- src/modules/bottle_drop/bottle_drop.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/modules/bottle_drop/bottle_drop.cpp b/src/modules/bottle_drop/bottle_drop.cpp index 4367d13432..1c5c0f57f2 100644 --- a/src/modules/bottle_drop/bottle_drop.cpp +++ b/src/modules/bottle_drop/bottle_drop.cpp @@ -684,7 +684,6 @@ BottleDrop::handle_command(struct vehicle_command_s *cmd) { switch (cmd->command) { case VEHICLE_CMD_CUSTOM_0: - /* * param1 and param2 set to 1: open and drop * param1 set to 1: open @@ -693,16 +692,19 @@ BottleDrop::handle_command(struct vehicle_command_s *cmd) if (cmd->param1 > 0.5f && cmd->param2 > 0.5f) { _open_door = true; _drop = true; + drop(); mavlink_log_info(_mavlink_fd, "#audio: drop bottle"); } else if (cmd->param1 > 0.5f) { _open_door = true; _drop = false; + open_bay(); mavlink_log_info(_mavlink_fd, "#audio: open doors"); } else { _open_door = false; _drop = false; + close_bay(); mavlink_log_info(_mavlink_fd, "#audio: close doors"); } From 584394d2bc1d88d291349cce2beee161c2a8448e Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Sat, 16 Aug 2014 17:53:23 +0200 Subject: [PATCH 100/411] Silenced commander --- src/modules/commander/commander.cpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 6c2c030702..db9ec90056 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -615,6 +615,11 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s case VEHICLE_CMD_PREFLIGHT_CALIBRATION: case VEHICLE_CMD_PREFLIGHT_SET_SENSOR_OFFSETS: case VEHICLE_CMD_PREFLIGHT_STORAGE: + case VEHICLE_CMD_CUSTOM_0: + case VEHICLE_CMD_CUSTOM_1: + case VEHICLE_CMD_CUSTOM_2: + case VEHICLE_CMD_PAYLOAD_PREPARE_DEPLOY: + case VEHICLE_CMD_PAYLOAD_CONTROL_DEPLOY: /* ignore commands that handled in low prio loop */ break; From 8a066a85faa7dd15265f705eba78a9dfe53f9f1d Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Sat, 16 Aug 2014 17:53:51 +0200 Subject: [PATCH 101/411] Fixed bottle drop to operate in GPS outdoor test, pending test flight --- src/modules/bottle_drop/bottle_drop.cpp | 400 +++++++++++++----------- 1 file changed, 216 insertions(+), 184 deletions(-) diff --git a/src/modules/bottle_drop/bottle_drop.cpp b/src/modules/bottle_drop/bottle_drop.cpp index 1c5c0f57f2..ddc41372ec 100644 --- a/src/modules/bottle_drop/bottle_drop.cpp +++ b/src/modules/bottle_drop/bottle_drop.cpp @@ -103,6 +103,7 @@ public: void open_bay(); void close_bay(); void drop(); + void lock_release(); private: bool _task_should_exit; /**< if true, task should exit */ @@ -119,8 +120,6 @@ private: struct actuator_controls_s _actuators; bool _drop_approval; - bool _open_door; - bool _drop; hrt_abstime _doors_opened; hrt_abstime _drop_time; @@ -133,6 +132,14 @@ private: bool initialized; } _target_position, _drop_position; + enum DROP_STATE { + DROP_STATE_INIT = 0, + DROP_STATE_TARGET_VALID, + DROP_STATE_BAY_OPEN, + DROP_STATE_DROPPED, + DROP_STATE_BAY_CLOSED + } _drop_state; + void task_main(); void handle_command(struct vehicle_command_s *cmd); @@ -168,11 +175,12 @@ BottleDrop::BottleDrop() : _actuator_pub(-1), _actuators {}, _drop_approval(false), - _open_door(false), - _drop(false), _doors_opened(0), _drop_time(0), - _alt_clearance(0) + _alt_clearance(0), + _target_position {}, + _drop_position {}, + _drop_state(DROP_STATE_INIT) { } @@ -226,8 +234,7 @@ BottleDrop::start() void BottleDrop::status() { - warnx("Doors: %s", _open_door ? "OPEN" : "CLOSED"); - warnx("Dropping: %s", _drop ? "YES" : "NO"); + warnx("drop state: %d", _drop_state); } void @@ -242,6 +249,8 @@ BottleDrop::open_bay() warnx("open doors"); actuators_publish(); + + usleep(500 * 1000); } void @@ -254,6 +263,9 @@ BottleDrop::close_bay() _doors_opened = 0; actuators_publish(); + + // delay until the bay is closed + usleep(500 * 1000); } void @@ -269,22 +281,29 @@ BottleDrop::drop() warnx("bay not ready, forced open"); } - while (hrt_elapsed_time(&_doors_opened) < 400000 && hrt_elapsed_time(&starttime) < 2000000) { + while (hrt_elapsed_time(&_doors_opened) < 500 * 1000 && hrt_elapsed_time(&starttime) < 2000000) { usleep(50000); warnx("delayed by door!"); } - if (_drop && _doors_opened != 0 && hrt_elapsed_time(&_doors_opened) < 500000) { - _actuators.control[2] = -1.0f; - } else { - _actuators.control[2] = 1.0f; - } + _actuators.control[2] = 1.0f; _drop_time = hrt_absolute_time(); + actuators_publish(); warnx("dropping now"); + // Give it time to drop + usleep(1000 * 1000); +} + +void +BottleDrop::lock_release() +{ + _actuators.control[2] = -1.0f; actuators_publish(); + + warnx("closing release"); } int @@ -401,9 +420,6 @@ BottleDrop::task_main() memset(&onboard_mission, 0, sizeof(onboard_mission)); orb_advert_t onboard_mission_pub = -1; - double latitude; - double longitude; - struct wind_estimate_s wind; /* wakeup source(s) */ @@ -414,6 +430,7 @@ BottleDrop::task_main() fds[0].events = POLLIN; // Whatever state the bay is in, we want it closed on startup + lock_release(); close_bay(); while (!_task_should_exit) { @@ -437,9 +454,6 @@ BottleDrop::task_main() if (updated) { /* copy global position */ orb_copy(ORB_ID(vehicle_global_position), vehicle_global_position_sub, &_global_pos); - - latitude = (double)_global_pos.lat / 1e7; - longitude = (double)_global_pos.lon / 1e7; } if (_global_pos.timestamp == 0) { @@ -452,32 +466,23 @@ BottleDrop::task_main() dt_runs = 1e6f / sleeptime_us; /* switch to faster updates during the drop */ - while (_drop) { - - /* 20s after drop, reset and close everything again */ - if (_drop && _doors_opened != 0 && hrt_elapsed_time(&_doors_opened) > 20000000) { - _open_door = false; - _drop = false; - } + while (_drop_state > DROP_STATE_INIT) { + // Get wind estimate orb_check(_wind_estimate_sub, &updated); - if (updated) { orb_copy(ORB_ID(wind_estimate), _wind_estimate_sub, &wind); } + // Get vehicle position orb_check(vehicle_global_position_sub, &updated); - if (updated) { /* copy global position */ orb_copy(ORB_ID(vehicle_global_position), vehicle_global_position_sub, &_global_pos); - - latitude = (double)_global_pos.lat / 1e7; - longitude = (double)_global_pos.lon / 1e7; } + // Get parameter updates orb_check(parameter_update_sub, &updated); - if (updated) { /* copy global position */ orb_copy(ORB_ID(parameter_update), parameter_update_sub, &update); @@ -488,177 +493,201 @@ BottleDrop::task_main() param_get(param_precision, &precision); } - // Initialization float windspeed_norm = sqrtf(wind.windspeed_north * wind.windspeed_north + wind.windspeed_east * wind.windspeed_east); float groundspeed_body = sqrtf(_global_pos.vel_n * _global_pos.vel_n + _global_pos.vel_e * _global_pos.vel_e); - - distance_open_door = fabsf(t_door * groundspeed_body); - + distance_real = fabsf(get_distance_to_next_waypoint(_global_pos.lat, _global_pos.lon, _drop_position.lat, _drop_position.lon)); //warnx("absolut wind speed = %.4f", vr); //////////////////////////////////////////////////////////////////// DEBUGGING //warnx("Initialization complete\n"); //////////////////////////////////////////////////////////////////// DEBUGGING + switch (_drop_state) { - if (_drop_approval && !state_drop) { - //warnx("approval given\n"); //////////////////////////////////////////////////////////////////// DEBUGGING - // drop here - //open_now = true; - //drop = false; - //drop_start = hrt_absolute_time(); + case DROP_STATE_INIT: + // exit inner loop, wait for new drop mission + break; - // Update drop point at 10 Hz - if (counter % 10 == 0) { + case DROP_STATE_TARGET_VALID: + { + //warnx("approval given\n"); //////////////////////////////////////////////////////////////////// DEBUGGING + // drop here + //open_now = true; + //drop = false; + //drop_start = hrt_absolute_time(); - az = g; // acceleration in z direction[m/s^2] - vz = 0; // velocity in z direction [m/s] - z = 0; // fallen distance [m] - h_0 = _global_pos.alt - _target_position.alt; // height over target at start[m] - h = h_0; // height over target [m] - ax = 0; // acceleration in x direction [m/s^2] - vx = groundspeed_body;// XXX project // ground speed in x direction [m/s] - x = 0; // traveled distance in x direction [m] - vw = 0; // wind speed [m/s] - vrx = 0; // relative velocity in x direction [m/s] - v = groundspeed_body; // relative speed vector [m/s] - Fd = 0; // Drag force [N] - Fdx = 0; // Drag force in x direction [N] - Fdz = 0; // Drag force in z direction [N] + // Update drop point at 10 Hz + if (counter % 10 == 0) { + + az = g; // acceleration in z direction[m/s^2] + vz = 0; // velocity in z direction [m/s] + z = 0; // fallen distance [m] + h_0 = _global_pos.alt - _target_position.alt; // height over target at start[m] + h = h_0; // height over target [m] + ax = 0; // acceleration in x direction [m/s^2] + vx = groundspeed_body;// XXX project // ground speed in x direction [m/s] + x = 0; // traveled distance in x direction [m] + vw = 0; // wind speed [m/s] + vrx = 0; // relative velocity in x direction [m/s] + v = groundspeed_body; // relative speed vector [m/s] + Fd = 0; // Drag force [N] + Fdx = 0; // Drag force in x direction [N] + Fdz = 0; // Drag force in z direction [N] - // Compute the distance the bottle will travel after it is dropped in body frame coordinates --> x - while (h > 0.05f) { - // z-direction - vz = vz + az * dt_freefall_prediction; - z = z + vz * dt_freefall_prediction; - h = h_0 - z; + // Compute the distance the bottle will travel after it is dropped in body frame coordinates --> x + while (h > 0.05f) { + // z-direction + vz = vz + az * dt_freefall_prediction; + z = z + vz * dt_freefall_prediction; + h = h_0 - z; - // x-direction - vw = windspeed_norm * logf(h / z_0) / logf(ground_distance / z_0); - vx = vx + ax * dt_freefall_prediction; - x = x + vx * dt_freefall_prediction; - vrx = vx + vw; + // x-direction + vw = windspeed_norm * logf(h / z_0) / logf(ground_distance / z_0); + vx = vx + ax * dt_freefall_prediction; + x = x + vx * dt_freefall_prediction; + vrx = vx + vw; - //Drag force - v = sqrtf(vz * vz + vrx * vrx); - Fd = 0.5f * rho * A * cd * powf(v, 2.0f); - Fdx = Fd * vrx / v; - Fdz = Fd * vz / v; + //Drag force + v = sqrtf(vz * vz + vrx * vrx); + Fd = 0.5f * rho * A * cd * powf(v, 2.0f); + Fdx = Fd * vrx / v; + Fdz = Fd * vz / v; - //acceleration - az = g - Fdz / m; - ax = -Fdx / m; + //acceleration + az = g - Fdz / m; + ax = -Fdx / m; + } + + // Compute drop vector + x = groundspeed_body * t_signal + x; } - // Compute drop vector - x = groundspeed_body * t_signal + x; + x_t = 0.0f; + y_t = 0.0f; + + float wind_direction_n, wind_direction_e; + + if (windspeed_norm < 0.5f) { // If there is no wind, an arbitrarily direction is chosen + wind_direction_n = 1.0f; + wind_direction_e = 0.0f; + + } else { + wind_direction_n = wind.windspeed_north / windspeed_norm; + wind_direction_e = wind.windspeed_east / windspeed_norm; + } + + x_drop = x_t + x * wind_direction_n; + y_drop = y_t + x * wind_direction_e; + map_projection_reproject(&ref, x_drop, y_drop, &_drop_position.lat, &_drop_position.lon); + //warnx("drop point: lat = %.7f , lon = %.7f", drop_position.lat, drop_position.lon); //////////////////////////////////////////////////////////////////// DEBUGGING + + + + // Compute flight vector + map_projection_reproject(&ref, x_drop + 2 * turn_radius * wind_direction_n, y_drop + 2 * turn_radius * wind_direction_n, + &(flight_vector_s.lat), &(flight_vector_s.lon)); + flight_vector_s.altitude = _drop_position.alt + _alt_clearance; + map_projection_reproject(&ref, x_drop - turn_radius * wind_direction_e, y_drop - turn_radius * wind_direction_e, + &flight_vector_e.lat, &flight_vector_e.lon); + flight_vector_e.altitude = _drop_position.alt + _alt_clearance; + //warnx("Flight vector: starting point = %.7f %.7f , end point = %.7f %.7f", flight_vector_s.lat,flight_vector_s.lon,flight_vector_e.lat,flight_vector_e.lon); //////////////////////////////////////////////////////////////////// DEBUGGING + + // Drop Cancellation if terms are not met + + // warnx("latitude:%.2f", _global_pos.lat); + // warnx("longitude:%.2f", _global_pos.lon); + // warnx("drop_position.lat:%.2f", drop_position.lat); + // warnx("drop_position.lon:%.2f", drop_position.lon); + + //warnx("position to-be: = %.7f %.7f" ,x_f_NED, y_f_NED ); //////////////////////////////////////////////////////////////////// DEBUGGING + + // if (counter % 10 ==0) { + // warnx("x: %.4f", x); + // warnx("drop_position.lat: %.4f, drop_position.lon: %.4f", drop_position.lat, drop_position.lon); + // warnx("latitude %.4f, longitude: %.4f", _global_pos.lat, _global_pos.lon); + // warnx("future_distance: %.2f, precision: %.2f", future_distance, precision); + // } + + /* Save WPs in datamanager */ + const ssize_t len = sizeof(struct mission_item_s); + + if (dm_write(DM_KEY_WAYPOINTS_ONBOARD, 0, DM_PERSIST_IN_FLIGHT_RESET, &flight_vector_s, len) != len) { + warnx("ERROR: could not save onboard WP"); + } + + if (dm_write(DM_KEY_WAYPOINTS_ONBOARD, 1, DM_PERSIST_IN_FLIGHT_RESET, &flight_vector_e, len) != len) { + warnx("ERROR: could not save onboard WP"); + } + + onboard_mission.count = 2; + + if (state_run && !state_drop) { + onboard_mission.current_seq = 0; + + } else { + onboard_mission.current_seq = -1; + } + + if (onboard_mission_pub > 0) { + orb_publish(ORB_ID(onboard_mission), onboard_mission_pub, &onboard_mission); + + } else { + onboard_mission_pub = orb_advertise(ORB_ID(onboard_mission), &onboard_mission); + } + + // We're close enough - open the bay + distance_open_door = math::max(3.0f, fabsf(t_door * groundspeed_body)); + + if (counter % 10 == 0) + warnx("dist real: %.2f, distance_open_door: %.2f, angle to wind: %.2f", + (double)distance_real, + (double)distance_open_door, + (double)(_wrap_pi(_global_pos.yaw - atan2f(wind.windspeed_north, wind.windspeed_east)))); + + if (isfinite(distance_real) && distance_real < distance_open_door && _drop_approval) { + open_bay(); + _drop_state = DROP_STATE_BAY_OPEN; + } } + break; - x_t = 0.0f; - y_t = 0.0f; + case DROP_STATE_BAY_OPEN: + { + map_projection_project(&ref, _global_pos.lat, _global_pos.lon, &x_l, &y_l); + x_f = x_l + _global_pos.vel_n * dt_runs; + y_f = y_l + _global_pos.vel_e * dt_runs; + map_projection_reproject(&ref, x_f, y_f, &x_f_NED, &y_f_NED); + future_distance = fabsf(get_distance_to_next_waypoint(x_f_NED, y_f_NED, _drop_position.lat, _drop_position.lon)); - float wind_direction_n, wind_direction_e; + warnx("Distance real: %.2f", (double)distance_real); - if (windspeed_norm < 0.5f) { // If there is no wind, an arbitrarily direction is chosen - wind_direction_n = 1.0f; - wind_direction_e = 0.0f; + if (isfinite(distance_real) && (((distance_real < precision) && (distance_real < future_distance)) || + /* we drop as well if we're really close */ (distance_real < precision / 10.0f))) { // Drop only if the distance between drop point and actual position is getting larger again + // XXX this check needs to be carefully validated - right now we prefer to drop if we're close to the goal - } else { - wind_direction_n = wind.windspeed_north / windspeed_norm; - wind_direction_e = wind.windspeed_east / windspeed_norm; - } + // if(fabsf(_wrap_pi(_global_pos.yaw-atan2f(wind.windspeed_north,wind_speed.windspeed_east)+M_PI_F)) < 80.0f/180.0f*M_PI_F) // if flight trajectory deviates more than 80 degrees from calculated path, it will no drop + // { + drop(); + _drop_state = DROP_STATE_DROPPED; + // } + // else + // { + // state_run = true; + // } + } + } + break; - x_drop = x_t + x * wind_direction_n; - y_drop = y_t + x * wind_direction_e; - map_projection_reproject(&ref, x_drop, y_drop, &_drop_position.lat, &_drop_position.lon); - //warnx("drop point: lat = %.7f , lon = %.7f", drop_position.lat, drop_position.lon); //////////////////////////////////////////////////////////////////// DEBUGGING - - - - // Compute flight vector - map_projection_reproject(&ref, x_drop + 2 * turn_radius * wind_direction_n, y_drop + 2 * turn_radius * wind_direction_n, - &(flight_vector_s.lat), &(flight_vector_s.lon)); - flight_vector_s.altitude = _drop_position.alt + _alt_clearance; - map_projection_reproject(&ref, x_drop - turn_radius * wind_direction_e, y_drop - turn_radius * wind_direction_e, - &flight_vector_e.lat, &flight_vector_e.lon); - flight_vector_e.altitude = _drop_position.alt + _alt_clearance; - //warnx("Flight vector: starting point = %.7f %.7f , end point = %.7f %.7f", flight_vector_s.lat,flight_vector_s.lon,flight_vector_e.lat,flight_vector_e.lon); //////////////////////////////////////////////////////////////////// DEBUGGING - - // Drop Cancellation if terms are not met - - // warnx("latitude:%.2f", latitude); - // warnx("longitude:%.2f", longitude); - // warnx("drop_position.lat:%.2f", drop_position.lat); - // warnx("drop_position.lon:%.2f", drop_position.lon); - - distance_real = fabsf(get_distance_to_next_waypoint(latitude, longitude, _drop_position.lat, _drop_position.lon)); - map_projection_project(&ref, latitude, longitude, &x_l, &y_l); - x_f = x_l + _global_pos.vel_n * dt_runs; - y_f = y_l + _global_pos.vel_e * dt_runs; - map_projection_reproject(&ref, x_f, y_f, &x_f_NED, &y_f_NED); - future_distance = fabsf(get_distance_to_next_waypoint(x_f_NED, y_f_NED, _drop_position.lat, _drop_position.lon)); - //warnx("position to-be: = %.7f %.7f" ,x_f_NED, y_f_NED ); //////////////////////////////////////////////////////////////////// DEBUGGING - - // if (counter % 10 ==0) { - // warnx("x: %.4f", x); - // warnx("drop_position.lat: %.4f, drop_position.lon: %.4f", drop_position.lat, drop_position.lon); - // warnx("latitude %.4f, longitude: %.4f", latitude, longitude); - // warnx("future_distance: %.2f, precision: %.2f", future_distance, precision); - // } - - /* Save WPs in datamanager */ - const ssize_t len = sizeof(struct mission_item_s); - - if (dm_write(DM_KEY_WAYPOINTS_ONBOARD, 0, DM_PERSIST_IN_FLIGHT_RESET, &flight_vector_s, len) != len) { - warnx("ERROR: could not save onboard WP"); - } - - if (dm_write(DM_KEY_WAYPOINTS_ONBOARD, 1, DM_PERSIST_IN_FLIGHT_RESET, &flight_vector_e, len) != len) { - warnx("ERROR: could not save onboard WP"); - } - - onboard_mission.count = 2; - - if (state_run && !state_drop) { - onboard_mission.current_seq = 0; - - } else { - onboard_mission.current_seq = -1; - } - - if (counter % 10 == 0) - warnx("Distance real: %.2f, distance_open_door: %.2f, angle to wind: %.2f", (double)distance_real, - (double)distance_open_door, - (double)(_wrap_pi(_global_pos.yaw - atan2f(wind.windspeed_north, wind.windspeed_east)))); - - if (onboard_mission_pub > 0) { - orb_publish(ORB_ID(onboard_mission), onboard_mission_pub, &onboard_mission); - - } else { - onboard_mission_pub = orb_advertise(ORB_ID(onboard_mission), &onboard_mission); - } - - } - - if (isfinite(distance_real) && distance_real < distance_open_door && _drop_approval) { - open_bay(); - } else { - close_bay(); - } - - if (isfinite(distance_real) && distance_real < precision && distance_real < future_distance) { // Drop only if the distance between drop point and actual position is getting larger again - // XXX this check needs to be carefully validated - right now we prefer to drop if we're close to the goal - - // if(fabsf(_wrap_pi(_global_pos.yaw-atan2f(wind.windspeed_north,wind_speed.windspeed_east)+M_PI_F)) < 80.0f/180.0f*M_PI_F) // if flight trajectory deviates more than 80 degrees from calculated path, it will no drop - // { - drop(); - // } - // else - // { - // state_run = true; - // } + case DROP_STATE_DROPPED: + /* 2s after drop, reset and close everything again */ + if ((hrt_elapsed_time(&_doors_opened) > 2 * 1000 * 1000)) { + _drop_state = DROP_STATE_INIT; + lock_release(); + close_bay(); + } + break; } counter++; @@ -690,20 +719,15 @@ BottleDrop::handle_command(struct vehicle_command_s *cmd) * else: close (and don't drop) */ if (cmd->param1 > 0.5f && cmd->param2 > 0.5f) { - _open_door = true; - _drop = true; + open_bay(); drop(); mavlink_log_info(_mavlink_fd, "#audio: drop bottle"); } else if (cmd->param1 > 0.5f) { - _open_door = true; - _drop = false; open_bay(); mavlink_log_info(_mavlink_fd, "#audio: open doors"); } else { - _open_door = false; - _drop = false; close_bay(); mavlink_log_info(_mavlink_fd, "#audio: close doors"); } @@ -720,6 +744,7 @@ BottleDrop::handle_command(struct vehicle_command_s *cmd) case 1: _drop_approval = true; + mavlink_log_info(_mavlink_fd, "#audio: prepare deploy approval"); break; default: @@ -734,6 +759,9 @@ BottleDrop::handle_command(struct vehicle_command_s *cmd) _target_position.lon = cmd->param6; _target_position.alt = cmd->param7; _target_position.initialized; + _drop_state = DROP_STATE_TARGET_VALID; + warnx("got target: %8.4f, %8.4f, %8.4f", (double)_target_position.lat, + (double)_target_position.lon, (double)_target_position.alt); map_projection_init(&ref, _target_position.lat, _target_position.lon); answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); break; @@ -746,6 +774,7 @@ BottleDrop::handle_command(struct vehicle_command_s *cmd) case 1: _drop_approval = true; + mavlink_log_info(_mavlink_fd, "#audio: control deploy approval"); break; default: @@ -848,6 +877,9 @@ int bottle_drop_main(int argc, char *argv[]) } else if (!strcmp(argv[1], "close")) { bottle_drop::g_bottle_drop->close_bay(); + } else if (!strcmp(argv[1], "lock")) { + bottle_drop::g_bottle_drop->lock_release(); + } else { usage(); } From ab9b234893448237d84b4e1f95b807be3a68e98f Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 16 Aug 2014 18:07:21 +0200 Subject: [PATCH 102/411] commander: mavlink output on flight termination --- src/modules/commander/commander.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 8a12e16ca6..2ab40a5eb0 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1313,9 +1313,10 @@ int commander_thread_main(int argc, char *argv[]) /* Check for geofence violation */ if (pos_sp_triplet.geofence_violated) { //XXX: make this configurable to select different actions (e.g. navigation modes) - /* this will only trigger if geofence is activated via param and a geofence file is present */ + /* this will only trigger if geofence is activated via param and a geofence file is present, also there is a circuit breaker to disable the actual flight termination in the px4io driver */ armed.force_failsafe = true; status_changed = true; + mavlink_log_emergency(mavlink_fd, "Geofence violated: terminating"); } // no reset is done here on purpose, on geofence violation we want to stay in flighttermination } From fc0bdfb6f5021f5cc2d155ea3cee9f5d5102cdef Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 16 Aug 2014 18:42:12 +0200 Subject: [PATCH 103/411] mc_pos_control: trajectory following, using previous and next waypoints --- .../mc_pos_control/mc_pos_control_main.cpp | 184 +++++++++++++++--- 1 file changed, 156 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 ecc40428de..488ca924a6 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -76,6 +76,7 @@ #define TILT_COS_MAX 0.7f #define SIGMA 0.000001f +#define MIN_DIST 0.01f /** * Multicopter position control app start / stop handling function @@ -229,6 +230,14 @@ private: */ void control_offboard(float dt); + bool cross_sphere_line(const math::Vector<3>& sphere_c, float sphere_r, + const math::Vector<3> line_a, const math::Vector<3> line_b, math::Vector<3>& res); + + /** + * Set position setpoint for AUTO + */ + void control_auto(); + /** * Select between barometric and global (AMSL) altitudes */ @@ -647,6 +656,152 @@ MulticopterPositionControl::control_offboard(float dt) } } +bool +MulticopterPositionControl::cross_sphere_line(const math::Vector<3>& sphere_c, float sphere_r, + const math::Vector<3> line_a, const math::Vector<3> line_b, math::Vector<3>& res) +{ + /* project center of sphere on line */ + /* normalized AB */ + math::Vector<3> ab_norm = line_b - line_a; + ab_norm.normalize(); + math::Vector<3> d = line_a + ab_norm * ((sphere_c - line_a) * ab_norm); + float cd_len = (sphere_c - d).length(); + + /* we have triangle CDX with known CD and CX = R, find DX */ + if (sphere_r > cd_len) { + /* have two roots, select one in A->B direction from D */ + float dx_len = sqrtf(sphere_r * sphere_r - cd_len * cd_len); + res = d + ab_norm * dx_len; + return true; + + } else { + /* have no roots, return D */ + res = d; + return false; + } +} + +void +MulticopterPositionControl::control_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) { + /* in case of interrupted mission don't go to waypoint but stay at current position */ + _reset_pos_sp = true; + _reset_alt_sp = true; + + /* project setpoint to local frame */ + math::Vector<3> curr_sp; + map_projection_project(&_ref_pos, + _pos_sp_triplet.current.lat, _pos_sp_triplet.current.lon, + &curr_sp.data[0], &curr_sp.data[1]); + curr_sp(2) = -(_pos_sp_triplet.current.alt - _ref_alt); + + /* by default use current setpoint as is */ + _pos_sp = curr_sp; + + if (_pos_sp_triplet.current.type == SETPOINT_TYPE_POSITION && _pos_sp_triplet.previous.valid) { + /* follow "previous - current" line */ + math::Vector<3> prev_sp; + map_projection_project(&_ref_pos, + _pos_sp_triplet.previous.lat, _pos_sp_triplet.previous.lon, + &prev_sp.data[0], &prev_sp.data[1]); + prev_sp(2) = -(_pos_sp_triplet.previous.alt - _ref_alt); + + if ((curr_sp - prev_sp).length() > MIN_DIST) { + /* scaled space: 1 == position error resulting max allowed speed, L1 = 1 in this space */ + math::Vector<3> scale = _params.pos_p.edivide(_params.vel_max); + + /* find X - cross point of L1 sphere and trajectory */ + math::Vector<3> pos_s = _pos.emult(scale); + math::Vector<3> prev_sp_s = prev_sp.emult(scale); + math::Vector<3> curr_sp_s = curr_sp.emult(scale); + math::Vector<3> curr_pos_s = pos_s - curr_sp_s; + float curr_pos_s_len = curr_pos_s.length(); + math::Vector<3> x; + if (curr_pos_s_len < 1.0f) { + /* copter is closer to waypoint than L1 radius */ + /* check next waypoint and use it to avoid slowing down when passing via waypoint */ + if (_pos_sp_triplet.next.valid) { + math::Vector<3> next_sp; + map_projection_project(&_ref_pos, + _pos_sp_triplet.next.lat, _pos_sp_triplet.next.lon, + &next_sp.data[0], &next_sp.data[1]); + next_sp(2) = -(_pos_sp_triplet.next.alt - _ref_alt); + + if ((next_sp - curr_sp).length() > MIN_DIST) { + math::Vector<3> next_sp_s = next_sp.emult(scale); + + /* calculate angle prev - curr - next */ + math::Vector<3> curr_next_s = next_sp_s - curr_sp_s; + math::Vector<3> prev_curr_s_norm = (curr_sp_s - prev_sp_s).normalized(); + + /* cos(a) = angle between current and next trajectory segments */ + float cos_a = prev_curr_s_norm * curr_next_s; + + /* cos(b) = angle pos - curr_sp - prev_sp */ + float cos_b = -curr_pos_s * prev_curr_s_norm / curr_pos_s_len; + + if (cos_a > 0.0f && cos_b > 0.0f) { + float curr_next_s_len = curr_next_s.length(); + /* if curr - next distance is larger than L1 radius, limit it */ + if (curr_next_s_len > 1.0f) { + cos_a /= curr_next_s_len; + } + + /* feed forward position setpoint offset */ + math::Vector<3> pos_ff = prev_curr_s_norm * + cos_a * cos_b * cos_b * (1.0f - curr_pos_s_len) * + (1.0f - expf(-curr_pos_s_len * curr_pos_s_len * 20.0f)); + _pos_sp += pos_ff.edivide(scale); + } + } + } + + } else { + bool near = cross_sphere_line(pos_s, 1.0f, prev_sp_s, curr_sp_s, x); + if (near) { + /* L1 sphere crosses trajectory */ + + } else { + /* copter is too far from trajectory */ + /* if copter is behind prev waypoint, go directly to prev waypoint */ + if ((x - prev_sp_s) * (curr_sp_s - prev_sp) < 0.0f) { + x = prev_sp_s; + } + + /* if copter is in front of curr waypoint, go directly to curr waypoint */ + if ((x - curr_sp_s) * (curr_sp_s - prev_sp) > 0.0f) { + x = curr_sp_s; + } + + x = pos_s + (x - pos_s).normalized(); + } + + /* scale result back to normal space */ + _pos_sp = x.edivide(scale); + } + } + } + + /* 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_pos_sp(); + reset_alt_sp(); + } +} + void MulticopterPositionControl::task_main() { @@ -757,34 +912,7 @@ MulticopterPositionControl::task_main() } else { /* 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) { - /* in case of interrupted mission don't go to waypoint but stay at current position */ - _reset_pos_sp = true; - _reset_alt_sp = true; - - /* project setpoint to local frame */ - map_projection_project(&_ref_pos, - _pos_sp_triplet.current.lat, _pos_sp_triplet.current.lon, - &_pos_sp.data[0], &_pos_sp.data[1]); - _pos_sp(2) = -(_pos_sp_triplet.current.alt - _ref_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_pos_sp(); - reset_alt_sp(); - } + control_auto(); } /* fill local position setpoint */ From 7b4448510436ddb3d5c548a4039bfa53c3aabfc4 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 16 Aug 2014 18:46:42 +0200 Subject: [PATCH 104/411] mission: takeoff and next waypoint related fixes --- src/modules/navigator/mission.cpp | 35 +++++++++++++++++++++---------- 1 file changed, 24 insertions(+), 11 deletions(-) diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 98a4340e1c..128dab7d77 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -344,6 +344,10 @@ Mission::set_mission_items() /* no mission available or mission finished, switch to loiter */ if (_mission_type != MISSION_TYPE_NONE) { mavlink_log_critical(_navigator->get_mavlink_fd(), "mission finished, loitering"); + + /* use last setpoint for loiter */ + _navigator->set_can_loiter_at_sp(true); + } else if (!user_feedback_done) { /* only tell users that we got no mission if there has not been any * better, more specific feedback yet @@ -360,6 +364,7 @@ Mission::set_mission_items() mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current); pos_sp_triplet->next.valid = false; + /* reuse setpoint for LOITER only if it's not IDLE */ _navigator->set_can_loiter_at_sp(pos_sp_triplet->current.type == SETPOINT_TYPE_LOITER); reset_mission_item_reached(); @@ -412,11 +417,13 @@ Mission::set_mission_items() if (_navigator->get_global_position()->alt < takeoff_alt - _navigator->get_acceptance_radius()) { mavlink_log_critical(_navigator->get_mavlink_fd(), "takeoff to %.1f meters above home", (double)(takeoff_alt - _navigator->get_home_position()->alt)); - _mission_item.nav_cmd = NAV_CMD_TAKEOFF; - _mission_item.lat = _navigator->get_global_position()->lat; - _mission_item.lon = _navigator->get_global_position()->lon; - _mission_item.altitude = takeoff_alt; - _mission_item.altitude_is_relative = false; + _mission_item.nav_cmd = NAV_CMD_TAKEOFF; + _mission_item.lat = _navigator->get_global_position()->lat; + _mission_item.lon = _navigator->get_global_position()->lon; + _mission_item.altitude = takeoff_alt; + _mission_item.altitude_is_relative = false; + _mission_item.autocontinue = true; + _mission_item.time_inside = 0; mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current); @@ -445,15 +452,21 @@ Mission::set_mission_items() } // TODO: report onboard mission item somehow - /* try to read next mission item */ - struct mission_item_s mission_item_next; + if (_mission_item.autocontinue && _mission_item.time_inside <= 0.001f) { + /* try to read next mission item */ + struct mission_item_s mission_item_next; - if (read_mission_item(_mission_type == MISSION_TYPE_ONBOARD, false, &mission_item_next)) { - /* got next mission item, update setpoint triplet */ - mission_item_to_position_setpoint(&mission_item_next, &pos_sp_triplet->next); + if (read_mission_item(_mission_type == MISSION_TYPE_ONBOARD, false, &mission_item_next)) { + /* got next mission item, update setpoint triplet */ + mission_item_to_position_setpoint(&mission_item_next, &pos_sp_triplet->next); + + } else { + /* next mission item is not available */ + pos_sp_triplet->next.valid = false; + } } else { - /* next mission item is not available */ + /* vehicle will be paused on current waypoint, don't set next item */ pos_sp_triplet->next.valid = false; } From 789f949e746cdd79e1214830a4ef822ef5634bbe Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Sun, 17 Aug 2014 10:19:48 +0200 Subject: [PATCH 105/411] Added debugging and small locking fix. --- src/modules/bottle_drop/bottle_drop.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/modules/bottle_drop/bottle_drop.cpp b/src/modules/bottle_drop/bottle_drop.cpp index ddc41372ec..14b9a8379a 100644 --- a/src/modules/bottle_drop/bottle_drop.cpp +++ b/src/modules/bottle_drop/bottle_drop.cpp @@ -728,6 +728,7 @@ BottleDrop::handle_command(struct vehicle_command_s *cmd) mavlink_log_info(_mavlink_fd, "#audio: open doors"); } else { + lock_release(); close_bay(); mavlink_log_info(_mavlink_fd, "#audio: close doors"); } @@ -760,7 +761,7 @@ BottleDrop::handle_command(struct vehicle_command_s *cmd) _target_position.alt = cmd->param7; _target_position.initialized; _drop_state = DROP_STATE_TARGET_VALID; - warnx("got target: %8.4f, %8.4f, %8.4f", (double)_target_position.lat, + mavlink_log_info(_mavlink_fd, "got target: %8.4f, %8.4f, %8.4f", (double)_target_position.lat, (double)_target_position.lon, (double)_target_position.alt); map_projection_init(&ref, _target_position.lat, _target_position.lon); answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); From b1008842204418f5d8cd0475547ecfb8f378b4c7 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 17 Aug 2014 12:07:02 +0200 Subject: [PATCH 106/411] geofence: support AMSL mode --- src/modules/navigator/geofence.cpp | 22 ++++++++----- src/modules/navigator/geofence.h | 40 +++++++++++++++--------- src/modules/navigator/geofence_params.c | 12 +++++++ src/modules/navigator/navigator.h | 8 +++++ src/modules/navigator/navigator_main.cpp | 27 ++++++++++++++-- 5 files changed, 86 insertions(+), 23 deletions(-) diff --git a/src/modules/navigator/geofence.cpp b/src/modules/navigator/geofence.cpp index 2662153084..2b9ce752b2 100644 --- a/src/modules/navigator/geofence.cpp +++ b/src/modules/navigator/geofence.cpp @@ -62,7 +62,8 @@ Geofence::Geofence() : _altitude_min(0), _altitude_max(0), _verticesCount(0), - param_geofence_on(this, "ON") + _param_geofence_on(this, "ON"), + _param_altitude_mode(this, "ALTMODE") { /* Load initial params */ updateParams(); @@ -74,19 +75,26 @@ Geofence::~Geofence() } -bool Geofence::inside(const struct vehicle_global_position_s *vehicle) +bool Geofence::inside(const struct vehicle_global_position_s &global_position) { - double lat = vehicle->lat / 1e7d; - double lon = vehicle->lon / 1e7d; - //float alt = vehicle->alt; + double lat = global_position.lat / 1e7d; + double lon = global_position.lon / 1e7d; - return inside(lat, lon, vehicle->alt); + return inside(lat, lon, global_position.alt); +} + +bool Geofence::inside(const struct vehicle_global_position_s &global_position, float baro_altitude_amsl) { + + double lat = global_position.lat / 1e7d; + double lon = global_position.lon / 1e7d; + + return inside(lat, lon, baro_altitude_amsl); } bool Geofence::inside(double lat, double lon, float altitude) { /* Return true if geofence is disabled */ - if (param_geofence_on.get() != 1) + if (_param_geofence_on.get() != 1) return true; if (valid()) { diff --git a/src/modules/navigator/geofence.h b/src/modules/navigator/geofence.h index 2eb126ab56..e2c0f08d87 100644 --- a/src/modules/navigator/geofence.h +++ b/src/modules/navigator/geofence.h @@ -42,6 +42,8 @@ #define GEOFENCE_H_ #include +#include +#include #include #include @@ -49,29 +51,25 @@ class Geofence : public control::SuperBlock { -private: - orb_advert_t _fence_pub; /**< publish fence topic */ - - float _altitude_min; - float _altitude_max; - - unsigned _verticesCount; - - /* Params */ - control::BlockParamInt param_geofence_on; public: Geofence(); ~Geofence(); + /* Altitude mode, corresponding to the param GF_ALTMODE */ + enum { + GF_ALT_MODE_GPS = 0, + GF_ALT_MODE_AMSL = 1 + }; + /** - * Return whether craft is inside geofence. + * Return whether system 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 + * @return true: system is inside fence, false: system is outside fence */ - bool inside(const struct vehicle_global_position_s *craft); + bool inside(const struct vehicle_global_position_s &global_position); + bool inside(const struct vehicle_global_position_s &global_position, float baro_altitude_amsl); bool inside(double lat, double lon, float altitude); int clearDm(); @@ -88,6 +86,20 @@ public: int loadFromFile(const char *filename); bool isEmpty() {return _verticesCount == 0;} + + int getAltitudeMode() { return _param_altitude_mode.get(); } + +private: + orb_advert_t _fence_pub; /**< publish fence topic */ + + float _altitude_min; + float _altitude_max; + + unsigned _verticesCount; + + /* Params */ + control::BlockParamInt _param_geofence_on; + control::BlockParamInt _param_altitude_mode; }; diff --git a/src/modules/navigator/geofence_params.c b/src/modules/navigator/geofence_params.c index 653b1ad849..29b42cd54e 100644 --- a/src/modules/navigator/geofence_params.c +++ b/src/modules/navigator/geofence_params.c @@ -58,3 +58,15 @@ * @group Geofence */ PARAM_DEFINE_INT32(GF_ON, 1); + +/** + * Geofence altitude mode + * + * Select which altitude reference should be used + * 0 = GPS, 1 = AMSL + * + * @min 0 + * @max 1 + * @group Geofence + */ +PARAM_DEFINE_INT32(GF_ALTMODE, 1); diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index 5369779723..59a6752a99 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -120,6 +120,7 @@ public: struct vehicle_status_s* get_vstatus() { return &_vstatus; } struct vehicle_control_mode_s* get_control_mode() { return &_control_mode; } struct vehicle_global_position_s* get_global_position() { return &_global_pos; } + struct sensor_combined_s* get_sensor_combined() { return &_sensor_combined; } struct home_position_s* get_home_position() { return &_home_pos; } struct position_setpoint_triplet_s* get_position_setpoint_triplet() { return &_pos_sp_triplet; } struct mission_result_s* get_mission_result() { return &_mission_result; } @@ -141,6 +142,7 @@ private: int _mavlink_fd; /**< the file descriptor to send messages over mavlink */ int _global_pos_sub; /**< global position subscription */ + int _sensor_combined_sub; /**< sensor combined subscription */ int _home_pos_sub; /**< home position subscription */ int _vstatus_sub; /**< vehicle status subscription */ int _capabilities_sub; /**< notification of vehicle capabilities updates */ @@ -156,6 +158,7 @@ private: vehicle_status_s _vstatus; /**< vehicle status */ vehicle_control_mode_s _control_mode; /**< vehicle control mode */ vehicle_global_position_s _global_pos; /**< global vehicle position */ + sensor_combined_s _sensor_combined; /**< sensor values */ home_position_s _home_pos; /**< home position for RTL */ mission_item_s _mission_item; /**< current mission item */ navigation_capabilities_s _nav_caps; /**< navigation capabilities */ @@ -195,6 +198,11 @@ private: */ void global_position_update(); + /** + * Retrieve sensor values + */ + void sensor_combined_update(); + /** * Retrieve home position */ diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 18bfc2cece..d77acf74e2 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -69,6 +69,7 @@ #include #include #include +#include #include #include @@ -112,6 +113,7 @@ Navigator::Navigator() : _vstatus{}, _control_mode{}, _global_pos{}, + _sensor_combined{}, _home_pos{}, _mission_item{}, _nav_caps{}, @@ -178,6 +180,12 @@ Navigator::global_position_update() orb_copy(ORB_ID(vehicle_global_position), _global_pos_sub, &_global_pos); } +void +Navigator::sensor_combined_update() +{ + orb_copy(ORB_ID(sensor_combined), _sensor_combined_sub, &_sensor_combined); +} + void Navigator::home_position_update() { @@ -248,6 +256,7 @@ Navigator::task_main() /* do subscriptions */ _global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); + _sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined)); _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)); @@ -261,6 +270,7 @@ Navigator::task_main() vehicle_status_update(); vehicle_control_mode_update(); global_position_update(); + sensor_combined_update(); home_position_update(); navigation_capabilities_update(); params_update(); @@ -272,7 +282,7 @@ Navigator::task_main() const hrt_abstime mavlink_open_interval = 500000; /* wakeup source(s) */ - struct pollfd fds[6]; + struct pollfd fds[7]; /* Setup of loop */ fds[0].fd = _global_pos_sub; @@ -287,6 +297,8 @@ Navigator::task_main() fds[4].events = POLLIN; fds[5].fd = _param_update_sub; fds[5].events = POLLIN; + fds[6].fd = _sensor_combined_sub; + fds[6].events = POLLIN; while (!_task_should_exit) { @@ -311,6 +323,11 @@ Navigator::task_main() _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); } + /* sensors combined updated */ + if (fds[6].revents & POLLIN) { + sensor_combined_update(); + } + /* parameters updated */ if (fds[5].revents & POLLIN) { params_update(); @@ -342,7 +359,13 @@ Navigator::task_main() global_position_update(); /* Check geofence violation */ - if (!_geofence.inside(&_global_pos)) { + bool inside = false; + if (_geofence.getAltitudeMode() == Geofence::GF_ALT_MODE_GPS) { + inside = _geofence.inside(_global_pos); + } else { + inside = _geofence.inside(_global_pos, _sensor_combined.baro_alt_meter); + } + if (!inside) { /* inform other apps via the sp triplet */ _pos_sp_triplet.geofence_violated = true; if (_pos_sp_triplet.geofence_violated != true) { From 5832948371866aec8f0c7f16b13869f270d36aad Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 17 Aug 2014 12:37:14 +0200 Subject: [PATCH 107/411] geofence: lat/lon is double types changed but the geofence implentation was not updated, this was forgotten in 58792c5ca6e42bc251dd3c92b0e79217ff5d5403 --- src/modules/navigator/geofence.cpp | 17 ++++++----------- 1 file changed, 6 insertions(+), 11 deletions(-) diff --git a/src/modules/navigator/geofence.cpp b/src/modules/navigator/geofence.cpp index 2b9ce752b2..7897c7ba0e 100644 --- a/src/modules/navigator/geofence.cpp +++ b/src/modules/navigator/geofence.cpp @@ -77,18 +77,12 @@ Geofence::~Geofence() bool Geofence::inside(const struct vehicle_global_position_s &global_position) { - double lat = global_position.lat / 1e7d; - double lon = global_position.lon / 1e7d; - - return inside(lat, lon, global_position.alt); + return inside(global_position.lat, global_position.lon, global_position.alt); } -bool Geofence::inside(const struct vehicle_global_position_s &global_position, float baro_altitude_amsl) { - - double lat = global_position.lat / 1e7d; - double lon = global_position.lon / 1e7d; - - return inside(lat, lon, baro_altitude_amsl); +bool Geofence::inside(const struct vehicle_global_position_s &global_position, float baro_altitude_amsl) +{ + return inside(global_position.lat, global_position.lon, baro_altitude_amsl); } bool Geofence::inside(double lat, double lon, float altitude) @@ -101,8 +95,9 @@ bool Geofence::inside(double lat, double lon, float altitude) if (!isEmpty()) { /* Vertical check */ - if (altitude > _altitude_max || altitude < _altitude_min) + if (altitude > _altitude_max || altitude < _altitude_min) { return false; + } /*Horizontal check */ /* Adaptation of algorithm originally presented as From 99860da9b70bfa87ef2834efa5e7b9ba96ee4e9b Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 17 Aug 2014 16:36:35 +0200 Subject: [PATCH 108/411] commander: remove unnecessary output (navigator already does the output) --- src/modules/commander/commander.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 2ab40a5eb0..84a4be948f 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1316,7 +1316,6 @@ int commander_thread_main(int argc, char *argv[]) /* this will only trigger if geofence is activated via param and a geofence file is present, also there is a circuit breaker to disable the actual flight termination in the px4io driver */ armed.force_failsafe = true; status_changed = true; - mavlink_log_emergency(mavlink_fd, "Geofence violated: terminating"); } // no reset is done here on purpose, on geofence violation we want to stay in flighttermination } From f31c3243b05fae6ffa4ab8cccb8e009470ca1294 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 17 Aug 2014 23:28:48 +0200 Subject: [PATCH 109/411] mc_pos_control: navigation fixes, smooth position setpoint movements --- .../mc_pos_control/mc_pos_control_main.cpp | 77 +++++++++++-------- 1 file changed, 44 insertions(+), 33 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 488ca924a6..847a92151c 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -236,7 +236,7 @@ private: /** * Set position setpoint for AUTO */ - void control_auto(); + void control_auto(float dt); /** * Select between barometric and global (AMSL) altitudes @@ -682,7 +682,7 @@ MulticopterPositionControl::cross_sphere_line(const math::Vector<3>& sphere_c, f } void -MulticopterPositionControl::control_auto() +MulticopterPositionControl::control_auto(float dt) { bool updated; orb_check(_pos_sp_triplet_sub, &updated); @@ -703,8 +703,14 @@ MulticopterPositionControl::control_auto() &curr_sp.data[0], &curr_sp.data[1]); curr_sp(2) = -(_pos_sp_triplet.current.alt - _ref_alt); + /* scaled space: 1 == position error resulting max allowed speed, L1 = 1 in this space */ + math::Vector<3> scale = _params.pos_p.edivide(_params.vel_max); // TODO add mult param here + + /* convert current setpoint to scaled space */ + math::Vector<3> curr_sp_s = curr_sp.emult(scale); + /* by default use current setpoint as is */ - _pos_sp = curr_sp; + math::Vector<3> pos_sp_s = curr_sp_s; if (_pos_sp_triplet.current.type == SETPOINT_TYPE_POSITION && _pos_sp_triplet.previous.valid) { /* follow "previous - current" line */ @@ -715,16 +721,13 @@ MulticopterPositionControl::control_auto() prev_sp(2) = -(_pos_sp_triplet.previous.alt - _ref_alt); if ((curr_sp - prev_sp).length() > MIN_DIST) { - /* scaled space: 1 == position error resulting max allowed speed, L1 = 1 in this space */ - math::Vector<3> scale = _params.pos_p.edivide(_params.vel_max); /* find X - cross point of L1 sphere and trajectory */ math::Vector<3> pos_s = _pos.emult(scale); math::Vector<3> prev_sp_s = prev_sp.emult(scale); - math::Vector<3> curr_sp_s = curr_sp.emult(scale); + math::Vector<3> prev_curr_s = curr_sp_s - prev_sp_s; math::Vector<3> curr_pos_s = pos_s - curr_sp_s; float curr_pos_s_len = curr_pos_s.length(); - math::Vector<3> x; if (curr_pos_s_len < 1.0f) { /* copter is closer to waypoint than L1 radius */ /* check next waypoint and use it to avoid slowing down when passing via waypoint */ @@ -740,56 +743,64 @@ MulticopterPositionControl::control_auto() /* calculate angle prev - curr - next */ math::Vector<3> curr_next_s = next_sp_s - curr_sp_s; - math::Vector<3> prev_curr_s_norm = (curr_sp_s - prev_sp_s).normalized(); + math::Vector<3> prev_curr_s_norm = prev_curr_s.normalized(); - /* cos(a) = angle between current and next trajectory segments */ - float cos_a = prev_curr_s_norm * curr_next_s; + /* cos(a) * curr_next, a = angle between current and next trajectory segments */ + float cos_a_curr_next = prev_curr_s_norm * curr_next_s; - /* cos(b) = angle pos - curr_sp - prev_sp */ + /* cos(b), b = angle pos - curr_sp - prev_sp */ float cos_b = -curr_pos_s * prev_curr_s_norm / curr_pos_s_len; - if (cos_a > 0.0f && cos_b > 0.0f) { + if (cos_a_curr_next > 0.0f && cos_b > 0.0f) { float curr_next_s_len = curr_next_s.length(); /* if curr - next distance is larger than L1 radius, limit it */ if (curr_next_s_len > 1.0f) { - cos_a /= curr_next_s_len; + cos_a_curr_next /= curr_next_s_len; } /* feed forward position setpoint offset */ math::Vector<3> pos_ff = prev_curr_s_norm * - cos_a * cos_b * cos_b * (1.0f - curr_pos_s_len) * + cos_a_curr_next * cos_b * cos_b * (1.0f - curr_pos_s_len) * (1.0f - expf(-curr_pos_s_len * curr_pos_s_len * 20.0f)); - _pos_sp += pos_ff.edivide(scale); + pos_sp_s += pos_ff; } } } } else { - bool near = cross_sphere_line(pos_s, 1.0f, prev_sp_s, curr_sp_s, x); + bool near = cross_sphere_line(pos_s, 1.0f, prev_sp_s, curr_sp_s, pos_sp_s); if (near) { /* L1 sphere crosses trajectory */ } else { /* copter is too far from trajectory */ /* if copter is behind prev waypoint, go directly to prev waypoint */ - if ((x - prev_sp_s) * (curr_sp_s - prev_sp) < 0.0f) { - x = prev_sp_s; + if ((pos_sp_s - prev_sp_s) * prev_curr_s < 0.0f) { + pos_sp_s = prev_sp_s; } /* if copter is in front of curr waypoint, go directly to curr waypoint */ - if ((x - curr_sp_s) * (curr_sp_s - prev_sp) > 0.0f) { - x = curr_sp_s; + if ((pos_sp_s - curr_sp_s) * prev_curr_s > 0.0f) { + pos_sp_s = curr_sp_s; } - x = pos_s + (x - pos_s).normalized(); + pos_sp_s = pos_s + (pos_sp_s - pos_s).normalized(); } - - /* scale result back to normal space */ - _pos_sp = x.edivide(scale); } } } + /* move setpoint not faster than max allowed speed */ + math::Vector<3> pos_sp_old_s = _pos_sp.emult(scale); + math::Vector<3> d_pos_s = pos_sp_s - pos_sp_old_s; + float d_pos_s_len = d_pos_s.length(); + if (d_pos_s_len > dt) { + pos_sp_s = pos_sp_old_s + d_pos_s / d_pos_s_len * dt; + } + + /* scale result back to normal space */ + _pos_sp = pos_sp_s.edivide(scale); + /* update yaw setpoint if needed */ if (isfinite(_pos_sp_triplet.current.yaw)) { _att_sp.yaw_body = _pos_sp_triplet.current.yaw; @@ -912,7 +923,7 @@ MulticopterPositionControl::task_main() } else { /* AUTO */ - control_auto(); + control_auto(dt); } /* fill local position setpoint */ @@ -975,14 +986,14 @@ MulticopterPositionControl::task_main() } - 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; - } - } +// 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); From 47b3f1253eb2a46c216aee1ca9f08d348266ea41 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 18 Aug 2014 16:47:30 +0200 Subject: [PATCH 110/411] Fix bottle drop logic --- src/modules/bottle_drop/bottle_drop.cpp | 43 +++++++++++-------------- 1 file changed, 18 insertions(+), 25 deletions(-) diff --git a/src/modules/bottle_drop/bottle_drop.cpp b/src/modules/bottle_drop/bottle_drop.cpp index 14b9a8379a..77f691f6c4 100644 --- a/src/modules/bottle_drop/bottle_drop.cpp +++ b/src/modules/bottle_drop/bottle_drop.cpp @@ -126,10 +126,9 @@ private: float _alt_clearance; struct position_s { - double lat; //degrees - double lon; //degrees - float alt; //m - bool initialized; + double lat; ///< degrees + double lon; ///< degrees + float alt; ///< m } _target_position, _drop_position; enum DROP_STATE { @@ -646,7 +645,7 @@ BottleDrop::task_main() (double)distance_open_door, (double)(_wrap_pi(_global_pos.yaw - atan2f(wind.windspeed_north, wind.windspeed_east)))); - if (isfinite(distance_real) && distance_real < distance_open_door && _drop_approval) { + if (isfinite(distance_real) && distance_real < distance_open_door) { open_bay(); _drop_state = DROP_STATE_BAY_OPEN; } @@ -655,27 +654,21 @@ BottleDrop::task_main() case DROP_STATE_BAY_OPEN: { - map_projection_project(&ref, _global_pos.lat, _global_pos.lon, &x_l, &y_l); - x_f = x_l + _global_pos.vel_n * dt_runs; - y_f = y_l + _global_pos.vel_e * dt_runs; - map_projection_reproject(&ref, x_f, y_f, &x_f_NED, &y_f_NED); - future_distance = fabsf(get_distance_to_next_waypoint(x_f_NED, y_f_NED, _drop_position.lat, _drop_position.lon)); + if (_drop_approval) { + map_projection_project(&ref, _global_pos.lat, _global_pos.lon, &x_l, &y_l); + x_f = x_l + _global_pos.vel_n * dt_runs; + y_f = y_l + _global_pos.vel_e * dt_runs; + map_projection_reproject(&ref, x_f, y_f, &x_f_NED, &y_f_NED); + future_distance = fabsf(get_distance_to_next_waypoint(x_f_NED, y_f_NED, _drop_position.lat, _drop_position.lon)); - warnx("Distance real: %.2f", (double)distance_real); + warnx("Distance real: %.2f", (double)distance_real); - if (isfinite(distance_real) && (((distance_real < precision) && (distance_real < future_distance)) || - /* we drop as well if we're really close */ (distance_real < precision / 10.0f))) { // Drop only if the distance between drop point and actual position is getting larger again - // XXX this check needs to be carefully validated - right now we prefer to drop if we're close to the goal - - // if(fabsf(_wrap_pi(_global_pos.yaw-atan2f(wind.windspeed_north,wind_speed.windspeed_east)+M_PI_F)) < 80.0f/180.0f*M_PI_F) // if flight trajectory deviates more than 80 degrees from calculated path, it will no drop - // { - drop(); - _drop_state = DROP_STATE_DROPPED; - // } - // else - // { - // state_run = true; - // } + if (isfinite(distance_real) && + (((distance_real < precision) && (distance_real < future_distance)) || + (distance_real < precision / 10.0f))) { + drop(); + _drop_state = DROP_STATE_DROPPED; + } } } break; @@ -684,6 +677,7 @@ BottleDrop::task_main() /* 2s after drop, reset and close everything again */ if ((hrt_elapsed_time(&_doors_opened) > 2 * 1000 * 1000)) { _drop_state = DROP_STATE_INIT; + _drop_approval = false; lock_release(); close_bay(); } @@ -759,7 +753,6 @@ BottleDrop::handle_command(struct vehicle_command_s *cmd) _target_position.lat = cmd->param5; _target_position.lon = cmd->param6; _target_position.alt = cmd->param7; - _target_position.initialized; _drop_state = DROP_STATE_TARGET_VALID; mavlink_log_info(_mavlink_fd, "got target: %8.4f, %8.4f, %8.4f", (double)_target_position.lat, (double)_target_position.lon, (double)_target_position.alt); From 7f9c996555975301288da58745f69b39f05facbe Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 19 Aug 2014 07:30:19 +0200 Subject: [PATCH 111/411] engine fail: small state machine fix --- src/modules/navigator/enginefailure.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/modules/navigator/enginefailure.cpp b/src/modules/navigator/enginefailure.cpp index de567f0dc6..e007b208b2 100644 --- a/src/modules/navigator/enginefailure.cpp +++ b/src/modules/navigator/enginefailure.cpp @@ -76,7 +76,8 @@ EngineFailure::on_inactive() void EngineFailure::on_activation() { - _ef_state = EF_STATE_LOITERDOWN; + _ef_state = EF_STATE_NONE; + advance_ef(); set_ef_item(); } @@ -139,6 +140,7 @@ EngineFailure::advance_ef() { switch (_ef_state) { case EF_STATE_NONE: + mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: Engine failure. Loitering down"); _ef_state = EF_STATE_LOITERDOWN; break; default: From 64ca94412e710164fb2ae69f6dfc3edbeff9c12b Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 19 Aug 2014 07:31:55 +0200 Subject: [PATCH 112/411] engine fail: fw pos control limits pitch and sets 0 throttle --- .../fw_pos_control_l1_main.cpp | 65 +++++++++++++++---- 1 file changed, 52 insertions(+), 13 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 350ce6dec1..0c35ccb2cc 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 @@ -139,7 +139,8 @@ private: int _pos_sp_triplet_sub; int _att_sub; /**< vehicle attitude subscription */ int _airspeed_sub; /**< airspeed subscription */ - int _control_mode_sub; /**< vehicle status subscription */ + int _control_mode_sub; /**< control mode subscription */ + int _vehicle_status_sub; /**< vehicle status subscription */ int _params_sub; /**< notification of parameter updates */ int _manual_control_sub; /**< notification of manual control updates */ int _sensor_combined_sub; /**< for body frame accelerations */ @@ -154,7 +155,8 @@ private: struct navigation_capabilities_s _nav_capabilities; /**< navigation capabilities */ struct manual_control_setpoint_s _manual; /**< r/c channel data */ struct airspeed_s _airspeed; /**< airspeed */ - struct vehicle_control_mode_s _control_mode; /**< vehicle status */ + struct vehicle_control_mode_s _control_mode; /**< control mode */ + struct vehicle_status_s _vehicle_status; /**< vehicle status */ struct vehicle_global_position_s _global_pos; /**< global vehicle position */ struct position_setpoint_triplet_s _pos_sp_triplet; /**< triplet of mission items */ struct sensor_combined_s _sensor_combined; /**< for body frame accelerations */ @@ -301,10 +303,15 @@ private: void control_update(); /** - * Check for changes in vehicle status. + * Check for changes in control mode */ void vehicle_control_mode_poll(); + /** + * Check for changes in vehicle status. + */ + void vehicle_status_poll(); + /** * Check for airspeed updates. */ @@ -408,6 +415,7 @@ FixedwingPositionControl::FixedwingPositionControl() : _att_sub(-1), _airspeed_sub(-1), _control_mode_sub(-1), + _vehicle_status_sub(-1), _params_sub(-1), _manual_control_sub(-1), _sensor_combined_sub(-1), @@ -425,6 +433,7 @@ FixedwingPositionControl::FixedwingPositionControl() : _manual(), _airspeed(), _control_mode(), + _vehicle_status(), _global_pos(), _pos_sp_triplet(), _sensor_combined(), @@ -627,16 +636,27 @@ FixedwingPositionControl::parameters_update() void FixedwingPositionControl::vehicle_control_mode_poll() { - bool vstatus_updated; + bool updated; - /* Check HIL state if vehicle status has changed */ - orb_check(_control_mode_sub, &vstatus_updated); + orb_check(_control_mode_sub, &updated); - if (vstatus_updated) { + if (updated) { orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode); } } +void +FixedwingPositionControl::vehicle_status_poll() +{ + bool updated; + + orb_check(_vehicle_status_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &_vehicle_status); + } +} + bool FixedwingPositionControl::vehicle_airspeed_poll() { @@ -1182,10 +1202,13 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } } - if (usePreTakeoffThrust) { - _att_sp.thrust = launchDetector.getThrottlePreTakeoff(); + if (_vehicle_status.engine_failure || _vehicle_status.engine_failure_cmd) { + /* Set thrrust to 0 to minimize damage */ + _att_sp.thrust = 0.0f; } - else { + else if (usePreTakeoffThrust) { + _att_sp.thrust = launchDetector.getThrottlePreTakeoff(); + } else { _att_sp.thrust = math::min(_mTecs.getEnabled() ? _mTecs.getThrottleSetpoint() : _tecs.get_throttle_demand(), throttle_max); } _att_sp.pitch_body = _mTecs.getEnabled() ? _mTecs.getPitchSetpoint() : _tecs.get_pitch_demand(); @@ -1212,13 +1235,16 @@ FixedwingPositionControl::task_main() _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)); + _vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status)); _airspeed_sub = orb_subscribe(ORB_ID(airspeed)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); _manual_control_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); _range_finder_sub = orb_subscribe(ORB_ID(sensor_range_finder)); - /* rate limit vehicle status updates to 5Hz */ + /* rate limit control mode updates to 5Hz */ orb_set_interval(_control_mode_sub, 200); + /* rate limit vehicle status updates to 5Hz */ + orb_set_interval(_vehicle_status_sub, 200); /* rate limit position updates to 50 Hz */ orb_set_interval(_global_pos_sub, 20); @@ -1257,9 +1283,12 @@ FixedwingPositionControl::task_main() perf_begin(_loop_perf); - /* check vehicle status for changes to publication state */ + /* check vehicle control mode for changes to publication state */ vehicle_control_mode_poll(); + /* check vehicle status for changes to publication state */ + vehicle_status_poll(); + /* only update parameters if they changed */ if (fds[0].revents & POLLIN) { /* read from param to clear updated flag */ @@ -1372,7 +1401,12 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_ flightPathAngle = -asinf(ground_speed(2)/ground_speed_length); } fwPosctrl::LimitOverride limitOverride; - if (climbout_mode) { + if (_vehicle_status.engine_failure || _vehicle_status.engine_failure_cmd) { + /* Force the slow downwards spiral */ + limitOverride.enablePitchMinOverride(-1.0f); + limitOverride.enablePitchMaxOverride(5.0f); + + } else if (climbout_mode) { limitOverride.enablePitchMinOverride(M_RAD_TO_DEG_F * climbout_pitch_min_rad); } else { limitOverride.disablePitchMinOverride(); @@ -1380,6 +1414,11 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_ _mTecs.updateAltitudeSpeed(flightPathAngle, altitude, alt_sp, _airspeed.true_airspeed_m_s, v_sp, mode, limitOverride); } else { + if (_vehicle_status.engine_failure || _vehicle_status.engine_failure_cmd) { + /* Force the slow downwards spiral */ + pitch_min_rad = M_DEG_TO_RAD_F * -1.0f; + pitch_max_rad = M_DEG_TO_RAD_F * 5.0f; + } /* Using tecs library */ _tecs.update_pitch_throttle(_R_nb, _att.pitch, altitude, alt_sp, v_sp, _airspeed.indicated_airspeed_m_s, eas2tas, From 57a250f9461f0ab8fe10cf6988f9e2dfd269bdf2 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 19 Aug 2014 09:23:41 +0200 Subject: [PATCH 113/411] navigator: remove offboard mode offboard setpoints are now forwarded directly from mavlink --- src/modules/navigator/module.mk | 1 - src/modules/navigator/navigator.h | 6 +- src/modules/navigator/navigator_main.cpp | 13 +-- src/modules/navigator/offboard.cpp | 142 ----------------------- src/modules/navigator/offboard.h | 72 ------------ 5 files changed, 4 insertions(+), 230 deletions(-) delete mode 100644 src/modules/navigator/offboard.cpp delete mode 100644 src/modules/navigator/offboard.h diff --git a/src/modules/navigator/module.mk b/src/modules/navigator/module.mk index b501989961..5d680948de 100644 --- a/src/modules/navigator/module.mk +++ b/src/modules/navigator/module.mk @@ -46,7 +46,6 @@ SRCS = navigator_main.cpp \ loiter.cpp \ rtl.cpp \ rtl_params.c \ - offboard.cpp \ mission_feasibility_checker.cpp \ geofence.cpp \ geofence_params.c diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index 8edbb63b35..742cd59ca5 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -56,14 +56,13 @@ #include "mission.h" #include "loiter.h" #include "rtl.h" -#include "offboard.h" #include "geofence.h" /** * Number of navigation modes that need on_active/on_inactive calls * Currently: mission, loiter, and rtl */ -#define NAVIGATOR_MODE_ARRAY_SIZE 4 +#define NAVIGATOR_MODE_ARRAY_SIZE 3 class Navigator : public control::SuperBlock { @@ -116,7 +115,6 @@ public: struct position_setpoint_triplet_s* get_position_setpoint_triplet() { return &_pos_sp_triplet; } int get_onboard_mission_sub() { return _onboard_mission_sub; } int get_offboard_mission_sub() { return _offboard_mission_sub; } - int get_offboard_control_sp_sub() { return _offboard_control_sp_sub; } Geofence& get_geofence() { return _geofence; } bool get_can_loiter_at_sp() { return _can_loiter_at_sp; } float get_loiter_radius() { return _param_loiter_radius.get(); } @@ -134,7 +132,6 @@ private: int _home_pos_sub; /**< home position subscription */ int _vstatus_sub; /**< vehicle status subscription */ int _capabilities_sub; /**< notification of vehicle capabilities updates */ - int _offboard_control_sp_sub; /*** offboard control subscription */ int _control_mode_sub; /**< vehicle control mode subscription */ int _onboard_mission_sub; /**< onboard mission subscription */ int _offboard_mission_sub; /**< offboard mission subscription */ @@ -164,7 +161,6 @@ private: Mission _mission; /**< class that handles the missions */ Loiter _loiter; /**< class that handles loiter */ RTL _rtl; /**< class that handles RTL */ - Offboard _offboard; /**< class that handles offboard */ NavigatorMode *_navigation_mode_array[NAVIGATOR_MODE_ARRAY_SIZE]; /**< array of navigation modes */ diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 331a9a728f..f11ae72c5d 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -67,7 +67,6 @@ #include #include #include -#include #include #include @@ -101,7 +100,6 @@ Navigator::Navigator() : _home_pos_sub(-1), _vstatus_sub(-1), _capabilities_sub(-1), - _offboard_control_sp_sub(-1), _control_mode_sub(-1), _onboard_mission_sub(-1), _offboard_mission_sub(-1), @@ -124,7 +122,6 @@ Navigator::Navigator() : _mission(this, "MIS"), _loiter(this, "LOI"), _rtl(this, "RTL"), - _offboard(this, "OFF"), _can_loiter_at_sp(false), _pos_sp_triplet_updated(false), _param_loiter_radius(this, "LOITER_RAD"), @@ -134,7 +131,6 @@ Navigator::Navigator() : _navigation_mode_array[0] = &_mission; _navigation_mode_array[1] = &_loiter; _navigation_mode_array[2] = &_rtl; - _navigation_mode_array[3] = &_offboard; updateParams(); } @@ -247,7 +243,6 @@ Navigator::task_main() _onboard_mission_sub = orb_subscribe(ORB_ID(onboard_mission)); _offboard_mission_sub = orb_subscribe(ORB_ID(offboard_mission)); _param_update_sub = orb_subscribe(ORB_ID(parameter_update)); - _offboard_control_sp_sub = orb_subscribe(ORB_ID(offboard_control_setpoint)); /* copy all topics first time */ vehicle_status_update(); @@ -353,6 +348,9 @@ Navigator::task_main() case NAVIGATION_STATE_ACRO: case NAVIGATION_STATE_ALTCTL: case NAVIGATION_STATE_POSCTL: + case NAVIGATION_STATE_LAND: + case NAVIGATION_STATE_TERMINATION: + case NAVIGATION_STATE_OFFBOARD: _navigation_mode = nullptr; _can_loiter_at_sp = false; break; @@ -368,11 +366,6 @@ Navigator::task_main() case NAVIGATION_STATE_AUTO_RTGS: _navigation_mode = &_rtl; /* TODO: change this to something else */ break; - case NAVIGATION_STATE_LAND: - case NAVIGATION_STATE_TERMINATION: - case NAVIGATION_STATE_OFFBOARD: - _navigation_mode = &_offboard; - break; default: _navigation_mode = nullptr; _can_loiter_at_sp = false; diff --git a/src/modules/navigator/offboard.cpp b/src/modules/navigator/offboard.cpp deleted file mode 100644 index fc4d183cdf..0000000000 --- a/src/modules/navigator/offboard.cpp +++ /dev/null @@ -1,142 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2014 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ -/** - * @file offboard.cpp - * - * Helper class for offboard commands - * - * @author Julian Oes - */ - -#include -#include -#include -#include -#include - -#include -#include - -#include -#include - -#include "navigator.h" -#include "offboard.h" - -Offboard::Offboard(Navigator *navigator, const char *name) : - NavigatorMode(navigator, name), - _offboard_control_sp({0}) -{ - /* load initial params */ - updateParams(); - /* initial reset */ - on_inactive(); -} - -Offboard::~Offboard() -{ -} - -void -Offboard::on_activation() -{ -} - -void -Offboard::on_active() -{ - struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); - - bool updated; - orb_check(_navigator->get_offboard_control_sp_sub(), &updated); - if (updated) { - update_offboard_control_setpoint(); - } - - /* copy offboard setpoints to the corresponding topics */ - if (_offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED) { - /* We accept position control only if none of the directions is ignored (as pos_sp_triplet does not - * support deactivation of individual directions) */ - if (_navigator->get_control_mode()->flag_control_position_enabled && - (!offboard_control_sp_ignore_position(_offboard_control_sp, 0) && - !offboard_control_sp_ignore_position(_offboard_control_sp, 1) && - !offboard_control_sp_ignore_position(_offboard_control_sp, 2))) { - /* position control */ - pos_sp_triplet->current.x = _offboard_control_sp.position[0]; - pos_sp_triplet->current.y = _offboard_control_sp.position[1]; - //pos_sp_triplet->current.yaw = _offboard_control_sp.position[2]; - //XXX: copy yaw - pos_sp_triplet->current.z = -_offboard_control_sp.position[2]; - - pos_sp_triplet->current.type = SETPOINT_TYPE_OFFBOARD; - pos_sp_triplet->current.valid = true; - pos_sp_triplet->current.position_valid = true; - - _navigator->set_position_setpoint_triplet_updated(); - } - /* We accept velocity control only if none of the directions is ignored (as pos_sp_triplet does not - * support deactivation of individual directions) */ - if (_navigator->get_control_mode()->flag_control_velocity_enabled && - (!offboard_control_sp_ignore_velocity(_offboard_control_sp, 0) && - !offboard_control_sp_ignore_velocity(_offboard_control_sp, 1) && - !offboard_control_sp_ignore_velocity(_offboard_control_sp, 2))) { - /* velocity control */ - pos_sp_triplet->current.vx = _offboard_control_sp.velocity[0]; - pos_sp_triplet->current.vy = _offboard_control_sp.velocity[1]; -// pos_sp_triplet->current.yawspeed = _offboard_control_sp.velocity[; -// //XXX: copy yaw speed - pos_sp_triplet->current.vz = _offboard_control_sp.velocity[2]; - - pos_sp_triplet->current.type = SETPOINT_TYPE_OFFBOARD; - pos_sp_triplet->current.valid = true; - pos_sp_triplet->current.velocity_valid = true; - - _navigator->set_position_setpoint_triplet_updated(); - } - - //XXX: map acceleration setpoint once supported in setpoint triplet - } - -} - -void -Offboard::on_inactive() -{ -} - -void -Offboard::update_offboard_control_setpoint() -{ - orb_copy(ORB_ID(offboard_control_setpoint), _navigator->get_offboard_control_sp_sub(), &_offboard_control_sp); - -} diff --git a/src/modules/navigator/offboard.h b/src/modules/navigator/offboard.h deleted file mode 100644 index 66b923bdb5..0000000000 --- a/src/modules/navigator/offboard.h +++ /dev/null @@ -1,72 +0,0 @@ -/*************************************************************************** - * - * Copyright (c) 2014 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ -/** - * @file offboard.h - * - * Helper class for offboard commands - * - * @author Julian Oes - */ - -#ifndef NAVIGATOR_OFFBOARD_H -#define NAVIGATOR_OFFBOARD_H - -#include -#include - -#include -#include - -#include "navigator_mode.h" - -class Navigator; - -class Offboard : public NavigatorMode -{ -public: - Offboard(Navigator *navigator, const char *name); - - ~Offboard(); - - virtual void on_inactive(); - - virtual void on_activation(); - - virtual void on_active(); -private: - void update_offboard_control_setpoint(); - - struct offboard_control_setpoint_s _offboard_control_sp; -}; - -#endif From 760a7ff548bcef6910f84beaa981600f3609358b Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 20 Aug 2014 07:45:01 +0200 Subject: [PATCH 114/411] gpsfailure: add skeleton class, activate in commander --- .../commander/state_machine_helper.cpp | 7 +- src/modules/navigator/datalinkloss.cpp | 2 +- src/modules/navigator/gpsfailure.cpp | 170 ++++++++++++++++++ src/modules/navigator/gpsfailure.h | 86 +++++++++ src/modules/navigator/module.mk | 3 +- src/modules/navigator/navigator.h | 4 +- src/modules/navigator/navigator_main.cpp | 5 + 7 files changed, 272 insertions(+), 5 deletions(-) create mode 100644 src/modules/navigator/gpsfailure.cpp create mode 100644 src/modules/navigator/gpsfailure.h diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 3f4bfaa1ca..404bafb042 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -499,12 +499,15 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en status->nav_state = NAVIGATION_STATE_AUTO_LANDENGFAIL; } else if (status->data_link_lost_cmd) { status->nav_state = NAVIGATION_STATE_AUTO_RTGS; - //} else if (status->gps_failure_cmd) { - //status->nav_state = NAVIGATION_STATE_AUTO_***; + } else if (status->gps_failure_cmd) { + status->nav_state = NAVIGATION_STATE_AUTO_LANDGPSFAIL; } else if (status->rc_signal_lost_cmd) { status->nav_state = NAVIGATION_STATE_AUTO_RTGS; //XXX + /* Finished handling commands which have priority , now handle failures */ } else if (status->engine_failure) { status->nav_state = NAVIGATION_STATE_AUTO_LANDENGFAIL; + } else if (status->gps_failure) { + status->nav_state = NAVIGATION_STATE_AUTO_LANDGPSFAIL; } else if (((status->data_link_lost && data_link_loss_enabled) && status->rc_signal_lost) || (!data_link_loss_enabled && status->rc_signal_lost && mission_finished)) { status->failsafe = true; diff --git a/src/modules/navigator/datalinkloss.cpp b/src/modules/navigator/datalinkloss.cpp index b914fdb347..d8a1de2297 100644 --- a/src/modules/navigator/datalinkloss.cpp +++ b/src/modules/navigator/datalinkloss.cpp @@ -32,7 +32,7 @@ ****************************************************************************/ /** * @file datalinkloss.cpp - * Helper class for Data Link Loss Mode acording to the OBC rules + * Helper class for Data Link Loss Mode according to the OBC rules * * @author Thomas Gubler */ diff --git a/src/modules/navigator/gpsfailure.cpp b/src/modules/navigator/gpsfailure.cpp new file mode 100644 index 0000000000..4c526b76ef --- /dev/null +++ b/src/modules/navigator/gpsfailure.cpp @@ -0,0 +1,170 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ +/** + * @file gpsfailure.cpp + * Helper class for gpsfailure mode according to the OBC rules + * + * @author Thomas Gubler + */ + +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include + +#include "navigator.h" +#include "gpsfailure.h" + +#define DELAY_SIGMA 0.01f + +GpsFailure::GpsFailure(Navigator *navigator, const char *name) : + MissionBlock(navigator, name), + _gpsf_state(GPSF_STATE_NONE) +{ + /* load initial params */ + updateParams(); + /* initial reset */ + on_inactive(); +} + +GpsFailure::~GpsFailure() +{ +} + +void +GpsFailure::on_inactive() +{ + /* reset GPSF state only if setpoint moved */ + if (!_navigator->get_can_loiter_at_sp()) { + _gpsf_state = GPSF_STATE_NONE; + } +} + +void +GpsFailure::on_activation() +{ + _gpsf_state = GPSF_STATE_NONE; + updateParams(); + advance_gpsf(); + set_gpsf_item(); +} + +void +GpsFailure::on_active() +{ + if (is_mission_item_reached()) { + updateParams(); + advance_gpsf(); + set_gpsf_item(); + } +} + +void +GpsFailure::set_gpsf_item() +{ + struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); + + set_previous_pos_setpoint(); + _navigator->set_can_loiter_at_sp(false); + + switch (_gpsf_state) { + case GPSF_STATE_LOITER: { + //_mission_item.lat = (double)(_param_commsholdlat.get()) * 1.0e-7; + //_mission_item.lon = (double)(_param_commsholdlon.get()) * 1.0e-7; + //_mission_item.altitude_is_relative = false; + //_mission_item.altitude = _param_commsholdalt.get(); + //_mission_item.yaw = NAN; + //_mission_item.loiter_radius = _navigator->get_loiter_radius(); + //_mission_item.loiter_direction = 1; + //_mission_item.nav_cmd = NAV_CMD_LOITER_TIME_LIMIT; + //_mission_item.acceptance_radius = _navigator->get_acceptance_radius(); + //_mission_item.time_inside = _param_commsholdwaittime.get() < 0.0f ? 0.0f : _param_commsholdwaittime.get(); + //_mission_item.pitch_min = 0.0f; + //_mission_item.autocontinue = true; + //_mission_item.origin = ORIGIN_ONBOARD; + + //_navigator->set_can_loiter_at_sp(true); + break; + } + case GPSF_STATE_TERMINATE: { + //_mission_item.lat = (double)(_param_airfieldhomelat.get()) * 1.0e-7; + //_mission_item.lon = (double)(_param_airfieldhomelon.get()) * 1.0e-7; + //_mission_item.altitude_is_relative = false; + //_mission_item.altitude = _param_airfieldhomealt.get(); + //_mission_item.yaw = NAN; + //_mission_item.loiter_radius = _navigator->get_loiter_radius(); + //_mission_item.loiter_direction = 1; + //_mission_item.nav_cmd = NAV_CMD_LOITER_UNLIMITED; + //_mission_item.acceptance_radius = _navigator->get_acceptance_radius(); + //_mission_item.pitch_min = 0.0f; + //_mission_item.autocontinue = true; + //_mission_item.origin = ORIGIN_ONBOARD; + + //_navigator->set_can_loiter_at_sp(true); + break; + } + default: + break; + } + + reset_mission_item_reached(); + + /* convert mission item to current position setpoint and make it valid */ + mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current); + pos_sp_triplet->next.valid = false; + + _navigator->set_position_setpoint_triplet_updated(); +} + +void +GpsFailure::advance_gpsf() +{ + switch (_gpsf_state) { + case GPSF_STATE_NONE: + _gpsf_state = GPSF_STATE_LOITER; + break; + case GPSF_STATE_LOITER: + _gpsf_state = GPSF_STATE_TERMINATE; + break; + default: + break; + } +} diff --git a/src/modules/navigator/gpsfailure.h b/src/modules/navigator/gpsfailure.h new file mode 100644 index 0000000000..88d386ec45 --- /dev/null +++ b/src/modules/navigator/gpsfailure.h @@ -0,0 +1,86 @@ +/*************************************************************************** + * + * Copyright (c) 2013-2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ +/** + * @file gpsfailure.h + * Helper class for Data Link Loss Mode according to the OBC rules + * + * @author Thomas Gubler + */ + +#ifndef NAVIGATOR_GPSFAILURE_H +#define NAVIGATOR_GPSFAILURE_H + +#include +#include + +#include + +#include "navigator_mode.h" +#include "mission_block.h" + +class Navigator; + +class GpsFailure : public MissionBlock +{ +public: + GpsFailure(Navigator *navigator, const char *name); + + ~GpsFailure(); + + virtual void on_inactive(); + + virtual void on_activation(); + + virtual void on_active(); + +private: + /* Params */ + + enum GPSFState { + GPSF_STATE_NONE = 0, + GPSF_STATE_LOITER = 1, + GPSF_STATE_TERMINATE = 2, + } _gpsf_state; + + /** + * Set the GPSF item + */ + void set_gpsf_item(); + + /** + * Move to next GPSF item + */ + void advance_gpsf(); + +}; +#endif diff --git a/src/modules/navigator/module.mk b/src/modules/navigator/module.mk index 0539087dfd..f6590605b4 100644 --- a/src/modules/navigator/module.mk +++ b/src/modules/navigator/module.mk @@ -52,7 +52,8 @@ SRCS = navigator_main.cpp \ geofence_params.c \ datalinkloss.cpp \ enginefailure.cpp \ - datalinkloss_params.c + datalinkloss_params.c \ + gpsfailure.cpp INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index 59a6752a99..b161c29849 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -61,12 +61,13 @@ #include "offboard.h" #include "datalinkloss.h" #include "enginefailure.h" +#include "gpsfailure.h" #include "geofence.h" /** * Number of navigation modes that need on_active/on_inactive calls */ -#define NAVIGATOR_MODE_ARRAY_SIZE 6 +#define NAVIGATOR_MODE_ARRAY_SIZE 7 class Navigator : public control::SuperBlock { @@ -184,6 +185,7 @@ private: DataLinkLoss _dataLinkLoss; /**< class that handles the OBC datalink loss mode */ EngineFailure _engineFailure; /**< class that handles the engine failure mode (FW only!) */ + GpsFailure _gpsFailure; /**< class that handles the OBC gpsfailure loss mode */ NavigatorMode *_navigation_mode_array[NAVIGATOR_MODE_ARRAY_SIZE]; /**< array of navigation modes */ diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index d77acf74e2..d778257157 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -132,6 +132,7 @@ Navigator::Navigator() : _offboard(this, "OFF"), _dataLinkLoss(this, "DLL"), _engineFailure(this, "EF"), + _gpsFailure(this, "GPSF"), _can_loiter_at_sp(false), _pos_sp_triplet_updated(false), _param_loiter_radius(this, "LOITER_RAD"), @@ -145,6 +146,7 @@ Navigator::Navigator() : _navigation_mode_array[3] = &_offboard; _navigation_mode_array[4] = &_dataLinkLoss; _navigation_mode_array[5] = &_engineFailure; + _navigation_mode_array[6] = &_gpsFailure; updateParams(); } @@ -421,6 +423,9 @@ Navigator::task_main() case NAVIGATION_STATE_AUTO_LANDENGFAIL: _navigation_mode = &_engineFailure; break; + case NAVIGATION_STATE_AUTO_LANDGPSFAIL: + _navigation_mode = &_gpsFailure; + break; case NAVIGATION_STATE_LAND: case NAVIGATION_STATE_TERMINATION: case NAVIGATION_STATE_OFFBOARD: From 2a99ff39dcdb78d97fd645f2c47bf6d36798a1dc Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 21 Aug 2014 15:14:55 +0200 Subject: [PATCH 115/411] Robustify bottle drop --- src/modules/bottle_drop/bottle_drop.cpp | 18 +++++++++++------- 1 file changed, 11 insertions(+), 7 deletions(-) diff --git a/src/modules/bottle_drop/bottle_drop.cpp b/src/modules/bottle_drop/bottle_drop.cpp index 77f691f6c4..35b2266e44 100644 --- a/src/modules/bottle_drop/bottle_drop.cpp +++ b/src/modules/bottle_drop/bottle_drop.cpp @@ -459,7 +459,7 @@ BottleDrop::task_main() continue; } - const unsigned sleeptime_us = 10000; + const unsigned sleeptime_us = 50000; hrt_abstime last_run = hrt_absolute_time(); dt_runs = 1e6f / sleeptime_us; @@ -648,6 +648,7 @@ BottleDrop::task_main() if (isfinite(distance_real) && distance_real < distance_open_door) { open_bay(); _drop_state = DROP_STATE_BAY_OPEN; + mavlink_log_info(_mavlink_fd, "#audio: opening bay"); } } break; @@ -664,10 +665,11 @@ BottleDrop::task_main() warnx("Distance real: %.2f", (double)distance_real); if (isfinite(distance_real) && - (((distance_real < precision) && (distance_real < future_distance)) || + (distance_real < precision) && ((distance_real < future_distance) || (distance_real < precision / 10.0f))) { drop(); _drop_state = DROP_STATE_DROPPED; + mavlink_log_info(_mavlink_fd, "#audio: payload dropped"); } } } @@ -680,6 +682,7 @@ BottleDrop::task_main() _drop_approval = false; lock_release(); close_bay(); + mavlink_log_info(_mavlink_fd, "#audio: closing bay"); } break; } @@ -688,7 +691,7 @@ BottleDrop::task_main() // update_actuators(); - // run at roughly 100 Hz + // run at roughly 20 Hz usleep(sleeptime_us); dt_runs = 1e6f / hrt_elapsed_time(&last_run); @@ -719,12 +722,12 @@ BottleDrop::handle_command(struct vehicle_command_s *cmd) } else if (cmd->param1 > 0.5f) { open_bay(); - mavlink_log_info(_mavlink_fd, "#audio: open doors"); + mavlink_log_info(_mavlink_fd, "#audio: opening bay"); } else { lock_release(); close_bay(); - mavlink_log_info(_mavlink_fd, "#audio: close doors"); + mavlink_log_info(_mavlink_fd, "#audio: closing bay"); } answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); @@ -735,11 +738,12 @@ BottleDrop::handle_command(struct vehicle_command_s *cmd) switch ((int)(cmd->param1 + 0.5f)) { case 0: _drop_approval = false; + mavlink_log_info(_mavlink_fd, "#audio: got drop position, no approval"); break; case 1: _drop_approval = true; - mavlink_log_info(_mavlink_fd, "#audio: prepare deploy approval"); + mavlink_log_info(_mavlink_fd, "#audio: got drop position and approval"); break; default: @@ -768,7 +772,7 @@ BottleDrop::handle_command(struct vehicle_command_s *cmd) case 1: _drop_approval = true; - mavlink_log_info(_mavlink_fd, "#audio: control deploy approval"); + mavlink_log_info(_mavlink_fd, "#audio: got drop approval"); break; default: From 0bdf28f9ba6f0875bfb70ff961fc61e73b813edc Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 21 Aug 2014 15:26:04 +0200 Subject: [PATCH 116/411] audio feedback on distance --- src/modules/bottle_drop/bottle_drop.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/modules/bottle_drop/bottle_drop.cpp b/src/modules/bottle_drop/bottle_drop.cpp index 35b2266e44..a899cbe60a 100644 --- a/src/modules/bottle_drop/bottle_drop.cpp +++ b/src/modules/bottle_drop/bottle_drop.cpp @@ -636,6 +636,10 @@ BottleDrop::task_main() onboard_mission_pub = orb_advertise(ORB_ID(onboard_mission), &onboard_mission); } + if (counter % 90 == 0) { + mavlink_log_info(_mavlink_fd, "#audio: drop distance %.2f", (double)distance_real); + } + // We're close enough - open the bay distance_open_door = math::max(3.0f, fabsf(t_door * groundspeed_body)); From 6051b2112efb584e5f84fb0648d4dddf5ac6c918 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 21 Aug 2014 15:31:06 +0200 Subject: [PATCH 117/411] Moved pos output --- src/modules/bottle_drop/bottle_drop.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/modules/bottle_drop/bottle_drop.cpp b/src/modules/bottle_drop/bottle_drop.cpp index a899cbe60a..6a501d97c9 100644 --- a/src/modules/bottle_drop/bottle_drop.cpp +++ b/src/modules/bottle_drop/bottle_drop.cpp @@ -497,6 +497,10 @@ BottleDrop::task_main() float groundspeed_body = sqrtf(_global_pos.vel_n * _global_pos.vel_n + _global_pos.vel_e * _global_pos.vel_e); distance_real = fabsf(get_distance_to_next_waypoint(_global_pos.lat, _global_pos.lon, _drop_position.lat, _drop_position.lon)); + if (counter % 90 == 0) { + mavlink_log_info(_mavlink_fd, "#audio: drop distance %.2f", (double)distance_real); + } + //warnx("absolut wind speed = %.4f", vr); //////////////////////////////////////////////////////////////////// DEBUGGING @@ -636,10 +640,6 @@ BottleDrop::task_main() onboard_mission_pub = orb_advertise(ORB_ID(onboard_mission), &onboard_mission); } - if (counter % 90 == 0) { - mavlink_log_info(_mavlink_fd, "#audio: drop distance %.2f", (double)distance_real); - } - // We're close enough - open the bay distance_open_door = math::max(3.0f, fabsf(t_door * groundspeed_body)); From 1026805422495e9b6d5351a38f2a2ca3ce70a352 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 21 Aug 2014 15:41:03 +0200 Subject: [PATCH 118/411] removed unnecessary switch statement --- src/modules/bottle_drop/bottle_drop.cpp | 4 ---- 1 file changed, 4 deletions(-) diff --git a/src/modules/bottle_drop/bottle_drop.cpp b/src/modules/bottle_drop/bottle_drop.cpp index 6a501d97c9..dbf94cb3d8 100644 --- a/src/modules/bottle_drop/bottle_drop.cpp +++ b/src/modules/bottle_drop/bottle_drop.cpp @@ -508,10 +508,6 @@ BottleDrop::task_main() switch (_drop_state) { - case DROP_STATE_INIT: - // exit inner loop, wait for new drop mission - break; - case DROP_STATE_TARGET_VALID: { //warnx("approval given\n"); //////////////////////////////////////////////////////////////////// DEBUGGING From 7c7dce3b440a7b6deb8f0f9340c4ba6b21daa800 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 21 Aug 2014 15:46:58 +0200 Subject: [PATCH 119/411] Open doors sooner for drop --- src/modules/bottle_drop/bottle_drop.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/bottle_drop/bottle_drop.cpp b/src/modules/bottle_drop/bottle_drop.cpp index dbf94cb3d8..bc182b5017 100644 --- a/src/modules/bottle_drop/bottle_drop.cpp +++ b/src/modules/bottle_drop/bottle_drop.cpp @@ -637,7 +637,7 @@ BottleDrop::task_main() } // We're close enough - open the bay - distance_open_door = math::max(3.0f, fabsf(t_door * groundspeed_body)); + distance_open_door = math::max(3.0f, 3.0f * fabsf(t_door * groundspeed_body)); if (counter % 10 == 0) warnx("dist real: %.2f, distance_open_door: %.2f, angle to wind: %.2f", From 8e7722f8948d3f8f21a5a661e9cdcc6097472fd9 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 21 Aug 2014 15:47:37 +0200 Subject: [PATCH 120/411] Open bay at a minimum distance of 5 meters, irrelevant of the velocity --- src/modules/bottle_drop/bottle_drop.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/bottle_drop/bottle_drop.cpp b/src/modules/bottle_drop/bottle_drop.cpp index bc182b5017..5d2343cfbe 100644 --- a/src/modules/bottle_drop/bottle_drop.cpp +++ b/src/modules/bottle_drop/bottle_drop.cpp @@ -637,7 +637,7 @@ BottleDrop::task_main() } // We're close enough - open the bay - distance_open_door = math::max(3.0f, 3.0f * fabsf(t_door * groundspeed_body)); + distance_open_door = math::max(5.0f, 3.0f * fabsf(t_door * groundspeed_body)); if (counter % 10 == 0) warnx("dist real: %.2f, distance_open_door: %.2f, angle to wind: %.2f", From 1e7dee439111f58fa9c0f737b93132a57dbf644d Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 21 Aug 2014 16:24:00 +0200 Subject: [PATCH 121/411] set_attitude_target msg: convert quaternion to R --- src/modules/mavlink/mavlink_receiver.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index da594412e7..60fc2937a9 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -689,9 +689,11 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg) att_sp.timestamp = hrt_absolute_time(); mavlink_quaternion_to_euler(set_attitude_target.q, &att_sp.roll_body, &att_sp.pitch_body, &att_sp.yaw_body); + mavlink_quaternion_to_dcm(set_attitude_target.q, att_sp.R_body); + att_sp.R_valid = true; att_sp.thrust = set_attitude_target.thrust; - att_sp.q_d_valid = true; memcpy(att_sp.q_d, set_attitude_target.q, sizeof(att_sp.q_d)); + att_sp.q_d_valid = true; if (_att_sp_pub < 0) { _att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp); } else { From 7187e29b5205ba6c5592c1b9e92ac5d28bb2539a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 21 Aug 2014 20:58:24 +0200 Subject: [PATCH 122/411] Fix altitude handling --- src/modules/bottle_drop/bottle_drop.cpp | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/src/modules/bottle_drop/bottle_drop.cpp b/src/modules/bottle_drop/bottle_drop.cpp index 5d2343cfbe..30c9ce8878 100644 --- a/src/modules/bottle_drop/bottle_drop.cpp +++ b/src/modules/bottle_drop/bottle_drop.cpp @@ -176,7 +176,7 @@ BottleDrop::BottleDrop() : _drop_approval(false), _doors_opened(0), _drop_time(0), - _alt_clearance(0), + _alt_clearance(70.0f), _target_position {}, _drop_position {}, _drop_state(DROP_STATE_INIT) @@ -340,8 +340,7 @@ BottleDrop::task_main() float turn_radius; // turn radius of the UAV float precision; // Expected precision of the UAV - // XXX we do not measure the exact ground altitude yet, but eventually we have to - float ground_distance = 70.0f; + float ground_distance = _alt_clearance; // Replace by closer estimate in loop // constant float g = CONSTANTS_ONE_G; // constant of gravity [m/s^2] @@ -349,7 +348,6 @@ BottleDrop::task_main() float rho = 1.2f; // air density [kg/m^3] float A = ((0.063f * 0.063f) / 4.0f * M_PI_F); // Bottle cross section [m^2] float dt_freefall_prediction = 0.01f; // step size of the free fall prediction [s] - float dt_runs = 0.05f; // step size 2 [s] // Has to be estimated by experiment float cd = 0.86f; // Drag coefficient for a cylinder with a d/l ratio of 1/3 [] @@ -462,7 +460,7 @@ BottleDrop::task_main() const unsigned sleeptime_us = 50000; hrt_abstime last_run = hrt_absolute_time(); - dt_runs = 1e6f / sleeptime_us; + float dt_runs = 1e6f / sleeptime_us; /* switch to faster updates during the drop */ while (_drop_state > DROP_STATE_INIT) { @@ -496,6 +494,7 @@ BottleDrop::task_main() float windspeed_norm = sqrtf(wind.windspeed_north * wind.windspeed_north + wind.windspeed_east * wind.windspeed_east); float groundspeed_body = sqrtf(_global_pos.vel_n * _global_pos.vel_n + _global_pos.vel_e * _global_pos.vel_e); distance_real = fabsf(get_distance_to_next_waypoint(_global_pos.lat, _global_pos.lon, _drop_position.lat, _drop_position.lon)); + ground_distance = _global_pos.alt - _target_position.alt; if (counter % 90 == 0) { mavlink_log_info(_mavlink_fd, "#audio: drop distance %.2f", (double)distance_real); @@ -550,7 +549,7 @@ BottleDrop::task_main() //Drag force v = sqrtf(vz * vz + vrx * vrx); - Fd = 0.5f * rho * A * cd * powf(v, 2.0f); + Fd = 0.5f * rho * A * cd * (v * v); Fdx = Fd * vrx / v; Fdz = Fd * vz / v; @@ -580,6 +579,7 @@ BottleDrop::task_main() x_drop = x_t + x * wind_direction_n; y_drop = y_t + x * wind_direction_e; map_projection_reproject(&ref, x_drop, y_drop, &_drop_position.lat, &_drop_position.lon); + _drop_position.alt = _target_position.alt + _alt_clearance; //warnx("drop point: lat = %.7f , lon = %.7f", drop_position.lat, drop_position.lon); //////////////////////////////////////////////////////////////////// DEBUGGING @@ -587,10 +587,10 @@ BottleDrop::task_main() // Compute flight vector map_projection_reproject(&ref, x_drop + 2 * turn_radius * wind_direction_n, y_drop + 2 * turn_radius * wind_direction_n, &(flight_vector_s.lat), &(flight_vector_s.lon)); - flight_vector_s.altitude = _drop_position.alt + _alt_clearance; + flight_vector_s.altitude = _drop_position.alt; map_projection_reproject(&ref, x_drop - turn_radius * wind_direction_e, y_drop - turn_radius * wind_direction_e, &flight_vector_e.lat, &flight_vector_e.lon); - flight_vector_e.altitude = _drop_position.alt + _alt_clearance; + flight_vector_e.altitude = _drop_position.alt; //warnx("Flight vector: starting point = %.7f %.7f , end point = %.7f %.7f", flight_vector_s.lat,flight_vector_s.lon,flight_vector_e.lat,flight_vector_e.lon); //////////////////////////////////////////////////////////////////// DEBUGGING // Drop Cancellation if terms are not met From e6a8eebd0a86c0dd7b8b23882772ccd3ba962ddc Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 21 Aug 2014 21:43:53 +0200 Subject: [PATCH 123/411] Fixed approach / navigation logic for bottle drop --- src/modules/bottle_drop/bottle_drop.cpp | 93 ++++++++++++++++--------- 1 file changed, 60 insertions(+), 33 deletions(-) diff --git a/src/modules/bottle_drop/bottle_drop.cpp b/src/modules/bottle_drop/bottle_drop.cpp index 30c9ce8878..ae97cc5707 100644 --- a/src/modules/bottle_drop/bottle_drop.cpp +++ b/src/modules/bottle_drop/bottle_drop.cpp @@ -134,6 +134,7 @@ private: enum DROP_STATE { DROP_STATE_INIT = 0, DROP_STATE_TARGET_VALID, + DROP_STATE_TARGET_SET, DROP_STATE_BAY_OPEN, DROP_STATE_DROPPED, DROP_STATE_BAY_CLOSED @@ -381,10 +382,6 @@ BottleDrop::task_main() float distance_real = 0; // The distance between the UAVs position and the drop point [m] float future_distance = 0; // The distance between the UAVs to-be position and the drop point [m] - // states - bool state_drop = false; // Drop occurred = true, Drop din't occur = false - bool state_run = false; // A drop was attempted = true, the drop is still in progress = false - unsigned counter = 0; param_t param_gproperties = param_find("BD_GPROPERTIES"); @@ -621,13 +618,7 @@ BottleDrop::task_main() } onboard_mission.count = 2; - - if (state_run && !state_drop) { - onboard_mission.current_seq = 0; - - } else { - onboard_mission.current_seq = -1; - } + onboard_mission.current_seq = 0; if (onboard_mission_pub > 0) { orb_publish(ORB_ID(onboard_mission), onboard_mission_pub, &onboard_mission); @@ -636,19 +627,27 @@ BottleDrop::task_main() onboard_mission_pub = orb_advertise(ORB_ID(onboard_mission), &onboard_mission); } - // We're close enough - open the bay - distance_open_door = math::max(5.0f, 3.0f * fabsf(t_door * groundspeed_body)); + _drop_state = DROP_STATE_TARGET_SET; + } + break; - if (counter % 10 == 0) - warnx("dist real: %.2f, distance_open_door: %.2f, angle to wind: %.2f", - (double)distance_real, - (double)distance_open_door, - (double)(_wrap_pi(_global_pos.yaw - atan2f(wind.windspeed_north, wind.windspeed_east)))); + case DROP_STATE_TARGET_SET - if (isfinite(distance_real) && distance_real < distance_open_door) { - open_bay(); - _drop_state = DROP_STATE_BAY_OPEN; - mavlink_log_info(_mavlink_fd, "#audio: opening bay"); + float distance_wp2 = get_distance_to_next_waypoint(flight_vector_e.lat, flight_vector_e.lon, _drop_position.lat, _drop_position.lon); + + if (distance_wp2 < distance_real) { + onboard_mission.current_seq = 0; + orb_publish(ORB_ID(onboard_mission), onboard_mission_pub, &onboard_mission); + } else { + + // We're close enough - open the bay + distance_open_door = math::max(5.0f, 3.0f * fabsf(t_door * groundspeed_body)); + + if (isfinite(distance_real) && distance_real < distance_open_door) { + open_bay(); + _drop_state = DROP_STATE_BAY_OPEN; + mavlink_log_info(_mavlink_fd, "#audio: opening bay"); + } } } break; @@ -670,6 +669,14 @@ BottleDrop::task_main() drop(); _drop_state = DROP_STATE_DROPPED; mavlink_log_info(_mavlink_fd, "#audio: payload dropped"); + } else { + + float distance_wp2 = get_distance_to_next_waypoint(flight_vector_e.lat, flight_vector_e.lon, _drop_position.lat, _drop_position.lon); + + if (distance_wp2 < distance_real) { + onboard_mission.current_seq = 0; + orb_publish(ORB_ID(onboard_mission), onboard_mission_pub, &onboard_mission); + } } } } @@ -683,6 +690,10 @@ BottleDrop::task_main() lock_release(); close_bay(); mavlink_log_info(_mavlink_fd, "#audio: closing bay"); + + // remove onboard mission + onboard_mission.current_seq = -1; + orb_publish(ORB_ID(onboard_mission), onboard_mission_pub, &onboard_mission); } break; } @@ -765,20 +776,36 @@ BottleDrop::handle_command(struct vehicle_command_s *cmd) break; case VEHICLE_CMD_PAYLOAD_CONTROL_DEPLOY: - switch ((int)(cmd->param1 + 0.5f)) { - case 0: - _drop_approval = false; - break; - case 1: - _drop_approval = true; - mavlink_log_info(_mavlink_fd, "#audio: got drop approval"); - break; + if (cmd->param1 < 0) { - default: + // Clear internal states _drop_approval = false; - break; - // XXX handle other values + _drop_state = DROP_STATE_INIT; + + // Abort if mission is present + onboard_mission.current_seq = -1; + + if (onboard_mission_pub > 0) { + orb_publish(ORB_ID(onboard_mission), onboard_mission_pub, &onboard_mission); + } + + } else { + switch ((int)(cmd->param1 + 0.5f)) { + case 0: + _drop_approval = false; + break; + + case 1: + _drop_approval = true; + mavlink_log_info(_mavlink_fd, "#audio: got drop approval"); + break; + + default: + _drop_approval = false; + break; + // XXX handle other values + } } answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); From 1a14ff250e5a2ead69576762fd5b7f176c4b6fac Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 22 Aug 2014 00:39:44 +0200 Subject: [PATCH 124/411] fw att control: use RC only if in manual --- src/modules/fw_att_control/fw_att_control_main.cpp | 9 ++++++++- 1 file changed, 8 insertions(+), 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 0cea13cc4d..ee112a40e7 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -717,7 +717,14 @@ FixedwingAttitudeControl::task_main() float pitch_sp = _parameters.pitchsp_offset_rad; float throttle_sp = 0.0f; - if (_vcontrol_mode.flag_control_velocity_enabled || _vcontrol_mode.flag_control_position_enabled) { + /* Read attitude setpoint from uorb if + * - velocity control or position control is enabled (pos controller is running) + * - manual control is disabled (another app may send the setpoint, but it should + * for sure not be set from the remote control values) + */ + if (_vcontrol_mode.flag_control_velocity_enabled || + _vcontrol_mode.flag_control_position_enabled || + !_vcontrol_mode.flag_control_manual_enabled) { /* read in attitude setpoint from attitude setpoint uorb topic */ roll_sp = _att_sp.roll_body + _parameters.rollsp_offset_rad; pitch_sp = _att_sp.pitch_body + _parameters.pitchsp_offset_rad; From 752a0a562564ccc6f7d49ceebe810de7e6a6d358 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 22 Aug 2014 00:40:45 +0200 Subject: [PATCH 125/411] add obc gps failure mode --- src/modules/commander/commander.cpp | 18 +++- src/modules/navigator/gpsfailure.cpp | 95 +++++++++--------- src/modules/navigator/gpsfailure.h | 13 ++- src/modules/navigator/gpsfailure_params.c | 97 +++++++++++++++++++ src/modules/navigator/module.mk | 3 +- src/modules/navigator/navigator.h | 12 +++ src/modules/navigator/navigator_main.cpp | 16 +++ .../uORB/topics/position_setpoint_triplet.h | 1 + 8 files changed, 207 insertions(+), 48 deletions(-) create mode 100644 src/modules/navigator/gpsfailure_params.c diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 84a4be948f..109ab403ac 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1311,11 +1311,12 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(position_setpoint_triplet), pos_sp_triplet_sub, &pos_sp_triplet); /* Check for geofence violation */ - if (pos_sp_triplet.geofence_violated) { + if (pos_sp_triplet.geofence_violated || pos_sp_triplet.flight_termination) { //XXX: make this configurable to select different actions (e.g. navigation modes) /* this will only trigger if geofence is activated via param and a geofence file is present, also there is a circuit breaker to disable the actual flight termination in the px4io driver */ - armed.force_failsafe = true; + armed.fosrce_failsafe = true; status_changed = true; + warnx("Flight termination"); } // no reset is done here on purpose, on geofence violation we want to stay in flighttermination } @@ -2060,6 +2061,7 @@ set_control_mode() case NAVIGATION_STATE_AUTO_LOITER: case NAVIGATION_STATE_AUTO_RTL: case NAVIGATION_STATE_AUTO_RTGS: + case NAVIGATION_STATE_AUTO_LANDENGFAIL: control_mode.flag_control_manual_enabled = false; control_mode.flag_control_auto_enabled = true; control_mode.flag_control_rates_enabled = true; @@ -2071,6 +2073,18 @@ set_control_mode() control_mode.flag_control_termination_enabled = false; break; + case NAVIGATION_STATE_AUTO_LANDGPSFAIL: + control_mode.flag_control_manual_enabled = false; + 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 = false; + control_mode.flag_control_climb_rate_enabled = true; + control_mode.flag_control_position_enabled = false; + control_mode.flag_control_velocity_enabled = false; + control_mode.flag_control_termination_enabled = false; + break; + case NAVIGATION_STATE_LAND: control_mode.flag_control_manual_enabled = false; control_mode.flag_control_auto_enabled = true; diff --git a/src/modules/navigator/gpsfailure.cpp b/src/modules/navigator/gpsfailure.cpp index 4c526b76ef..02e766ffba 100644 --- a/src/modules/navigator/gpsfailure.cpp +++ b/src/modules/navigator/gpsfailure.cpp @@ -57,7 +57,12 @@ GpsFailure::GpsFailure(Navigator *navigator, const char *name) : MissionBlock(navigator, name), - _gpsf_state(GPSF_STATE_NONE) + _param_loitertime(this, "LT"), + _param_openlooploiter_roll(this, "R"), + _param_openlooploiter_pitch(this, "P"), + _param_openlooploiter_thrust(this, "TR"), + _gpsf_state(GPSF_STATE_NONE), + _timestamp_activation(0) { /* load initial params */ updateParams(); @@ -82,6 +87,7 @@ void GpsFailure::on_activation() { _gpsf_state = GPSF_STATE_NONE; + _timestamp_activation = hrt_absolute_time(); updateParams(); advance_gpsf(); set_gpsf_item(); @@ -90,10 +96,34 @@ GpsFailure::on_activation() void GpsFailure::on_active() { - if (is_mission_item_reached()) { - updateParams(); - advance_gpsf(); + + switch (_gpsf_state) { + case GPSF_STATE_LOITER: { + /* Position controller does not run in this mode: + * navigator has to publish an attitude setpoint */ + _navigator->get_att_sp()->roll_body = M_DEG_TO_RAD_F * _param_openlooploiter_roll.get(); + _navigator->get_att_sp()->pitch_body = M_DEG_TO_RAD_F * _param_openlooploiter_pitch.get(); + _navigator->get_att_sp()->thrust = _param_openlooploiter_thrust.get(); + _navigator->publish_att_sp(); + + /* Measure time */ + hrt_abstime elapsed = hrt_elapsed_time(&_timestamp_activation); + + //warnx("open loop loiter, posctl enabled %u, elapsed %.1fs, thrust %.2f", + //_navigator->get_control_mode()->flag_control_position_enabled, elapsed * 1e-6, (double)_param_openlooploiter_thrust.get()); + if (elapsed > _param_loitertime.get() * 1e6f) { + /* no recovery, adavance the state machine */ + warnx("gps not recovered, switch to next state"); + advance_gpsf(); + } + break; + } + case GPSF_STATE_TERMINATE: set_gpsf_item(); + advance_gpsf(); + break; + default: + break; } } @@ -102,68 +132,45 @@ GpsFailure::set_gpsf_item() { struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); - set_previous_pos_setpoint(); - _navigator->set_can_loiter_at_sp(false); + /* Set pos sp triplet to invalid to stop pos controller */ + pos_sp_triplet->previous.valid = false; + pos_sp_triplet->current.valid = false; + pos_sp_triplet->next.valid = false; switch (_gpsf_state) { - case GPSF_STATE_LOITER: { - //_mission_item.lat = (double)(_param_commsholdlat.get()) * 1.0e-7; - //_mission_item.lon = (double)(_param_commsholdlon.get()) * 1.0e-7; - //_mission_item.altitude_is_relative = false; - //_mission_item.altitude = _param_commsholdalt.get(); - //_mission_item.yaw = NAN; - //_mission_item.loiter_radius = _navigator->get_loiter_radius(); - //_mission_item.loiter_direction = 1; - //_mission_item.nav_cmd = NAV_CMD_LOITER_TIME_LIMIT; - //_mission_item.acceptance_radius = _navigator->get_acceptance_radius(); - //_mission_item.time_inside = _param_commsholdwaittime.get() < 0.0f ? 0.0f : _param_commsholdwaittime.get(); - //_mission_item.pitch_min = 0.0f; - //_mission_item.autocontinue = true; - //_mission_item.origin = ORIGIN_ONBOARD; - - //_navigator->set_can_loiter_at_sp(true); - break; - } case GPSF_STATE_TERMINATE: { - //_mission_item.lat = (double)(_param_airfieldhomelat.get()) * 1.0e-7; - //_mission_item.lon = (double)(_param_airfieldhomelon.get()) * 1.0e-7; - //_mission_item.altitude_is_relative = false; - //_mission_item.altitude = _param_airfieldhomealt.get(); - //_mission_item.yaw = NAN; - //_mission_item.loiter_radius = _navigator->get_loiter_radius(); - //_mission_item.loiter_direction = 1; - //_mission_item.nav_cmd = NAV_CMD_LOITER_UNLIMITED; - //_mission_item.acceptance_radius = _navigator->get_acceptance_radius(); - //_mission_item.pitch_min = 0.0f; - //_mission_item.autocontinue = true; - //_mission_item.origin = ORIGIN_ONBOARD; - - //_navigator->set_can_loiter_at_sp(true); - break; + /* Request flight termination from the commander */ + pos_sp_triplet->flight_termination = true; + warnx("request flight termination"); } default: break; } reset_mission_item_reached(); - - /* convert mission item to current position setpoint and make it valid */ - mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current); - pos_sp_triplet->next.valid = false; - _navigator->set_position_setpoint_triplet_updated(); } void GpsFailure::advance_gpsf() { + updateParams(); + switch (_gpsf_state) { case GPSF_STATE_NONE: _gpsf_state = GPSF_STATE_LOITER; + warnx("gpsf loiter"); + mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: open loop loiter"); break; case GPSF_STATE_LOITER: _gpsf_state = GPSF_STATE_TERMINATE; + warnx("gpsf terminate"); + mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: no gps recovery, termination"); + warnx("mavlink sent"); break; + case GPSF_STATE_TERMINATE: + warnx("gpsf end"); + _gpsf_state = GPSF_STATE_END; default: break; } diff --git a/src/modules/navigator/gpsfailure.h b/src/modules/navigator/gpsfailure.h index 88d386ec45..e9e72babf8 100644 --- a/src/modules/navigator/gpsfailure.h +++ b/src/modules/navigator/gpsfailure.h @@ -43,7 +43,11 @@ #include #include -#include +#include +#include +#include + +#include #include "navigator_mode.h" #include "mission_block.h" @@ -65,13 +69,20 @@ public: private: /* Params */ + control::BlockParamFloat _param_loitertime; + control::BlockParamFloat _param_openlooploiter_roll; + control::BlockParamFloat _param_openlooploiter_pitch; + control::BlockParamFloat _param_openlooploiter_thrust; enum GPSFState { GPSF_STATE_NONE = 0, GPSF_STATE_LOITER = 1, GPSF_STATE_TERMINATE = 2, + GPSF_STATE_END = 3, } _gpsf_state; + hrt_abstime _timestamp_activation; //*< timestamp when this mode was activated */ + /** * Set the GPSF item */ diff --git a/src/modules/navigator/gpsfailure_params.c b/src/modules/navigator/gpsfailure_params.c new file mode 100644 index 0000000000..39d179eed7 --- /dev/null +++ b/src/modules/navigator/gpsfailure_params.c @@ -0,0 +1,97 @@ +/**************************************************************************** + * + * Copyright (c) 2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * AS IS AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file gpsfailure_params.c + * + * Parameters for GPSF navigation mode + * + * @author Thomas Gubler + */ + +#include + +#include + +/* + * GPS Failure Navigation Mode parameters, accessible via MAVLink + */ + +/** + * Loiter time + * + * The amount of time in seconds the system should do open loop loiter and wait for gps recovery + * before it goes into flight termination. + * + * @unit seconds + * @min 0.0 + * @group GPSF + */ +PARAM_DEFINE_FLOAT(NAV_GPSF_LT, 30.0f); + +/** + * Open loop loiter roll + * + * Roll in degrees during the open loop loiter + * + * @unit deg + * @min 0.0 + * @max 30.0 + * @group GPSF + */ +PARAM_DEFINE_FLOAT(NAV_GPSF_R, 15.0f); + +/** + * Open loop loiter pitch + * + * Pitch in degrees during the open loop loiter + * + * @unit deg + * @min -30.0 + * @max 30.0 + * @group GPSF + */ +PARAM_DEFINE_FLOAT(NAV_GPSF_P, 0.0f); + +/** + * Open loop loiter thrust + * + * Thrust value which is set during the open loop loiter + * + * @min 0.0 + * @max 1.0 + * @group GPSF + */ +PARAM_DEFINE_FLOAT(NAV_GPSF_TR, 0.7f); + + diff --git a/src/modules/navigator/module.mk b/src/modules/navigator/module.mk index f6590605b4..9e4ad053c9 100644 --- a/src/modules/navigator/module.mk +++ b/src/modules/navigator/module.mk @@ -53,7 +53,8 @@ SRCS = navigator_main.cpp \ datalinkloss.cpp \ enginefailure.cpp \ datalinkloss_params.c \ - gpsfailure.cpp + gpsfailure.cpp \ + gpsfailure_params.c INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index b161c29849..ec6e538e34 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -53,6 +53,7 @@ #include #include #include +#include #include "navigator_mode.h" #include "mission.h" @@ -108,6 +109,12 @@ public: * Publish the mission result so commander and mavlink know what is going on */ void publish_mission_result(); + + /** + * Publish the attitude sp, only to be used in very special modes when position control is deactivated + * Example: mode that is triggered on gps failure + */ + void publish_att_sp(); /** * Setters @@ -125,6 +132,7 @@ public: struct home_position_s* get_home_position() { return &_home_pos; } struct position_setpoint_triplet_s* get_position_setpoint_triplet() { return &_pos_sp_triplet; } struct mission_result_s* get_mission_result() { return &_mission_result; } + struct vehicle_attitude_setpoint_s* get_att_sp() { return &_att_sp; } int get_onboard_mission_sub() { return _onboard_mission_sub; } int get_offboard_mission_sub() { return _offboard_mission_sub; } @@ -155,6 +163,9 @@ private: orb_advert_t _pos_sp_triplet_pub; /**< publish position setpoint triplet */ orb_advert_t _mission_result_pub; + orb_advert_t _att_sp_pub; /**< publish att sp + used only in very special failsafe modes + when pos control is deactivated */ vehicle_status_s _vstatus; /**< vehicle status */ vehicle_control_mode_s _control_mode; /**< vehicle control mode */ @@ -166,6 +177,7 @@ private: position_setpoint_triplet_s _pos_sp_triplet; /**< triplet of position setpoints */ mission_result_s _mission_result; + vehicle_attitude_setpoint_s _att_sp; bool _mission_item_valid; /**< flags if the current mission item is valid */ diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index bc9951bf2e..e0913bb575 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -110,6 +110,7 @@ Navigator::Navigator() : _param_update_sub(-1), _pos_sp_triplet_pub(-1), _mission_result_pub(-1), + _att_sp_pub(-1), _vstatus{}, _control_mode{}, _global_pos{}, @@ -119,6 +120,7 @@ Navigator::Navigator() : _nav_caps{}, _pos_sp_triplet{}, _mission_result{}, + _att_sp{}, _mission_item_valid(false), _loop_perf(perf_alloc(PC_ELAPSED, "navigator")), _geofence{}, @@ -609,3 +611,17 @@ Navigator::publish_mission_result() _mission_result.reached = false; _mission_result.finished = false; } + +void +Navigator::publish_att_sp() +{ + /* lazily publish the attitude sp only once available */ + if (_att_sp_pub > 0) { + /* publish att sp*/ + orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &_att_sp); + + } else { + /* advertise and publish */ + _att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_att_sp); + } +} diff --git a/src/modules/uORB/topics/position_setpoint_triplet.h b/src/modules/uORB/topics/position_setpoint_triplet.h index 1c78f53305..e2b1525a23 100644 --- a/src/modules/uORB/topics/position_setpoint_triplet.h +++ b/src/modules/uORB/topics/position_setpoint_triplet.h @@ -98,6 +98,7 @@ struct position_setpoint_triplet_s unsigned nav_state; /**< report the navigation state */ bool geofence_violated; /**< true if the geofence is violated */ + bool flight_termination; /**< true if the navigator demands a flight termination from the commander app */ }; /** From 04ad990254e51ceb68c91e7ef601f1e281fd7062 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 22 Aug 2014 07:08:30 +0200 Subject: [PATCH 126/411] Fixed build issues --- src/modules/bottle_drop/bottle_drop.cpp | 43 +++++++++++++------------ 1 file changed, 22 insertions(+), 21 deletions(-) diff --git a/src/modules/bottle_drop/bottle_drop.cpp b/src/modules/bottle_drop/bottle_drop.cpp index ae97cc5707..6c3bdb0026 100644 --- a/src/modules/bottle_drop/bottle_drop.cpp +++ b/src/modules/bottle_drop/bottle_drop.cpp @@ -140,6 +140,9 @@ private: DROP_STATE_BAY_CLOSED } _drop_state; + struct mission_s _onboard_mission; + orb_advert_t _onboard_mission_pub; + void task_main(); void handle_command(struct vehicle_command_s *cmd); @@ -180,7 +183,9 @@ BottleDrop::BottleDrop() : _alt_clearance(70.0f), _target_position {}, _drop_position {}, - _drop_state(DROP_STATE_INIT) + _drop_state(DROP_STATE_INIT), + _onboard_mission {}, + _onboard_mission_pub(-1) { } @@ -410,10 +415,6 @@ BottleDrop::task_main() flight_vector_e.acceptance_radius = 50; // TODO: make parameter flight_vector_e.autocontinue = true; - struct mission_s onboard_mission; - memset(&onboard_mission, 0, sizeof(onboard_mission)); - orb_advert_t onboard_mission_pub = -1; - struct wind_estimate_s wind; /* wakeup source(s) */ @@ -617,27 +618,27 @@ BottleDrop::task_main() warnx("ERROR: could not save onboard WP"); } - onboard_mission.count = 2; - onboard_mission.current_seq = 0; + _onboard_mission.count = 2; + _onboard_mission.current_seq = 0; - if (onboard_mission_pub > 0) { - orb_publish(ORB_ID(onboard_mission), onboard_mission_pub, &onboard_mission); + if (_onboard_mission_pub > 0) { + orb_publish(ORB_ID(onboard_mission), _onboard_mission_pub, &_onboard_mission); } else { - onboard_mission_pub = orb_advertise(ORB_ID(onboard_mission), &onboard_mission); + _onboard_mission_pub = orb_advertise(ORB_ID(onboard_mission), &_onboard_mission); } _drop_state = DROP_STATE_TARGET_SET; } break; - case DROP_STATE_TARGET_SET - + case DROP_STATE_TARGET_SET: + { float distance_wp2 = get_distance_to_next_waypoint(flight_vector_e.lat, flight_vector_e.lon, _drop_position.lat, _drop_position.lon); if (distance_wp2 < distance_real) { - onboard_mission.current_seq = 0; - orb_publish(ORB_ID(onboard_mission), onboard_mission_pub, &onboard_mission); + _onboard_mission.current_seq = 0; + orb_publish(ORB_ID(onboard_mission), _onboard_mission_pub, &_onboard_mission); } else { // We're close enough - open the bay @@ -674,8 +675,8 @@ BottleDrop::task_main() float distance_wp2 = get_distance_to_next_waypoint(flight_vector_e.lat, flight_vector_e.lon, _drop_position.lat, _drop_position.lon); if (distance_wp2 < distance_real) { - onboard_mission.current_seq = 0; - orb_publish(ORB_ID(onboard_mission), onboard_mission_pub, &onboard_mission); + _onboard_mission.current_seq = 0; + orb_publish(ORB_ID(onboard_mission), _onboard_mission_pub, &_onboard_mission); } } } @@ -692,8 +693,8 @@ BottleDrop::task_main() mavlink_log_info(_mavlink_fd, "#audio: closing bay"); // remove onboard mission - onboard_mission.current_seq = -1; - orb_publish(ORB_ID(onboard_mission), onboard_mission_pub, &onboard_mission); + _onboard_mission.current_seq = -1; + orb_publish(ORB_ID(onboard_mission), _onboard_mission_pub, &_onboard_mission); } break; } @@ -784,10 +785,10 @@ BottleDrop::handle_command(struct vehicle_command_s *cmd) _drop_state = DROP_STATE_INIT; // Abort if mission is present - onboard_mission.current_seq = -1; + _onboard_mission.current_seq = -1; - if (onboard_mission_pub > 0) { - orb_publish(ORB_ID(onboard_mission), onboard_mission_pub, &onboard_mission); + if (_onboard_mission_pub > 0) { + orb_publish(ORB_ID(onboard_mission), _onboard_mission_pub, &_onboard_mission); } } else { From 0f7c3926143a917b3e444f0f2c6c2cb673f073e4 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 22 Aug 2014 14:30:34 +0200 Subject: [PATCH 127/411] Initialize altitude is relative field for drop locations --- src/modules/bottle_drop/bottle_drop.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/modules/bottle_drop/bottle_drop.cpp b/src/modules/bottle_drop/bottle_drop.cpp index 5d2343cfbe..8e96c3e093 100644 --- a/src/modules/bottle_drop/bottle_drop.cpp +++ b/src/modules/bottle_drop/bottle_drop.cpp @@ -404,16 +404,18 @@ BottleDrop::task_main() memset(&update, 0, sizeof(update)); int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update)); - struct mission_item_s flight_vector_s; - struct mission_item_s flight_vector_e; + struct mission_item_s flight_vector_s {}; + struct mission_item_s flight_vector_e {}; flight_vector_s.nav_cmd = NAV_CMD_WAYPOINT; flight_vector_s.acceptance_radius = 50; // TODO: make parameter flight_vector_s.autocontinue = true; + flight_vector_s.altitude_is_relative = false; flight_vector_e.nav_cmd = NAV_CMD_WAYPOINT; flight_vector_e.acceptance_radius = 50; // TODO: make parameter flight_vector_e.autocontinue = true; + flight_vector_s.altitude_is_relative = false; struct mission_s onboard_mission; memset(&onboard_mission, 0, sizeof(onboard_mission)); From bdd4f028eec79f09335f888b4857db44968aa07d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 22 Aug 2014 14:36:31 +0200 Subject: [PATCH 128/411] Fixed copy-paste errors in drop waypoint coordinates --- src/modules/bottle_drop/bottle_drop.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/bottle_drop/bottle_drop.cpp b/src/modules/bottle_drop/bottle_drop.cpp index 058e908292..5b7f7bd7ff 100644 --- a/src/modules/bottle_drop/bottle_drop.cpp +++ b/src/modules/bottle_drop/bottle_drop.cpp @@ -587,10 +587,10 @@ BottleDrop::task_main() // Compute flight vector - map_projection_reproject(&ref, x_drop + 2 * turn_radius * wind_direction_n, y_drop + 2 * turn_radius * wind_direction_n, + map_projection_reproject(&ref, x_drop + 2 * turn_radius * wind_direction_n, y_drop + 2 * turn_radius * wind_direction_e, &(flight_vector_s.lat), &(flight_vector_s.lon)); flight_vector_s.altitude = _drop_position.alt; - map_projection_reproject(&ref, x_drop - turn_radius * wind_direction_e, y_drop - turn_radius * wind_direction_e, + map_projection_reproject(&ref, x_drop - turn_radius * wind_direction_n, y_drop - turn_radius * wind_direction_e, &flight_vector_e.lat, &flight_vector_e.lon); flight_vector_e.altitude = _drop_position.alt; //warnx("Flight vector: starting point = %.7f %.7f , end point = %.7f %.7f", flight_vector_s.lat,flight_vector_s.lon,flight_vector_e.lat,flight_vector_e.lon); //////////////////////////////////////////////////////////////////// DEBUGGING From 3f37efc046c9f3d2d6ee4a39f67ed5e20a6866ac Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 22 Aug 2014 14:41:35 +0200 Subject: [PATCH 129/411] Fix a bunch of stupid mistakes on coordinates / distances, set number of waypoints to zero once reached --- src/modules/bottle_drop/bottle_drop.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/modules/bottle_drop/bottle_drop.cpp b/src/modules/bottle_drop/bottle_drop.cpp index 81d1238cfd..2bd1f679b4 100644 --- a/src/modules/bottle_drop/bottle_drop.cpp +++ b/src/modules/bottle_drop/bottle_drop.cpp @@ -636,7 +636,7 @@ BottleDrop::task_main() case DROP_STATE_TARGET_SET: { - float distance_wp2 = get_distance_to_next_waypoint(flight_vector_e.lat, flight_vector_e.lon, _drop_position.lat, _drop_position.lon); + float distance_wp2 = get_distance_to_next_waypoint(_global_pos.lat, _global_pos.lon, flight_vector_e.lat, flight_vector_e.lon); if (distance_wp2 < distance_real) { _onboard_mission.current_seq = 0; @@ -674,7 +674,7 @@ BottleDrop::task_main() mavlink_log_info(_mavlink_fd, "#audio: payload dropped"); } else { - float distance_wp2 = get_distance_to_next_waypoint(flight_vector_e.lat, flight_vector_e.lon, _drop_position.lat, _drop_position.lon); + float distance_wp2 = get_distance_to_next_waypoint(_global_pos.lat, _global_pos.lon, flight_vector_e.lat, flight_vector_e.lon); if (distance_wp2 < distance_real) { _onboard_mission.current_seq = 0; @@ -696,6 +696,7 @@ BottleDrop::task_main() // remove onboard mission _onboard_mission.current_seq = -1; + _onboard_mission.count = 0; orb_publish(ORB_ID(onboard_mission), _onboard_mission_pub, &_onboard_mission); } break; From 6d80ebfc8933091c1f2cf70a42107bf80d3db74f Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 22 Aug 2014 16:09:20 +0200 Subject: [PATCH 130/411] update mavlink version --- mavlink/include/mavlink/v1.0 | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mavlink/include/mavlink/v1.0 b/mavlink/include/mavlink/v1.0 index 4d7487c2bc..c8a6e736e2 160000 --- a/mavlink/include/mavlink/v1.0 +++ b/mavlink/include/mavlink/v1.0 @@ -1 +1 @@ -Subproject commit 4d7487c2bc5f5ccf87bca82970fb2c08d6d8bd50 +Subproject commit c8a6e736e2bf71e14b023cf26733c3499d6d515b From d604985e080f1c04278f5f28acf0eaaf3b334041 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 22 Aug 2014 17:22:35 +0200 Subject: [PATCH 131/411] Bottle drop: Fix signs and comments --- src/modules/bottle_drop/bottle_drop.cpp | 48 +++++-------------------- 1 file changed, 9 insertions(+), 39 deletions(-) diff --git a/src/modules/bottle_drop/bottle_drop.cpp b/src/modules/bottle_drop/bottle_drop.cpp index 2bd1f679b4..3eec5a879a 100644 --- a/src/modules/bottle_drop/bottle_drop.cpp +++ b/src/modules/bottle_drop/bottle_drop.cpp @@ -462,7 +462,7 @@ BottleDrop::task_main() hrt_abstime last_run = hrt_absolute_time(); float dt_runs = 1e6f / sleeptime_us; - /* switch to faster updates during the drop */ + // switch to faster updates during the drop while (_drop_state > DROP_STATE_INIT) { // Get wind estimate @@ -474,17 +474,17 @@ BottleDrop::task_main() // Get vehicle position orb_check(vehicle_global_position_sub, &updated); if (updated) { - /* copy global position */ + // copy global position orb_copy(ORB_ID(vehicle_global_position), vehicle_global_position_sub, &_global_pos); } // Get parameter updates orb_check(parameter_update_sub, &updated); if (updated) { - /* copy global position */ + // copy global position orb_copy(ORB_ID(parameter_update), parameter_update_sub, &update); - /* update all parameters */ + // update all parameters param_get(param_gproperties, &z_0); param_get(param_turn_radius, &turn_radius); param_get(param_precision, &precision); @@ -500,20 +500,10 @@ BottleDrop::task_main() mavlink_log_info(_mavlink_fd, "#audio: drop distance %.2f", (double)distance_real); } - //warnx("absolut wind speed = %.4f", vr); //////////////////////////////////////////////////////////////////// DEBUGGING - - - //warnx("Initialization complete\n"); //////////////////////////////////////////////////////////////////// DEBUGGING - switch (_drop_state) { case DROP_STATE_TARGET_VALID: { - //warnx("approval given\n"); //////////////////////////////////////////////////////////////////// DEBUGGING - // drop here - //open_now = true; - //drop = false; - //drop_start = hrt_absolute_time(); // Update drop point at 10 Hz if (counter % 10 == 0) { @@ -547,18 +537,18 @@ BottleDrop::task_main() x = x + vx * dt_freefall_prediction; vrx = vx + vw; - //Drag force + // drag force v = sqrtf(vz * vz + vrx * vrx); Fd = 0.5f * rho * A * cd * (v * v); Fdx = Fd * vrx / v; Fdz = Fd * vz / v; - //acceleration + // acceleration az = g - Fdz / m; ax = -Fdx / m; } - // Compute drop vector + // compute drop vector x = groundspeed_body * t_signal + x; } @@ -580,9 +570,6 @@ BottleDrop::task_main() y_drop = y_t + x * wind_direction_e; map_projection_reproject(&ref, x_drop, y_drop, &_drop_position.lat, &_drop_position.lon); _drop_position.alt = _target_position.alt + _alt_clearance; - //warnx("drop point: lat = %.7f , lon = %.7f", drop_position.lat, drop_position.lon); //////////////////////////////////////////////////////////////////// DEBUGGING - - // Compute flight vector map_projection_reproject(&ref, x_drop + 2 * turn_radius * wind_direction_n, y_drop + 2 * turn_radius * wind_direction_e, @@ -591,25 +578,8 @@ BottleDrop::task_main() map_projection_reproject(&ref, x_drop - turn_radius * wind_direction_n, y_drop - turn_radius * wind_direction_e, &flight_vector_e.lat, &flight_vector_e.lon); flight_vector_e.altitude = _drop_position.alt; - //warnx("Flight vector: starting point = %.7f %.7f , end point = %.7f %.7f", flight_vector_s.lat,flight_vector_s.lon,flight_vector_e.lat,flight_vector_e.lon); //////////////////////////////////////////////////////////////////// DEBUGGING - // Drop Cancellation if terms are not met - - // warnx("latitude:%.2f", _global_pos.lat); - // warnx("longitude:%.2f", _global_pos.lon); - // warnx("drop_position.lat:%.2f", drop_position.lat); - // warnx("drop_position.lon:%.2f", drop_position.lon); - - //warnx("position to-be: = %.7f %.7f" ,x_f_NED, y_f_NED ); //////////////////////////////////////////////////////////////////// DEBUGGING - - // if (counter % 10 ==0) { - // warnx("x: %.4f", x); - // warnx("drop_position.lat: %.4f, drop_position.lon: %.4f", drop_position.lat, drop_position.lon); - // warnx("latitude %.4f, longitude: %.4f", _global_pos.lat, _global_pos.lon); - // warnx("future_distance: %.2f, precision: %.2f", future_distance, precision); - // } - - /* Save WPs in datamanager */ + // Save WPs in datamanager const ssize_t len = sizeof(struct mission_item_s); if (dm_write(DM_KEY_WAYPOINTS_ONBOARD, 0, DM_PERSIST_IN_FLIGHT_RESET, &flight_vector_s, len) != len) { @@ -662,7 +632,7 @@ BottleDrop::task_main() x_f = x_l + _global_pos.vel_n * dt_runs; y_f = y_l + _global_pos.vel_e * dt_runs; map_projection_reproject(&ref, x_f, y_f, &x_f_NED, &y_f_NED); - future_distance = fabsf(get_distance_to_next_waypoint(x_f_NED, y_f_NED, _drop_position.lat, _drop_position.lon)); + future_distance = get_distance_to_next_waypoint(x_f_NED, y_f_NED, _drop_position.lat, _drop_position.lon); warnx("Distance real: %.2f", (double)distance_real); From 406a52e6399b171cfe454b85774f12f9add40aee Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 22 Aug 2014 20:24:04 +0200 Subject: [PATCH 132/411] fix typo --- 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 109ab403ac..243546787b 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1314,7 +1314,7 @@ int commander_thread_main(int argc, char *argv[]) if (pos_sp_triplet.geofence_violated || pos_sp_triplet.flight_termination) { //XXX: make this configurable to select different actions (e.g. navigation modes) /* this will only trigger if geofence is activated via param and a geofence file is present, also there is a circuit breaker to disable the actual flight termination in the px4io driver */ - armed.fosrce_failsafe = true; + armed.force_failsafe = true; status_changed = true; warnx("Flight termination"); } // no reset is done here on purpose, on geofence violation we want to stay in flighttermination From b22acadb8a255cfdf7a8c77ea0ea84587ff7d5e0 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 22 Aug 2014 21:02:33 +0200 Subject: [PATCH 133/411] DL loss && gps fail: flight termination --- src/modules/commander/commander.cpp | 13 ++++++++++++- 1 file changed, 12 insertions(+), 1 deletion(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 243546787b..8ae5a441aa 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1316,7 +1316,7 @@ int commander_thread_main(int argc, char *argv[]) /* this will only trigger if geofence is activated via param and a geofence file is present, also there is a circuit breaker to disable the actual flight termination in the px4io driver */ armed.force_failsafe = true; status_changed = true; - warnx("Flight termination"); + warnx("Flight termination because of navigator request or geofence"); } // no reset is done here on purpose, on geofence violation we want to stay in flighttermination } @@ -1579,6 +1579,17 @@ int commander_thread_main(int argc, char *argv[]) } } + /* At this point the data link and the gps system have been checked + * If both failed we want to terminate the flight */ + if ((status.data_link_lost && status.gps_failure) || + (status.data_link_lost_cmd && status.gps_failure_cmd)) { + armed.force_failsafe = true; + status_changed = true; + warnx("Flight termination because of data link loss && gps failure"); + mavlink_log_critical(mavlink_fd, "DL and GPS lost: flight termination"); + } + + hrt_abstime t1 = hrt_absolute_time(); /* print new state */ From d50135b611605891cb1fb8841490c41474c3ec31 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 22 Aug 2014 21:40:58 +0200 Subject: [PATCH 134/411] rc loss && gps loss: flight termination --- src/modules/commander/commander.cpp | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 8ae5a441aa..9968ab8e6e 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1579,6 +1579,7 @@ int commander_thread_main(int argc, char *argv[]) } } + /* Check for failure combinations which lead to flight termination */ /* At this point the data link and the gps system have been checked * If both failed we want to terminate the flight */ if ((status.data_link_lost && status.gps_failure) || @@ -1589,6 +1590,21 @@ int commander_thread_main(int argc, char *argv[]) mavlink_log_critical(mavlink_fd, "DL and GPS lost: flight termination"); } + /* At this point the rc signal and the gps system have been checked + * If we are in manual (controlled with RC): + * if both failed we want to terminate the flight */ + if ((status.main_state == MAIN_STATE_ACRO || + status.main_state == MAIN_STATE_MANUAL || + status.main_state == MAIN_STATE_ALTCTL || + status.main_state == MAIN_STATE_POSCTL) && + ((status.rc_signal_lost && status.rc_signal_lost) || + (status.rc_signal_lost && status.gps_failure_cmd))) { + armed.force_failsafe = true; + status_changed = true; + warnx("Flight termination because of RC signal loss && gps failure"); + mavlink_log_critical(mavlink_fd, "RC and GPS lost: flight termination"); + } + hrt_abstime t1 = hrt_absolute_time(); From 4d75222b67b8ae93f28c1bbec782ce7fd0ab9919 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 22 Aug 2014 21:41:24 +0200 Subject: [PATCH 135/411] switch to rc loss mode if rc loss commanded --- 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 404bafb042..d105e4bdf6 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -451,7 +451,7 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en case MAIN_STATE_ALTCTL: case MAIN_STATE_POSCTL: /* require RC for all manual modes */ - if (status->rc_signal_lost && armed) { + if ((status->rc_signal_lost || status->rc_signal_lost_cmd) && armed) { status->failsafe = true; if (status->condition_global_position_valid && status->condition_home_position_valid) { From 6ae8800ad09caf9194240ec75067af5ef56a5a23 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 22 Aug 2014 23:05:19 +0200 Subject: [PATCH 136/411] move and rename params airfield home is general --- src/modules/navigator/datalinkloss.cpp | 6 +-- src/modules/navigator/datalinkloss_params.c | 33 ---------------- src/modules/navigator/navigator_params.c | 43 +++++++++++++++++++++ 3 files changed, 46 insertions(+), 36 deletions(-) diff --git a/src/modules/navigator/datalinkloss.cpp b/src/modules/navigator/datalinkloss.cpp index d8a1de2297..893d6d93ae 100644 --- a/src/modules/navigator/datalinkloss.cpp +++ b/src/modules/navigator/datalinkloss.cpp @@ -61,9 +61,9 @@ DataLinkLoss::DataLinkLoss(Navigator *navigator, const char *name) : _param_commsholdlat(this, "CH_LAT"), _param_commsholdlon(this, "CH_LON"), _param_commsholdalt(this, "CH_ALT"), - _param_airfieldhomelat(this, "AH_LAT"), - _param_airfieldhomelon(this, "AH_LON"), - _param_airfieldhomealt(this, "AH_ALT"), + _param_airfieldhomelat(this, "NAV_AH_LAT", false), + _param_airfieldhomelon(this, "NAV_AH_LON", false), + _param_airfieldhomealt(this, "NAV_AH_ALT", false), _param_numberdatalinklosses(this, "N"), _dll_state(DLL_STATE_NONE) { diff --git a/src/modules/navigator/datalinkloss_params.c b/src/modules/navigator/datalinkloss_params.c index 77a8763cb1..02f7ca4c31 100644 --- a/src/modules/navigator/datalinkloss_params.c +++ b/src/modules/navigator/datalinkloss_params.c @@ -91,39 +91,6 @@ PARAM_DEFINE_INT32(NAV_DLL_CH_LON, 1518453890); */ PARAM_DEFINE_FLOAT(NAV_DLL_CH_ALT, 600.0f); -/** - * Airfield home Lat - * - * Latitude of airfield home waypoint - * - * @unit degrees * 1e7 - * @min 0.0 - * @group DLL - */ -PARAM_DEFINE_INT32(NAV_DLL_AH_LAT, -265847810); - -/** - * Airfield home Lon - * - * Longitude of airfield home waypoint - * - * @unit degrees * 1e7 - * @min 0.0 - * @group DLL - */ -PARAM_DEFINE_INT32(NAV_DLL_AH_LON, 1518423250); - -/** - * Airfield home alt - * - * Altitude of airfield home waypoint - * - * @unit m - * @min 0.0 - * @group DLL - */ -PARAM_DEFINE_FLOAT(NAV_DLL_AH_ALT, 600.0f); - /** * Number of allowed Datalink timeouts * diff --git a/src/modules/navigator/navigator_params.c b/src/modules/navigator/navigator_params.c index afaf1c3c31..1f40e634ed 100644 --- a/src/modules/navigator/navigator_params.c +++ b/src/modules/navigator/navigator_params.c @@ -75,3 +75,46 @@ PARAM_DEFINE_FLOAT(NAV_ACC_RAD, 25.0f); * @group Mission */ PARAM_DEFINE_INT32(NAV_DLL_OBC, 0); + +/** + * Set OBC mode for rc loss + * + * If set to 1 the behaviour on data link loss is set to a mode according to the OBC rules + * + * @min 0 + * @group Mission + */ +PARAM_DEFINE_INT32(NAV_RCL_OBC, 0); + +/** + * Airfield home Lat + * + * Latitude of airfield home waypoint + * + * @unit degrees * 1e7 + * @min 0.0 + * @group DLL + */ +PARAM_DEFINE_INT32(NAV_AH_LAT, -265847810); + +/** + * Airfield home Lon + * + * Longitude of airfield home waypoint + * + * @unit degrees * 1e7 + * @min 0.0 + * @group DLL + */ +PARAM_DEFINE_INT32(NAV_AH_LON, 1518423250); + +/** + * Airfield home alt + * + * Altitude of airfield home waypoint + * + * @unit m + * @min 0.0 + * @group DLL + */ +PARAM_DEFINE_FLOAT(NAV_AH_ALT, 600.0f); From fd3746a233b0ef16758e0171da0ee7e71ff58887 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 22 Aug 2014 23:06:14 +0200 Subject: [PATCH 137/411] add OBC RC loss mode to navigator --- src/modules/navigator/gpsfailure.cpp | 2 +- src/modules/navigator/module.mk | 4 +- src/modules/navigator/navigator.h | 8 +- src/modules/navigator/navigator_main.cpp | 13 +- src/modules/navigator/rcloss.cpp | 172 +++++++++++++++++++++++ src/modules/navigator/rcloss.h | 88 ++++++++++++ src/modules/navigator/rcloss_params.c | 59 ++++++++ 7 files changed, 339 insertions(+), 7 deletions(-) create mode 100644 src/modules/navigator/rcloss.cpp create mode 100644 src/modules/navigator/rcloss.h create mode 100644 src/modules/navigator/rcloss_params.c diff --git a/src/modules/navigator/gpsfailure.cpp b/src/modules/navigator/gpsfailure.cpp index 02e766ffba..f0bbbcf1cb 100644 --- a/src/modules/navigator/gpsfailure.cpp +++ b/src/modules/navigator/gpsfailure.cpp @@ -141,7 +141,7 @@ GpsFailure::set_gpsf_item() case GPSF_STATE_TERMINATE: { /* Request flight termination from the commander */ pos_sp_triplet->flight_termination = true; - warnx("request flight termination"); + warnx("gps fail: request flight termination"); } default: break; diff --git a/src/modules/navigator/module.mk b/src/modules/navigator/module.mk index 9e4ad053c9..c075903b75 100644 --- a/src/modules/navigator/module.mk +++ b/src/modules/navigator/module.mk @@ -51,8 +51,10 @@ SRCS = navigator_main.cpp \ geofence.cpp \ geofence_params.c \ datalinkloss.cpp \ - enginefailure.cpp \ datalinkloss_params.c \ + rcloss.cpp \ + rcloss_params.c \ + enginefailure.cpp \ gpsfailure.cpp \ gpsfailure_params.c diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index ec6e538e34..709e3e7242 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -63,12 +63,13 @@ #include "datalinkloss.h" #include "enginefailure.h" #include "gpsfailure.h" +#include "rcloss.h" #include "geofence.h" /** * Number of navigation modes that need on_active/on_inactive calls */ -#define NAVIGATOR_MODE_ARRAY_SIZE 7 +#define NAVIGATOR_MODE_ARRAY_SIZE 8 class Navigator : public control::SuperBlock { @@ -109,7 +110,7 @@ public: * Publish the mission result so commander and mavlink know what is going on */ void publish_mission_result(); - + /** * Publish the attitude sp, only to be used in very special modes when position control is deactivated * Example: mode that is triggered on gps failure @@ -193,6 +194,8 @@ private: Mission _mission; /**< class that handles the missions */ Loiter _loiter; /**< class that handles loiter */ RTL _rtl; /**< class that handles RTL */ + RCLoss _rcLoss; /**< class that handles RTL according to + OBC rules (rc loss mode) */ Offboard _offboard; /**< class that handles offboard */ DataLinkLoss _dataLinkLoss; /**< class that handles the OBC datalink loss mode */ EngineFailure _engineFailure; /**< class that handles the engine failure mode @@ -207,6 +210,7 @@ private: control::BlockParamFloat _param_loiter_radius; /**< loiter radius for fixedwing */ control::BlockParamFloat _param_acceptance_radius; /**< acceptance for takeoff */ control::BlockParamInt _param_datalinkloss_obc; /**< if true: obc mode on data link loss enabled */ + control::BlockParamInt _param_rcloss_obc; /**< if true: obc mode on rc loss enabled */ /** * Retrieve global position */ diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index e0913bb575..c569ee7b54 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -131,6 +131,7 @@ Navigator::Navigator() : _mission(this, "MIS"), _loiter(this, "LOI"), _rtl(this, "RTL"), + _rcLoss(this, "RCL"), _offboard(this, "OFF"), _dataLinkLoss(this, "DLL"), _engineFailure(this, "EF"), @@ -139,7 +140,8 @@ Navigator::Navigator() : _pos_sp_triplet_updated(false), _param_loiter_radius(this, "LOITER_RAD"), _param_acceptance_radius(this, "ACC_RAD"), - _param_datalinkloss_obc(this, "DLL_OBC") + _param_datalinkloss_obc(this, "DLL_OBC"), + _param_rcloss_obc(this, "RCL_OBC") { /* Create a list of our possible navigation types */ _navigation_mode_array[0] = &_mission; @@ -149,6 +151,7 @@ Navigator::Navigator() : _navigation_mode_array[4] = &_dataLinkLoss; _navigation_mode_array[5] = &_engineFailure; _navigation_mode_array[6] = &_gpsFailure; + _navigation_mode_array[7] = &_rcLoss; updateParams(); } @@ -413,7 +416,11 @@ Navigator::task_main() _navigation_mode = &_loiter; break; case NAVIGATION_STATE_AUTO_RTL: - _navigation_mode = &_rtl; + if (_param_rcloss_obc.get() != 0) { + _navigation_mode = &_rcLoss; + } else { + _navigation_mode = &_rtl; + } break; case NAVIGATION_STATE_AUTO_RTGS: //XXX OBC: differentiate between rc loss and dl loss here /* Use complex data link loss mode only when enabled via param @@ -421,7 +428,7 @@ Navigator::task_main() if (_param_datalinkloss_obc.get() != 0) { _navigation_mode = &_dataLinkLoss; } else { - _navigation_mode = &_rtl; /* TODO: change this to something else */ + _navigation_mode = &_rtl; } break; case NAVIGATION_STATE_AUTO_LANDENGFAIL: diff --git a/src/modules/navigator/rcloss.cpp b/src/modules/navigator/rcloss.cpp new file mode 100644 index 0000000000..b8b659efe0 --- /dev/null +++ b/src/modules/navigator/rcloss.cpp @@ -0,0 +1,172 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ +/** + * @file rcloss.cpp + * Helper class for RC Loss Mode according to the OBC rules + * + * @author Thomas Gubler + */ + +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include + +#include "navigator.h" +#include "datalinkloss.h" + +#define DELAY_SIGMA 0.01f + +RCLoss::RCLoss(Navigator *navigator, const char *name) : + MissionBlock(navigator, name), + _param_loitertime(this, "LT"), + _rcl_state(RCL_STATE_NONE) +{ + /* load initial params */ + updateParams(); + /* initial reset */ + on_inactive(); +} + +RCLoss::~RCLoss() +{ +} + +void +RCLoss::on_inactive() +{ + /* reset RCL state only if setpoint moved */ + if (!_navigator->get_can_loiter_at_sp()) { + _rcl_state = RCL_STATE_NONE; + } +} + +void +RCLoss::on_activation() +{ + _rcl_state = RCL_STATE_NONE; + updateParams(); + advance_rcl(); + set_rcl_item(); +} + +void +RCLoss::on_active() +{ + if (is_mission_item_reached()) { + updateParams(); + advance_rcl(); + set_rcl_item(); + } +} + +void +RCLoss::set_rcl_item() +{ + struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); + + set_previous_pos_setpoint(); + _navigator->set_can_loiter_at_sp(false); + + switch (_rcl_state) { + case RCL_STATE_LOITER: { + _mission_item.lat = _navigator->get_global_position()->lat; + _mission_item.lon = _navigator->get_global_position()->lon; + _mission_item.altitude_is_relative = false; + _mission_item.yaw = NAN; + _mission_item.loiter_radius = _navigator->get_loiter_radius(); + _mission_item.loiter_direction = 1; + _mission_item.nav_cmd = NAV_CMD_LOITER_TIME_LIMIT; + _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); + _mission_item.time_inside = _param_loitertime.get() < 0.0f ? 0.0f : _param_loitertime.get(); + _mission_item.pitch_min = 0.0f; + _mission_item.autocontinue = true; + _mission_item.origin = ORIGIN_ONBOARD; + + _navigator->set_can_loiter_at_sp(true); + break; + } + case RCL_STATE_TERMINATE: { + /* Request flight termination from the commander */ + pos_sp_triplet->flight_termination = true; + warnx("gps fail: request flight termination"); + pos_sp_triplet->previous.valid = false; + pos_sp_triplet->current.valid = false; + pos_sp_triplet->next.valid = false; + } + default: + break; + } + + reset_mission_item_reached(); + + /* convert mission item to current position setpoint and make it valid */ + mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current); + pos_sp_triplet->next.valid = false; + + _navigator->set_position_setpoint_triplet_updated(); +} + +void +RCLoss::advance_rcl() +{ + switch (_rcl_state) { + case RCL_STATE_NONE: + /* Check the number of data link losses. If above home fly home directly */ + warnx("RC loss, OBC mode, loiter"); + mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: rc los, loitering"); + _rcl_state = RCL_STATE_LOITER; + break; + case RCL_STATE_LOITER: + _rcl_state = RCL_STATE_TERMINATE; + warnx("time is up, no RC regain, terminating"); + mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RC not regained, terminating"); + _navigator->get_mission_result()->stay_in_failsafe = true; + _navigator->publish_mission_result(); + break; + case RCL_STATE_TERMINATE: + warnx("rcl end"); + _rcl_state = RCL_STATE_END; + break; + default: + break; + } +} diff --git a/src/modules/navigator/rcloss.h b/src/modules/navigator/rcloss.h new file mode 100644 index 0000000000..bcb74d8778 --- /dev/null +++ b/src/modules/navigator/rcloss.h @@ -0,0 +1,88 @@ +/*************************************************************************** + * + * Copyright (c) 2013-2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ +/** + * @file rcloss.h + * Helper class for RC Loss Mode acording to the OBC rules + * + * @author Thomas Gubler + */ + +#ifndef NAVIGATOR_RCLOSS_H +#define NAVIGATOR_RCLOSS_H + +#include +#include + +#include + +#include "navigator_mode.h" +#include "mission_block.h" + +class Navigator; + +class RCLoss : public MissionBlock +{ +public: + RCLoss(Navigator *navigator, const char *name); + + ~RCLoss(); + + virtual void on_inactive(); + + virtual void on_activation(); + + virtual void on_active(); + +private: + /* Params */ + control::BlockParamFloat _param_loitertime; + + enum RCLState { + RCL_STATE_NONE = 0, + RCL_STATE_LOITER = 1, + RCL_STATE_TERMINATE = 2, + RCL_STATE_END = 3 + } _rcl_state; + + /** + * Set the RCL item + */ + void set_rcl_item(); + + /** + * Move to next RCL item + */ + void advance_rcl(); + +}; +#endif diff --git a/src/modules/navigator/rcloss_params.c b/src/modules/navigator/rcloss_params.c new file mode 100644 index 0000000000..83d23cf499 --- /dev/null +++ b/src/modules/navigator/rcloss_params.c @@ -0,0 +1,59 @@ +/**************************************************************************** + * + * Copyright (c) 2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * AS IS AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file rcloss_params.c + * + * Parameters for RC Loss (OBC) + * + * @author Thomas Gubler + */ + +#include + +#include + +/* + * OBC RC Loss mode parameters, accessible via MAVLink + */ + +/** + * Loiter Time + * + * The amount of time in seconds the system should loiter at current position before termination + * + * @unit seconds + * @min 0.0 + * @group RCL + */ +PARAM_DEFINE_FLOAT(NAV_RCL_LT, 120.0f); From 29715102894121f711fd40acfcf6ec8fb4fa6630 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 23 Aug 2014 13:00:28 +0200 Subject: [PATCH 138/411] commander: flight termination, require arming --- src/modules/commander/commander.cpp | 48 +++++++++++++++-------------- 1 file changed, 25 insertions(+), 23 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 9968ab8e6e..692e5e8ec4 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1311,7 +1311,7 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(position_setpoint_triplet), pos_sp_triplet_sub, &pos_sp_triplet); /* Check for geofence violation */ - if (pos_sp_triplet.geofence_violated || pos_sp_triplet.flight_termination) { + if (armed.armed && (pos_sp_triplet.geofence_violated || pos_sp_triplet.flight_termination)) { //XXX: make this configurable to select different actions (e.g. navigation modes) /* this will only trigger if geofence is activated via param and a geofence file is present, also there is a circuit breaker to disable the actual flight termination in the px4io driver */ armed.force_failsafe = true; @@ -1580,29 +1580,31 @@ int commander_thread_main(int argc, char *argv[]) } /* Check for failure combinations which lead to flight termination */ - /* At this point the data link and the gps system have been checked - * If both failed we want to terminate the flight */ - if ((status.data_link_lost && status.gps_failure) || - (status.data_link_lost_cmd && status.gps_failure_cmd)) { - armed.force_failsafe = true; - status_changed = true; - warnx("Flight termination because of data link loss && gps failure"); - mavlink_log_critical(mavlink_fd, "DL and GPS lost: flight termination"); - } + if (armed.armed) { + /* At this point the data link and the gps system have been checked + * If both failed we want to terminate the flight */ + if ((status.data_link_lost && status.gps_failure) || + (status.data_link_lost_cmd && status.gps_failure_cmd)) { + armed.force_failsafe = true; + status_changed = true; + warnx("Flight termination because of data link loss && gps failure"); + mavlink_log_critical(mavlink_fd, "DL and GPS lost: flight termination"); + } - /* At this point the rc signal and the gps system have been checked - * If we are in manual (controlled with RC): - * if both failed we want to terminate the flight */ - if ((status.main_state == MAIN_STATE_ACRO || - status.main_state == MAIN_STATE_MANUAL || - status.main_state == MAIN_STATE_ALTCTL || - status.main_state == MAIN_STATE_POSCTL) && - ((status.rc_signal_lost && status.rc_signal_lost) || - (status.rc_signal_lost && status.gps_failure_cmd))) { - armed.force_failsafe = true; - status_changed = true; - warnx("Flight termination because of RC signal loss && gps failure"); - mavlink_log_critical(mavlink_fd, "RC and GPS lost: flight termination"); + /* At this point the rc signal and the gps system have been checked + * If we are in manual (controlled with RC): + * if both failed we want to terminate the flight */ + if ((status.main_state == MAIN_STATE_ACRO || + status.main_state == MAIN_STATE_MANUAL || + status.main_state == MAIN_STATE_ALTCTL || + status.main_state == MAIN_STATE_POSCTL) && + ((status.rc_signal_lost && status.rc_signal_lost) || + (status.rc_signal_lost && status.gps_failure_cmd))) { + armed.force_failsafe = true; + status_changed = true; + warnx("Flight termination because of RC signal loss && gps failure"); + mavlink_log_critical(mavlink_fd, "RC and GPS lost: flight termination"); + } } From cc12e6051d1ae8100c035e935fe6dd1a453ce887 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 23 Aug 2014 13:13:51 +0200 Subject: [PATCH 139/411] obc rcloss: set altitude --- src/modules/navigator/rcloss.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/modules/navigator/rcloss.cpp b/src/modules/navigator/rcloss.cpp index b8b659efe0..427001a013 100644 --- a/src/modules/navigator/rcloss.cpp +++ b/src/modules/navigator/rcloss.cpp @@ -110,6 +110,7 @@ RCLoss::set_rcl_item() case RCL_STATE_LOITER: { _mission_item.lat = _navigator->get_global_position()->lat; _mission_item.lon = _navigator->get_global_position()->lon; + _mission_item.altitude = _navigator->get_global_position()->alt; _mission_item.altitude_is_relative = false; _mission_item.yaw = NAN; _mission_item.loiter_radius = _navigator->get_loiter_radius(); @@ -127,7 +128,7 @@ RCLoss::set_rcl_item() case RCL_STATE_TERMINATE: { /* Request flight termination from the commander */ pos_sp_triplet->flight_termination = true; - warnx("gps fail: request flight termination"); + warnx("rc not recovered: request flight termination"); pos_sp_triplet->previous.valid = false; pos_sp_triplet->current.valid = false; pos_sp_triplet->next.valid = false; @@ -152,7 +153,7 @@ RCLoss::advance_rcl() case RCL_STATE_NONE: /* Check the number of data link losses. If above home fly home directly */ warnx("RC loss, OBC mode, loiter"); - mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: rc los, loitering"); + mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: rc loss, loitering"); _rcl_state = RCL_STATE_LOITER; break; case RCL_STATE_LOITER: From ffd2fa7386f9fe3ab017d98a2b0e8e21b0975833 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 23 Aug 2014 13:25:50 +0200 Subject: [PATCH 140/411] commander: fix check for rc && gps loss --- src/modules/commander/commander.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 692e5e8ec4..c4b76b3918 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1598,8 +1598,8 @@ int commander_thread_main(int argc, char *argv[]) status.main_state == MAIN_STATE_MANUAL || status.main_state == MAIN_STATE_ALTCTL || status.main_state == MAIN_STATE_POSCTL) && - ((status.rc_signal_lost && status.rc_signal_lost) || - (status.rc_signal_lost && status.gps_failure_cmd))) { + ((status.rc_signal_lost && status.gps_failure) || + (status.rc_signal_lost_cmd && status.gps_failure_cmd))) { armed.force_failsafe = true; status_changed = true; warnx("Flight termination because of RC signal loss && gps failure"); From 3c10b78e2044ec2cbe36d4de35c7ac8a936521ae Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 23 Aug 2014 14:02:22 +0200 Subject: [PATCH 141/411] stae machine helper: remove unnecessary check for RC loss --- src/modules/commander/commander.cpp | 2 +- src/modules/commander/state_machine_helper.cpp | 15 +-------------- 2 files changed, 2 insertions(+), 15 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index c4b76b3918..0bf5cfe343 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -566,7 +566,7 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s status_local->rc_signal_lost_cmd = false; if ((int)cmd->param2 <= 0) { /* reset all commanded failure modes */ - warnx("revert to normal mode"); + warnx("reset all non-flighttermination failsafe commands"); } else if ((int)cmd->param2 == 1) { /* trigger engine failure mode */ status_local->engine_failure_cmd = true; diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index d105e4bdf6..4506942ab2 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -548,22 +548,9 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en break; case MAIN_STATE_AUTO_LOITER: - /* go into failsafe on a engine failure or if datalink and RC is lost */ + /* go into failsafe on a engine failure */ if (status->engine_failure) { status->nav_state = NAVIGATION_STATE_AUTO_LANDENGFAIL; - } else if ((status->data_link_lost && data_link_loss_enabled) && status->rc_signal_lost) { - status->failsafe = true; - - if (status->condition_global_position_valid && status->condition_home_position_valid) { - status->nav_state = NAVIGATION_STATE_AUTO_RTL; - } else if (status->condition_local_position_valid) { - status->nav_state = NAVIGATION_STATE_LAND; - } else if (status->condition_local_altitude_valid) { - status->nav_state = NAVIGATION_STATE_DESCEND; - } else { - status->nav_state = NAVIGATION_STATE_TERMINATION; - } - /* also go into failsafe if just datalink is lost */ } else if (status->data_link_lost && data_link_loss_enabled) { status->failsafe = true; From 8262739b6219f54e7ea31de93cd81304df311ab9 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 24 Aug 2014 11:14:15 +0200 Subject: [PATCH 142/411] geofence: can select gps instead of global position --- src/modules/navigator/geofence.cpp | 23 +++++++++++++- src/modules/navigator/geofence.h | 14 ++++++++- src/modules/navigator/geofence_params.c | 17 +++++++++-- src/modules/navigator/navigator.h | 9 ++++++ src/modules/navigator/navigator_main.cpp | 38 +++++++++++++++++++----- 5 files changed, 89 insertions(+), 12 deletions(-) diff --git a/src/modules/navigator/geofence.cpp b/src/modules/navigator/geofence.cpp index 7897c7ba0e..4a02a0c3f3 100644 --- a/src/modules/navigator/geofence.cpp +++ b/src/modules/navigator/geofence.cpp @@ -63,7 +63,8 @@ Geofence::Geofence() : _altitude_max(0), _verticesCount(0), _param_geofence_on(this, "ON"), - _param_altitude_mode(this, "ALTMODE") + _param_altitude_mode(this, "ALTMODE"), + _param_source(this, "SOURCE") { /* Load initial params */ updateParams(); @@ -85,6 +86,26 @@ bool Geofence::inside(const struct vehicle_global_position_s &global_position, f return inside(global_position.lat, global_position.lon, baro_altitude_amsl); } + +bool Geofence::inside(const struct vehicle_global_position_s &global_position, + const struct vehicle_gps_position_s &gps_position,float baro_altitude_amsl) { + if (getAltitudeMode() == Geofence::GF_ALT_MODE_WGS84) { + if (getSource() == Geofence::GF_SOURCE_GLOBALPOS) { + return inside(global_position); + } else { + return inside((double)gps_position.lat * 1.0e-7, (double)gps_position.lon * 1.0e-7, + (double)gps_position.alt * 1.0e-3); + } + } else { + if (getSource() == Geofence::GF_SOURCE_GLOBALPOS) { + return inside(global_position, baro_altitude_amsl); + } else { + return inside((double)gps_position.lat * 1.0e-7, (double)gps_position.lon * 1.0e-7, + baro_altitude_amsl); + } + } +} + bool Geofence::inside(double lat, double lon, float altitude) { /* Return true if geofence is disabled */ diff --git a/src/modules/navigator/geofence.h b/src/modules/navigator/geofence.h index e2c0f08d87..91c74572e3 100644 --- a/src/modules/navigator/geofence.h +++ b/src/modules/navigator/geofence.h @@ -43,6 +43,7 @@ #include #include +#include #include #include #include @@ -57,10 +58,16 @@ public: /* Altitude mode, corresponding to the param GF_ALTMODE */ enum { - GF_ALT_MODE_GPS = 0, + GF_ALT_MODE_WGS84 = 0, GF_ALT_MODE_AMSL = 1 }; + /* Source, corresponding to the param GF_SOURCE */ + enum { + GF_SOURCE_GLOBALPOS = 0, + GF_SOURCE_GPS = 1 + }; + /** * Return whether system is inside geofence. * @@ -71,6 +78,8 @@ public: bool inside(const struct vehicle_global_position_s &global_position); bool inside(const struct vehicle_global_position_s &global_position, float baro_altitude_amsl); bool inside(double lat, double lon, float altitude); + bool inside(const struct vehicle_global_position_s &global_position, + const struct vehicle_gps_position_s &gps_position,float baro_altitude_amsl); int clearDm(); @@ -88,6 +97,8 @@ public: bool isEmpty() {return _verticesCount == 0;} int getAltitudeMode() { return _param_altitude_mode.get(); } + + int getSource() { return _param_source.get(); } private: orb_advert_t _fence_pub; /**< publish fence topic */ @@ -100,6 +111,7 @@ private: /* Params */ control::BlockParamInt _param_geofence_on; control::BlockParamInt _param_altitude_mode; + control::BlockParamInt _param_source; }; diff --git a/src/modules/navigator/geofence_params.c b/src/modules/navigator/geofence_params.c index 29b42cd54e..32902ee97a 100644 --- a/src/modules/navigator/geofence_params.c +++ b/src/modules/navigator/geofence_params.c @@ -63,10 +63,23 @@ PARAM_DEFINE_INT32(GF_ON, 1); * Geofence altitude mode * * Select which altitude reference should be used - * 0 = GPS, 1 = AMSL + * 0 = WGS84, 1 = AMSL * * @min 0 * @max 1 * @group Geofence */ -PARAM_DEFINE_INT32(GF_ALTMODE, 1); +PARAM_DEFINE_INT32(GF_ALTMODE, 0); + +/** + * Geofence source + * + * Select which position source should be used. Selecting GPS instead of global position makes sure that there is + * no dependence on the position estimator + * 0 = global position, 1 = GPS + * + * @min 0 + * @max 1 + * @group Geofence + */ +PARAM_DEFINE_INT32(GF_SOURCE, 0); diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index 709e3e7242..840b43f1b8 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -51,6 +51,7 @@ #include #include #include +#include #include #include #include @@ -129,6 +130,7 @@ public: struct vehicle_status_s* get_vstatus() { return &_vstatus; } struct vehicle_control_mode_s* get_control_mode() { return &_control_mode; } struct vehicle_global_position_s* get_global_position() { return &_global_pos; } + struct vehicle_gps_position_s* get_gps_position() { return &_gps_pos; } struct sensor_combined_s* get_sensor_combined() { return &_sensor_combined; } struct home_position_s* get_home_position() { return &_home_pos; } struct position_setpoint_triplet_s* get_position_setpoint_triplet() { return &_pos_sp_triplet; } @@ -152,6 +154,7 @@ private: int _mavlink_fd; /**< the file descriptor to send messages over mavlink */ int _global_pos_sub; /**< global position subscription */ + int _gps_pos_sub; /**< gps position subscription */ int _sensor_combined_sub; /**< sensor combined subscription */ int _home_pos_sub; /**< home position subscription */ int _vstatus_sub; /**< vehicle status subscription */ @@ -171,6 +174,7 @@ private: vehicle_status_s _vstatus; /**< vehicle status */ vehicle_control_mode_s _control_mode; /**< vehicle control mode */ vehicle_global_position_s _global_pos; /**< global vehicle position */ + vehicle_gps_position_s _gps_pos; /**< gps position */ sensor_combined_s _sensor_combined; /**< sensor values */ home_position_s _home_pos; /**< home position for RTL */ mission_item_s _mission_item; /**< current mission item */ @@ -216,6 +220,11 @@ private: */ void global_position_update(); + /** + * Retrieve gps position + */ + void gps_position_update(); + /** * Retrieve sensor values */ diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index c569ee7b54..81ceaaca2d 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -100,6 +100,7 @@ Navigator::Navigator() : _navigator_task(-1), _mavlink_fd(-1), _global_pos_sub(-1), + _gps_pos_sub(-1), _home_pos_sub(-1), _vstatus_sub(-1), _capabilities_sub(-1), @@ -114,6 +115,7 @@ Navigator::Navigator() : _vstatus{}, _control_mode{}, _global_pos{}, + _gps_pos{}, _sensor_combined{}, _home_pos{}, _mission_item{}, @@ -187,6 +189,12 @@ Navigator::global_position_update() orb_copy(ORB_ID(vehicle_global_position), _global_pos_sub, &_global_pos); } +void +Navigator::gps_position_update() +{ + orb_copy(ORB_ID(vehicle_gps_position), _gps_pos_sub, &_gps_pos); +} + void Navigator::sensor_combined_update() { @@ -263,6 +271,7 @@ Navigator::task_main() /* do subscriptions */ _global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); + _gps_pos_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); _sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined)); _capabilities_sub = orb_subscribe(ORB_ID(navigation_capabilities)); _vstatus_sub = orb_subscribe(ORB_ID(vehicle_status)); @@ -277,6 +286,7 @@ Navigator::task_main() vehicle_status_update(); vehicle_control_mode_update(); global_position_update(); + gps_position_update(); sensor_combined_update(); home_position_update(); navigation_capabilities_update(); @@ -289,7 +299,7 @@ Navigator::task_main() const hrt_abstime mavlink_open_interval = 500000; /* wakeup source(s) */ - struct pollfd fds[7]; + struct pollfd fds[8]; /* Setup of loop */ fds[0].fd = _global_pos_sub; @@ -306,6 +316,8 @@ Navigator::task_main() fds[5].events = POLLIN; fds[6].fd = _sensor_combined_sub; fds[6].events = POLLIN; + fds[7].fd = _gps_pos_sub; + fds[7].events = POLLIN; while (!_task_should_exit) { @@ -330,6 +342,16 @@ Navigator::task_main() _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); } + bool have_geofence_position_data = false; + + /* gps updated */ + if (fds[7].revents & POLLIN) { + gps_position_update(); + if (_geofence.getSource() == Geofence::GF_SOURCE_GPS) { + have_geofence_position_data = true; + } + } + /* sensors combined updated */ if (fds[6].revents & POLLIN) { sensor_combined_update(); @@ -364,14 +386,14 @@ Navigator::task_main() /* global position updated */ if (fds[0].revents & POLLIN) { global_position_update(); - - /* Check geofence violation */ - bool inside = false; - if (_geofence.getAltitudeMode() == Geofence::GF_ALT_MODE_GPS) { - inside = _geofence.inside(_global_pos); - } else { - inside = _geofence.inside(_global_pos, _sensor_combined.baro_alt_meter); + if (_geofence.getSource() == Geofence::GF_SOURCE_GLOBALPOS) { + have_geofence_position_data = true; } + } + + /* Check geofence violation */ + if (have_geofence_position_data) { + bool inside = _geofence.inside(_global_pos, _gps_pos, _sensor_combined.baro_alt_meter); if (!inside) { /* inform other apps via the sp triplet */ _pos_sp_triplet.geofence_violated = true; From 81adc52671d920ffe184948267fcc1f9fbb027cc Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 24 Aug 2014 11:30:02 +0200 Subject: [PATCH 143/411] geofence: add counter threshold for subsequent detections --- src/modules/navigator/geofence.cpp | 22 +++++++++++++++++++++- src/modules/navigator/geofence.h | 8 ++++++-- src/modules/navigator/geofence_params.c | 11 +++++++++++ 3 files changed, 38 insertions(+), 3 deletions(-) diff --git a/src/modules/navigator/geofence.cpp b/src/modules/navigator/geofence.cpp index 4a02a0c3f3..a3805dc0fd 100644 --- a/src/modules/navigator/geofence.cpp +++ b/src/modules/navigator/geofence.cpp @@ -64,7 +64,9 @@ Geofence::Geofence() : _verticesCount(0), _param_geofence_on(this, "ON"), _param_altitude_mode(this, "ALTMODE"), - _param_source(this, "SOURCE") + _param_source(this, "SOURCE"), + _param_counter_threshold(this, "COUNT"), + _outside_counter(0) { /* Load initial params */ updateParams(); @@ -107,6 +109,24 @@ bool Geofence::inside(const struct vehicle_global_position_s &global_position, } bool Geofence::inside(double lat, double lon, float altitude) +{ + bool inside_fence = inside_polygon(lat, lon, altitude); + + if (inside_fence) { + _outside_counter = 0; + return inside_fence; + } { + _outside_counter++; + if(_outside_counter > _param_counter_threshold.get()) { + return inside_fence; + } else { + return true; + } + } +} + + +bool Geofence::inside_polygon(double lat, double lon, float altitude) { /* Return true if geofence is disabled */ if (_param_geofence_on.get() != 1) diff --git a/src/modules/navigator/geofence.h b/src/modules/navigator/geofence.h index 91c74572e3..65ebb0c3db 100644 --- a/src/modules/navigator/geofence.h +++ b/src/modules/navigator/geofence.h @@ -77,9 +77,10 @@ public: */ bool inside(const struct vehicle_global_position_s &global_position); bool inside(const struct vehicle_global_position_s &global_position, float baro_altitude_amsl); - bool inside(double lat, double lon, float altitude); bool inside(const struct vehicle_global_position_s &global_position, const struct vehicle_gps_position_s &gps_position,float baro_altitude_amsl); + bool inside(double lat, double lon, float altitude); + bool inside_polygon(double lat, double lon, float altitude); int clearDm(); @@ -97,7 +98,7 @@ public: bool isEmpty() {return _verticesCount == 0;} int getAltitudeMode() { return _param_altitude_mode.get(); } - + int getSource() { return _param_source.get(); } private: @@ -112,6 +113,9 @@ private: control::BlockParamInt _param_geofence_on; control::BlockParamInt _param_altitude_mode; control::BlockParamInt _param_source; + control::BlockParamInt _param_counter_threshold; + + uint8_t _outside_counter; }; diff --git a/src/modules/navigator/geofence_params.c b/src/modules/navigator/geofence_params.c index 32902ee97a..fca3918e1d 100644 --- a/src/modules/navigator/geofence_params.c +++ b/src/modules/navigator/geofence_params.c @@ -83,3 +83,14 @@ PARAM_DEFINE_INT32(GF_ALTMODE, 0); * @group Geofence */ PARAM_DEFINE_INT32(GF_SOURCE, 0); + +/** + * Geofence counter limit + * + * Set how many subsequent position measurements outside of the fence are needed before geofence violation is triggered + * + * @min -1 + * @max 10 + * @group Geofence + */ +PARAM_DEFINE_INT32(GF_COUNT, -1); From ef0a0a1a6e86aff79a0fd8829ca5c86244fa39bc Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 24 Aug 2014 12:05:12 +0200 Subject: [PATCH 144/411] Fix drop offset: We want to drop so that the wind carries the bottle into the drop zone --- src/modules/bottle_drop/bottle_drop.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/bottle_drop/bottle_drop.cpp b/src/modules/bottle_drop/bottle_drop.cpp index 3eec5a879a..65eca1a62f 100644 --- a/src/modules/bottle_drop/bottle_drop.cpp +++ b/src/modules/bottle_drop/bottle_drop.cpp @@ -566,8 +566,8 @@ BottleDrop::task_main() wind_direction_e = wind.windspeed_east / windspeed_norm; } - x_drop = x_t + x * wind_direction_n; - y_drop = y_t + x * wind_direction_e; + x_drop = x_t - x * wind_direction_n; + y_drop = y_t - x * wind_direction_e; map_projection_reproject(&ref, x_drop, y_drop, &_drop_position.lat, &_drop_position.lon); _drop_position.alt = _target_position.alt + _alt_clearance; From c037cfe6f2daef8ff96cad965c4b040a9d8c62f9 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 24 Aug 2014 12:29:30 +0200 Subject: [PATCH 145/411] datalink loss (obc): add termination after loitering at airfield home --- src/modules/navigator/datalinkloss.cpp | 25 ++++++++++++++++++++- src/modules/navigator/datalinkloss.h | 3 +++ src/modules/navigator/datalinkloss_params.c | 11 +++++++++ src/modules/navigator/rcloss.cpp | 1 + 4 files changed, 39 insertions(+), 1 deletion(-) diff --git a/src/modules/navigator/datalinkloss.cpp b/src/modules/navigator/datalinkloss.cpp index 893d6d93ae..3310984b0b 100644 --- a/src/modules/navigator/datalinkloss.cpp +++ b/src/modules/navigator/datalinkloss.cpp @@ -64,6 +64,7 @@ DataLinkLoss::DataLinkLoss(Navigator *navigator, const char *name) : _param_airfieldhomelat(this, "NAV_AH_LAT", false), _param_airfieldhomelon(this, "NAV_AH_LON", false), _param_airfieldhomealt(this, "NAV_AH_ALT", false), + _param_airfieldhomewaittime(this, "AH_T"), _param_numberdatalinklosses(this, "N"), _dll_state(DLL_STATE_NONE) { @@ -140,7 +141,8 @@ DataLinkLoss::set_dll_item() _mission_item.yaw = NAN; _mission_item.loiter_radius = _navigator->get_loiter_radius(); _mission_item.loiter_direction = 1; - _mission_item.nav_cmd = NAV_CMD_LOITER_UNLIMITED; + _mission_item.nav_cmd = NAV_CMD_LOITER_TIME_LIMIT; + _mission_item.time_inside = _param_airfieldhomewaittime.get() < 0.0f ? 0.0f : _param_airfieldhomewaittime.get(); _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); _mission_item.pitch_min = 0.0f; _mission_item.autocontinue = true; @@ -149,6 +151,15 @@ DataLinkLoss::set_dll_item() _navigator->set_can_loiter_at_sp(true); break; } + case DLL_STATE_TERMINATE: { + /* Request flight termination from the commander */ + pos_sp_triplet->flight_termination = true; + warnx("not switched to manual: request flight termination"); + pos_sp_triplet->previous.valid = false; + pos_sp_triplet->current.valid = false; + pos_sp_triplet->next.valid = false; + break; + } default: break; } @@ -185,6 +196,18 @@ DataLinkLoss::advance_dll() _navigator->get_mission_result()->stay_in_failsafe = true; _navigator->publish_mission_result(); break; + case DLL_STATE_FLYTOAIRFIELDHOMEWP: + _dll_state = DLL_STATE_TERMINATE; + warnx("time is up, state should have been changed manually by now"); + mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: no manual control, terminating"); + _navigator->get_mission_result()->stay_in_failsafe = true; + _navigator->publish_mission_result(); + break; + case DLL_STATE_TERMINATE: + warnx("dll end"); + _dll_state = DLL_STATE_END; + break; + default: break; } diff --git a/src/modules/navigator/datalinkloss.h b/src/modules/navigator/datalinkloss.h index 96b4ce010d..d0c9ad09af 100644 --- a/src/modules/navigator/datalinkloss.h +++ b/src/modules/navigator/datalinkloss.h @@ -72,12 +72,15 @@ private: control::BlockParamInt _param_airfieldhomelat; // * 1e7 control::BlockParamInt _param_airfieldhomelon; // * 1e7 control::BlockParamFloat _param_airfieldhomealt; + control::BlockParamFloat _param_airfieldhomewaittime; control::BlockParamInt _param_numberdatalinklosses; enum DLLState { DLL_STATE_NONE = 0, DLL_STATE_FLYTOCOMMSHOLDWP = 1, DLL_STATE_FLYTOAIRFIELDHOMEWP = 2, + DLL_STATE_TERMINATE = 3, + DLL_STATE_END = 4 } _dll_state; /** diff --git a/src/modules/navigator/datalinkloss_params.c b/src/modules/navigator/datalinkloss_params.c index 02f7ca4c31..db307c904f 100644 --- a/src/modules/navigator/datalinkloss_params.c +++ b/src/modules/navigator/datalinkloss_params.c @@ -91,6 +91,17 @@ PARAM_DEFINE_INT32(NAV_DLL_CH_LON, 1518453890); */ PARAM_DEFINE_FLOAT(NAV_DLL_CH_ALT, 600.0f); +/** + * Aifield hole wait time + * + * The amount of time in seconds the system should wait at the airfield home waypoint + * + * @unit seconds + * @min 0.0 + * @group DLL + */ +PARAM_DEFINE_FLOAT(NAV_DLL_AH_T, 120.0f); + /** * Number of allowed Datalink timeouts * diff --git a/src/modules/navigator/rcloss.cpp b/src/modules/navigator/rcloss.cpp index 427001a013..54f1813f5f 100644 --- a/src/modules/navigator/rcloss.cpp +++ b/src/modules/navigator/rcloss.cpp @@ -132,6 +132,7 @@ RCLoss::set_rcl_item() pos_sp_triplet->previous.valid = false; pos_sp_triplet->current.valid = false; pos_sp_triplet->next.valid = false; + break; } default: break; From ae7c99393606373e3a549946481fe07de6fb4c84 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 24 Aug 2014 12:40:19 +0200 Subject: [PATCH 146/411] datalink loss: add param to allow skipping of comms hold wp --- src/modules/navigator/datalinkloss.cpp | 24 +++++++++++++++------ src/modules/navigator/datalinkloss.h | 1 + src/modules/navigator/datalinkloss_params.c | 14 +++++++++++- 3 files changed, 31 insertions(+), 8 deletions(-) diff --git a/src/modules/navigator/datalinkloss.cpp b/src/modules/navigator/datalinkloss.cpp index 3310984b0b..4e3d258403 100644 --- a/src/modules/navigator/datalinkloss.cpp +++ b/src/modules/navigator/datalinkloss.cpp @@ -66,6 +66,7 @@ DataLinkLoss::DataLinkLoss(Navigator *navigator, const char *name) : _param_airfieldhomealt(this, "NAV_AH_ALT", false), _param_airfieldhomewaittime(this, "AH_T"), _param_numberdatalinklosses(this, "N"), + _param_skipcommshold(this, "CHSK"), _dll_state(DLL_STATE_NONE) { /* load initial params */ @@ -179,14 +180,23 @@ DataLinkLoss::advance_dll() switch (_dll_state) { case DLL_STATE_NONE: /* Check the number of data link losses. If above home fly home directly */ - if (_navigator->get_vstatus()->data_link_lost_counter > _param_numberdatalinklosses.get()) { - warnx("%d data link losses, limit is %d, fly to airfield home", _navigator->get_vstatus()->data_link_lost_counter, _param_numberdatalinklosses.get()); - mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: too many DL losses, fly to home"); - _dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP; + if (!_param_skipcommshold.get()) { + /* if number of data link losses limit is not reached fly to comms hold wp */ + if (_navigator->get_vstatus()->data_link_lost_counter > _param_numberdatalinklosses.get()) { + warnx("%d data link losses, limit is %d, fly to airfield home", + _navigator->get_vstatus()->data_link_lost_counter, _param_numberdatalinklosses.get()); + mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: too many DL losses, fly to home"); + _dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP; + } else { + warnx("fly to comms hold, datalink loss counter: %d", _navigator->get_vstatus()->data_link_lost_counter); + mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: fly to comms hold"); + _dll_state = DLL_STATE_FLYTOCOMMSHOLDWP; + } } else { - warnx("fly to comms hold, datalink loss counter: %d", _navigator->get_vstatus()->data_link_lost_counter); - mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: fly to comms hold"); - _dll_state = DLL_STATE_FLYTOCOMMSHOLDWP; + /* comms hold wp not active, fly to airfield home directly */ + warnx("Skipping comms hold wp. Flying directly to airfield home"); + mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: fly to airfield home, comms hold skipped"); + _dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP; } break; case DLL_STATE_FLYTOCOMMSHOLDWP: diff --git a/src/modules/navigator/datalinkloss.h b/src/modules/navigator/datalinkloss.h index d0c9ad09af..31e0e39078 100644 --- a/src/modules/navigator/datalinkloss.h +++ b/src/modules/navigator/datalinkloss.h @@ -74,6 +74,7 @@ private: control::BlockParamFloat _param_airfieldhomealt; control::BlockParamFloat _param_airfieldhomewaittime; control::BlockParamInt _param_numberdatalinklosses; + control::BlockParamInt _param_skipcommshold; enum DLLState { DLL_STATE_NONE = 0, diff --git a/src/modules/navigator/datalinkloss_params.c b/src/modules/navigator/datalinkloss_params.c index db307c904f..6780c0c88c 100644 --- a/src/modules/navigator/datalinkloss_params.c +++ b/src/modules/navigator/datalinkloss_params.c @@ -107,8 +107,20 @@ PARAM_DEFINE_FLOAT(NAV_DLL_AH_T, 120.0f); * * After more than this number of data link timeouts the aircraft returns home directly * - * @group commander + * @group DLL * @min 0 * @max 1000 */ PARAM_DEFINE_INT32(NAV_DLL_N, 2); + +/** + * Skip comms hold wp + * + * If set to 1 the system will skip the comms hold wp on data link loss and will directly fly to + * airfield home + * + * @group DLL + * @min 0 + * @max 1 + */ +PARAM_DEFINE_INT32(NAV_DLL_CHSK, 0); From c0975af375c168be98804f025192bbb30710355d Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 24 Aug 2014 13:44:43 +0200 Subject: [PATCH 147/411] commander: check if baro is healthy --- src/modules/commander/commander.cpp | 19 +++++++++++++++++++ src/modules/uORB/topics/vehicle_status.h | 2 ++ 2 files changed, 21 insertions(+) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 0bf5cfe343..ddfbd65a19 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1081,6 +1081,25 @@ int commander_thread_main(int argc, char *argv[]) if (updated) { orb_copy(ORB_ID(sensor_combined), sensor_sub, &sensors); + /* Check if the barometer is healthy and issue a warning in the GCS if not so. + * Because the barometer is used for calculating AMSL altitude which is used to ensure + * vertical separation from other airtraffic the operator has to know when the + * barometer is inoperational. + * */ + if (hrt_elapsed_time(&sensors.baro_timestamp) < FAILSAFE_DEFAULT_TIMEOUT) { + /* handle the case where baro was regained */ + if (status.barometer_failure) { + status.barometer_failure = false; + status_changed = true; + mavlink_log_critical(mavlink_fd, "baro healthy"); + } + } else { + if (!status.barometer_failure) { + status.barometer_failure = true; + status_changed = true; + mavlink_log_critical(mavlink_fd, "baro failed"); + } + } } orb_check(diff_pres_sub, &updated); diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index b465f8407d..ad92f5b264 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -211,6 +211,8 @@ struct vehicle_status_s { bool gps_failure; /** Set to true if a gps failure is detected */ bool gps_failure_cmd; /** Set to true if a gps failure mode is commanded */ + bool barometer_failure; /** Set to true if a barometer failure is detected */ + bool offboard_control_signal_found_once; bool offboard_control_signal_lost; bool offboard_control_signal_weak; From 8f2fa6da25ffcb83b27e732e4d22b845a652500f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 24 Aug 2014 14:40:39 +0200 Subject: [PATCH 148/411] Adjust rates for attitude and attitude SP --- src/modules/mavlink/mavlink_main.cpp | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index ed7e879d3d..d5c68c011b 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1387,23 +1387,26 @@ Mavlink::task_main(int argc, char *argv[]) configure_stream("SYS_STATUS", 1.0f); configure_stream("GPS_GLOBAL_ORIGIN", 0.5f); configure_stream("HIGHRES_IMU", 1.0f); - configure_stream("ATTITUDE", 10.0f); + configure_stream("ATTITUDE", 15.0f); configure_stream("VFR_HUD", 8.0f); configure_stream("GPS_RAW_INT", 1.0f); configure_stream("GLOBAL_POSITION_INT", 3.0f); configure_stream("LOCAL_POSITION_NED", 3.0f); configure_stream("RC_CHANNELS_RAW", 1.0f); configure_stream("POSITION_TARGET_GLOBAL_INT", 3.0f); - configure_stream("ATTITUDE_TARGET", 3.0f); + configure_stream("ATTITUDE_TARGET", 15.0f); configure_stream("DISTANCE_SENSOR", 0.5f); configure_stream("OPTICAL_FLOW", 0.5f); break; case MAVLINK_MODE_ONBOARD: configure_stream("SYS_STATUS", 1.0f); - configure_stream("ATTITUDE", 15.0f); + // XXX OBC change back + configure_stream("ATTITUDE", 50.0f); configure_stream("GLOBAL_POSITION_INT", 15.0f); configure_stream("CAMERA_CAPTURE", 1.0f); + configure_stream("ATTITUDE_TARGET", 50.0f); + configure_stream("POSITION_TARGET_GLOBAL_INT", 20.0f); break; default: From bdccd69030e56381d906afeabc8305dbe18e2de6 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 24 Aug 2014 15:27:31 +0200 Subject: [PATCH 149/411] geofence: make some functions private, correctly update params --- src/modules/navigator/geofence.cpp | 2 ++ src/modules/navigator/geofence.h | 7 ++++--- src/modules/navigator/mission_feasibility_checker.cpp | 2 +- 3 files changed, 7 insertions(+), 4 deletions(-) diff --git a/src/modules/navigator/geofence.cpp b/src/modules/navigator/geofence.cpp index a3805dc0fd..5504239c5e 100644 --- a/src/modules/navigator/geofence.cpp +++ b/src/modules/navigator/geofence.cpp @@ -91,6 +91,8 @@ bool Geofence::inside(const struct vehicle_global_position_s &global_position, f bool Geofence::inside(const struct vehicle_global_position_s &global_position, const struct vehicle_gps_position_s &gps_position,float baro_altitude_amsl) { + updateParams(); + if (getAltitudeMode() == Geofence::GF_ALT_MODE_WGS84) { if (getSource() == Geofence::GF_SOURCE_GLOBALPOS) { return inside(global_position); diff --git a/src/modules/navigator/geofence.h b/src/modules/navigator/geofence.h index 65ebb0c3db..65e5b4042b 100644 --- a/src/modules/navigator/geofence.h +++ b/src/modules/navigator/geofence.h @@ -75,11 +75,8 @@ public: * @param craft pointer craft coordinates * @return true: system is inside fence, false: system is outside fence */ - bool inside(const struct vehicle_global_position_s &global_position); - bool inside(const struct vehicle_global_position_s &global_position, float baro_altitude_amsl); bool inside(const struct vehicle_global_position_s &global_position, const struct vehicle_gps_position_s &gps_position,float baro_altitude_amsl); - bool inside(double lat, double lon, float altitude); bool inside_polygon(double lat, double lon, float altitude); int clearDm(); @@ -116,6 +113,10 @@ private: control::BlockParamInt _param_counter_threshold; uint8_t _outside_counter; + + bool inside(double lat, double lon, float altitude); + bool inside(const struct vehicle_global_position_s &global_position); + bool inside(const struct vehicle_global_position_s &global_position, float baro_altitude_amsl); }; diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp index 606521f200..937e4fa5ad 100644 --- a/src/modules/navigator/mission_feasibility_checker.cpp +++ b/src/modules/navigator/mission_feasibility_checker.cpp @@ -109,7 +109,7 @@ bool MissionFeasibilityChecker::checkGeofence(dm_item_t dm_current, size_t nMiss return false; } - if (!geofence.inside(missionitem.lat, missionitem.lon, missionitem.altitude)) { + if (!geofence.inside_polygon(missionitem.lat, missionitem.lon, missionitem.altitude)) { mavlink_log_info(_mavlink_fd, "#audio: Geofence violation waypoint %d", i); return false; } From a432d0493c0761da075c7734c0f54f44d6121e78 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 24 Aug 2014 17:44:36 +0200 Subject: [PATCH 150/411] correctly initialize stay in failsafe flag --- src/modules/navigator/navigator_mode.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/navigator/navigator_mode.cpp b/src/modules/navigator/navigator_mode.cpp index d91f7ab181..3807c5ea82 100644 --- a/src/modules/navigator/navigator_mode.cpp +++ b/src/modules/navigator/navigator_mode.cpp @@ -64,7 +64,7 @@ NavigatorMode::run(bool active) { /* first run */ _first_run = false; /* Reset stay in failsafe flag */ - _navigator->get_mission_result()->stay_in_failsafe = true; + _navigator->get_mission_result()->stay_in_failsafe = false; _navigator->publish_mission_result(); on_activation(); From 06317046f2da215db328be660f900d265cdf9102 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 24 Aug 2014 17:45:15 +0200 Subject: [PATCH 151/411] move flight termination and geofence flags from setpoint triplet to mission result --- src/modules/commander/commander.cpp | 18 +++++------ src/modules/navigator/datalinkloss.cpp | 31 ++++++++++--------- src/modules/navigator/gpsfailure.cpp | 3 +- src/modules/navigator/navigator_main.cpp | 16 ++++------ src/modules/navigator/rcloss.cpp | 3 +- src/modules/uORB/topics/mission_result.h | 8 +++-- .../uORB/topics/position_setpoint_triplet.h | 2 -- 7 files changed, 41 insertions(+), 40 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index ddfbd65a19..f05f970e58 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1328,15 +1328,6 @@ int commander_thread_main(int argc, char *argv[]) if (updated) { orb_copy(ORB_ID(position_setpoint_triplet), pos_sp_triplet_sub, &pos_sp_triplet); - - /* Check for geofence violation */ - if (armed.armed && (pos_sp_triplet.geofence_violated || pos_sp_triplet.flight_termination)) { - //XXX: make this configurable to select different actions (e.g. navigation modes) - /* this will only trigger if geofence is activated via param and a geofence file is present, also there is a circuit breaker to disable the actual flight termination in the px4io driver */ - armed.force_failsafe = true; - status_changed = true; - warnx("Flight termination because of navigator request or geofence"); - } // no reset is done here on purpose, on geofence violation we want to stay in flighttermination } if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) { @@ -1429,6 +1420,15 @@ int commander_thread_main(int argc, char *argv[]) if (updated) { orb_copy(ORB_ID(mission_result), mission_result_sub, &mission_result); + + /* Check for geofence violation */ + if (armed.armed && (mission_result.geofence_violated || mission_result.flight_termination)) { + //XXX: make this configurable to select different actions (e.g. navigation modes) + /* this will only trigger if geofence is activated via param and a geofence file is present, also there is a circuit breaker to disable the actual flight termination in the px4io driver */ + armed.force_failsafe = true; + status_changed = true; + warnx("Flight termination because of navigator request or geofence"); + } // no reset is done here on purpose, on geofence violation we want to stay in flighttermination } /* RC input check */ diff --git a/src/modules/navigator/datalinkloss.cpp b/src/modules/navigator/datalinkloss.cpp index 4e3d258403..66f1c8c731 100644 --- a/src/modules/navigator/datalinkloss.cpp +++ b/src/modules/navigator/datalinkloss.cpp @@ -154,7 +154,8 @@ DataLinkLoss::set_dll_item() } case DLL_STATE_TERMINATE: { /* Request flight termination from the commander */ - pos_sp_triplet->flight_termination = true; + _navigator->get_mission_result()->flight_termination = true; + _navigator->publish_mission_result(); warnx("not switched to manual: request flight termination"); pos_sp_triplet->previous.valid = false; pos_sp_triplet->current.valid = false; @@ -180,23 +181,25 @@ DataLinkLoss::advance_dll() switch (_dll_state) { case DLL_STATE_NONE: /* Check the number of data link losses. If above home fly home directly */ - if (!_param_skipcommshold.get()) { - /* if number of data link losses limit is not reached fly to comms hold wp */ - if (_navigator->get_vstatus()->data_link_lost_counter > _param_numberdatalinklosses.get()) { - warnx("%d data link losses, limit is %d, fly to airfield home", - _navigator->get_vstatus()->data_link_lost_counter, _param_numberdatalinklosses.get()); - mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: too many DL losses, fly to home"); - _dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP; - } else { + /* if number of data link losses limit is not reached fly to comms hold wp */ + if (_navigator->get_vstatus()->data_link_lost_counter > _param_numberdatalinklosses.get()) { + warnx("%d data link losses, limit is %d, fly to airfield home", + _navigator->get_vstatus()->data_link_lost_counter, _param_numberdatalinklosses.get()); + mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: too many DL losses, fly to airfield home"); + _navigator->get_mission_result()->stay_in_failsafe = true; + _navigator->publish_mission_result(); + _dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP; + } else { + if (!_param_skipcommshold.get()) { warnx("fly to comms hold, datalink loss counter: %d", _navigator->get_vstatus()->data_link_lost_counter); mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: fly to comms hold"); _dll_state = DLL_STATE_FLYTOCOMMSHOLDWP; + } else { + /* comms hold wp not active, fly to airfield home directly */ + warnx("Skipping comms hold wp. Flying directly to airfield home"); + mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: fly to airfield home, comms hold skipped"); + _dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP; } - } else { - /* comms hold wp not active, fly to airfield home directly */ - warnx("Skipping comms hold wp. Flying directly to airfield home"); - mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: fly to airfield home, comms hold skipped"); - _dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP; } break; case DLL_STATE_FLYTOCOMMSHOLDWP: diff --git a/src/modules/navigator/gpsfailure.cpp b/src/modules/navigator/gpsfailure.cpp index f0bbbcf1cb..cd55f60b06 100644 --- a/src/modules/navigator/gpsfailure.cpp +++ b/src/modules/navigator/gpsfailure.cpp @@ -140,7 +140,8 @@ GpsFailure::set_gpsf_item() switch (_gpsf_state) { case GPSF_STATE_TERMINATE: { /* Request flight termination from the commander */ - pos_sp_triplet->flight_termination = true; + _navigator->get_mission_result()->flight_termination = true; + _navigator->publish_mission_result(); warnx("gps fail: request flight termination"); } default: diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 81ceaaca2d..c173ecd504 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -395,11 +395,9 @@ Navigator::task_main() if (have_geofence_position_data) { bool inside = _geofence.inside(_global_pos, _gps_pos, _sensor_combined.baro_alt_meter); if (!inside) { - /* inform other apps via the sp triplet */ - _pos_sp_triplet.geofence_violated = true; - if (_pos_sp_triplet.geofence_violated != true) { - _pos_sp_triplet_updated = true; - } + /* inform other apps via the mission result */ + _mission_result.geofence_violated = true; + publish_mission_result(); /* Issue a warning about the geofence violation once */ if (!_geofence_violation_warning_sent) { @@ -407,11 +405,9 @@ Navigator::task_main() _geofence_violation_warning_sent = true; } } else { - /* inform other apps via the sp triplet */ - _pos_sp_triplet.geofence_violated = false; - if (_pos_sp_triplet.geofence_violated != false) { - _pos_sp_triplet_updated = true; - } + /* inform other apps via the mission result */ + _mission_result.geofence_violated = false; + publish_mission_result(); /* Reset the _geofence_violation_warning_sent field */ _geofence_violation_warning_sent = false; } diff --git a/src/modules/navigator/rcloss.cpp b/src/modules/navigator/rcloss.cpp index 54f1813f5f..651e311840 100644 --- a/src/modules/navigator/rcloss.cpp +++ b/src/modules/navigator/rcloss.cpp @@ -127,7 +127,8 @@ RCLoss::set_rcl_item() } case RCL_STATE_TERMINATE: { /* Request flight termination from the commander */ - pos_sp_triplet->flight_termination = true; + _navigator->get_mission_result()->flight_termination = true; + _navigator->publish_mission_result(); warnx("rc not recovered: request flight termination"); pos_sp_triplet->previous.valid = false; pos_sp_triplet->current.valid = false; diff --git a/src/modules/uORB/topics/mission_result.h b/src/modules/uORB/topics/mission_result.h index 65ddfb4ad4..c7d25d1f08 100644 --- a/src/modules/uORB/topics/mission_result.h +++ b/src/modules/uORB/topics/mission_result.h @@ -55,9 +55,11 @@ struct mission_result_s { unsigned seq_reached; /**< Sequence of the mission item which has been reached */ unsigned seq_current; /**< Sequence of the current mission item */ - bool reached; /**< true if mission has been reached */ - bool finished; /**< true if mission has been completed */ - bool stay_in_failsafe; /**< true if the commander should not switch out of the failsafe mode*/ + bool reached; /**< true if mission has been reached */ + bool finished; /**< true if mission has been completed */ + bool stay_in_failsafe; /**< true if the commander should not switch out of the failsafe mode*/ + bool geofence_violated; /**< true if the geofence is violated */ + bool flight_termination; /**< true if the navigator demands a flight termination from the commander app */ }; /** diff --git a/src/modules/uORB/topics/position_setpoint_triplet.h b/src/modules/uORB/topics/position_setpoint_triplet.h index e2b1525a23..ec2131abd8 100644 --- a/src/modules/uORB/topics/position_setpoint_triplet.h +++ b/src/modules/uORB/topics/position_setpoint_triplet.h @@ -97,8 +97,6 @@ struct position_setpoint_triplet_s struct position_setpoint_s next; unsigned nav_state; /**< report the navigation state */ - bool geofence_violated; /**< true if the geofence is violated */ - bool flight_termination; /**< true if the navigator demands a flight termination from the commander app */ }; /** From 98e74ed0e74e02f866fc7538c7d4153ae3c36743 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 24 Aug 2014 22:18:01 +0200 Subject: [PATCH 152/411] commander: limit the output of a warnx --- src/modules/commander/commander.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index f05f970e58..2fcf7bebb1 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1427,7 +1427,11 @@ int commander_thread_main(int argc, char *argv[]) /* this will only trigger if geofence is activated via param and a geofence file is present, also there is a circuit breaker to disable the actual flight termination in the px4io driver */ armed.force_failsafe = true; status_changed = true; - warnx("Flight termination because of navigator request or geofence"); + static bool flight_termination_printed = false; + if (!flight_termination_printed) { + warnx("Flight termination because of navigator request or geofence"); + flight_termination_printed = true; + } } // no reset is done here on purpose, on geofence violation we want to stay in flighttermination } From 55fde23233de3d311d6c8d0b2cd368e45af3caac Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 25 Aug 2014 09:19:36 +0200 Subject: [PATCH 153/411] support new yaw and yawrate fields in mavlnk position_target message --- src/modules/mavlink/mavlink_receiver.cpp | 35 ++++++++++++++++--- .../uORB/topics/offboard_control_setpoint.h | 22 +++++++++++- .../uORB/topics/position_setpoint_triplet.h | 2 ++ 3 files changed, 54 insertions(+), 5 deletions(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 60fc2937a9..3ac4d3b03d 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -474,6 +474,8 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t offboard_control_sp.acceleration[0] = set_position_target_local_ned.afx; offboard_control_sp.acceleration[1] = set_position_target_local_ned.afy; offboard_control_sp.acceleration[2] = set_position_target_local_ned.afz; + offboard_control_sp.yaw = set_position_target_local_ned.yaw; + offboard_control_sp.yaw_rate = set_position_target_local_ned.yaw_rate; offboard_control_sp.isForceSetpoint = (bool)(set_position_target_local_ned.type_mask & (1 << 9)); /* If we are in force control mode, for now set offboard mode to force control */ @@ -486,7 +488,12 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t offboard_control_sp.ignore &= ~(1 << i); offboard_control_sp.ignore |= (set_position_target_local_ned.type_mask & (1 << i)); } - + offboard_control_sp.ignore &= ~(1 << OFB_IGN_BIT_YAW); + offboard_control_sp.ignore |= (set_position_target_local_ned.type_mask & (1 << 10)) << + OFB_IGN_BIT_YAW; + offboard_control_sp.ignore &= ~(1 << OFB_IGN_BIT_YAWRATE); + offboard_control_sp.ignore |= (set_position_target_local_ned.type_mask & (1 << 11)) << + OFB_IGN_BIT_YAWRATE; offboard_control_sp.timestamp = hrt_absolute_time(); @@ -527,12 +534,12 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t 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_POSITION; //XXX support others /* set the local pos values if the setpoint type is 'local pos' and none * of the local pos fields is set to 'ignore' */ if (offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED && !offboard_control_sp_ignore_position_some(offboard_control_sp)) { - pos_sp_triplet.current.type = SETPOINT_TYPE_POSITION; //XXX support others pos_sp_triplet.current.position_valid = true; pos_sp_triplet.current.x = offboard_control_sp.position[0]; pos_sp_triplet.current.y = offboard_control_sp.position[1]; @@ -545,7 +552,6 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t * of the local vel fields is set to 'ignore' */ if (offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED && !offboard_control_sp_ignore_velocity_some(offboard_control_sp)) { - pos_sp_triplet.current.type = SETPOINT_TYPE_POSITION; //XXX support others pos_sp_triplet.current.velocity_valid = true; pos_sp_triplet.current.vx = offboard_control_sp.velocity[0]; pos_sp_triplet.current.vy = offboard_control_sp.velocity[1]; @@ -558,7 +564,6 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t * of the accelerations fields is set to 'ignore' */ if (offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED && !offboard_control_sp_ignore_acceleration_some(offboard_control_sp)) { - pos_sp_triplet.current.type = SETPOINT_TYPE_POSITION; //XXX support others pos_sp_triplet.current.acceleration_valid = true; pos_sp_triplet.current.a_x = offboard_control_sp.acceleration[0]; pos_sp_triplet.current.a_y = offboard_control_sp.acceleration[1]; @@ -569,6 +574,28 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t } else { pos_sp_triplet.current.acceleration_valid = false; } + + /* set the yaw sp value if the setpoint type is 'local pos' and the yaw + * field is not set to 'ignore' */ + if (offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED && + !offboard_control_sp_ignore_yaw(offboard_control_sp)) { + pos_sp_triplet.current.yaw_valid = true; + pos_sp_triplet.current.yaw = offboard_control_sp.yaw; + + } else { + pos_sp_triplet.current.yaw_valid = false; + } + + /* set the yawrate sp value if the setpoint type is 'local pos' and the yawrate + * field is not set to 'ignore' */ + if (offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED && + !offboard_control_sp_ignore_yawrate(offboard_control_sp)) { + pos_sp_triplet.current.yawspeed_valid = true; + pos_sp_triplet.current.yawspeed = offboard_control_sp.yaw_rate; + + } else { + pos_sp_triplet.current.yawspeed_valid = false; + } //XXX handle global pos setpoints (different MAV frames) diff --git a/src/modules/uORB/topics/offboard_control_setpoint.h b/src/modules/uORB/topics/offboard_control_setpoint.h index 6e37896afe..72a28e5017 100644 --- a/src/modules/uORB/topics/offboard_control_setpoint.h +++ b/src/modules/uORB/topics/offboard_control_setpoint.h @@ -86,7 +86,9 @@ enum {OFB_IGN_BIT_POS_X, OFB_IGN_BIT_BODYRATE_Y, OFB_IGN_BIT_BODYRATE_Z, OFB_IGN_BIT_ATT, - OFB_IGN_BIT_THRUST + OFB_IGN_BIT_THRUST, + OFB_IGN_BIT_YAW, + OFB_IGN_BIT_YAWRATE, }; /** @@ -105,6 +107,10 @@ struct offboard_control_setpoint_s { float attitude[4]; /**< attitude of vehicle (quaternion) */ float attitude_rate[3]; /**< body angular rates (x, y, z) */ float thrust; /**< thrust */ + float yaw; /**< yaw: this is the yaw from the position_target message + (not from the full attitude_target message) */ + float yaw_rate; /**< yaw rate: this is the yaw from the position_target message + (not from the full attitude_target message) */ uint16_t ignore; /**< if field i is set to true, the value should be ignored, see definition at top of file for mapping */ @@ -249,6 +255,20 @@ inline bool offboard_control_sp_ignore_thrust(const struct offboard_control_setp return (bool)(offboard_control_sp.ignore & (1 << OFB_IGN_BIT_THRUST)); } +/** + * Returns true if the yaw setpoint should be ignored + */ +inline bool offboard_control_sp_ignore_yaw(const struct offboard_control_setpoint_s &offboard_control_sp) { + return (bool)(offboard_control_sp.ignore & (1 << OFB_IGN_BIT_YAW)); +} + +/** + * Returns true if the yaw rate setpoint should be ignored + */ +inline bool offboard_control_sp_ignore_yawrate(const struct offboard_control_setpoint_s &offboard_control_sp) { + return (bool)(offboard_control_sp.ignore & (1 << OFB_IGN_BIT_YAWRATE)); +} + /* register this as object request broker structure */ ORB_DECLARE(offboard_control_setpoint); diff --git a/src/modules/uORB/topics/position_setpoint_triplet.h b/src/modules/uORB/topics/position_setpoint_triplet.h index 3ea3f671e0..cb2262534d 100644 --- a/src/modules/uORB/topics/position_setpoint_triplet.h +++ b/src/modules/uORB/topics/position_setpoint_triplet.h @@ -79,7 +79,9 @@ struct position_setpoint_s double lon; /**< longitude, in deg */ float alt; /**< altitude AMSL, in m */ float yaw; /**< yaw (only for multirotors), in rad [-PI..PI), NaN = hold current yaw */ + bool yaw_valid; /**< true if yaw setpoint valid */ float yawspeed; /**< yawspeed (only for multirotors, in rad/s) */ + bool yawspeed_valid; /**< true if yawspeed setpoint valid */ 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 9bda573151ae1b5fa87686ee58596ea14e052941 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 25 Aug 2014 13:15:30 +0200 Subject: [PATCH 154/411] mc pos control: offboard: set yaw and yawspeed depending on valid flags --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 6 +++++- 1 file changed, 5 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 ecc40428de..4f5c62a592 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -614,7 +614,6 @@ MulticopterPositionControl::control_offboard(float dt) _pos_sp(0) = _pos_sp_triplet.current.x; _pos_sp(1) = _pos_sp_triplet.current.y; _pos_sp(2) = _pos_sp_triplet.current.z; - _att_sp.yaw_body = _pos_sp_triplet.current.yaw; } else if (_control_mode.flag_control_velocity_enabled && _pos_sp_triplet.current.velocity_valid) { /* control velocity */ @@ -624,6 +623,11 @@ MulticopterPositionControl::control_offboard(float dt) /* set position setpoint move rate */ _sp_move_rate(0) = _pos_sp_triplet.current.vx; _sp_move_rate(1) = _pos_sp_triplet.current.vy; + } + + if (_pos_sp_triplet.current.yaw_valid) { + _att_sp.yaw_body = _pos_sp_triplet.current.yaw; + } else if (_pos_sp_triplet.current.yawspeed_valid) { _att_sp.yaw_body = _att_sp.yaw_body + _pos_sp_triplet.current.yawspeed * dt; } From 8a9da209d194b4f35000935379901ed6091091f9 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 25 Aug 2014 23:17:56 +0200 Subject: [PATCH 155/411] limit warnx output on flight termination --- src/modules/commander/commander.cpp | 12 ++++++++++-- 1 file changed, 10 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 2fcf7bebb1..8b065560f4 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1610,7 +1610,11 @@ int commander_thread_main(int argc, char *argv[]) (status.data_link_lost_cmd && status.gps_failure_cmd)) { armed.force_failsafe = true; status_changed = true; - warnx("Flight termination because of data link loss && gps failure"); + static bool flight_termination_printed = false; + if (!flight_termination_printed) { + warnx("Flight termination because of data link loss && gps failure"); + flight_termination_printed = true; + } mavlink_log_critical(mavlink_fd, "DL and GPS lost: flight termination"); } @@ -1625,7 +1629,11 @@ int commander_thread_main(int argc, char *argv[]) (status.rc_signal_lost_cmd && status.gps_failure_cmd))) { armed.force_failsafe = true; status_changed = true; - warnx("Flight termination because of RC signal loss && gps failure"); + static bool flight_termination_printed = false; + if (!flight_termination_printed) { + warnx("Flight termination because of RC signal loss && gps failure"); + flight_termination_printed = true; + } mavlink_log_critical(mavlink_fd, "RC and GPS lost: flight termination"); } } From ccdfab245bac02565521f2d76e604f54ba3f5bd5 Mon Sep 17 00:00:00 2001 From: Holger Steinhaus Date: Tue, 26 Aug 2014 20:26:47 +0200 Subject: [PATCH 156/411] mixer_multirotor: topic for motor limit notification --- src/modules/uORB/objects_common.cpp | 3 + .../uORB/topics/multirotor_motor_limits.h | 69 +++++++++++++++++++ 2 files changed, 72 insertions(+) create mode 100644 src/modules/uORB/topics/multirotor_motor_limits.h diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index f7d5f87374..b921865ebe 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -192,6 +192,9 @@ ORB_DEFINE(actuator_outputs_1, struct actuator_outputs_s); ORB_DEFINE(actuator_outputs_2, struct actuator_outputs_s); ORB_DEFINE(actuator_outputs_3, struct actuator_outputs_s); +#include "topics/multirotor_motor_limits.h" +ORB_DEFINE(multirotor_motor_limits, struct multirotor_motor_limits_s); + #include "topics/telemetry_status.h" ORB_DEFINE(telemetry_status_0, struct telemetry_status_s); ORB_DEFINE(telemetry_status_1, struct telemetry_status_s); diff --git a/src/modules/uORB/topics/multirotor_motor_limits.h b/src/modules/uORB/topics/multirotor_motor_limits.h new file mode 100644 index 0000000000..44c51e4d98 --- /dev/null +++ b/src/modules/uORB/topics/multirotor_motor_limits.h @@ -0,0 +1,69 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +/** + * @file multirotor_motor_limits.h + * + * Definition of multirotor_motor_limits topic + */ + +#ifndef MULTIROTOR_MOTOR_LIMITS_H_ +#define MULTIROTOR_MOTOR_LIMITS_H_ + +#include "../uORB.h" +#include + +/** + * @addtogroup topics + * @{ + */ + +/** + * Motor limits + */ +struct multirotor_motor_limits_s { + uint8_t roll_pitch : 1; // roll/pitch limit reached + uint8_t yaw : 1; // yaw limit reached + uint8_t throttle_lower : 1; // lower throttle limit reached + uint8_t throttle_upper : 1; // upper throttle limit reached + uint8_t reserved : 4; +}; + +/** + * @} + */ + +/* register this as object request broker structure */ +ORB_DECLARE(multirotor_motor_limits); + +#endif From 5aafa1b021a80868a3b5aebde13cd54d407e6e41 Mon Sep 17 00:00:00 2001 From: Holger Steinhaus Date: Tue, 26 Aug 2014 20:36:02 +0200 Subject: [PATCH 157/411] mixer_multirotor: motor limit notification for PX4FMU --- src/modules/systemlib/mixer/mixer.h | 6 +++++ .../systemlib/mixer/mixer_multirotor.cpp | 22 ++++++++++++++++++- 2 files changed, 27 insertions(+), 1 deletion(-) diff --git a/src/modules/systemlib/mixer/mixer.h b/src/modules/systemlib/mixer/mixer.h index cdf60febc7..17989558e6 100644 --- a/src/modules/systemlib/mixer/mixer.h +++ b/src/modules/systemlib/mixer/mixer.h @@ -130,6 +130,9 @@ #include #include "drivers/drv_mixer.h" + +#include + #include "mixer_load.h" /** @@ -531,6 +534,9 @@ private: float _yaw_scale; float _idle_speed; + orb_advert_t _limits_pub; + multirotor_motor_limits_s _limits; + unsigned _rotor_count; const Rotor *_rotors; diff --git a/src/modules/systemlib/mixer/mixer_multirotor.cpp b/src/modules/systemlib/mixer/mixer_multirotor.cpp index 4b22a46d0b..57e17b67d2 100644 --- a/src/modules/systemlib/mixer/mixer_multirotor.cpp +++ b/src/modules/systemlib/mixer/mixer_multirotor.cpp @@ -36,7 +36,8 @@ * * Multi-rotor mixers. */ - +#include +#include #include #include @@ -302,6 +303,11 @@ MultirotorMixer::mix(float *outputs, unsigned space) float min_out = 0.0f; float max_out = 0.0f; + _limits.roll_pitch = false; + _limits.yaw = false; + _limits.throttle_upper = false; + _limits.throttle_lower = false; + /* perform initial mix pass yielding unbounded outputs, ignore yaw */ for (unsigned i = 0; i < _rotor_count; i++) { float out = roll * _rotors[i].roll_scale + @@ -311,6 +317,7 @@ MultirotorMixer::mix(float *outputs, unsigned space) /* limit yaw if it causes outputs clipping */ if (out >= 0.0f && out < -yaw * _rotors[i].yaw_scale) { yaw = -out / _rotors[i].yaw_scale; + _limits.yaw = true; } /* calculate min and max output values */ @@ -332,6 +339,7 @@ MultirotorMixer::mix(float *outputs, unsigned space) for (unsigned i = 0; i < _rotor_count; i++) { outputs[i] = scale_in * (roll * _rotors[i].roll_scale + pitch * _rotors[i].pitch_scale) + thrust; } + _limits.roll_pitch = true; } else { /* roll/pitch mixed without limiting, add yaw control */ @@ -344,6 +352,7 @@ MultirotorMixer::mix(float *outputs, unsigned space) float scale_out; if (max_out > 1.0f) { scale_out = 1.0f / max_out; + _limits.throttle_upper = true; } else { scale_out = 1.0f; @@ -351,9 +360,20 @@ MultirotorMixer::mix(float *outputs, unsigned space) /* scale outputs to range _idle_speed..1, and do final limiting */ for (unsigned i = 0; i < _rotor_count; i++) { + if (outputs[i] < _idle_speed) { + _limits.throttle_lower = true; + } outputs[i] = constrain(_idle_speed + (outputs[i] * (1.0f - _idle_speed) * scale_out), _idle_speed, 1.0f); } +#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1) || defined(CONFIG_ARCH_BOARD_PX4FMU_V2) + /* publish/advertise motor limits if running on FMU */ + if (_limits_pub > 0) { + orb_publish(ORB_ID(multirotor_motor_limits), _limits_pub, &_limits); + } else { + _limits_pub = orb_advertise(ORB_ID(multirotor_motor_limits), &_limits); + } +#endif return _rotor_count; } From 6ece4cf7c44869d2fd6e027ad6239baf3af860df Mon Sep 17 00:00:00 2001 From: Holger Steinhaus Date: Tue, 26 Aug 2014 21:08:16 +0200 Subject: [PATCH 158/411] ORB topic for drive testing requests --- src/modules/uORB/objects_common.cpp | 3 ++ src/modules/uORB/topics/test_motor.h | 70 ++++++++++++++++++++++++++++ 2 files changed, 73 insertions(+) create mode 100644 src/modules/uORB/topics/test_motor.h diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index f7d5f87374..b8cdb45eb4 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -198,6 +198,9 @@ ORB_DEFINE(telemetry_status_1, struct telemetry_status_s); ORB_DEFINE(telemetry_status_2, struct telemetry_status_s); ORB_DEFINE(telemetry_status_3, struct telemetry_status_s); +#include "topics/test_motor.h" +ORB_DEFINE(test_motor, struct test_motor_s); + #include "topics/debug_key_value.h" ORB_DEFINE(debug_key_value, struct debug_key_value_s); diff --git a/src/modules/uORB/topics/test_motor.h b/src/modules/uORB/topics/test_motor.h new file mode 100644 index 0000000000..491096934a --- /dev/null +++ b/src/modules/uORB/topics/test_motor.h @@ -0,0 +1,70 @@ +/**************************************************************************** + * + * Copyright (C) 2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file test_motor.h + * + * Test a single drive motor while the vehicle is disarmed + * + * Publishing values to this topic causes a single motor to spin, e.g. for + * ground test purposes. This topic will be ignored while the vehicle is armed. + * + */ + +#ifndef TOPIC_TEST_MOTOR_H +#define TOPIC_TEST_MOTOR_H + +#include +#include "../uORB.h" + +#define NUM_MOTOR_OUTPUTS 8 + +/** + * @addtogroup topics + * @{ + */ + +struct test_motor_s { + uint64_t timestamp; /**< output timestamp in us since system boot */ + unsigned motor_number; /**< number of motor to spin */ + float value; /**< output data, in natural output units */ +}; + +/** + * @} + */ + +/* actuator output sets; this list can be expanded as more drivers emerge */ +ORB_DECLARE(test_motor); + +#endif From f8ba5f8dae146529e3ff28f4330cdc4daaa6f260 Mon Sep 17 00:00:00 2001 From: Holger Steinhaus Date: Tue, 26 Aug 2014 21:10:39 +0200 Subject: [PATCH 159/411] cmd line utility for controlling drive tests --- src/systemcmds/motor_test/module.mk | 41 ++++++++ src/systemcmds/motor_test/motor_test.c | 128 +++++++++++++++++++++++++ 2 files changed, 169 insertions(+) create mode 100644 src/systemcmds/motor_test/module.mk create mode 100644 src/systemcmds/motor_test/motor_test.c diff --git a/src/systemcmds/motor_test/module.mk b/src/systemcmds/motor_test/module.mk new file mode 100644 index 0000000000..eb36d2ded8 --- /dev/null +++ b/src/systemcmds/motor_test/module.mk @@ -0,0 +1,41 @@ +############################################################################ +# +# Copyright (c) 2014 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +# +# Build the motor_test tool. +# + +MODULE_COMMAND = motor_test +SRCS = motor_test.c + +MODULE_STACKSIZE = 4096 diff --git a/src/systemcmds/motor_test/motor_test.c b/src/systemcmds/motor_test/motor_test.c new file mode 100644 index 0000000000..079f99674a --- /dev/null +++ b/src/systemcmds/motor_test/motor_test.c @@ -0,0 +1,128 @@ +/**************************************************************************** + * + * Copyright (C) 2014 PX4 Development Team. All rights reserved. + * Author: Holger Steinhaus + * + * 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 motor_test.c + * + * Tool for drive testing + */ + +#include + +#include +#include +#include +#include + +#include +#include +#include + + +#include "systemlib/systemlib.h" +#include "systemlib/err.h" + + +__EXPORT int motor_test_main(int argc, char *argv[]); +static void motor_test(unsigned channel, float value); +static void usage(const char *reason); + +void motor_test(unsigned channel, float value) +{ + orb_advert_t _test_motor_pub; + struct test_motor_s _test_motor; + + _test_motor.motor_number = channel; + _test_motor.timestamp = hrt_absolute_time(); + _test_motor.value = value; + + if (_test_motor_pub > 0) { + /* publish armed state */ + orb_publish(ORB_ID(test_motor), _test_motor_pub, &_test_motor); + } else { + /* advertise and publish */ + _test_motor_pub = orb_advertise(ORB_ID(test_motor), &_test_motor); + } +} + +static void usage(const char *reason) +{ + if (reason != NULL) + warnx("%s", reason); + + errx(1, + "usage:\n" + "motor_test\n" + " -m Motor to test (0..7)\n" + " -p Power (0..100)\n"); +} + +int motor_test_main(int argc, char *argv[]) +{ + unsigned long channel, lval; + float value; + int ch; + + if (argc != 5) { + usage("please specify motor and power"); + } + + while ((ch = getopt(argc, argv, "m:p:")) != EOF) { + switch (ch) { + + case 'm': + /* Read in channels supplied as one int and convert to mask: 1234 -> 0xF */ + channel = strtoul(optarg, NULL, 0); + break; + + case 'p': + /* Read in custom low value */ + lval = strtoul(optarg, NULL, 0); + + if (lval > 100) + usage("value invalid"); + + value = (float)lval/100.f; + break; + default: + usage(NULL); + } + } + + motor_test(channel, value); + + printf("motor %d set to %.2f\n", channel, value); + + exit(0); +} From 3d528a2c979e7d0df1171afc1f038759c7b01383 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 26 Aug 2014 22:22:59 +0200 Subject: [PATCH 160/411] introduce new nav state to allow normal rtl with RC switch --- src/modules/commander/state_machine_helper.cpp | 2 +- src/modules/navigator/navigator_main.cpp | 5 ++++- src/modules/uORB/topics/vehicle_status.h | 1 + 3 files changed, 6 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 098ff1a3d9..e3b5d30e49 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -462,7 +462,7 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en status->failsafe = true; if (status->condition_global_position_valid && status->condition_home_position_valid) { - status->nav_state = NAVIGATION_STATE_AUTO_RTL; + status->nav_state = NAVIGATION_STATE_AUTO_RCRECOVER; } else if (status->condition_local_position_valid) { status->nav_state = NAVIGATION_STATE_LAND; } else if (status->condition_local_altitude_valid) { diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index c173ecd504..9a8c54e7e6 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -433,13 +433,16 @@ Navigator::task_main() case NAVIGATION_STATE_AUTO_LOITER: _navigation_mode = &_loiter; break; - case NAVIGATION_STATE_AUTO_RTL: + case NAVIGATION_STATE_AUTO_RCRECOVER: if (_param_rcloss_obc.get() != 0) { _navigation_mode = &_rcLoss; } else { _navigation_mode = &_rtl; } break; + case NAVIGATION_STATE_AUTO_RTL: + _navigation_mode = &_rtl; + break; case NAVIGATION_STATE_AUTO_RTGS: //XXX OBC: differentiate between rc loss and dl loss here /* Use complex data link loss mode only when enabled via param * otherwise use rtl */ diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index ad92f5b264..9dccb2309d 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -102,6 +102,7 @@ typedef enum { NAVIGATION_STATE_AUTO_MISSION, /**< Auto mission mode */ NAVIGATION_STATE_AUTO_LOITER, /**< Auto loiter mode */ NAVIGATION_STATE_AUTO_RTL, /**< Auto return to launch mode */ + NAVIGATION_STATE_AUTO_RCRECOVER, /**< RC recover mode */ NAVIGATION_STATE_AUTO_RTGS, /**< Auto return to groundstation on data link loss */ NAVIGATION_STATE_AUTO_LANDENGFAIL, /**< Auto land on engine failure */ NAVIGATION_STATE_AUTO_LANDGPSFAIL, /**< Auto land on gps failure (e.g. open loop loiter down) */ From 5e5322c593c6bd8ccd894f47ab8fd88b72e51677 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 26 Aug 2014 22:46:09 +0200 Subject: [PATCH 161/411] fix flight termination circuit breaker name, tested --- src/drivers/px4io/px4io.cpp | 2 +- src/modules/systemlib/circuit_breaker.c | 2 +- src/modules/systemlib/circuit_breaker.h | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index d5f569d71a..b5c5ef17c9 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1170,7 +1170,7 @@ PX4IO::io_set_arming_state() } /* Do not set failsafe if circuit breaker is enabled */ - if (armed.force_failsafe && !circuit_breaker_enabled("CBRK_FLIGHTTERMINATION", CBRK_FLIGHTTERMINATION_KEY)) { + if (armed.force_failsafe && !circuit_breaker_enabled("CBRK_FLIGHTTERM", CBRK_FLIGHTTERM_KEY)) { set |= PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE; } else { clear |= PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE; diff --git a/src/modules/systemlib/circuit_breaker.c b/src/modules/systemlib/circuit_breaker.c index 8a0b51b71b..b0f95aedf2 100644 --- a/src/modules/systemlib/circuit_breaker.c +++ b/src/modules/systemlib/circuit_breaker.c @@ -106,7 +106,7 @@ PARAM_DEFINE_INT32(CBRK_AIRSPD_CHK, 0); * @max 121212 * @group Circuit Breaker */ -PARAM_DEFINE_INT32(CBRK_FLIGHTTERMINATION, 0); +PARAM_DEFINE_INT32(CBRK_FLIGHTTERM, 0); bool circuit_breaker_enabled(const char* breaker, int32_t magic) { diff --git a/src/modules/systemlib/circuit_breaker.h b/src/modules/systemlib/circuit_breaker.h index 54c4fced62..445a89d3a7 100644 --- a/src/modules/systemlib/circuit_breaker.h +++ b/src/modules/systemlib/circuit_breaker.h @@ -53,7 +53,7 @@ #define CBRK_RATE_CTRL_KEY 140253 #define CBRK_IO_SAFETY_KEY 22027 #define CBRK_AIRSPD_CHK_KEY 162128 -#define CBRK_FLIGHTTERMINATION_KEY 121212 +#define CBRK_FLIGHTTERM_KEY 121212 #include From 9cc1f1ab9db4af9af18e6879ba82cbcfa8e588f3 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 26 Aug 2014 23:12:28 +0200 Subject: [PATCH 162/411] flight termination on gps failure && datalink loss: do not activate in manual modes --- src/modules/commander/commander.cpp | 11 ++++++++--- 1 file changed, 8 insertions(+), 3 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 8b065560f4..5673037b88 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1605,9 +1605,14 @@ int commander_thread_main(int argc, char *argv[]) /* Check for failure combinations which lead to flight termination */ if (armed.armed) { /* At this point the data link and the gps system have been checked - * If both failed we want to terminate the flight */ - if ((status.data_link_lost && status.gps_failure) || - (status.data_link_lost_cmd && status.gps_failure_cmd)) { + * If we are not in a manual (RC stick controlled mode) + * and both failed we want to terminate the flight */ + if (status.main_state != MAIN_STATE_MANUAL && + status.main_state != MAIN_STATE_ACRO && + status.main_state != MAIN_STATE_ALTCTL && + status.main_state != MAIN_STATE_POSCTL && + ((status.data_link_lost && status.gps_failure) || + (status.data_link_lost_cmd && status.gps_failure_cmd))) { armed.force_failsafe = true; status_changed = true; static bool flight_termination_printed = false; From c79bc7073eb65f08b724a3893d183d20781f06e2 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 27 Aug 2014 07:59:34 +0200 Subject: [PATCH 163/411] Support for termination failsafe in PWM out driver --- src/drivers/drv_pwm_output.h | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/src/drivers/drv_pwm_output.h b/src/drivers/drv_pwm_output.h index 5aff6825be..bc586f3950 100644 --- a/src/drivers/drv_pwm_output.h +++ b/src/drivers/drv_pwm_output.h @@ -205,10 +205,13 @@ ORB_DECLARE(output_pwm); #define PWM_SERVO_GET_DISABLE_LOCKDOWN _IOC(_PWM_SERVO_BASE, 22) /** force safety switch off (to disable use of safety switch) */ -#define PWM_SERVO_SET_FORCE_SAFETY_OFF _IOC(_PWM_SERVO_BASE, 23) +#define PWM_SERVO_SET_FORCE_SAFETY_OFF _IOC(_PWM_SERVO_BASE, 23) /** force failsafe mode (failsafe values are set immediately even if failsafe condition not met) */ -#define PWM_SERVO_SET_FORCE_FAILSAFE _IOC(_PWM_SERVO_BASE, 24) +#define PWM_SERVO_SET_FORCE_FAILSAFE _IOC(_PWM_SERVO_BASE, 24) + +/** make failsafe non-recoverable (termination) if it occurs */ +#define PWM_SERVO_SET_TERMINATION_FAILSAFE _IOC(_PWM_SERVO_BASE, 25) /* * From 1fbdca4ee988d5816eebbd9fef95ce498bacfd14 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 27 Aug 2014 07:59:53 +0200 Subject: [PATCH 164/411] Add command to run termination failsafe --- src/systemcmds/pwm/pwm.c | 21 ++++++++++++++++++++- 1 file changed, 20 insertions(+), 1 deletion(-) diff --git a/src/systemcmds/pwm/pwm.c b/src/systemcmds/pwm/pwm.c index c8d698b86a..478c2a772f 100644 --- a/src/systemcmds/pwm/pwm.c +++ b/src/systemcmds/pwm/pwm.c @@ -654,9 +654,28 @@ pwm_main(int argc, char *argv[]) } } exit(0); + } else if (!strcmp(argv[1], "terminatefail")) { + + if (argc < 3) { + errx(1, "arg missing [on|off]"); + } else { + + if (!strcmp(argv[2], "on")) { + /* force failsafe */ + ret = ioctl(fd, PWM_SERVO_SET_TERMINATION_FAILSAFE, 1); + } else { + /* force failsafe */ + ret = ioctl(fd, PWM_SERVO_SET_TERMINATION_FAILSAFE, 0); + } + + if (ret != OK) { + warnx("FAILED setting termination failsafe %s", argv[2]); + } + } + exit(0); } - usage("specify arm|disarm|rate|failsafe|disarmed|min|max|test|info|forcefail"); + usage("specify arm|disarm|rate|failsafe\n\t\tdisarmed|min|max|test|info|forcefail|terminatefail"); return 0; } From 49846d476f77290093c24097e05d5a8d60d1a4f1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 27 Aug 2014 08:00:12 +0200 Subject: [PATCH 165/411] IO firmware supports termination failsafe --- src/modules/px4iofirmware/mixer.cpp | 10 ++++++++++ src/modules/px4iofirmware/protocol.h | 3 ++- src/modules/px4iofirmware/registers.c | 18 ++++++++++++++++-- 3 files changed, 28 insertions(+), 3 deletions(-) diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index 606c639f94..1eacda97a6 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -154,6 +154,16 @@ mixer_tick(void) } } + /* + * Check if failsafe termination is set - if yes, + * set the force failsafe flag once entering the first + * failsafe condition. + */ + if ((r_setup_arming & PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE) && + (source == MIX_FAILSAFE)) { + r_setup_arming |= PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE; + } + /* * Check if we should force failsafe - and do it if we have to */ diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h index 0507836871..eae7f9567a 100644 --- a/src/modules/px4iofirmware/protocol.h +++ b/src/modules/px4iofirmware/protocol.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. + * 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 @@ -180,6 +180,7 @@ #define PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED (1 << 6) /* Disable the IO-internal evaluation of the RC */ #define PX4IO_P_SETUP_ARMING_LOCKDOWN (1 << 7) /* If set, the system operates normally, but won't actuate any servos */ #define PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE (1 << 8) /* If set, the system will always output the failsafe values */ +#define PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE (1 << 9) /* If set, the system will never return from a failsafe, but remain in failsafe once triggered. */ #define PX4IO_P_SETUP_PWM_RATES 2 /* bitmask, 0 = low rate, 1 = high rate */ #define PX4IO_P_SETUP_PWM_DEFAULTRATE 3 /* 'low' PWM frame output rate in Hz */ diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index 0da778b6f6..7a5a5e4846 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. + * 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 @@ -190,7 +190,8 @@ volatile uint16_t r_page_setup[] = PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE | \ PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED | \ PX4IO_P_SETUP_ARMING_LOCKDOWN | \ - PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE) + PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE | \ + PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE) #define PX4IO_P_SETUP_RATES_VALID ((1 << PX4IO_SERVO_COUNT) - 1) #define PX4IO_P_SETUP_RELAYS_VALID ((1 << PX4IO_RELAY_CHANNELS) - 1) @@ -518,6 +519,19 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) r_status_flags |= PX4IO_P_STATUS_FLAGS_INIT_OK; } + /* + * If the failsafe termination flag is set, do not allow the autopilot to unset it + */ + value |= (r_setup_arming & PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE); + + /* + * If failsafe termination is enabled and force failsafe bit is set, do not allow + * the autopilot to clear it. + */ + if (r_setup_arming & PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE) { + value |= (r_setup_arming & PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE); + } + r_setup_arming = value; break; From a7109609ecfeba0a11121c1b83a46b1463f55931 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 27 Aug 2014 08:00:28 +0200 Subject: [PATCH 166/411] support termination failsafe in IO driver --- src/drivers/px4io/px4io.cpp | 22 +++++++++++++++++++++- 1 file changed, 21 insertions(+), 1 deletion(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 97919538ff..3fdb1dd1f8 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1175,6 +1175,14 @@ PX4IO::io_set_arming_state() clear |= PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE; } + // XXX this is for future support in the commander + // but can be removed if unneeded + // if (armed.termination_failsafe) { + // set |= PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE; + // } else { + // clear |= PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE; + // } + if (armed.ready_to_arm) { set |= PX4IO_P_SETUP_ARMING_IO_ARM_OK; @@ -2038,7 +2046,8 @@ PX4IO::print_status(bool extended_status) ((arming & PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK) ? " INAIR_RESTART_OK" : ""), ((arming & PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE) ? " ALWAYS_PWM_ENABLE" : ""), ((arming & PX4IO_P_SETUP_ARMING_LOCKDOWN) ? " LOCKDOWN" : ""), - ((arming & PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE) ? " FORCE_FAILSAFE" : "") + ((arming & PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE) ? " FORCE_FAILSAFE" : ""), + ((arming & PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE) ? " TERM_FAILSAFE" : "") ); #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 printf("rates 0x%04x default %u alt %u relays 0x%04x\n", @@ -2262,6 +2271,17 @@ PX4IO::ioctl(file * filep, int cmd, unsigned long arg) } break; + case PWM_SERVO_SET_TERMINATION_FAILSAFE: + /* if failsafe occurs, do not allow the system to recover */ + if (arg == 0) { + /* clear termination failsafe flag */ + ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE, 0); + } else { + /* set termination failsafe flag */ + ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, 0, PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE); + } + break; + case DSM_BIND_START: /* only allow DSM2, DSM-X and DSM-X with more than 7 channels */ From 91d50301c61cf495e83cab59621ef83cff24da3a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 27 Aug 2014 10:46:10 +0200 Subject: [PATCH 167/411] Do not enter RC override if FMU is lost and termination failsafe mode requested --- src/modules/px4iofirmware/mixer.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index 1eacda97a6..0c65b76425 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -139,7 +139,9 @@ mixer_tick(void) (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)) { + !(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) && + /* do not enter manual override if we asked for termination failsafe and FMU is lost */ + !(r_setup_arming & PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE)) { /* if allowed, mix from RC inputs directly */ source = MIX_OVERRIDE; From c85d7aae06b49f43e32dde05c00bcee1310aee9f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 28 Aug 2014 20:36:59 +0200 Subject: [PATCH 168/411] Make sure we got to valid input at least once before kicking in failsafe --- src/modules/px4iofirmware/mixer.cpp | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index 0c65b76425..17751bd6d2 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -71,6 +71,7 @@ extern "C" { static bool mixer_servos_armed = false; static bool should_arm = false; static bool should_always_enable_pwm = false; +static bool input_valid_initialized = false; /* the input was valid at least once */ static volatile bool in_mixer = false; /* selected control values and count for mixing */ @@ -162,7 +163,8 @@ mixer_tick(void) * failsafe condition. */ if ((r_setup_arming & PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE) && - (source == MIX_FAILSAFE)) { + (source == MIX_FAILSAFE) && + input_valid_initialized) { r_setup_arming |= PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE; } @@ -180,6 +182,9 @@ mixer_tick(void) r_status_flags |= PX4IO_P_STATUS_FLAGS_FAILSAFE; } else { r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_FAILSAFE); + + /* we got valid input, kick off the full failsafe checks */ + input_valid_initialized = true; } /* From 6773eb988017063366dce95bbfec8fae1f1913f0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 28 Aug 2014 21:39:33 +0200 Subject: [PATCH 169/411] Introduce FMU initialised status flag, only run failsafe trap if initialized once --- src/modules/px4iofirmware/mixer.cpp | 67 +++++++++++++++------------- src/modules/px4iofirmware/protocol.h | 1 + 2 files changed, 36 insertions(+), 32 deletions(-) diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index 17751bd6d2..3e19333d8e 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -58,7 +58,7 @@ extern "C" { /* * Maximum interval in us before FMU signal is considered lost */ -#define FMU_INPUT_DROP_LIMIT_US 200000 +#define FMU_INPUT_DROP_LIMIT_US 500000 /* XXX need to move the RC_CHANNEL_FUNCTION out of rc_channels.h and into systemlib */ #define ROLL 0 @@ -71,7 +71,6 @@ extern "C" { static bool mixer_servos_armed = false; static bool should_arm = false; static bool should_always_enable_pwm = false; -static bool input_valid_initialized = false; /* the input was valid at least once */ static volatile bool in_mixer = false; /* selected control values and count for mixing */ @@ -99,7 +98,8 @@ mixer_tick(void) { /* check that we are receiving fresh data from the FMU */ - if (hrt_elapsed_time(&system_state.fmu_data_received_time) > FMU_INPUT_DROP_LIMIT_US) { + if ((system_state.fmu_data_received_time == 0) || + hrt_elapsed_time(&system_state.fmu_data_received_time) > FMU_INPUT_DROP_LIMIT_US) { /* too long without FMU input, time to go to failsafe */ if (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) { @@ -110,6 +110,9 @@ mixer_tick(void) } else { r_status_flags |= PX4IO_P_STATUS_FLAGS_FMU_OK; + + /* this flag is never cleared once OK */ + r_status_flags |= PX4IO_P_STATUS_FLAGS_FMU_INITIALIZED; } /* default to failsafe mixing - it will be forced below if flag is set */ @@ -157,14 +160,41 @@ mixer_tick(void) } } + /* + * Decide whether the servos should be armed right now. + * + * We must be armed, and we must have a PWM source; either raw from + * FMU or from the mixer. + * + */ + should_arm = ( + /* IO initialised without error */ (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK) + /* and IO is armed */ && (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) + /* and FMU is armed */ && ( + ((r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) + /* and there is valid input via or mixer */ && (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) ) + /* or direct PWM is set */ || (r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) + /* or failsafe was set manually */ || ((r_setup_arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM) && !(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK)) + ) + ); + + should_always_enable_pwm = (r_setup_arming & PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE) + && (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK) + && (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK); + /* * Check if failsafe termination is set - if yes, * set the force failsafe flag once entering the first * failsafe condition. */ - if ((r_setup_arming & PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE) && + if ( /* if we have requested flight termination style failsafe (noreturn) */ + (r_setup_arming & PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE) && + /* and we ended up in a failsafe condition */ (source == MIX_FAILSAFE) && - input_valid_initialized) { + /* and we should be armed, so we intended to provide outputs */ + should_arm && + /* and FMU is initialized */ + (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_INITIALIZED)) { r_setup_arming |= PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE; } @@ -182,35 +212,8 @@ mixer_tick(void) r_status_flags |= PX4IO_P_STATUS_FLAGS_FAILSAFE; } else { r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_FAILSAFE); - - /* we got valid input, kick off the full failsafe checks */ - input_valid_initialized = true; } - /* - * Decide whether the servos should be armed right now. - * - * We must be armed, and we must have a PWM source; either raw from - * FMU or from the mixer. - * - * XXX correct behaviour for failsafe may require an additional case - * here. - */ - should_arm = ( - /* IO initialised without error */ (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK) - /* and IO is armed */ && (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) - /* and FMU is armed */ && ( - ((r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) - /* and there is valid input via or mixer */ && (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) ) - /* or direct PWM is set */ || (r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) - /* or failsafe was set manually */ || ((r_setup_arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM) && !(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK)) - ) - ); - - should_always_enable_pwm = (r_setup_arming & PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE) - && (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK) - && (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK); - /* * Run the mixers. */ diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h index eae7f9567a..4739f6e400 100644 --- a/src/modules/px4iofirmware/protocol.h +++ b/src/modules/px4iofirmware/protocol.h @@ -111,6 +111,7 @@ #define PX4IO_P_STATUS_FLAGS_INIT_OK (1 << 10) /* initialisation of the IO completed without error */ #define PX4IO_P_STATUS_FLAGS_FAILSAFE (1 << 11) /* failsafe is active */ #define PX4IO_P_STATUS_FLAGS_SAFETY_OFF (1 << 12) /* safety is off */ +#define PX4IO_P_STATUS_FLAGS_FMU_INITIALIZED (1 << 13) /* FMU was initialized and OK once */ #define PX4IO_P_STATUS_ALARMS 3 /* alarm flags - alarms latch, write 1 to a bit to clear it */ #define PX4IO_P_STATUS_ALARMS_VBATT_LOW (1 << 0) /* [1] VBatt is very close to regulator dropout */ From 592f6f2bcb632d6bb86c63b1055560d0c8b526b2 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 30 Aug 2014 16:44:30 +0200 Subject: [PATCH 170/411] Revert "Fix drop offset: We want to drop so that the wind carries the bottle into the drop zone" This reverts commit ef0a0a1a6e86aff79a0fd8829ca5c86244fa39bc. --- src/modules/bottle_drop/bottle_drop.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/bottle_drop/bottle_drop.cpp b/src/modules/bottle_drop/bottle_drop.cpp index 65eca1a62f..3eec5a879a 100644 --- a/src/modules/bottle_drop/bottle_drop.cpp +++ b/src/modules/bottle_drop/bottle_drop.cpp @@ -566,8 +566,8 @@ BottleDrop::task_main() wind_direction_e = wind.windspeed_east / windspeed_norm; } - x_drop = x_t - x * wind_direction_n; - y_drop = y_t - x * wind_direction_e; + x_drop = x_t + x * wind_direction_n; + y_drop = y_t + x * wind_direction_e; map_projection_reproject(&ref, x_drop, y_drop, &_drop_position.lat, &_drop_position.lon); _drop_position.alt = _target_position.alt + _alt_clearance; From 056693df44561a8d1e19501585aa5f680b7aa086 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 31 Aug 2014 15:55:03 +0200 Subject: [PATCH 171/411] Add drag coefficients to adjust bottle drop to other objects to ease testing --- src/modules/bottle_drop/bottle_drop.cpp | 10 ++- src/modules/bottle_drop/bottle_drop_params.c | 89 +++++++++++++++++++- 2 files changed, 95 insertions(+), 4 deletions(-) diff --git a/src/modules/bottle_drop/bottle_drop.cpp b/src/modules/bottle_drop/bottle_drop.cpp index 3eec5a879a..30ee782cc7 100644 --- a/src/modules/bottle_drop/bottle_drop.cpp +++ b/src/modules/bottle_drop/bottle_drop.cpp @@ -392,11 +392,17 @@ BottleDrop::task_main() param_t param_gproperties = param_find("BD_GPROPERTIES"); param_t param_turn_radius = param_find("BD_TURNRADIUS"); param_t param_precision = param_find("BD_PRECISION"); + param_t param_cd = param_find("BD_OBJ_CD"); + param_t param_mass = param_find("BD_OBJ_MASS"); + param_t param_surface = param_find("BD_OBJ_SURFACE"); param_get(param_precision, &precision); param_get(param_turn_radius, &turn_radius); param_get(param_gproperties, &z_0); + param_get(param_cd, &cd); + param_get(param_mass, &M); + param_get(param_surface, &A); int vehicle_global_position_sub = orb_subscribe(ORB_ID(vehicle_global_position)); @@ -419,10 +425,10 @@ BottleDrop::task_main() struct wind_estimate_s wind; - /* wakeup source(s) */ + // wakeup source(s) struct pollfd fds[1]; - /* Setup of loop */ + // Setup of loop fds[0].fd = _command_sub; fds[0].events = POLLIN; diff --git a/src/modules/bottle_drop/bottle_drop_params.c b/src/modules/bottle_drop/bottle_drop_params.c index 22e9baf8aa..e5d35bf0a2 100644 --- a/src/modules/bottle_drop/bottle_drop_params.c +++ b/src/modules/bottle_drop/bottle_drop_params.c @@ -41,6 +41,91 @@ #include #include +/** + * Ground drag property + * + * This parameter encodes the ground drag coefficient and the corresponding + * decrease in wind speed from the plane altitude to ground altitude. + * + * @unit unknown + * @min 0.001 + * @max 0.1 + * @group Payload drop + */ PARAM_DEFINE_FLOAT(BD_GPROPERTIES, 0.03f); -PARAM_DEFINE_FLOAT(BD_TURNRADIUS, 90.0f); -PARAM_DEFINE_FLOAT(BD_PRECISION, 10.0f); + +/** + * Plane turn radius + * + * The planes known minimal turn radius - use a higher value + * to make the plane maneuver more distant from the actual drop + * position. This is to ensure the wings are level during the drop. + * + * @unit meter + * @min 30.0 + * @max 500.0 + * @group Payload drop + */ +PARAM_DEFINE_FLOAT(BD_TURNRADIUS, 120.0f); + +/** + * Drop precision + * + * If the system is closer than this distance on passing over the + * drop position, it will release the payload. This is a safeguard + * to prevent a drop out of the required accuracy. + * + * @unit meter + * @min 1.0 + * @max 80.0 + * @group Payload drop + */ +PARAM_DEFINE_FLOAT(BD_PRECISION, 30.0f); + +/** + * Payload drag coefficient of the dropped object + * + * The drag coefficient (cd) is the typical drag + * constant for air. It is in general object specific, + * but the closest primitive shape to the actual object + * should give good results: + * http://en.wikipedia.org/wiki/Drag_coefficient + * + * @unit meter + * @min 1.0 + * @max 80.0 + * @group Payload drop + */ +PARAM_DEFINE_FLOAT(BD_OBJ_CD, 0.86f); + +/** + * Payload mass + * + * A typical small toy ball: + * 0.025 kg + * + * OBC water bottle: + * 0.6 kg + * + * @unit kilogram + * @min 0.001 + * @max 5.0 + * @group Payload drop + */ +PARAM_DEFINE_FLOAT(BD_OBJ_MASS, 0.6f); + +/** + * Payload front surface area + * + * A typical small toy ball: + * (0.045 * 0.045) / 4.0 * pi = 0.001590 m^2 + * + * OBC water bottle: + * (0.063 * 0.063) / 4.0 * pi = 0.003117 m^2 + * + * @unit m^2 + * @min 0.001 + * @max 0.5 + * @group Payload drop + */ +PARAM_DEFINE_FLOAT(BD_OBJ_SURFACE, 0.00311724531f); From 706e5809a14ed57f75724a2f5aabdfda157fac5d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 31 Aug 2014 17:02:33 +0200 Subject: [PATCH 172/411] Fix variable name in param --- src/modules/bottle_drop/bottle_drop.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/bottle_drop/bottle_drop.cpp b/src/modules/bottle_drop/bottle_drop.cpp index 30ee782cc7..de3f20a952 100644 --- a/src/modules/bottle_drop/bottle_drop.cpp +++ b/src/modules/bottle_drop/bottle_drop.cpp @@ -401,7 +401,7 @@ BottleDrop::task_main() param_get(param_turn_radius, &turn_radius); param_get(param_gproperties, &z_0); param_get(param_cd, &cd); - param_get(param_mass, &M); + param_get(param_mass, &m); param_get(param_surface, &A); int vehicle_global_position_sub = orb_subscribe(ORB_ID(vehicle_global_position)); From 69109b622796dacd6f78ebafa92b10379ba0d7b4 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 1 Sep 2014 00:45:41 +0200 Subject: [PATCH 173/411] Compile and link ST24 parser in IO firmware --- src/lib/rc/st24.c | 120 ++++++++++++++++++++++++++- src/lib/rc/st24.h | 48 ++++++----- src/modules/px4iofirmware/controls.c | 27 +++++- src/modules/px4iofirmware/module.mk | 3 +- 4 files changed, 174 insertions(+), 24 deletions(-) diff --git a/src/lib/rc/st24.c b/src/lib/rc/st24.c index addbcd899d..956a72bf35 100644 --- a/src/lib/rc/st24.c +++ b/src/lib/rc/st24.c @@ -39,6 +39,23 @@ * @author Lorenz Meier */ +#include +#include "st24.h" + +enum ST24_DECODE_STATE { + ST24_DECODE_STATE_UNSYNCED, + ST24_DECODE_STATE_GOT_STX1, + ST24_DECODE_STATE_GOT_STX2, + ST24_DECODE_STATE_GOT_LEN, + ST24_DECODE_STATE_GOT_TYPE, + ST24_DECODE_STATE_GOT_DATA +}; + +static enum ST24_DECODE_STATE _decode_state = ST24_DECODE_STATE_UNSYNCED; +static unsigned _rxlen; + +static ReceiverFcPacket _rxpacket; + uint8_t st24_common_crc8(uint8_t *ptr, uint8_t len) { uint8_t i, crc ; @@ -66,6 +83,105 @@ uint8_t st24_common_crc8(uint8_t *ptr, uint8_t len) } -uint8_t st24_decode(uint8_t byte, uint8_t *rssi, uint8_t* rx_count, uint16_t *channels, uint16_t max_chan_count) { +uint8_t st24_decode(uint8_t byte, uint8_t *rssi, uint8_t* rx_count, uint16_t *channels, uint16_t max_chan_count) +{ -} \ No newline at end of file + bool ret = false; + + switch (_decode_state) { + case ST24_DECODE_STATE_UNSYNCED: + if (byte == ST24_STX1) { + _decode_state = ST24_DECODE_STATE_GOT_STX1; + } + break; + + case ST24_DECODE_STATE_GOT_STX1: + if (byte == ST24_STX2) { + _decode_state = ST24_DECODE_STATE_GOT_STX2; + } else { + _decode_state = ST24_DECODE_STATE_UNSYNCED; + } + break; + + case ST24_DECODE_STATE_GOT_STX2: + _rxpacket.length = byte; + _rxlen = 0; + _decode_state = ST24_DECODE_STATE_GOT_LEN; + break; + + case ST24_DECODE_STATE_GOT_LEN: + _rxpacket.type = byte; + _rxlen++; + _decode_state = ST24_DECODE_STATE_GOT_TYPE; + break; + + case ST24_DECODE_STATE_GOT_TYPE: + if (_rxlen < (_rxpacket.length - 1)) { + _rxpacket.st24_data[_rxlen] = byte; + _rxlen++; + } else { + _decode_state = ST24_DECODE_STATE_GOT_DATA; + } + break; + + case ST24_DECODE_STATE_GOT_DATA: + _rxpacket.crc8 = byte; + _rxlen++; + + if (st24_common_crc8((uint8_t*)&(_rxpacket.length), sizeof(_rxpacket.length) + + sizeof(_rxpacket.st24_data) + sizeof(_rxpacket.type)) == _rxpacket.crc8) { + + ret = true; + + /* decode the actual packet */ + + switch (_rxpacket.type) { + + case ST24_PACKET_TYPE_CHANNELDATA12: + { + ChannelData12* d = (ChannelData12*)&_rxpacket; + + *rssi = d->rssi; + *rx_count = d->packet_count; + + for (unsigned i = 0; i < 1; i += 2) { + channels[i] = ((uint16_t)d->channel[i]) << 8; + channels[i] |= (0xF & d->channel[i+1]); + + channels[i+1] = ((uint16_t)(0xF & d->channel[i+1])) << 4; + channels[i+1] |= d->channel[i+2]; + } + } + break; + + case ST24_PACKET_TYPE_CHANNELDATA24: + { + ChannelData24* d = (ChannelData12*)&_rxpacket; + + *rssi = d->rssi; + *rx_count = d->packet_count; + + for (unsigned i = 0; i < 1; i += 2) { + channels[i] = ((uint16_t)d->channel[i]) << 8; + channels[i] |= (0xF & d->channel[i+1]); + + channels[i+1] = ((uint16_t)(0xF & d->channel[i+1])) << 4; + channels[i+1] |= d->channel[i+2]; + } + } + break; + + default: + ret = false; + break; + } + + } else { + /* decoding failed */ + _decode_state = ST24_DECODE_STATE_UNSYNCED; + } + break; + } + + return ret; +} diff --git a/src/lib/rc/st24.h b/src/lib/rc/st24.h index 3621e8506e..24dbf8e51a 100644 --- a/src/lib/rc/st24.h +++ b/src/lib/rc/st24.h @@ -39,22 +39,30 @@ * @author Lorenz Meier */ -#define ST24_DATA_LEN_MAX 64 +#pragma once -enum { +#include + +__BEGIN_DECLS + +#define ST24_DATA_LEN_MAX 64 +#define ST24_STX1 0x55 +#define ST24_STX2 0x55 + +enum ST24_PACKET_TYPE { ST24_PACKET_TYPE_CHANNELDATA12 = 0, ST24_PACKET_TYPE_CHANNELDATA24, ST24_PACKET_TYPE_TRANSMITTERGPSDATA -} ST24_PACKET_TYPE; +}; #pragma pack(push, 1) typedef struct { - uint8_t header1; ///< 0x55 for a valid packet - uint8_t header2; ///< 0x55 for a valid packet + uint8_t header1; ///< 0x55 for a valid packet + uint8_t header2; ///< 0x55 for a valid packet uint8_t length; ///< length includes type, data, and crc = sizeof(type)+sizeof(data[payload_len])+sizeof(crc8) - uint8_t type; ///< from enum ST24_PACKET_TYPE + uint8_t type; ///< from enum ST24_PACKET_TYPE uint8_t st24_data[ST24_DATA_LEN_MAX]; - uint8_t crc8; ///< crc8 checksum, calculated by st24_common_crc8 and including fields length, type and st24_data + uint8_t crc8; ///< crc8 checksum, calculated by st24_common_crc8 and including fields length, type and st24_data } ReceiverFcPacket; /** @@ -63,8 +71,8 @@ typedef struct { * This is incoming from the ST24 */ typedef struct { - uint16_t t; ///< packet counter or clock - uint8_t rssi ///< signal strength + uint16_t t; ///< packet counter or clock + uint8_t rssi; ///< signal strength uint8_t packet_count; ///< Number of UART packets sent since reception of last RF frame (this tells something about age / rate) uint8_t channel[18]; ///< channel data, 12 channels (12 bit numbers) } ChannelData12; @@ -74,8 +82,8 @@ typedef struct { * */ typedef struct { - uint16_t t; ///< packet counter or clock - uint8_t rssi ///< signal strength + uint16_t t; ///< packet counter or clock + uint8_t rssi; ///< signal strength uint8_t packet_count; ///< Number of UART packets sent since reception of last RF frame (this tells something about age / rate) uint8_t channel[36]; ///< channel data, 24 channels (12 bit numbers) } ChannelData24; @@ -114,17 +122,17 @@ typedef struct { * */ typedef struct { - uint16_t t; ///< packet counter or clock - int32_t lat; ///< lattitude (degrees) +/- 90 deg - int32_t lon; ///< longitude (degrees) +/- 180 deg - int32_t alt; ///< 0.01m resolution, altitude (meters) + uint16_t t; ///< packet counter or clock + int32_t lat; ///< lattitude (degrees) +/- 90 deg + int32_t lon; ///< longitude (degrees) +/- 180 deg + int32_t alt; ///< 0.01m resolution, altitude (meters) int16_t vx, vy, vz; ///< velocity 0.01m res, +/-320.00 North-East- Down - uint8_t nsat; /// #include #include +#include #include "px4io.h" @@ -51,11 +52,21 @@ #define RC_CHANNEL_LOW_THRESH -8000 /* 10% threshold */ static bool ppm_input(uint16_t *values, uint16_t *num_values, uint16_t *frame_len); +static bool dsm_port_input(void); static perf_counter_t c_gather_dsm; static perf_counter_t c_gather_sbus; static perf_counter_t c_gather_ppm; +static int _dsm_fd; + +bool dsm_port_input() +{ + /* get data from FD and attempt to parse with DSM and ST24 libs */ + + return false; +} + void controls_init(void) { @@ -65,7 +76,7 @@ controls_init(void) system_state.rc_channels_timestamp_valid = 0; /* DSM input (USART1) */ - dsm_init("/dev/ttyS0"); + _dsm_fd = dsm_init("/dev/ttyS0"); /* S.bus input (USART3) */ sbus_init("/dev/ttyS2"); @@ -175,6 +186,18 @@ controls_tick() { } perf_end(c_gather_ppm); + uint8_t st24_rssi, rx_count; + uint8_t st24_maxchans = 18; + bool st24_updated = st24_decode(0, &st24_rssi, &rx_count, r_raw_rc_values, st24_maxchans); + if (st24_updated) { + + rssi = st24_rssi; + + r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_PPM; + r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP); + r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE); + } + /* limit number of channels to allowable data size */ if (r_raw_rc_count > PX4IO_RC_INPUT_CHANNELS) r_raw_rc_count = PX4IO_RC_INPUT_CHANNELS; @@ -191,7 +214,7 @@ controls_tick() { /* * If we received a new frame from any of the RC sources, process it. */ - if (dsm_updated || sbus_updated || ppm_updated) { + if (dsm_updated || sbus_updated || ppm_updated || st24_updated) { /* record a bitmask of channels assigned */ unsigned assigned_channels = 0; diff --git a/src/modules/px4iofirmware/module.mk b/src/modules/px4iofirmware/module.mk index 01869569f6..eb99e8a966 100644 --- a/src/modules/px4iofirmware/module.mk +++ b/src/modules/px4iofirmware/module.mk @@ -14,7 +14,8 @@ SRCS = adc.c \ ../systemlib/mixer/mixer_group.cpp \ ../systemlib/mixer/mixer_multirotor.cpp \ ../systemlib/mixer/mixer_simple.cpp \ - ../systemlib/pwm_limit/pwm_limit.c + ../systemlib/pwm_limit/pwm_limit.c \ + ../../lib/rc/st24.c ifeq ($(BOARD),px4io-v1) SRCS += i2c.c From 63e741824cfde42ac563c457e615cf58f10a2529 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 1 Sep 2014 11:27:28 +0200 Subject: [PATCH 174/411] fw landing: use terrain estimate from global pos --- .../fw_pos_control_l1_main.cpp | 82 ++++++------------- 1 file changed, 24 insertions(+), 58 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 deccab4820..064b8a6fae 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 @@ -88,7 +88,6 @@ #include #include #include -#include #include "landingslope.h" #include "mtecs/mTecs.h" @@ -143,7 +142,6 @@ private: int _params_sub; /**< notification of parameter updates */ int _manual_control_sub; /**< notification of manual control updates */ int _sensor_combined_sub; /**< for body frame accelerations */ - int _range_finder_sub; /**< range finder subscription */ orb_advert_t _attitude_sp_pub; /**< attitude setpoint */ orb_advert_t _tecs_status_pub; /**< TECS status publication */ @@ -158,7 +156,6 @@ private: struct vehicle_global_position_s _global_pos; /**< global vehicle position */ struct position_setpoint_triplet_s _pos_sp_triplet; /**< triplet of mission items */ struct sensor_combined_s _sensor_combined; /**< for body frame accelerations */ - struct range_finder_report _range_finder; /**< range finder report */ perf_counter_t _loop_perf; /**< loop performance counter */ @@ -239,7 +236,6 @@ private: float land_flare_alt_relative; float land_thrust_lim_alt_relative; float land_heading_hold_horizontal_distance; - float range_finder_rel_alt; } _parameters; /**< local copies of interesting parameters */ @@ -284,7 +280,6 @@ private: param_t land_flare_alt_relative; param_t land_thrust_lim_alt_relative; param_t land_heading_hold_horizontal_distance; - param_t range_finder_rel_alt; } _parameter_handles; /**< handles for interesting parameters */ @@ -310,12 +305,6 @@ private: */ bool vehicle_airspeed_poll(); - /** - * Check for range finder updates. - */ - bool range_finder_poll(); - - /** * Check for position updates. */ @@ -337,9 +326,9 @@ private: void navigation_capabilities_publish(); /** - * Get the relative alt either from the difference between estimate and waypoint or from the laser range finder + * Return the terrain estimate during landing: uses the wp altitude value or the terrain estimate if available */ - float get_relative_landingalt(float land_setpoint_alt, float current_alt, const struct range_finder_report &range_finder, float range_finder_use_relative_alt); + float get_terrain_altitude_landing(float land_setpoint_alt, const struct vehicle_global_position_s &global_pos); /** * Control position. @@ -411,7 +400,6 @@ FixedwingPositionControl::FixedwingPositionControl() : _params_sub(-1), _manual_control_sub(-1), _sensor_combined_sub(-1), - _range_finder_sub(-1), /* publications */ _attitude_sp_pub(-1), @@ -428,7 +416,6 @@ FixedwingPositionControl::FixedwingPositionControl() : _global_pos(), _pos_sp_triplet(), _sensor_combined(), - _range_finder(), /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "fw l1 control")), @@ -477,7 +464,6 @@ FixedwingPositionControl::FixedwingPositionControl() : _parameter_handles.land_flare_alt_relative = param_find("FW_LND_FLALT"); _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.range_finder_rel_alt = param_find("FW_LND_RFRALT"); _parameter_handles.time_const = param_find("FW_T_TIME_CONST"); _parameter_handles.time_const_throt = param_find("FW_T_THRO_CONST"); @@ -577,8 +563,6 @@ FixedwingPositionControl::parameters_update() param_get(_parameter_handles.land_heading_hold_horizontal_distance, &(_parameters.land_heading_hold_horizontal_distance)); - param_get(_parameter_handles.range_finder_rel_alt, &(_parameters.range_finder_rel_alt)); - _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)); @@ -669,20 +653,6 @@ FixedwingPositionControl::vehicle_airspeed_poll() return airspeed_updated; } -bool -FixedwingPositionControl::range_finder_poll() -{ - /* check if there is a range finder measurement */ - bool range_finder_updated; - orb_check(_range_finder_sub, &range_finder_updated); - - if (range_finder_updated) { - orb_copy(ORB_ID(sensor_range_finder), _range_finder_sub, &_range_finder); - } - - return range_finder_updated; -} - void FixedwingPositionControl::vehicle_attitude_poll() { @@ -820,21 +790,14 @@ void FixedwingPositionControl::navigation_capabilities_publish() } } -float FixedwingPositionControl::get_relative_landingalt(float land_setpoint_alt, float current_alt, const struct range_finder_report &range_finder, float range_finder_use_relative_alt) +float FixedwingPositionControl::get_terrain_altitude_landing(float land_setpoint_alt, const struct vehicle_global_position_s &global_pos) { - float rel_alt_estimated = current_alt - land_setpoint_alt; - - /* only use range finder if: - * parameter (range_finder_use_relative_alt) > 0 - * the measurement is valid - * the estimated relative altitude (from global altitude estimate and landing waypoint) <= range_finder_use_relative_alt - */ - if (range_finder_use_relative_alt < 0 || !range_finder.valid || range_finder.distance > range_finder_use_relative_alt ) { - return rel_alt_estimated; + if (global_pos.terrain_alt_valid && + isfinite(global_pos.terrain_alt)) { + return global_pos.terrain_alt; + } else { + return land_setpoint_alt; } - - return range_finder.distance; - } bool @@ -991,15 +954,19 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi float airspeed_land = 1.3f * _parameters.airspeed_min; float airspeed_approach = 1.3f * _parameters.airspeed_min; + /* Get an estimate of the terrain altitude if available, otherwise terrain_alt will be + * equal to _pos_sp_triplet.current.alt */ + float terrain_alt = get_terrain_altitude_landing(_pos_sp_triplet.current.alt, _global_pos); + /* Calculate distance (to landing waypoint) and altitude of last ordinary waypoint L */ - float L_altitude_rel = _pos_sp_triplet.previous.valid ? _pos_sp_triplet.previous.alt - _pos_sp_triplet.current.alt : 0.0f; + float L_altitude_rel = _pos_sp_triplet.previous.valid ? + _pos_sp_triplet.previous.alt - terrain_alt : 0.0f; 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_rel_desired = landingslope.getLandingSlopeRelativeAltitudeSave(wp_distance, bearing_lastwp_currwp, bearing_airplane_currwp); - float relative_alt = get_relative_landingalt(_pos_sp_triplet.current.alt, _global_pos.alt, _range_finder, _parameters.range_finder_rel_alt); - if ( (relative_alt < landingslope.flare_relative_alt()) || land_noreturn_vertical) { //checking for land_noreturn to avoid unwanted climb out + if ( (_global_pos.alt < terrain_alt + landingslope.flare_relative_alt()) || land_noreturn_vertical) { //checking for land_noreturn to avoid unwanted climb out /* land with minimal speed */ @@ -1009,7 +976,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* kill the throttle if param requests it */ throttle_max = _parameters.throttle_max; - if (relative_alt < landingslope.motor_lim_relative_alt() || land_motor_lim) { + if (_global_pos.alt < terrain_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; @@ -1027,12 +994,12 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi land_stayonground = true; } - tecs_update_pitch_throttle(_pos_sp_triplet.current.alt + flare_curve_alt_rel, + tecs_update_pitch_throttle(terrain_alt + flare_curve_alt_rel, calculate_target_airspeed(airspeed_land), eas2tas, flare_pitch_angle_rad, math::radians(15.0f), 0.0f, throttle_max, throttle_land, false, flare_pitch_angle_rad, - _pos_sp_triplet.current.alt + relative_alt, ground_speed, + _global_pos.alt, ground_speed, land_motor_lim ? TECS_MODE_LAND_THROTTLELIM : TECS_MODE_LAND); if (!land_noreturn_vertical) { @@ -1053,8 +1020,8 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi * if current position is below the slope continue at previous wp altitude * until the intersection with slope * */ - float altitude_desired_rel = relative_alt; - if (relative_alt > landing_slope_alt_rel_desired || land_onslope) { + float altitude_desired_rel; + if (_global_pos.alt > terrain_alt + landing_slope_alt_rel_desired || land_onslope) { /* stay on slope */ altitude_desired_rel = landing_slope_alt_rel_desired; if (!land_onslope) { @@ -1063,10 +1030,11 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } } else { /* continue horizontally */ - altitude_desired_rel = _pos_sp_triplet.previous.valid ? L_altitude_rel : relative_alt; + altitude_desired_rel = _pos_sp_triplet.previous.valid ? L_altitude_rel : + _global_pos.alt - terrain_alt; } - tecs_update_pitch_throttle(_pos_sp_triplet.current.alt + altitude_desired_rel, + tecs_update_pitch_throttle(terrain_alt + altitude_desired_rel, calculate_target_airspeed(airspeed_approach), eas2tas, math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max), @@ -1075,7 +1043,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi _parameters.throttle_cruise, false, math::radians(_parameters.pitch_limit_min), - _pos_sp_triplet.current.alt + relative_alt, + _global_pos.alt, ground_speed); } @@ -1221,7 +1189,6 @@ FixedwingPositionControl::task_main() _airspeed_sub = orb_subscribe(ORB_ID(airspeed)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); _manual_control_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); - _range_finder_sub = orb_subscribe(ORB_ID(sensor_range_finder)); /* rate limit vehicle status updates to 5Hz */ orb_set_interval(_control_mode_sub, 200); @@ -1295,7 +1262,6 @@ FixedwingPositionControl::task_main() vehicle_setpoint_poll(); vehicle_sensor_combined_poll(); vehicle_airspeed_poll(); - range_finder_poll(); // vehicle_baro_poll(); math::Vector<3> ground_speed(_global_pos.vel_n, _global_pos.vel_e, _global_pos.vel_d); From 8ad1aa789b6f10769dd9977151c9a39971b1ebaf Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 1 Sep 2014 18:13:29 +0200 Subject: [PATCH 175/411] mc_pos_control: reset position setpoint on entering to AUTO mode --- .../mc_pos_control/mc_pos_control_main.cpp | 19 ++++++++++++++----- 1 file changed, 14 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 847a92151c..01fdb4c653 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -180,6 +180,7 @@ private: bool _reset_pos_sp; bool _reset_alt_sp; + bool _mode_auto; math::Vector<3> _pos; math::Vector<3> _pos_sp; @@ -292,7 +293,8 @@ MulticopterPositionControl::MulticopterPositionControl() : _ref_timestamp(0), _reset_pos_sp(true), - _reset_alt_sp(true) + _reset_alt_sp(true), + _mode_auto(false) { memset(&_att, 0, sizeof(_att)); memset(&_att_sp, 0, sizeof(_att_sp)); @@ -684,6 +686,13 @@ MulticopterPositionControl::cross_sphere_line(const math::Vector<3>& sphere_c, f void MulticopterPositionControl::control_auto(float dt) { + if (!_mode_auto) { + _mode_auto = true; + /* reset position setpoint on AUTO mode activation */ + reset_pos_sp(); + reset_alt_sp(); + } + bool updated; orb_check(_pos_sp_triplet_sub, &updated); @@ -807,9 +816,7 @@ MulticopterPositionControl::control_auto(float dt) } } else { - /* no waypoint, loiter, reset position setpoint if needed */ - reset_pos_sp(); - reset_alt_sp(); + /* no waypoint, do nothing, setpoint was already reset */ } } @@ -916,10 +923,12 @@ MulticopterPositionControl::task_main() if (_control_mode.flag_control_manual_enabled) { /* manual control */ control_manual(dt); + _mode_auto = false; } else if (_control_mode.flag_control_offboard_enabled) { /* offboard control */ control_offboard(dt); + _mode_auto = false; } else { /* AUTO */ @@ -1271,9 +1280,9 @@ MulticopterPositionControl::task_main() /* position controller disabled, reset setpoints */ _reset_alt_sp = true; _reset_pos_sp = true; + _mode_auto = false; reset_int_z = true; reset_int_xy = true; - } /* reset altitude controller integral (hovering throttle) to manual throttle after manual throttle control */ From 132c9180ead0c64c2edcc36dec2bf8896679a040 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 1 Sep 2014 18:47:56 +0200 Subject: [PATCH 176/411] mc_pos_control: move position offset limiting to separate method --- .../mc_pos_control/mc_pos_control_main.cpp | 28 +++++++++++++++++++ 1 file changed, 28 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 01fdb4c653..ce26f7dc25 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -221,6 +221,11 @@ private: */ void reset_alt_sp(); + /** + * Check if position setpoint is too far from current position and adjust it if needed. + */ + void limit_pos_sp_offset(); + /** * Set position setpoint using manual control */ @@ -544,6 +549,29 @@ MulticopterPositionControl::reset_alt_sp() } } +void +MulticopterPositionControl::limit_pos_sp_offset() +{ + 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.sp_offs_max); + } +} + void MulticopterPositionControl::control_manual(float dt) { From 7f0ba70b1e8e86334d1acb827d8d2d4bf7305870 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 3 Sep 2014 08:39:26 +0200 Subject: [PATCH 177/411] fw landing: if using terrain estimate, don't switch back during landing --- .../fw_pos_control_l1_main.cpp | 23 +++++++++++-------- 1 file changed, 14 insertions(+), 9 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 064b8a6fae..0db857727a 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,12 +160,12 @@ private: perf_counter_t _loop_perf; /**< loop performance counter */ /* land states */ - /* not in non-abort mode for landing yet */ bool land_noreturn_horizontal; bool land_noreturn_vertical; bool land_stayonground; bool land_motor_lim; bool land_onslope; + bool land_useterrain; /* takeoff/launch states */ bool launch_detected; @@ -425,6 +425,7 @@ FixedwingPositionControl::FixedwingPositionControl() : land_stayonground(false), land_motor_lim(false), land_onslope(false), + land_useterrain(false), launch_detected(false), usePreTakeoffThrust(false), last_manual(false), @@ -792,8 +793,14 @@ void FixedwingPositionControl::navigation_capabilities_publish() float FixedwingPositionControl::get_terrain_altitude_landing(float land_setpoint_alt, const struct vehicle_global_position_s &global_pos) { - if (global_pos.terrain_alt_valid && - isfinite(global_pos.terrain_alt)) { + if (!isfinite(global_pos.terrain_alt)) { + return land_setpoint_alt; + } + + /* Decide if the terrain estimation can be used, once we switched to using the terrain we stick with it + * for the whole landing */ + if (global_pos.terrain_alt_valid || land_useterrain) { + land_useterrain = true; return global_pos.terrain_alt; } else { return land_setpoint_alt; @@ -941,11 +948,6 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* 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.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 @@ -967,7 +969,6 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi if ( (_global_pos.alt < terrain_alt + landingslope.flare_relative_alt()) || land_noreturn_vertical) { //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 */ @@ -993,6 +994,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi flare_curve_alt_rel = 0.0f; // stay on ground land_stayonground = true; } + warnx("flare: alt %4.3f, sp %4.3f", (double)_global_pos.alt, (double)(terrain_alt + flare_curve_alt_rel)); tecs_update_pitch_throttle(terrain_alt + flare_curve_alt_rel, calculate_target_airspeed(airspeed_land), eas2tas, @@ -1034,6 +1036,8 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi _global_pos.alt - terrain_alt; } + warnx("approach: alt %4.3f, sp %4.3f", (double)_global_pos.alt, (double)(terrain_alt + altitude_desired_rel)); + tecs_update_pitch_throttle(terrain_alt + altitude_desired_rel, calculate_target_airspeed(airspeed_approach), eas2tas, math::radians(_parameters.pitch_limit_min), @@ -1326,6 +1330,7 @@ void FixedwingPositionControl::reset_landing_state() land_stayonground = false; land_motor_lim = false; land_onslope = false; + land_useterrain = false; } void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_sp, float eas2tas, From 3d01da35d02505e751536e2cc09637797b37bed6 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 3 Sep 2014 10:34:27 +0200 Subject: [PATCH 178/411] write sysid & compid to telemetry status --- src/modules/mavlink/mavlink_receiver.cpp | 2 ++ src/modules/uORB/topics/telemetry_status.h | 2 ++ 2 files changed, 4 insertions(+) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 1115304d42..43222880e9 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -534,6 +534,8 @@ MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg) tstatus.remote_noise = rstatus.remnoise; tstatus.rxerrors = rstatus.rxerrors; tstatus.fixed = rstatus.fixed; + tstatus.system_id = msg->sysid; + tstatus.component_id = msg->compid; if (_telemetry_status_pub < 0) { _telemetry_status_pub = orb_advertise(telemetry_status_orb_id[_mavlink->get_channel()], &tstatus); diff --git a/src/modules/uORB/topics/telemetry_status.h b/src/modules/uORB/topics/telemetry_status.h index 1297c1a9d3..93193f32ba 100644 --- a/src/modules/uORB/topics/telemetry_status.h +++ b/src/modules/uORB/topics/telemetry_status.h @@ -67,6 +67,8 @@ struct telemetry_status_s { 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 */ + uint8_t system_id; /**< system id of the remote system */ + uint8_t component_id; /**< component id of the remote system */ }; /** From 3f8793210b47bd8e09ed2adaabc2fab966db5df6 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 3 Sep 2014 10:34:52 +0200 Subject: [PATCH 179/411] datalink check: ignore onboard computer --- src/modules/commander/commander.cpp | 20 +++++++++++++++++--- src/modules/commander/commander_params.c | 10 ++++++++++ 2 files changed, 27 insertions(+), 3 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 9885176b70..07fcb5d40e 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -685,6 +685,7 @@ int commander_thread_main(int argc, char *argv[]) param_t _param_enable_datalink_loss = param_find("COM_DL_LOSS_EN"); param_t _param_datalink_loss_timeout = param_find("COM_DL_LOSS_T"); param_t _param_datalink_regain_timeout = param_find("COM_DL_REG_T"); + param_t _param_onboard_sysid = param_find("COM_ONBSYSID"); /* welcome user */ warnx("starting"); @@ -879,12 +880,14 @@ int commander_thread_main(int argc, char *argv[]) uint64_t telemetry_last_heartbeat[TELEMETRY_STATUS_ORB_ID_NUM]; uint64_t telemetry_last_dl_loss[TELEMETRY_STATUS_ORB_ID_NUM]; bool telemetry_lost[TELEMETRY_STATUS_ORB_ID_NUM]; + uint8_t telemetry_sysid[TELEMETRY_STATUS_ORB_ID_NUM]; for (int i = 0; i < TELEMETRY_STATUS_ORB_ID_NUM; i++) { telemetry_subs[i] = orb_subscribe(telemetry_status_orb_id[i]); telemetry_last_heartbeat[i] = 0; telemetry_last_dl_loss[i] = 0; telemetry_lost[i] = true; + telemetry_sysid[i] = 0; } /* Subscribe to global position */ @@ -971,6 +974,8 @@ int commander_thread_main(int argc, char *argv[]) int32_t datalink_loss_enabled = false; int32_t datalink_loss_timeout = 10; int32_t datalink_regain_timeout = 0; + uint8_t onboard_sysid = 42; /**< systemid of the onboard computer, telemetry from this sysid + is not validated for the datalink loss check */ /* check which state machines for changes, clear "changed" flag */ bool arming_state_changed = false; @@ -1033,6 +1038,7 @@ int commander_thread_main(int argc, char *argv[]) param_get(_param_enable_datalink_loss, &datalink_loss_enabled); param_get(_param_datalink_loss_timeout, &datalink_loss_timeout); param_get(_param_datalink_regain_timeout, &datalink_regain_timeout); + param_get(_param_onboard_sysid, &onboard_sysid); } orb_check(sp_man_sub, &updated); @@ -1079,6 +1085,7 @@ int commander_thread_main(int argc, char *argv[]) } telemetry_last_heartbeat[i] = telemetry.heartbeat_time; + telemetry_sysid[i] = telemetry.system_id; } } @@ -1562,10 +1569,17 @@ int commander_thread_main(int argc, char *argv[]) mavlink_log_critical(mavlink_fd, "data link %i regained", i); telemetry_lost[i] = false; - have_link = true; - } else if (!telemetry_lost[i]) { + + /* If this is not an onboard link/onboard computer: + * set flag that we have a valid link */ + if (telemetry_sysid[i] != onboard_sysid) { + have_link = true; + } + } else if (!telemetry_lost[i] && telemetry_sysid[i] != onboard_sysid) { /* telemetry was healthy also in last iteration - * we don't have to check a timeout */ + * we don't have to check a timeout, + * telemetry from onboard computers is not accepted as a valid datalink + */ have_link = true; } diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index 30159dad9c..98c0982b21 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -128,3 +128,13 @@ PARAM_DEFINE_INT32(COM_DL_LOSS_T, 10); * @max 30 */ PARAM_DEFINE_INT32(COM_DL_REG_T, 0); + +/** Onboard computer system id + * + * The system id of the onboard computer. Heartbeats from this system are ignored during the datalink check + * + * @group commander + * @min 0 + * @max 255 + */ +PARAM_DEFINE_INT32(COM_ONBSYSID, 42); From 484f2864d188fa04cbbbd767278b547b7f9969af Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 3 Sep 2014 12:36:44 +0200 Subject: [PATCH 180/411] remove warnx --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 3 --- 1 file changed, 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 0db857727a..6e06b2b788 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 @@ -994,7 +994,6 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi flare_curve_alt_rel = 0.0f; // stay on ground land_stayonground = true; } - warnx("flare: alt %4.3f, sp %4.3f", (double)_global_pos.alt, (double)(terrain_alt + flare_curve_alt_rel)); tecs_update_pitch_throttle(terrain_alt + flare_curve_alt_rel, calculate_target_airspeed(airspeed_land), eas2tas, @@ -1036,8 +1035,6 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi _global_pos.alt - terrain_alt; } - warnx("approach: alt %4.3f, sp %4.3f", (double)_global_pos.alt, (double)(terrain_alt + altitude_desired_rel)); - tecs_update_pitch_throttle(terrain_alt + altitude_desired_rel, calculate_target_airspeed(airspeed_approach), eas2tas, math::radians(_parameters.pitch_limit_min), From 5134015800f014e1eeb4895718823d8b0ac508c1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 3 Sep 2014 18:15:12 +0200 Subject: [PATCH 181/411] always obey commands in bottle drop --- src/modules/bottle_drop/bottle_drop.cpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/src/modules/bottle_drop/bottle_drop.cpp b/src/modules/bottle_drop/bottle_drop.cpp index de3f20a952..0bb65252f9 100644 --- a/src/modules/bottle_drop/bottle_drop.cpp +++ b/src/modules/bottle_drop/bottle_drop.cpp @@ -496,6 +496,12 @@ BottleDrop::task_main() param_get(param_precision, &precision); } + orb_check(_command_sub, &updated); + if (updated) { + orb_copy(ORB_ID(vehicle_command), _command_sub, &_command); + handle_command(&_command); + } + float windspeed_norm = sqrtf(wind.windspeed_north * wind.windspeed_north + wind.windspeed_east * wind.windspeed_east); float groundspeed_body = sqrtf(_global_pos.vel_n * _global_pos.vel_n + _global_pos.vel_e * _global_pos.vel_e); From 75e9497ab1fea94985b4e64b02069343e16821b6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 3 Sep 2014 18:15:30 +0200 Subject: [PATCH 182/411] Fix FX79 mixer --- ROMFS/px4fmu_common/mixers/FMU_FX79.mix | 13 +++++-------- 1 file changed, 5 insertions(+), 8 deletions(-) diff --git a/ROMFS/px4fmu_common/mixers/FMU_FX79.mix b/ROMFS/px4fmu_common/mixers/FMU_FX79.mix index 112d9b891c..b8879af9ee 100755 --- a/ROMFS/px4fmu_common/mixers/FMU_FX79.mix +++ b/ROMFS/px4fmu_common/mixers/FMU_FX79.mix @@ -52,21 +52,18 @@ M: 1 O: 10000 10000 0 -10000 10000 S: 0 3 0 20000 -10000 -10000 10000 -Gimbal / flaps / payload mixer for last four channels +Inputs to the mixer come from channel group 2 (payload), channels 0 +(bay servo 1), 1 (bay servo 2) and 3 (drop release). ----------------------------------------------------- M: 1 O: 10000 10000 0 -10000 10000 -S: 0 4 10000 10000 0 -10000 10000 +S: 2 0 10000 10000 0 -10000 10000 M: 1 O: 10000 10000 0 -10000 10000 -S: 0 5 10000 10000 0 -10000 10000 +S: 2 1 10000 10000 0 -10000 10000 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 +S: 2 2 -10000 -10000 0 -10000 10000 From 139ca8c327a80b61213e2d75dd8819cf119d22d7 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 3 Sep 2014 18:53:05 +0200 Subject: [PATCH 183/411] fw landing: impose horizontal limit for start of flare --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 6 +++++- 1 file changed, 5 insertions(+), 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 84c14be018..e71e7a2ac7 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 @@ -999,7 +999,11 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi float relative_alt = get_relative_landingalt(_pos_sp_triplet.current.alt, _global_pos.alt, _range_finder, _parameters.range_finder_rel_alt); - if ( (relative_alt < landingslope.flare_relative_alt()) || land_noreturn_vertical) { //checking for land_noreturn to avoid unwanted climb out + /* Check if we should start flaring with a vertical and a + * horizontal limit (with some tolerance) */ + if ( ((relative_alt < landingslope.flare_relative_alt()) && + (wp_distance < landingslope.flare_length() + 5.0f)) || + land_noreturn_vertical) { //checking for land_noreturn to avoid unwanted climb out /* land with minimal speed */ From dd1945bb7644b41e765866aefd3c6cc14e433b37 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 4 Sep 2014 10:59:35 +0200 Subject: [PATCH 184/411] add heightrate ff for tecs --- src/lib/external_lgpl/tecs/tecs.cpp | 2 +- src/lib/external_lgpl/tecs/tecs.h | 6 + .../fw_pos_control_l1_main.cpp | 5 + .../fw_pos_control_l1_params.c | 125 +++++++++--------- 4 files changed, 78 insertions(+), 60 deletions(-) diff --git a/src/lib/external_lgpl/tecs/tecs.cpp b/src/lib/external_lgpl/tecs/tecs.cpp index d27bf776fb..023bd71bfc 100644 --- a/src/lib/external_lgpl/tecs/tecs.cpp +++ b/src/lib/external_lgpl/tecs/tecs.cpp @@ -238,7 +238,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)*_heightrate_p; + _hgt_rate_dem = (_hgt_dem_adj-state)*_heightrate_p + _heightrate_ff * (_hgt_dem_adj - _hgt_dem_adj_last)/_DT; // 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 36ae4ecaf2..8ac31de6f5 100644 --- a/src/lib/external_lgpl/tecs/tecs.h +++ b/src/lib/external_lgpl/tecs/tecs.h @@ -47,6 +47,7 @@ public: _rollComp(0.0f), _spdWeight(0.5f), _heightrate_p(0.0f), + _heightrate_ff(0.0f), _speedrate_p(0.0f), _throttle_dem(0.0f), _pitch_dem(0.0f), @@ -220,6 +221,10 @@ public: _heightrate_p = heightrate_p; } + void set_heightrate_ff(float heightrate_ff) { + _heightrate_ff = heightrate_ff; + } + void set_speedrate_p(float speedrate_p) { _speedrate_p = speedrate_p; } @@ -256,6 +261,7 @@ private: float _rollComp; float _spdWeight; float _heightrate_p; + float _heightrate_ff; float _speedrate_p; // throttle demand in the range from 0.0 to 1.0 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 84c14be018..d339b1c4d0 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 @@ -211,6 +211,7 @@ private: float max_climb_rate; float climbout_diff; float heightrate_p; + float heightrate_ff; float speedrate_p; float throttle_damp; float integrator_gain; @@ -256,6 +257,7 @@ private: param_t max_climb_rate; param_t climbout_diff; param_t heightrate_p; + param_t heightrate_ff; param_t speedrate_p; param_t throttle_damp; param_t integrator_gain; @@ -494,6 +496,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.heightrate_ff = param_find("FW_T_HRATE_FF"); _parameter_handles.speedrate_p = param_find("FW_T_SRATE_P"); /* fetch initial parameter values */ @@ -563,6 +566,7 @@ FixedwingPositionControl::parameters_update() param_get(_parameter_handles.climbout_diff, &(_parameters.climbout_diff)); param_get(_parameter_handles.heightrate_p, &(_parameters.heightrate_p)); + param_get(_parameter_handles.heightrate_ff, &(_parameters.heightrate_ff)); param_get(_parameter_handles.speedrate_p, &(_parameters.speedrate_p)); param_get(_parameter_handles.land_slope_angle, &(_parameters.land_slope_angle)); @@ -600,6 +604,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_heightrate_ff(_parameters.heightrate_ff); _tecs.set_speedrate_p(_parameters.speedrate_p); /* sanity check parameters */ 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 890ab3bad2..847ecbb5cb 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 @@ -131,8 +131,8 @@ 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 + * 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 @@ -142,10 +142,10 @@ 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 + * 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 @@ -158,7 +158,7 @@ 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, + * This throttle value will be set as throttle limit at FW_LND_TLALT, * before arcraft will flare. * * @group L1 Control @@ -180,17 +180,17 @@ PARAM_DEFINE_FLOAT(FW_CLMBOUT_DIFF, 25.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 + * 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. * * @group L1 Control @@ -200,8 +200,8 @@ 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 + * 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 Fixed Wing TECS @@ -211,10 +211,10 @@ 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 + * 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. * * @group Fixed Wing TECS @@ -224,7 +224,7 @@ PARAM_DEFINE_FLOAT(FW_T_SINK_MAX, 5.0f); /** * TECS time constant * - * This is the time constant of the TECS control algorithm (in seconds). + * 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. * @@ -235,7 +235,7 @@ PARAM_DEFINE_FLOAT(FW_T_TIME_CONST, 5.0f); /** * TECS Throttle time constant * - * This is the time constant of the TECS throttle control algorithm (in seconds). + * This is the time constant of the TECS throttle control algorithm (in seconds). * Smaller values make it faster to respond, larger values make it slower * to respond. * @@ -246,7 +246,7 @@ PARAM_DEFINE_FLOAT(FW_T_THRO_CONST, 8.0f); /** * Throttle damping factor * - * This is the damping gain for the throttle demand loop. + * This is the damping gain for the throttle demand loop. * Increase to add damping to correct for oscillations in speed and height. * * @group Fixed Wing TECS @@ -256,9 +256,9 @@ 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 + * 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 Fixed Wing TECS @@ -269,9 +269,9 @@ PARAM_DEFINE_FLOAT(FW_T_INTEG_GAIN, 0.1f); * Maximum vertical acceleration * * This is the maximum vertical acceleration (in metres/second square) - * 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 + * 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. * * @group Fixed Wing TECS @@ -281,10 +281,10 @@ 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 + * 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. * * @group Fixed Wing TECS @@ -294,10 +294,10 @@ 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 + * 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 + * more towards use of the arispeed sensor, whilst reducing it weights the * solution more towards use of the accelerometer data. * * @group Fixed Wing TECS @@ -307,13 +307,13 @@ 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 + * 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. * * @group Fixed Wing TECS @@ -323,15 +323,15 @@ 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 + * 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). * * @group Fixed Wing TECS @@ -341,9 +341,9 @@ 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 + * 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 Fixed Wing TECS @@ -357,6 +357,13 @@ PARAM_DEFINE_FLOAT(FW_T_PTCH_DAMP, 0.0f); */ PARAM_DEFINE_FLOAT(FW_T_HRATE_P, 0.05f); +/** + * Height rate FF factor + * + * @group Fixed Wing TECS + */ +PARAM_DEFINE_FLOAT(FW_T_HRATE_FF, 0.0f); + /** * Speed rate P factor * From bf3f8861ddbf2af80fc121589ab6189538c1cadb Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 4 Sep 2014 11:11:03 +0200 Subject: [PATCH 185/411] fw landing: horiz flare check: fix wp distance better calculation of wp distance when behind wp --- .../fw_pos_control_l1/fw_pos_control_l1_main.cpp | 14 +++++++++++--- 1 file changed, 11 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 409b57d63e..e8e316064d 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 @@ -909,10 +909,17 @@ 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)); + float bearing_airplane_currwp = get_bearing_to_next_waypoint(current_position(0), current_position(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)); + /* calculate a waypoint distance value which is 0 when the aircraft is behind the waypoint */ + float wp_distance_save = wp_distance; + if (fabsf(bearing_airplane_currwp - bearing_lastwp_currwp) >= math::radians(90.0f)) { + wp_distance_save = 0.0f; + } + //warnx("wp dist: %d, alt err: %d, noret: %s", (int)wp_distance, (int)altitude_error, (land_noreturn) ? "YES" : "NO"); if (wp_distance < _parameters.land_heading_hold_horizontal_distance || land_noreturn_horizontal) { @@ -964,13 +971,14 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi float L_altitude_rel = _pos_sp_triplet.previous.valid ? _pos_sp_triplet.previous.alt - terrain_alt : 0.0f; - 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_rel_desired = landingslope.getLandingSlopeRelativeAltitudeSave(wp_distance, bearing_lastwp_currwp, bearing_airplane_currwp); /* Check if we should start flaring with a vertical and a - * horizontal limit (with some tolerance) */ + * horizontal limit (with some tolerance) + * The horizontal limit is only applied when we are in front of the wp + */ if (((_global_pos.alt < terrain_alt + landingslope.flare_relative_alt()) && - (wp_distance < landingslope.flare_length() + 5.0f)) || + (wp_distance_save < landingslope.flare_length() + 5.0f)) || land_noreturn_vertical) { //checking for land_noreturn to avoid unwanted climb out /* land with minimal speed */ From 4e9a52fe45655aa853bf9af10223d32767bb60c4 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 4 Sep 2014 12:32:10 +0200 Subject: [PATCH 186/411] heightrate ff: fix order of calculations --- src/lib/external_lgpl/tecs/tecs.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/lib/external_lgpl/tecs/tecs.cpp b/src/lib/external_lgpl/tecs/tecs.cpp index 023bd71bfc..da99aa5b10 100644 --- a/src/lib/external_lgpl/tecs/tecs.cpp +++ b/src/lib/external_lgpl/tecs/tecs.cpp @@ -236,9 +236,8 @@ void TECS::_update_height_demand(float demand, float state) // // _hgt_rate_dem); _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)*_heightrate_p + _heightrate_ff * (_hgt_dem_adj - _hgt_dem_adj_last)/_DT; + _hgt_dem_adj_last = _hgt_dem_adj; // Limit height rate of change if (_hgt_rate_dem > _maxClimbRate) { _hgt_rate_dem = _maxClimbRate; From 2d3b6a88dee5220c1b35d6e32f264217da8e36b8 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 4 Sep 2014 17:18:06 +0200 Subject: [PATCH 187/411] swissfang: don't build MC apps on FMU1 --- makefiles/config_px4fmu-v1_default.mk | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk index 97eddfdd2b..99c0eb8c1a 100644 --- a/makefiles/config_px4fmu-v1_default.mk +++ b/makefiles/config_px4fmu-v1_default.mk @@ -67,17 +67,17 @@ MODULES += modules/gpio_led # # Estimation modules (EKF / other filters) # -MODULES += modules/attitude_estimator_ekf +#MODULES += modules/attitude_estimator_ekf MODULES += modules/ekf_att_pos_estimator -MODULES += modules/position_estimator_inav +#MODULES += modules/position_estimator_inav # # Vehicle Control # MODULES += modules/fw_pos_control_l1 MODULES += modules/fw_att_control -MODULES += modules/mc_att_control -MODULES += modules/mc_pos_control +#MODULES += modules/mc_att_control +#MODULES += modules/mc_pos_control # # Logging From 3684bf71d5068a03587005ba365c8705e327e936 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 4 Sep 2014 18:12:20 +0200 Subject: [PATCH 188/411] make flare pitch angle a param --- .../fw_pos_control_l1_main.cpp | 14 +- .../fw_pos_control_l1_params.c | 134 ++++++++++-------- 2 files changed, 86 insertions(+), 62 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 e8e316064d..c0c238ba9b 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 @@ -237,6 +237,8 @@ private: float land_flare_alt_relative; float land_thrust_lim_alt_relative; float land_heading_hold_horizontal_distance; + float land_flare_pitch_min_deg; + float land_flare_pitch_max_deg; } _parameters; /**< local copies of interesting parameters */ @@ -281,6 +283,8 @@ private: param_t land_flare_alt_relative; param_t land_thrust_lim_alt_relative; param_t land_heading_hold_horizontal_distance; + param_t land_flare_pitch_min_deg; + param_t land_flare_pitch_max_deg; } _parameter_handles; /**< handles for interesting parameters */ @@ -465,6 +469,8 @@ FixedwingPositionControl::FixedwingPositionControl() : _parameter_handles.land_flare_alt_relative = param_find("FW_LND_FLALT"); _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.land_flare_pitch_min_deg = param_find("FW_FLARE_PMIN"); + _parameter_handles.land_flare_pitch_max_deg = param_find("FW_FLARE_PMAX"); _parameter_handles.time_const = param_find("FW_T_TIME_CONST"); _parameter_handles.time_const_throt = param_find("FW_T_THRO_CONST"); @@ -563,6 +569,8 @@ FixedwingPositionControl::parameters_update() } param_get(_parameter_handles.land_heading_hold_horizontal_distance, &(_parameters.land_heading_hold_horizontal_distance)); + param_get(_parameter_handles.land_flare_pitch_min_deg, &(_parameters.land_flare_pitch_min_deg)); + param_get(_parameter_handles.land_flare_pitch_max_deg, &(_parameters.land_flare_pitch_max_deg)); _l1_control.set_l1_damping(_parameters.l1_damping); _l1_control.set_l1_period(_parameters.l1_period); @@ -958,7 +966,6 @@ 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(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; @@ -1008,9 +1015,10 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi tecs_update_pitch_throttle(terrain_alt + flare_curve_alt_rel, calculate_target_airspeed(airspeed_land), eas2tas, - flare_pitch_angle_rad, math::radians(15.0f), + math::radians(_parameters.land_flare_pitch_min_deg), + math::radians(_parameters.land_flare_pitch_max_deg), 0.0f, throttle_max, throttle_land, - false, flare_pitch_angle_rad, + false, math::radians(_parameters.land_flare_pitch_min_deg), _global_pos.alt, ground_speed, land_motor_lim ? TECS_MODE_LAND_THROTTLELIM : TECS_MODE_LAND); 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 890ab3bad2..f2e8c422f3 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 @@ -131,8 +131,8 @@ 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 + * 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 @@ -142,10 +142,10 @@ 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 + * 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 @@ -158,7 +158,7 @@ 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, + * This throttle value will be set as throttle limit at FW_LND_TLALT, * before arcraft will flare. * * @group L1 Control @@ -180,17 +180,17 @@ PARAM_DEFINE_FLOAT(FW_CLMBOUT_DIFF, 25.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 + * 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. * * @group L1 Control @@ -200,8 +200,8 @@ 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 + * 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 Fixed Wing TECS @@ -211,10 +211,10 @@ 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 + * 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. * * @group Fixed Wing TECS @@ -224,7 +224,7 @@ PARAM_DEFINE_FLOAT(FW_T_SINK_MAX, 5.0f); /** * TECS time constant * - * This is the time constant of the TECS control algorithm (in seconds). + * 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. * @@ -235,7 +235,7 @@ PARAM_DEFINE_FLOAT(FW_T_TIME_CONST, 5.0f); /** * TECS Throttle time constant * - * This is the time constant of the TECS throttle control algorithm (in seconds). + * This is the time constant of the TECS throttle control algorithm (in seconds). * Smaller values make it faster to respond, larger values make it slower * to respond. * @@ -246,7 +246,7 @@ PARAM_DEFINE_FLOAT(FW_T_THRO_CONST, 8.0f); /** * Throttle damping factor * - * This is the damping gain for the throttle demand loop. + * This is the damping gain for the throttle demand loop. * Increase to add damping to correct for oscillations in speed and height. * * @group Fixed Wing TECS @@ -256,9 +256,9 @@ 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 + * 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 Fixed Wing TECS @@ -269,9 +269,9 @@ PARAM_DEFINE_FLOAT(FW_T_INTEG_GAIN, 0.1f); * Maximum vertical acceleration * * This is the maximum vertical acceleration (in metres/second square) - * 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 + * 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. * * @group Fixed Wing TECS @@ -281,10 +281,10 @@ 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 + * 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. * * @group Fixed Wing TECS @@ -294,10 +294,10 @@ 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 + * 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 + * more towards use of the arispeed sensor, whilst reducing it weights the * solution more towards use of the accelerometer data. * * @group Fixed Wing TECS @@ -307,13 +307,13 @@ 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 + * 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. * * @group Fixed Wing TECS @@ -323,15 +323,15 @@ 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 + * 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). * * @group Fixed Wing TECS @@ -341,9 +341,9 @@ 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 + * 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 Fixed Wing TECS @@ -414,3 +414,19 @@ PARAM_DEFINE_FLOAT(FW_LND_HHDIST, 15.0f); * @group L1 Control */ PARAM_DEFINE_FLOAT(FW_LND_RFRALT, -1.0f); + +/** + * Flare, minimum pitch + * + * Minimum pitch during flare, a positive sign means nose up + * + */ +PARAM_DEFINE_FLOAT(FW_FLARE_PMIN, 2.5f); + +/** + * Flare, maximum pitch + * + * Maximum pitch during flare, a positive sign means nose up + * + */ +PARAM_DEFINE_FLOAT(FW_FLARE_PMAX, 15.0f); From 033e4892ca2864c61e68c11c952d867ca6dc129e Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 5 Sep 2014 08:59:00 +0200 Subject: [PATCH 189/411] engine failure detection --- src/modules/commander/commander.cpp | 52 ++++++++++++++++++++++-- src/modules/commander/commander_params.c | 32 +++++++++++++++ src/modules/systemlib/circuit_breaker.c | 14 +++++++ src/modules/systemlib/circuit_breaker.h | 1 + 4 files changed, 95 insertions(+), 4 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 07fcb5d40e..447e2189d5 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -686,6 +686,9 @@ int commander_thread_main(int argc, char *argv[]) param_t _param_datalink_loss_timeout = param_find("COM_DL_LOSS_T"); param_t _param_datalink_regain_timeout = param_find("COM_DL_REG_T"); param_t _param_onboard_sysid = param_find("COM_ONBSYSID"); + param_t _param_ef_throttle_thres = param_find("COM_EF_THROT"); + param_t _param_ef_current2throttle_thres = param_find("COM_EF_C2T"); + param_t _param_ef_time_thres = param_find("COM_EF_TIME"); /* welcome user */ warnx("starting"); @@ -974,8 +977,16 @@ int commander_thread_main(int argc, char *argv[]) int32_t datalink_loss_enabled = false; int32_t datalink_loss_timeout = 10; int32_t datalink_regain_timeout = 0; - uint8_t onboard_sysid = 42; /**< systemid of the onboard computer, telemetry from this sysid - is not validated for the datalink loss check */ + int32_t onboard_sysid = 42; /**< systemid of the onboard computer, + telemetry from this sysid is not + validated for the datalink loss check */ + + /* Thresholds for engine failure detection */ + int32_t ef_throttle_thres = 1.0f; + int32_t ef_current2throttle_thres = 0.0f; + int32_t ef_time_thres = 1000.0f; + uint64_t timestamp_engine_healthy = 0; /**< absolute time when engine + was healty*/ /* check which state machines for changes, clear "changed" flag */ bool arming_state_changed = false; @@ -1039,6 +1050,9 @@ int commander_thread_main(int argc, char *argv[]) param_get(_param_datalink_loss_timeout, &datalink_loss_timeout); param_get(_param_datalink_regain_timeout, &datalink_regain_timeout); param_get(_param_onboard_sysid, &onboard_sysid); + param_get(_param_ef_throttle_thres, &ef_throttle_thres); + param_get(_param_ef_current2throttle_thres, &ef_current2throttle_thres); + param_get(_param_ef_time_thres, &ef_time_thres); } orb_check(sp_man_sub, &updated); @@ -1572,10 +1586,11 @@ int commander_thread_main(int argc, char *argv[]) /* If this is not an onboard link/onboard computer: * set flag that we have a valid link */ - if (telemetry_sysid[i] != onboard_sysid) { + if (telemetry_sysid[i] != (uint8_t)onboard_sysid) { have_link = true; } - } else if (!telemetry_lost[i] && telemetry_sysid[i] != onboard_sysid) { + } else if (!telemetry_lost[i] && telemetry_sysid[i] != + (uint8_t)onboard_sysid) { /* telemetry was healthy also in last iteration * we don't have to check a timeout, * telemetry from onboard computers is not accepted as a valid datalink @@ -1608,6 +1623,35 @@ int commander_thread_main(int argc, char *argv[]) } } + /* Check engine failure + * only for fixed wing for now + */ + if (!circuit_breaker_enabled("CBRK_ENGINEFAIL", CBRK_ENGINEFAIL_KEY) && + status.is_rotary_wing == false && + armed.armed && + ((actuator_controls.control[3] > ef_throttle_thres && + battery.current_a/actuator_controls.control[3] < + ef_current2throttle_thres) || + (status.engine_failure))) { + /* potential failure, measure time */ + if (timestamp_engine_healthy > 0 && + hrt_elapsed_time(×tamp_engine_healthy) > + ef_time_thres * 1e6 && + !status.engine_failure) { + status.engine_failure = true; + status_changed = true; + mavlink_log_critical(mavlink_fd, "Engine Failure"); + } + } else { + /* no failure reset flag */ + timestamp_engine_healthy = hrt_absolute_time(); + if (status.engine_failure) { + status.engine_failure = false; + status_changed = true; + } + } + + /* handle commands last, as the system needs to be updated to handle them */ orb_check(cmd_sub, &updated); diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index 98c0982b21..15c299a8bd 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -138,3 +138,35 @@ PARAM_DEFINE_INT32(COM_DL_REG_T, 0); * @max 255 */ PARAM_DEFINE_INT32(COM_ONBSYSID, 42); + +/** Engine Failure Throttle Threshold + * + * Engine failure triggers only above this throttle value + * + * @group commander + * @min 0.0f + * @max 1.0f + */ +PARAM_DEFINE_FLOAT(COM_EF_THROT, 0.5f); + +/** Engine Failure Current/Throttle Threshold + * + * Engine failure triggers only below this current/throttle value + * + * @group commander + * @min 0.0f + * @max 7.0f + */ +PARAM_DEFINE_FLOAT(COM_EF_C2T, 5.0f); + +/** Engine Failure Time Threshold + * + * Engine failure triggers only if the throttle threshold and the + * current to throttle threshold are violated for this time + * + * @group commander + * @unit second + * @min 0.0f + * @max 7.0f + */ +PARAM_DEFINE_FLOAT(COM_EF_TIME, 5.0f); diff --git a/src/modules/systemlib/circuit_breaker.c b/src/modules/systemlib/circuit_breaker.c index b0f95aedf2..9e5429988b 100644 --- a/src/modules/systemlib/circuit_breaker.c +++ b/src/modules/systemlib/circuit_breaker.c @@ -108,6 +108,20 @@ PARAM_DEFINE_INT32(CBRK_AIRSPD_CHK, 0); */ PARAM_DEFINE_INT32(CBRK_FLIGHTTERM, 0); +/** + * Circuit breaker for engine failure detection + * + * Setting this parameter to 284953 will disable the engine failure detection. + * If the aircraft is in engine failure mode the enine failure flag will be + * set to healthy + * WARNING: ENABLING THIS CIRCUIT BREAKER IS AT OWN RISK + * + * @min 0 + * @max 284953 + * @group Circuit Breaker + */ +PARAM_DEFINE_INT32(CBRK_ENGINEFAIL, 0); + bool circuit_breaker_enabled(const char* breaker, int32_t magic) { int32_t val; diff --git a/src/modules/systemlib/circuit_breaker.h b/src/modules/systemlib/circuit_breaker.h index 445a89d3a7..6a55e4948d 100644 --- a/src/modules/systemlib/circuit_breaker.h +++ b/src/modules/systemlib/circuit_breaker.h @@ -54,6 +54,7 @@ #define CBRK_IO_SAFETY_KEY 22027 #define CBRK_AIRSPD_CHK_KEY 162128 #define CBRK_FLIGHTTERM_KEY 121212 +#define CBRK_ENGINEFAIL_KEY 284953 #include From d7f93f4ece19a4244cacf0227d0818f418a0f214 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 5 Sep 2014 10:04:42 +0200 Subject: [PATCH 190/411] loiter mission items: better reached check for FW --- src/modules/navigator/mission_block.cpp | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index 4adf77dce6..157a1cf712 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -113,6 +113,19 @@ MissionBlock::is_mission_item_reached() if (dist >= 0.0f && dist <= _navigator->get_acceptance_radius()) { _waypoint_position_reached = true; } + } else if (!_navigator->get_vstatus()->is_rotary_wing && + (_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)) { + /* Loiter mission item on a non rotary wing: the aircraft is going to circle the + * coordinates with a radius equal to the loiter_radius field + * Therefore the item is marked as reached once the system reaches the loiter + * radius (+ some margin). Time inside and turn count is handled elsewhere. + */ + if (dist >= 0.0f && dist <= _mission_item.loiter_radius * 1.2f) { + _waypoint_position_reached = true; + } + } else { /* for normal mission items used their acceptance radius */ if (dist >= 0.0f && dist <= _mission_item.acceptance_radius) { From 0a6267e010a1401ba111d5477c26a0b8ff17ffea Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 5 Sep 2014 10:18:25 +0200 Subject: [PATCH 191/411] update comment --- src/modules/navigator/mission_block.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index 157a1cf712..723caec7ce 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -118,14 +118,14 @@ MissionBlock::is_mission_item_reached() _mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || _mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT)) { /* Loiter mission item on a non rotary wing: the aircraft is going to circle the - * coordinates with a radius equal to the loiter_radius field + * coordinates with a radius equal to the loiter_radius field. It is not flying + * through the waypoint center. * Therefore the item is marked as reached once the system reaches the loiter * radius (+ some margin). Time inside and turn count is handled elsewhere. */ if (dist >= 0.0f && dist <= _mission_item.loiter_radius * 1.2f) { _waypoint_position_reached = true; } - } else { /* for normal mission items used their acceptance radius */ if (dist >= 0.0f && dist <= _mission_item.acceptance_radius) { From 1d9c99956f3f84e4350734f1eef41df4c03411f8 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 5 Sep 2014 12:06:05 +0200 Subject: [PATCH 192/411] make rc loss timeout a param --- src/modules/commander/commander.cpp | 7 +++++-- src/modules/commander/commander_params.c | 11 +++++++++++ 2 files changed, 16 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 447e2189d5..8dd6fb0861 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -128,7 +128,6 @@ extern struct system_load_s system_load; #define POSITION_TIMEOUT (2 * 1000 * 1000) /**< consider the local or global position estimate invalid after 600ms */ #define FAILSAFE_DEFAULT_TIMEOUT (3 * 1000 * 1000) /**< hysteresis time - the failsafe will trigger after 3 seconds in this state */ -#define RC_TIMEOUT 500000 #define OFFBOARD_TIMEOUT 500000 #define DIFFPRESS_TIMEOUT 2000000 @@ -684,6 +683,7 @@ int commander_thread_main(int argc, char *argv[]) param_t _param_enable_parachute = param_find("NAV_PARACHUTE_EN"); param_t _param_enable_datalink_loss = param_find("COM_DL_LOSS_EN"); param_t _param_datalink_loss_timeout = param_find("COM_DL_LOSS_T"); + param_t _param_rc_loss_timeout = param_find("COM_RC_LOSS_T"); param_t _param_datalink_regain_timeout = param_find("COM_DL_REG_T"); param_t _param_onboard_sysid = param_find("COM_ONBSYSID"); param_t _param_ef_throttle_thres = param_find("COM_EF_THROT"); @@ -976,6 +976,7 @@ int commander_thread_main(int argc, char *argv[]) int32_t datalink_loss_enabled = false; int32_t datalink_loss_timeout = 10; + float rc_loss_timeout = 0.5; int32_t datalink_regain_timeout = 0; int32_t onboard_sysid = 42; /**< systemid of the onboard computer, telemetry from this sysid is not @@ -1048,6 +1049,7 @@ int commander_thread_main(int argc, char *argv[]) param_get(_param_enable_parachute, ¶chute_enabled); param_get(_param_enable_datalink_loss, &datalink_loss_enabled); param_get(_param_datalink_loss_timeout, &datalink_loss_timeout); + param_get(_param_rc_loss_timeout, &rc_loss_timeout); param_get(_param_datalink_regain_timeout, &datalink_regain_timeout); param_get(_param_onboard_sysid, &onboard_sysid); param_get(_param_ef_throttle_thres, &ef_throttle_thres); @@ -1462,7 +1464,8 @@ int commander_thread_main(int argc, char *argv[]) } /* RC input check */ - if (!status.rc_input_blocked && sp_man.timestamp != 0 && hrt_absolute_time() < sp_man.timestamp + RC_TIMEOUT) { + if (!status.rc_input_blocked && sp_man.timestamp != 0 && + hrt_absolute_time() < sp_man.timestamp + (uint64_t)(rc_loss_timeout * 1e6f)) { /* handle the case where RC signal was regained */ if (!status.rc_signal_found_once) { status.rc_signal_found_once = true; diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index 15c299a8bd..7d06003c91 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -170,3 +170,14 @@ PARAM_DEFINE_FLOAT(COM_EF_C2T, 5.0f); * @max 7.0f */ PARAM_DEFINE_FLOAT(COM_EF_TIME, 5.0f); + +/** RC loss time threshold + * + * After this amount of seconds without RC connection the rc lost flag is set to true + * + * @group commander + * @unit second + * @min 0 + * @max 35 + */ +PARAM_DEFINE_FLOAT(COM_RC_LOSS_T, 0.5); From 960587b10ff351c2ba9dc0ef4e9b5d5b2fabfbe7 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 6 Sep 2014 11:03:32 +0200 Subject: [PATCH 193/411] Drop at the exact timing, drop only if facing into the right direction --- src/modules/bottle_drop/bottle_drop.cpp | 15 +++++++++------ 1 file changed, 9 insertions(+), 6 deletions(-) diff --git a/src/modules/bottle_drop/bottle_drop.cpp b/src/modules/bottle_drop/bottle_drop.cpp index 0bb65252f9..56c09970d3 100644 --- a/src/modules/bottle_drop/bottle_drop.cpp +++ b/src/modules/bottle_drop/bottle_drop.cpp @@ -626,9 +626,15 @@ BottleDrop::task_main() } else { // We're close enough - open the bay - distance_open_door = math::max(5.0f, 3.0f * fabsf(t_door * groundspeed_body)); + distance_open_door = math::max(10.0f, 3.0f * fabsf(t_door * groundspeed_body)); - if (isfinite(distance_real) && distance_real < distance_open_door) { + float ground_direction = atan2f(_global_pos.vel_e, _global_pos.vel_n); + float approach_direction = get_bearing_to_next_waypoint(flight_vector_s.lat, flight_vector_s.lon, flight_vector_e.lat, flight_vector_e.lon); + + float approach_error = math::wrap_pi(ground_direction - approach_direction); + + if (isfinite(distance_real) && distance_real < distance_open_door && + approach_error < math::radians(20.0f)) { open_bay(); _drop_state = DROP_STATE_BAY_OPEN; mavlink_log_info(_mavlink_fd, "#audio: opening bay"); @@ -646,11 +652,8 @@ BottleDrop::task_main() map_projection_reproject(&ref, x_f, y_f, &x_f_NED, &y_f_NED); future_distance = get_distance_to_next_waypoint(x_f_NED, y_f_NED, _drop_position.lat, _drop_position.lon); - warnx("Distance real: %.2f", (double)distance_real); - if (isfinite(distance_real) && - (distance_real < precision) && ((distance_real < future_distance) || - (distance_real < precision / 10.0f))) { + (distance_real < precision) && ((distance_real < future_distance))) { drop(); _drop_state = DROP_STATE_DROPPED; mavlink_log_info(_mavlink_fd, "#audio: payload dropped"); From 3834796ce94d424701e20c047b1bf29fcc4ffd61 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 6 Sep 2014 11:39:56 +0200 Subject: [PATCH 194/411] More user feedback on approach angle and approach error. Fix check for approach error --- src/modules/bottle_drop/bottle_drop.cpp | 27 ++++++++++++++++--------- 1 file changed, 18 insertions(+), 9 deletions(-) diff --git a/src/modules/bottle_drop/bottle_drop.cpp b/src/modules/bottle_drop/bottle_drop.cpp index 56c09970d3..a3eb721e35 100644 --- a/src/modules/bottle_drop/bottle_drop.cpp +++ b/src/modules/bottle_drop/bottle_drop.cpp @@ -384,6 +384,7 @@ BottleDrop::task_main() float x_f, y_f; // to-be position of the UAV after dt_runs seconds in projected coordinates double x_f_NED, y_f_NED; // to-be position of the UAV after dt_runs seconds in NED float distance_open_door; // The distance the UAV travels during its doors open [m] + float approach_error = 0.0f; // The error in radians between current ground vector and desired ground vector float distance_real = 0; // The distance between the UAVs position and the drop point [m] float future_distance = 0; // The distance between the UAVs to-be position and the drop point [m] @@ -505,11 +506,21 @@ BottleDrop::task_main() float windspeed_norm = sqrtf(wind.windspeed_north * wind.windspeed_north + wind.windspeed_east * wind.windspeed_east); float groundspeed_body = sqrtf(_global_pos.vel_n * _global_pos.vel_n + _global_pos.vel_e * _global_pos.vel_e); - distance_real = fabsf(get_distance_to_next_waypoint(_global_pos.lat, _global_pos.lon, _drop_position.lat, _drop_position.lon)); ground_distance = _global_pos.alt - _target_position.alt; - if (counter % 90 == 0) { - mavlink_log_info(_mavlink_fd, "#audio: drop distance %.2f", (double)distance_real); + // Distance to drop position and angle error to approach vector + // are relevant in all states greater than target valid (which calculates these positions) + if (_drop_state > DROP_STATE_TARGET_VALID) { + distance_real = fabsf(get_distance_to_next_waypoint(_global_pos.lat, _global_pos.lon, _drop_position.lat, _drop_position.lon)); + + float ground_direction = atan2f(_global_pos.vel_e, _global_pos.vel_n); + float approach_direction = get_bearing_to_next_waypoint(flight_vector_s.lat, flight_vector_s.lon, flight_vector_e.lat, flight_vector_e.lon); + + approach_error = _wrap_pi(ground_direction - approach_direction); + + if (counter % 90 == 0) { + mavlink_log_critical(_mavlink_fd, "drop distance %u, heading: %u", (unsigned)distance_real, (unsigned)math::degrees(approach_error)); + } } switch (_drop_state) { @@ -612,6 +623,9 @@ BottleDrop::task_main() _onboard_mission_pub = orb_advertise(ORB_ID(onboard_mission), &_onboard_mission); } + float approach_direction = get_bearing_to_next_waypoint(flight_vector_s.lat, flight_vector_s.lon, flight_vector_e.lat, flight_vector_e.lon); + mavlink_log_critical(_mavlink_fd, "position set, approach heading: %u", (unsigned)distance_real, (unsigned)math::degrees(approach_direction + M_PI_F)); + _drop_state = DROP_STATE_TARGET_SET; } break; @@ -628,13 +642,8 @@ BottleDrop::task_main() // We're close enough - open the bay distance_open_door = math::max(10.0f, 3.0f * fabsf(t_door * groundspeed_body)); - float ground_direction = atan2f(_global_pos.vel_e, _global_pos.vel_n); - float approach_direction = get_bearing_to_next_waypoint(flight_vector_s.lat, flight_vector_s.lon, flight_vector_e.lat, flight_vector_e.lon); - - float approach_error = math::wrap_pi(ground_direction - approach_direction); - if (isfinite(distance_real) && distance_real < distance_open_door && - approach_error < math::radians(20.0f)) { + fabsf(approach_error) < math::radians(20.0f)) { open_bay(); _drop_state = DROP_STATE_BAY_OPEN; mavlink_log_info(_mavlink_fd, "#audio: opening bay"); From cb11d1f99e6c8dd37a844eabdbd77d3fde2f2398 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 6 Sep 2014 12:00:37 +0200 Subject: [PATCH 195/411] Better message formatting --- src/modules/bottle_drop/bottle_drop.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/bottle_drop/bottle_drop.cpp b/src/modules/bottle_drop/bottle_drop.cpp index a3eb721e35..91f1b1c950 100644 --- a/src/modules/bottle_drop/bottle_drop.cpp +++ b/src/modules/bottle_drop/bottle_drop.cpp @@ -519,7 +519,7 @@ BottleDrop::task_main() approach_error = _wrap_pi(ground_direction - approach_direction); if (counter % 90 == 0) { - mavlink_log_critical(_mavlink_fd, "drop distance %u, heading: %u", (unsigned)distance_real, (unsigned)math::degrees(approach_error)); + mavlink_log_critical(_mavlink_fd, "drop distance %u, heading error %u", (unsigned)distance_real, (unsigned)math::degrees(approach_error)); } } From 8296a14e9befae6b36ef04aff78c502f3c7b08a7 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 6 Sep 2014 14:36:25 +0200 Subject: [PATCH 196/411] Bottle drop: Better cd default --- src/modules/bottle_drop/bottle_drop_params.c | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/modules/bottle_drop/bottle_drop_params.c b/src/modules/bottle_drop/bottle_drop_params.c index e5d35bf0a2..51ebfb9a19 100644 --- a/src/modules/bottle_drop/bottle_drop_params.c +++ b/src/modules/bottle_drop/bottle_drop_params.c @@ -92,11 +92,11 @@ PARAM_DEFINE_FLOAT(BD_PRECISION, 30.0f); * http://en.wikipedia.org/wiki/Drag_coefficient * * @unit meter - * @min 1.0 - * @max 80.0 + * @min 0.08 + * @max 1.5 * @group Payload drop */ -PARAM_DEFINE_FLOAT(BD_OBJ_CD, 0.86f); +PARAM_DEFINE_FLOAT(BD_OBJ_CD, 0.1f); /** * Payload mass From dbe35d64ca4288de551ec11b4d5bff43650e8012 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 6 Sep 2014 15:32:12 +0200 Subject: [PATCH 197/411] Fixed delay calculation of laser --- .../ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index 97abb76a9c..2c50e5c755 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -1369,7 +1369,7 @@ FixedwingEstimator::task_main() if (newRangeData) { _ekf->fuseRngData = true; _ekf->useRangeFinder = true; - _ekf->RecallStates(_ekf->statesAtRngTime, (IMUmsec - 500.0f)); + _ekf->RecallStates(_ekf->statesAtRngTime, (IMUmsec - 100.0f)); _ekf->GroundEKF(); } From cf601c09bf14ac4141bc66f2f71934fcf65da2a6 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 4 Sep 2014 17:18:06 +0200 Subject: [PATCH 198/411] swissfang: don't build MC apps on FMU1 --- makefiles/config_px4fmu-v1_default.mk | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk index 97eddfdd2b..99c0eb8c1a 100644 --- a/makefiles/config_px4fmu-v1_default.mk +++ b/makefiles/config_px4fmu-v1_default.mk @@ -67,17 +67,17 @@ MODULES += modules/gpio_led # # Estimation modules (EKF / other filters) # -MODULES += modules/attitude_estimator_ekf +#MODULES += modules/attitude_estimator_ekf MODULES += modules/ekf_att_pos_estimator -MODULES += modules/position_estimator_inav +#MODULES += modules/position_estimator_inav # # Vehicle Control # MODULES += modules/fw_pos_control_l1 MODULES += modules/fw_att_control -MODULES += modules/mc_att_control -MODULES += modules/mc_pos_control +#MODULES += modules/mc_att_control +#MODULES += modules/mc_pos_control # # Logging From 5418412777ca96362cb5d866b2d0f5bb7efc3131 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 7 Sep 2014 13:05:03 +0200 Subject: [PATCH 199/411] do send termination info only once --- src/modules/commander/commander.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 8dd6fb0861..bd5b464aa4 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1684,9 +1684,9 @@ int commander_thread_main(int argc, char *argv[]) static bool flight_termination_printed = false; if (!flight_termination_printed) { warnx("Flight termination because of data link loss && gps failure"); + mavlink_log_critical(mavlink_fd, "DL and GPS lost: flight termination"); flight_termination_printed = true; } - mavlink_log_critical(mavlink_fd, "DL and GPS lost: flight termination"); } /* At this point the rc signal and the gps system have been checked @@ -1703,9 +1703,9 @@ int commander_thread_main(int argc, char *argv[]) static bool flight_termination_printed = false; if (!flight_termination_printed) { warnx("Flight termination because of RC signal loss && gps failure"); + mavlink_log_critical(mavlink_fd, "RC and GPS lost: flight termination"); flight_termination_printed = true; } - mavlink_log_critical(mavlink_fd, "RC and GPS lost: flight termination"); } } From 74601939a82174fcb6ef2d681e1f32b14a0a4ba0 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 7 Sep 2014 13:45:21 +0200 Subject: [PATCH 200/411] Revert "do send termination info only once" This reverts commit 5418412777ca96362cb5d866b2d0f5bb7efc3131. --- src/modules/commander/commander.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index bd5b464aa4..8dd6fb0861 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1684,9 +1684,9 @@ int commander_thread_main(int argc, char *argv[]) static bool flight_termination_printed = false; if (!flight_termination_printed) { warnx("Flight termination because of data link loss && gps failure"); - mavlink_log_critical(mavlink_fd, "DL and GPS lost: flight termination"); flight_termination_printed = true; } + mavlink_log_critical(mavlink_fd, "DL and GPS lost: flight termination"); } /* At this point the rc signal and the gps system have been checked @@ -1703,9 +1703,9 @@ int commander_thread_main(int argc, char *argv[]) static bool flight_termination_printed = false; if (!flight_termination_printed) { warnx("Flight termination because of RC signal loss && gps failure"); - mavlink_log_critical(mavlink_fd, "RC and GPS lost: flight termination"); flight_termination_printed = true; } + mavlink_log_critical(mavlink_fd, "RC and GPS lost: flight termination"); } } From 9608e7adeb069878d5cedd3ece9edc7f4ac40ce4 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 7 Sep 2014 15:29:53 +0200 Subject: [PATCH 201/411] flight termination mavlink outtput: limit rate --- src/modules/commander/commander.cpp | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 8dd6fb0861..3a82720913 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1684,9 +1684,12 @@ int commander_thread_main(int argc, char *argv[]) static bool flight_termination_printed = false; if (!flight_termination_printed) { warnx("Flight termination because of data link loss && gps failure"); + mavlink_log_critical(mavlink_fd, "DL and GPS lost: flight termination"); flight_termination_printed = true; } - mavlink_log_critical(mavlink_fd, "DL and GPS lost: flight termination"); + if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0 ) { + mavlink_log_critical(mavlink_fd, "DL and GPS lost: flight termination"); + } } /* At this point the rc signal and the gps system have been checked @@ -1705,7 +1708,9 @@ int commander_thread_main(int argc, char *argv[]) warnx("Flight termination because of RC signal loss && gps failure"); flight_termination_printed = true; } - mavlink_log_critical(mavlink_fd, "RC and GPS lost: flight termination"); + if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0 ) { + mavlink_log_critical(mavlink_fd, "RC and GPS lost: flight termination"); + } } } From eb90979ba8254d7ade8ce530c7e4643d48c4196b Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 7 Sep 2014 15:31:58 +0200 Subject: [PATCH 202/411] if V_RCL_LT < 0 go directly to termination --- src/modules/navigator/rcloss.cpp | 15 +++++++++++---- src/modules/navigator/rcloss_params.c | 3 ++- 2 files changed, 13 insertions(+), 5 deletions(-) diff --git a/src/modules/navigator/rcloss.cpp b/src/modules/navigator/rcloss.cpp index 651e311840..5564a1c42e 100644 --- a/src/modules/navigator/rcloss.cpp +++ b/src/modules/navigator/rcloss.cpp @@ -153,10 +153,17 @@ RCLoss::advance_rcl() { switch (_rcl_state) { case RCL_STATE_NONE: - /* Check the number of data link losses. If above home fly home directly */ - warnx("RC loss, OBC mode, loiter"); - mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: rc loss, loitering"); - _rcl_state = RCL_STATE_LOITER; + if (_param_loitertime.get() > 0.0f) { + warnx("RC loss, OBC mode, loiter"); + mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: rc loss, loitering"); + _rcl_state = RCL_STATE_LOITER; + } else { + warnx("RC loss, OBC mode, slip loiter, terminate"); + mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: rc loss, terminating"); + _rcl_state = RCL_STATE_TERMINATE; + _navigator->get_mission_result()->stay_in_failsafe = true; + _navigator->publish_mission_result(); + } break; case RCL_STATE_LOITER: _rcl_state = RCL_STATE_TERMINATE; diff --git a/src/modules/navigator/rcloss_params.c b/src/modules/navigator/rcloss_params.c index 83d23cf499..f1a01c73b5 100644 --- a/src/modules/navigator/rcloss_params.c +++ b/src/modules/navigator/rcloss_params.c @@ -51,9 +51,10 @@ * Loiter Time * * The amount of time in seconds the system should loiter at current position before termination + * Set to -1 to make the system skip loitering * * @unit seconds - * @min 0.0 + * @min -1.0 * @group RCL */ PARAM_DEFINE_FLOAT(NAV_RCL_LT, 120.0f); From 97f7c0088f07cb86463501e81b58c5dba971a50c Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 7 Sep 2014 15:32:28 +0200 Subject: [PATCH 203/411] fix typo in comment --- 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 79992bf451..d177f75522 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 @@ -1223,7 +1223,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } if (_vehicle_status.engine_failure || _vehicle_status.engine_failure_cmd) { - /* Set thrrust to 0 to minimize damage */ + /* Set thrust to 0 to minimize damage */ _att_sp.thrust = 0.0f; } else if (pos_sp_triplet.current.type == SETPOINT_TYPE_TAKEOFF && launch_detection_state != LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS) { From e8503ab3f90353957136d60dbc3fa7668249119c Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 7 Sep 2014 15:33:11 +0200 Subject: [PATCH 204/411] gps failure has priority over engine falure, in case both fail make sure that the gps failure mode does not turn on the engine --- src/modules/commander/state_machine_helper.cpp | 4 ++-- src/modules/fw_att_control/fw_att_control_main.cpp | 8 ++++++-- 2 files changed, 8 insertions(+), 4 deletions(-) diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index e3b5d30e49..9568752aed 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -511,10 +511,10 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en } else if (status->rc_signal_lost_cmd) { status->nav_state = NAVIGATION_STATE_AUTO_RTGS; //XXX /* Finished handling commands which have priority , now handle failures */ - } else if (status->engine_failure) { - status->nav_state = NAVIGATION_STATE_AUTO_LANDENGFAIL; } else if (status->gps_failure) { status->nav_state = NAVIGATION_STATE_AUTO_LANDGPSFAIL; + } else if (status->engine_failure) { + status->nav_state = NAVIGATION_STATE_AUTO_LANDENGFAIL; } else if (((status->data_link_lost && data_link_loss_enabled) && status->rc_signal_lost) || (!data_link_loss_enabled && status->rc_signal_lost && mission_finished)) { status->failsafe = true; 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 bef2f7ca53..e770c11a27 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -893,8 +893,12 @@ FixedwingAttitudeControl::task_main() } } - /* throttle passed through */ - _actuators.control[3] = (isfinite(throttle_sp)) ? throttle_sp : 0.0f; + /* throttle passed through if it is finite and if no engine failure was + * detected */ + _actuators.control[3] = (isfinite(throttle_sp) && + !(_vehicle_status.engine_failure || + _vehicle_status.engine_failure_cmd)) ? + throttle_sp : 0.0f; if (!isfinite(throttle_sp)) { if (_debug && loop_counter % 10 == 0) { warnx("throttle_sp %.4f", (double)throttle_sp); From bc880a3ff9a297d39c7f53068bbe3ad1be572d44 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 11 Sep 2014 01:02:35 +0200 Subject: [PATCH 205/411] Fix dt calculation which is used to calculate correct drop time. --- src/modules/bottle_drop/bottle_drop.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/bottle_drop/bottle_drop.cpp b/src/modules/bottle_drop/bottle_drop.cpp index 0bb65252f9..8114703082 100644 --- a/src/modules/bottle_drop/bottle_drop.cpp +++ b/src/modules/bottle_drop/bottle_drop.cpp @@ -466,7 +466,7 @@ BottleDrop::task_main() const unsigned sleeptime_us = 50000; hrt_abstime last_run = hrt_absolute_time(); - float dt_runs = 1e6f / sleeptime_us; + float dt_runs = sleeptime_us / 1e6f; // switch to faster updates during the drop while (_drop_state > DROP_STATE_INIT) { @@ -691,7 +691,7 @@ BottleDrop::task_main() // run at roughly 20 Hz usleep(sleeptime_us); - dt_runs = 1e6f / hrt_elapsed_time(&last_run); + dt_runs = hrt_elapsed_time(&last_run) / 1e6f; last_run = hrt_absolute_time(); } } From 1512bf727c7ad19400fc4777fbccce7b6e951437 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 11 Sep 2014 01:04:02 +0200 Subject: [PATCH 206/411] Remove useless modulo throttling, which is a nice source of potential non-trivial non-determinism / timing issues. --- src/modules/bottle_drop/bottle_drop.cpp | 76 ++++++++++++------------- 1 file changed, 36 insertions(+), 40 deletions(-) diff --git a/src/modules/bottle_drop/bottle_drop.cpp b/src/modules/bottle_drop/bottle_drop.cpp index 8114703082..128e406191 100644 --- a/src/modules/bottle_drop/bottle_drop.cpp +++ b/src/modules/bottle_drop/bottle_drop.cpp @@ -517,53 +517,49 @@ BottleDrop::task_main() case DROP_STATE_TARGET_VALID: { - // Update drop point at 10 Hz - if (counter % 10 == 0) { - - az = g; // acceleration in z direction[m/s^2] - vz = 0; // velocity in z direction [m/s] - z = 0; // fallen distance [m] - h_0 = _global_pos.alt - _target_position.alt; // height over target at start[m] - h = h_0; // height over target [m] - ax = 0; // acceleration in x direction [m/s^2] - vx = groundspeed_body;// XXX project // ground speed in x direction [m/s] - x = 0; // traveled distance in x direction [m] - vw = 0; // wind speed [m/s] - vrx = 0; // relative velocity in x direction [m/s] - v = groundspeed_body; // relative speed vector [m/s] - Fd = 0; // Drag force [N] - Fdx = 0; // Drag force in x direction [N] - Fdz = 0; // Drag force in z direction [N] + az = g; // acceleration in z direction[m/s^2] + vz = 0; // velocity in z direction [m/s] + z = 0; // fallen distance [m] + h_0 = _global_pos.alt - _target_position.alt; // height over target at start[m] + h = h_0; // height over target [m] + ax = 0; // acceleration in x direction [m/s^2] + vx = groundspeed_body;// XXX project // ground speed in x direction [m/s] + x = 0; // traveled distance in x direction [m] + vw = 0; // wind speed [m/s] + vrx = 0; // relative velocity in x direction [m/s] + v = groundspeed_body; // relative speed vector [m/s] + Fd = 0; // Drag force [N] + Fdx = 0; // Drag force in x direction [N] + Fdz = 0; // Drag force in z direction [N] - // Compute the distance the bottle will travel after it is dropped in body frame coordinates --> x - while (h > 0.05f) { - // z-direction - vz = vz + az * dt_freefall_prediction; - z = z + vz * dt_freefall_prediction; - h = h_0 - z; + // Compute the distance the bottle will travel after it is dropped in body frame coordinates --> x + while (h > 0.05f) { + // z-direction + vz = vz + az * dt_freefall_prediction; + z = z + vz * dt_freefall_prediction; + h = h_0 - z; - // x-direction - vw = windspeed_norm * logf(h / z_0) / logf(ground_distance / z_0); - vx = vx + ax * dt_freefall_prediction; - x = x + vx * dt_freefall_prediction; - vrx = vx + vw; + // x-direction + vw = windspeed_norm * logf(h / z_0) / logf(ground_distance / z_0); + vx = vx + ax * dt_freefall_prediction; + x = x + vx * dt_freefall_prediction; + vrx = vx + vw; - // drag force - v = sqrtf(vz * vz + vrx * vrx); - Fd = 0.5f * rho * A * cd * (v * v); - Fdx = Fd * vrx / v; - Fdz = Fd * vz / v; + // drag force + v = sqrtf(vz * vz + vrx * vrx); + Fd = 0.5f * rho * A * cd * (v * v); + Fdx = Fd * vrx / v; + Fdz = Fd * vz / v; - // acceleration - az = g - Fdz / m; - ax = -Fdx / m; - } - - // compute drop vector - x = groundspeed_body * t_signal + x; + // acceleration + az = g - Fdz / m; + ax = -Fdx / m; } + // compute drop vector + x = groundspeed_body * t_signal + x; + x_t = 0.0f; y_t = 0.0f; From 6791ab72a910b00818026ac60d95d8df20bfa0d3 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 11 Sep 2014 01:06:30 +0200 Subject: [PATCH 207/411] Run faster for better accuracy. --- src/modules/bottle_drop/bottle_drop.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/bottle_drop/bottle_drop.cpp b/src/modules/bottle_drop/bottle_drop.cpp index 128e406191..53071630df 100644 --- a/src/modules/bottle_drop/bottle_drop.cpp +++ b/src/modules/bottle_drop/bottle_drop.cpp @@ -463,7 +463,7 @@ BottleDrop::task_main() continue; } - const unsigned sleeptime_us = 50000; + const unsigned sleeptime_us = 9500; hrt_abstime last_run = hrt_absolute_time(); float dt_runs = sleeptime_us / 1e6f; @@ -684,7 +684,7 @@ BottleDrop::task_main() // update_actuators(); - // run at roughly 20 Hz + // run at roughly 100 Hz usleep(sleeptime_us); dt_runs = hrt_elapsed_time(&last_run) / 1e6f; From 940264f6ab565caee7a5426cfa1023b306c97e74 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 13 Sep 2014 16:23:28 +0200 Subject: [PATCH 208/411] Revert "datalink check: ignore onboard computer" This reverts commit 3f8793210b47bd8e09ed2adaabc2fab966db5df6. Conflicts: src/modules/commander/commander.cpp src/modules/commander/commander_params.c --- src/modules/commander/commander.cpp | 23 +++-------------------- src/modules/commander/commander_params.c | 10 ---------- 2 files changed, 3 insertions(+), 30 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 3a82720913..c53b143491 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -685,7 +685,6 @@ int commander_thread_main(int argc, char *argv[]) param_t _param_datalink_loss_timeout = param_find("COM_DL_LOSS_T"); param_t _param_rc_loss_timeout = param_find("COM_RC_LOSS_T"); param_t _param_datalink_regain_timeout = param_find("COM_DL_REG_T"); - param_t _param_onboard_sysid = param_find("COM_ONBSYSID"); param_t _param_ef_throttle_thres = param_find("COM_EF_THROT"); param_t _param_ef_current2throttle_thres = param_find("COM_EF_C2T"); param_t _param_ef_time_thres = param_find("COM_EF_TIME"); @@ -883,14 +882,12 @@ int commander_thread_main(int argc, char *argv[]) uint64_t telemetry_last_heartbeat[TELEMETRY_STATUS_ORB_ID_NUM]; uint64_t telemetry_last_dl_loss[TELEMETRY_STATUS_ORB_ID_NUM]; bool telemetry_lost[TELEMETRY_STATUS_ORB_ID_NUM]; - uint8_t telemetry_sysid[TELEMETRY_STATUS_ORB_ID_NUM]; for (int i = 0; i < TELEMETRY_STATUS_ORB_ID_NUM; i++) { telemetry_subs[i] = orb_subscribe(telemetry_status_orb_id[i]); telemetry_last_heartbeat[i] = 0; telemetry_last_dl_loss[i] = 0; telemetry_lost[i] = true; - telemetry_sysid[i] = 0; } /* Subscribe to global position */ @@ -978,9 +975,6 @@ int commander_thread_main(int argc, char *argv[]) int32_t datalink_loss_timeout = 10; float rc_loss_timeout = 0.5; int32_t datalink_regain_timeout = 0; - int32_t onboard_sysid = 42; /**< systemid of the onboard computer, - telemetry from this sysid is not - validated for the datalink loss check */ /* Thresholds for engine failure detection */ int32_t ef_throttle_thres = 1.0f; @@ -988,7 +982,6 @@ int commander_thread_main(int argc, char *argv[]) int32_t ef_time_thres = 1000.0f; uint64_t timestamp_engine_healthy = 0; /**< absolute time when engine was healty*/ - /* check which state machines for changes, clear "changed" flag */ bool arming_state_changed = false; bool main_state_changed = false; @@ -1051,7 +1044,6 @@ int commander_thread_main(int argc, char *argv[]) param_get(_param_datalink_loss_timeout, &datalink_loss_timeout); param_get(_param_rc_loss_timeout, &rc_loss_timeout); param_get(_param_datalink_regain_timeout, &datalink_regain_timeout); - param_get(_param_onboard_sysid, &onboard_sysid); param_get(_param_ef_throttle_thres, &ef_throttle_thres); param_get(_param_ef_current2throttle_thres, &ef_current2throttle_thres); param_get(_param_ef_time_thres, &ef_time_thres); @@ -1101,7 +1093,6 @@ int commander_thread_main(int argc, char *argv[]) } telemetry_last_heartbeat[i] = telemetry.heartbeat_time; - telemetry_sysid[i] = telemetry.system_id; } } @@ -1586,18 +1577,10 @@ int commander_thread_main(int argc, char *argv[]) mavlink_log_critical(mavlink_fd, "data link %i regained", i); telemetry_lost[i] = false; - - /* If this is not an onboard link/onboard computer: - * set flag that we have a valid link */ - if (telemetry_sysid[i] != (uint8_t)onboard_sysid) { - have_link = true; - } - } else if (!telemetry_lost[i] && telemetry_sysid[i] != - (uint8_t)onboard_sysid) { + have_link = true; + } else if (!telemetry_lost[i]) { /* telemetry was healthy also in last iteration - * we don't have to check a timeout, - * telemetry from onboard computers is not accepted as a valid datalink - */ + * we don't have to check a timeout */ have_link = true; } diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index 7d06003c91..f4bc7fd7fb 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -129,16 +129,6 @@ PARAM_DEFINE_INT32(COM_DL_LOSS_T, 10); */ PARAM_DEFINE_INT32(COM_DL_REG_T, 0); -/** Onboard computer system id - * - * The system id of the onboard computer. Heartbeats from this system are ignored during the datalink check - * - * @group commander - * @min 0 - * @max 255 - */ -PARAM_DEFINE_INT32(COM_ONBSYSID, 42); - /** Engine Failure Throttle Threshold * * Engine failure triggers only above this throttle value From deda5d0a04300f2ee67194c4fc9a60da37de722a Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Sat, 13 Sep 2014 19:59:44 -0700 Subject: [PATCH 209/411] Upgraded unit test framework --- ROMFS/px4fmu_test/init.d/rcS | 30 +++++ makefiles/config_px4fmu-v2_test.mk | 1 + .../commander_tests/commander_tests.cpp | 4 +- .../state_machine_helper_test.cpp | 13 +- .../state_machine_helper_test.h | 2 +- .../mavlink_tests/mavlink_ftp_test.cpp | 10 +- .../mavlink/mavlink_tests/mavlink_ftp_test.h | 9 +- .../mavlink/mavlink_tests/mavlink_tests.cpp | 7 +- src/modules/mavlink/mavlink_tests/module.mk | 2 + src/modules/unit_test/unit_test.cpp | 24 ++-- src/modules/unit_test/unit_test.h | 125 ++++++++++-------- 11 files changed, 139 insertions(+), 88 deletions(-) diff --git a/ROMFS/px4fmu_test/init.d/rcS b/ROMFS/px4fmu_test/init.d/rcS index 56482d1407..bc248ac04c 100644 --- a/ROMFS/px4fmu_test/init.d/rcS +++ b/ROMFS/px4fmu_test/init.d/rcS @@ -75,3 +75,33 @@ if [ -f /fs/microsd/mount_test_cmds.txt ] then tests mount fi + +# +# Run unit tests at board boot, reporting failure as needed. +# Add new unit tests using the same pattern as below. +# + +set unit_test_failure 0 + +if mavlink_tests +then +else + set unit_test_failure 1 + set unit_test_failure_list "${unit_test_failure_list} mavlink_tests" +fi + +if commander_tests +then +else + set unit_test_failure 1 + set unit_test_failure_list "${unit_test_failure_list} commander_tests" +fi + +if [ $unit_test_failure == 0 ] +then + echo + echo "All Unit Tests PASSED" +else + echo + echo "Some Unit Tests FAILED:${unit_test_failure_list}" +fi \ No newline at end of file diff --git a/makefiles/config_px4fmu-v2_test.mk b/makefiles/config_px4fmu-v2_test.mk index a41670a77b..2f4d9d6a47 100644 --- a/makefiles/config_px4fmu-v2_test.mk +++ b/makefiles/config_px4fmu-v2_test.mk @@ -56,6 +56,7 @@ LIBRARIES += lib/mathlib/CMSIS MODULES += modules/unit_test MODULES += modules/mavlink/mavlink_tests +MODULES += modules/commander/commander_tests # # Transitional support - add commands from the NuttX export archive. diff --git a/src/modules/commander/commander_tests/commander_tests.cpp b/src/modules/commander/commander_tests/commander_tests.cpp index 0abb84a82c..2bfa5d0bb1 100644 --- a/src/modules/commander/commander_tests/commander_tests.cpp +++ b/src/modules/commander/commander_tests/commander_tests.cpp @@ -48,7 +48,5 @@ extern "C" __EXPORT int commander_tests_main(int argc, char *argv[]); int commander_tests_main(int argc, char *argv[]) { - stateMachineHelperTest(); - - return 0; + return stateMachineHelperTest() ? 0 : -1; } diff --git a/src/modules/commander/commander_tests/state_machine_helper_test.cpp b/src/modules/commander/commander_tests/state_machine_helper_test.cpp index 08dda2fabf..874090e93e 100644 --- a/src/modules/commander/commander_tests/state_machine_helper_test.cpp +++ b/src/modules/commander/commander_tests/state_machine_helper_test.cpp @@ -49,7 +49,7 @@ public: StateMachineHelperTest(); virtual ~StateMachineHelperTest(); - virtual void runTests(void); + virtual bool run_tests(void); private: bool armingStateTransitionTest(); @@ -488,16 +488,13 @@ bool StateMachineHelperTest::isSafeTest(void) return true; } -void StateMachineHelperTest::runTests(void) +bool StateMachineHelperTest::run_tests(void) { ut_run_test(armingStateTransitionTest); ut_run_test(mainStateTransitionTest); ut_run_test(isSafeTest); + + return (_tests_failed == 0); } -void stateMachineHelperTest(void) -{ - StateMachineHelperTest* test = new StateMachineHelperTest(); - test->runTests(); - test->printResults(); -} +ut_declare_test(stateMachineHelperTest, StateMachineHelperTest) \ No newline at end of file diff --git a/src/modules/commander/commander_tests/state_machine_helper_test.h b/src/modules/commander/commander_tests/state_machine_helper_test.h index bbf66255ea..cf67190955 100644 --- a/src/modules/commander/commander_tests/state_machine_helper_test.h +++ b/src/modules/commander/commander_tests/state_machine_helper_test.h @@ -39,6 +39,6 @@ #ifndef STATE_MACHINE_HELPER_TEST_H_ #define STATE_MACHINE_HELPER_TEST_ -void stateMachineHelperTest(void); +bool stateMachineHelperTest(void); #endif /* STATE_MACHINE_HELPER_TEST_H_ */ diff --git a/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.cpp b/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.cpp index 022041c74b..a5fb2117ea 100644 --- a/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.cpp +++ b/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.cpp @@ -65,7 +65,7 @@ MavlinkFtpTest::~MavlinkFtpTest() } /// @brief Called before every test to initialize the FTP Server. -void MavlinkFtpTest::init(void) +void MavlinkFtpTest::_init(void) { _ftp_server = new MavlinkFTP;; _ftp_server->set_unittest_worker(MavlinkFtpTest::receive_message, this); @@ -74,7 +74,7 @@ void MavlinkFtpTest::init(void) } /// @brief Called after every test to take down the FTP Server. -void MavlinkFtpTest::cleanup(void) +void MavlinkFtpTest::_cleanup(void) { delete _ftp_server; @@ -738,7 +738,7 @@ void MavlinkFtpTest::_cleanup_microsd(void) } /// @brief Runs all the unit tests -void MavlinkFtpTest::runTests(void) +bool MavlinkFtpTest::run_tests(void) { ut_run_test(_ack_test); ut_run_test(_bad_opcode_test); @@ -753,5 +753,9 @@ void MavlinkFtpTest::runTests(void) ut_run_test(_removedirectory_test); ut_run_test(_createdirectory_test); ut_run_test(_removefile_test); + + return (_tests_failed == 0); + } +ut_declare_test(mavlink_ftp_test, MavlinkFtpTest) diff --git a/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.h b/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.h index babd909dac..2696192cc6 100644 --- a/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.h +++ b/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.h @@ -46,10 +46,7 @@ public: MavlinkFtpTest(); virtual ~MavlinkFtpTest(); - virtual void init(void); - virtual void cleanup(void); - - virtual void runTests(void); + virtual bool run_tests(void); static void receive_message(const mavlink_message_t *msg, MavlinkFtpTest* ftpTest); @@ -65,6 +62,9 @@ public: MavlinkFtpTest& operator=(const MavlinkFtpTest&); private: + virtual void _init(void); + virtual void _cleanup(void); + bool _ack_test(void); bool _bad_opcode_test(void); bool _bad_datasize_test(void); @@ -105,3 +105,4 @@ private: static const char _unittest_microsd_file[]; }; +bool mavlink_ftp_test(void); diff --git a/src/modules/mavlink/mavlink_tests/mavlink_tests.cpp b/src/modules/mavlink/mavlink_tests/mavlink_tests.cpp index d9c1e413a5..10cbcb0ecc 100644 --- a/src/modules/mavlink/mavlink_tests/mavlink_tests.cpp +++ b/src/modules/mavlink/mavlink_tests/mavlink_tests.cpp @@ -43,10 +43,5 @@ extern "C" __EXPORT int mavlink_tests_main(int argc, char *argv[]); int mavlink_tests_main(int argc, char *argv[]) { - MavlinkFtpTest* test = new MavlinkFtpTest; - - test->runTests(); - test->printResults(); - - return 0; + return mavlink_ftp_test() ? 0 : -1; } diff --git a/src/modules/mavlink/mavlink_tests/module.mk b/src/modules/mavlink/mavlink_tests/module.mk index 07cfce1dc9..1cc28cce15 100644 --- a/src/modules/mavlink/mavlink_tests/module.mk +++ b/src/modules/mavlink/mavlink_tests/module.mk @@ -45,4 +45,6 @@ INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink MODULE_STACKSIZE = 5000 +MAXOPTIMIZATION = -Os + EXTRACXXFLAGS = -Weffc++ -DMAVLINK_FTP_UNIT_TEST diff --git a/src/modules/unit_test/unit_test.cpp b/src/modules/unit_test/unit_test.cpp index be3b9461a8..b7568ce3a4 100644 --- a/src/modules/unit_test/unit_test.cpp +++ b/src/modules/unit_test/unit_test.cpp @@ -36,7 +36,11 @@ #include -UnitTest::UnitTest() +UnitTest::UnitTest() : + _tests_run(0), + _tests_failed(0), + _tests_passed(0), + _assertions(0) { } @@ -44,20 +48,22 @@ UnitTest::~UnitTest() { } -void UnitTest::printResults(void) +void UnitTest::print_results(void) { - warnx(mu_tests_failed() ? "SOME TESTS FAILED" : "ALL TESTS PASSED"); - warnx(" Tests passed : %d", mu_tests_passed()); - warnx(" Tests failed : %d", mu_tests_failed()); - warnx(" Assertions : %d", mu_assertion()); + warnx(_tests_failed ? "SOME TESTS FAILED" : "ALL TESTS PASSED"); + warnx(" Tests passed : %d", _tests_passed); + warnx(" Tests failed : %d", _tests_failed); + warnx(" Assertions : %d", _assertions); } -void UnitTest::printAssert(const char* msg, const char* test, const char* file, int line) +/// @brief Used internally to the ut_assert macro to print assert failures. +void UnitTest::_print_assert(const char* msg, const char* test, const char* file, int line) { - warnx("Assertion failed: %s - %s (%s:%d)", msg, test, file, line); + warnx("Assertion failed: %s - %s (%s:%d)", msg, test, file, line); } -void UnitTest::printCompare(const char* msg, const char *v1_text, int v1, const char *v2_text, int v2, const char* file, int line) +/// @brief Used internally to the ut_compare macro to print assert failures. +void UnitTest::_print_compare(const char* msg, const char *v1_text, int v1, const char *v2_text, int v2, const char* file, int line) { warnx("Compare failed: %s - (%s:%d) (%s:%d) (%s:%d)", msg, v1_text, v1, v2_text, v2, file, line); } diff --git a/src/modules/unit_test/unit_test.h b/src/modules/unit_test/unit_test.h index 1a54897094..8ed4efadf9 100644 --- a/src/modules/unit_test/unit_test.h +++ b/src/modules/unit_test/unit_test.h @@ -37,68 +37,85 @@ #include +/// @brief Base class to be used for unit tests. class __EXPORT UnitTest { - public: -#define INLINE_GLOBAL(type,func) inline type& func() { static type x; return x; } - -INLINE_GLOBAL(int, mu_tests_run) -INLINE_GLOBAL(int, mu_tests_failed) -INLINE_GLOBAL(int, mu_tests_passed) -INLINE_GLOBAL(int, mu_assertion) -INLINE_GLOBAL(int, mu_line) -INLINE_GLOBAL(const char*, mu_last_test) UnitTest(); - virtual ~UnitTest(); + virtual ~UnitTest(); - virtual void init(void) { }; - virtual void cleanup(void) { }; - - virtual void runTests(void) = 0; - void printResults(void); - - void printAssert(const char* msg, const char* test, const char* file, int line); - void printCompare(const char* msg, const char *v1_text, int v1, const char *v2_text, int v2, const char* file, int line); - -#define ut_assert(message, test) \ - do { \ - if (!(test)) { \ - printAssert(message, #test, __FILE__, __LINE__); \ - return false; \ - } else { \ - mu_assertion()++; \ - } \ - } while (0) - -#define ut_compare(message, v1, v2) \ - do { \ - int _v1 = v1; \ - int _v2 = v2; \ - if (_v1 != _v2) { \ - printCompare(message, #v1, _v1, #v2, _v2, __FILE__, __LINE__); \ - return false; \ - } else { \ - mu_assertion()++; \ - } \ + /// @brief Override to run your unit tests. Unit tests should be called using ut_run_test macro. + /// @return true: all unit tests succeeded, false: one or more unit tests failed + virtual bool run_tests(void) = 0; + + /// @brief Prints results from running of unit tests. + void print_results(void); + +/// @brief Macro to create a function which will run a unit test class and print results. +#define ut_declare_test(test_function, test_class) \ + bool test_function(void) \ + { \ + test_class* test = new test_class(); \ + bool success = test->run_tests(); \ + test->print_results(); \ + return success; \ + } + +protected: + +/// @brief Runs a single unit test. Unit tests must have the function signature of bool test(void). The unit +/// test should return true if it succeeded, false for fail. +#define ut_run_test(test) \ + do { \ + warnx("RUNNING TEST: %s", #test); \ + _tests_run++; \ + _init(); \ + if (!test()) { \ + warnx("TEST FAILED: %s", #test); \ + _tests_failed++; \ + } else { \ + warnx("TEST PASSED: %s", #test); \ + _tests_passed++; \ + } \ + _cleanup(); \ } while (0) - -#define ut_run_test(test) \ - do { \ - warnx("RUNNING TEST: %s", #test); \ - mu_tests_run()++; \ - init(); \ - if (!test()) { \ - warnx("TEST FAILED: %s", #test); \ - mu_tests_failed()++; \ - } else { \ - warnx("TEST PASSED: %s", #test); \ - mu_tests_passed()++; \ - } \ - cleanup(); \ - } while (0) + +/// @brief Used to assert a value within a unit test. +#define ut_assert(message, test) \ + do { \ + if (!(test)) { \ + _print_assert(message, #test, __FILE__, __LINE__); \ + return false; \ + } else { \ + _assertions++; \ + } \ + } while (0) + +/// @brief Used to compare two integer values within a unit test. If possible use ut_compare instead of ut_assert +/// since it will give you better error reporting of the actual values being compared. +#define ut_compare(message, v1, v2) \ + do { \ + int _v1 = v1; \ + int _v2 = v2; \ + if (_v1 != _v2) { \ + _print_compare(message, #v1, _v1, #v2, _v2, __FILE__, __LINE__); \ + return false; \ + } else { \ + _assertions++; \ + } \ + } while (0) + + virtual void _init(void) { }; ///< Run before each unit test. Override to provide custom behavior. + virtual void _cleanup(void) { }; ///< Run after each unit test. Override to provide custom behavior. + + void _print_assert(const char* msg, const char* test, const char* file, int line); + void _print_compare(const char* msg, const char *v1_text, int v1, const char *v2_text, int v2, const char* file, int line); + int _tests_run; ///< The number of individual unit tests run + int _tests_failed; ///< The number of unit tests which failed + int _tests_passed; ///< The number of unit tests which passed + int _assertions; ///< Total number of assertions tested by all unit tests }; #endif /* UNIT_TEST_H_ */ From 60c63f48729f028ec9f5b6de93da91873d152bfd Mon Sep 17 00:00:00 2001 From: Vladimir Ermakov Date: Wed, 17 Sep 2014 10:07:29 +0400 Subject: [PATCH 210/411] FTP: Add new open command for write. All open commands now return file size. --- src/modules/mavlink/mavlink_ftp.cpp | 46 ++++++++++++++--------------- src/modules/mavlink/mavlink_ftp.h | 7 +++-- 2 files changed, 27 insertions(+), 26 deletions(-) diff --git a/src/modules/mavlink/mavlink_ftp.cpp b/src/modules/mavlink/mavlink_ftp.cpp index 2a85c3702a..b17036c8ee 100644 --- a/src/modules/mavlink/mavlink_ftp.cpp +++ b/src/modules/mavlink/mavlink_ftp.cpp @@ -180,12 +180,16 @@ MavlinkFTP::_process_request(Request *req) errorCode = _workList(payload); break; - case kCmdOpenFile: - errorCode = _workOpen(payload, false); + case kCmdOpenFileRO: + errorCode = _workOpen(payload, O_RDONLY); break; case kCmdCreateFile: - errorCode = _workOpen(payload, true); + errorCode = _workOpen(payload, O_CREAT | O_EXCL | O_WRONLY); + break; + + case kCmdOpenFileWO: + errorCode = _workOpen(payload, O_CREAT | O_WRONLY); break; case kCmdReadFile: @@ -396,27 +400,27 @@ MavlinkFTP::_workList(PayloadHeader* payload) /// @brief Responds to an Open command MavlinkFTP::ErrorCode -MavlinkFTP::_workOpen(PayloadHeader* payload, bool create) +MavlinkFTP::_workOpen(PayloadHeader* payload, int oflag) { int session_index = _find_unused_session(); if (session_index < 0) { warnx("FTP: Open failed - out of sessions\n"); return kErrNoSessionsAvailable; } - - char *filename = _data_as_cstring(payload); - - uint32_t fileSize = 0; - if (!create) { - struct stat st; - if (stat(filename, &st) != 0) { - return kErrFailErrno; - } - fileSize = st.st_size; - } - int oflag = create ? (O_CREAT | O_EXCL | O_APPEND) : O_RDONLY; - + char *filename = _data_as_cstring(payload); + + uint32_t fileSize = 0; + struct stat st; + if (stat(filename, &st) != 0) { + // fail only if requested open for read + if (oflag & O_RDONLY) + return kErrFailErrno; + else + st.st_size = 0; + } + fileSize = st.st_size; + int fd = ::open(filename, oflag); if (fd < 0) { return kErrFailErrno; @@ -424,12 +428,8 @@ MavlinkFTP::_workOpen(PayloadHeader* payload, bool create) _session_fds[session_index] = fd; payload->session = session_index; - if (create) { - payload->size = 0; - } else { - payload->size = sizeof(uint32_t); - *((uint32_t*)payload->data) = fileSize; - } + payload->size = sizeof(uint32_t); + *((uint32_t*)payload->data) = fileSize; return kErrNone; } diff --git a/src/modules/mavlink/mavlink_ftp.h b/src/modules/mavlink/mavlink_ftp.h index 657e2f855b..1c873fe9f0 100644 --- a/src/modules/mavlink/mavlink_ftp.h +++ b/src/modules/mavlink/mavlink_ftp.h @@ -89,13 +89,14 @@ public: kCmdTerminateSession, ///< Terminates open Read session kCmdResetSessions, ///< Terminates all open Read sessions kCmdListDirectory, ///< List files in from - kCmdOpenFile, ///< Opens file at for reading, returns + kCmdOpenFileRO, ///< Opens file at for reading, returns kCmdReadFile, ///< Reads bytes from in kCmdCreateFile, ///< Creates file at for writing, returns - kCmdWriteFile, ///< Appends bytes to file in + kCmdWriteFile, ///< Writes bytes to in kCmdRemoveFile, ///< Remove file at kCmdCreateDirectory, ///< Creates directory at kCmdRemoveDirectory, ///< Removes Directory at , must be empty + kCmdOpenFileWO, ///< Opens file at for writing, returns kRspAck = 128, ///< Ack response kRspNak ///< Nak response @@ -140,7 +141,7 @@ private: void _reply(Request *req); ErrorCode _workList(PayloadHeader *payload); - ErrorCode _workOpen(PayloadHeader *payload, bool create); + ErrorCode _workOpen(PayloadHeader *payload, int oflag); ErrorCode _workRead(PayloadHeader *payload); ErrorCode _workWrite(PayloadHeader *payload); ErrorCode _workTerminate(PayloadHeader *payload); From 72887e14d9964cf12f8ca0a48d0d185f8d469ecb Mon Sep 17 00:00:00 2001 From: Vladimir Ermakov Date: Wed, 17 Sep 2014 13:04:21 +0400 Subject: [PATCH 211/411] FTP: Implement write command. --- src/modules/mavlink/mavlink_ftp.cpp | 45 +++++++++++++++++------------ 1 file changed, 26 insertions(+), 19 deletions(-) diff --git a/src/modules/mavlink/mavlink_ftp.cpp b/src/modules/mavlink/mavlink_ftp.cpp index b17036c8ee..695dcb0dcc 100644 --- a/src/modules/mavlink/mavlink_ftp.cpp +++ b/src/modules/mavlink/mavlink_ftp.cpp @@ -470,29 +470,36 @@ MavlinkFTP::_workRead(PayloadHeader* payload) MavlinkFTP::ErrorCode MavlinkFTP::_workWrite(PayloadHeader* payload) { -#if 0 - // NYI: Coming soon - auto hdr = req->header(); + int session_index = payload->session; - // look up session - auto session = getSession(hdr->session); - if (session == nullptr) { - return kErrNoSession; + if (!_valid_session(session_index)) { + return kErrInvalidSession; } - // append to file - int result = session->append(hdr->offset, &hdr->data[0], hdr->size); - - if (result < 0) { - // XXX might also be no space, I/O, etc. - return kErrNotAppend; - } - - hdr->size = result; - return kErrNone; -#else - return kErrUnknownCommand; + // Seek to the specified position +#ifdef MAVLINK_FTP_DEBUG + warnx("seek %d", payload->offset); #endif + if (lseek(_session_fds[session_index], payload->offset, SEEK_SET) < 0) { + // Unable to see to the specified location + warnx("seek fail"); + return kErrFailErrno; + } + + int bytes_written = ::write(_session_fds[session_index], &payload->data[0], payload->size); + if (bytes_written < 0) { + // Negative return indicates error other than eof + warnx("write fail %d", bytes_written); + return kErrFailErrno; + } + + // need to decide how to indicate how much data was written + // current: old code set hdr->size + payload->size = bytes_written; + //payload->size = sizeof(uint32_t); + //*((uint32_t*)payload->data) = bytes_written; + + return kErrNone; } /// @brief Responds to a RemoveFile command From f55d20a133397b2529826f015bec3452294ad4c2 Mon Sep 17 00:00:00 2001 From: Vladimir Ermakov Date: Wed, 17 Sep 2014 18:42:00 +0400 Subject: [PATCH 212/411] FTP: Add truncate command. Unfortunately NuttX not provides truncate(), had to be emulated by copying with O_TRUNC flag. --- src/modules/mavlink/mavlink_ftp.cpp | 126 ++++++++++++++++++++++++++++ src/modules/mavlink/mavlink_ftp.h | 3 + 2 files changed, 129 insertions(+) diff --git a/src/modules/mavlink/mavlink_ftp.cpp b/src/modules/mavlink/mavlink_ftp.cpp index 695dcb0dcc..6865655da1 100644 --- a/src/modules/mavlink/mavlink_ftp.cpp +++ b/src/modules/mavlink/mavlink_ftp.cpp @@ -204,6 +204,10 @@ MavlinkFTP::_process_request(Request *req) errorCode = _workRemoveFile(payload); break; + case kCmdTruncateFile: + errorCode = _workTruncateFile(payload); + break; + case kCmdCreateDirectory: errorCode = _workCreateDirectory(payload); break; @@ -517,6 +521,82 @@ MavlinkFTP::_workRemoveFile(PayloadHeader* payload) } } +/// @brief Responds to a TruncateFile command +MavlinkFTP::ErrorCode +MavlinkFTP::_workTruncateFile(PayloadHeader* payload) +{ + char file[kMaxDataLength]; + const char temp_file[] = "/fs/microsd/.trunc.tmp"; + strncpy(file, _data_as_cstring(payload), kMaxDataLength); + payload->size = 0; + + // emulate truncate(file, payload->offset) by + // copying to temp and overwrite with O_TRUNC flag. + + struct stat st; + if (stat(file, &st) != 0) { + return kErrFailErrno; + } + + if (!S_ISREG(st.st_mode)) { + errno = EISDIR; + return kErrFailErrno; + } + + // check perms allow us to write (not romfs) + int mode = st.st_mode & (S_IRWXU | S_IRWXG | S_IRWXO); + if (!(mode & ~(S_IRUSR | S_IXUSR | S_IRGRP | S_IXGRP | S_IROTH | S_IXOTH))) { + errno = EROFS; + return kErrFailErrno; + } + + if (payload->offset == st.st_size) { + // nothing to do + return kErrNone; + } + else if (payload->offset == 0) { + // 1: truncate all data + int fd = ::open(file, O_TRUNC | O_WRONLY); + if (fd < 0) { + return kErrFailErrno; + } + + ::close(fd); + return kErrNone; + } + else if (payload->offset > st.st_size) { + // 2: extend file + int fd = ::open(file, O_WRONLY); + if (fd < 0) { + return kErrFailErrno; + } + + if (lseek(fd, payload->offset - 1, SEEK_SET) != 0) { + ::close(fd); + return kErrFailErrno; + } + + bool ok = 1 == ::write(fd, "", 1); + ::close(fd); + + return (ok)? kErrNone : kErrFailErrno; + } + else { + // 3: truncate + if (_copy_file(file, temp_file, payload->offset) != 0) { + return kErrFailErrno; + } + if (_copy_file(temp_file, file, payload->offset) != 0) { + return kErrFailErrno; + } + if (::unlink(temp_file) != 0) { + return kErrFailErrno; + } + + return kErrNone; + } +} + /// @brief Responds to a Terminate command MavlinkFTP::ErrorCode MavlinkFTP::_workTerminate(PayloadHeader* payload) @@ -652,3 +732,49 @@ MavlinkFTP::_return_request(Request *req) _unlock_request_queue(); } +/// @brief Copy file (with limited space) +int +MavlinkFTP::_copy_file(const char *src_path, const char *dst_path, ssize_t length) +{ + char buff[512]; + int src_fd = -1, dst_fd = -1; + + src_fd = ::open(src_path, O_RDONLY); + if (src_fd < 0) { + return -1; + } + + dst_fd = ::open(dst_path, O_CREAT | O_TRUNC | O_WRONLY); + if (dst_fd < 0) { + ::close(src_fd); + return -1; + } + + while (length > 0) { + ssize_t bytes_read, bytes_written; + size_t blen = (length > sizeof(buff))? sizeof(buff) : length; + + bytes_read = ::read(src_fd, buff, blen); + if (bytes_read == 0) { + // EOF + break; + } + else if (bytes_read < 0) { + warnx("cp: read"); + break; + } + + bytes_written = ::write(dst_fd, buff, bytes_read); + if (bytes_written != bytes_read) { + warnx("cp: short write"); + break; + } + + length -= bytes_written; + } + + ::close(src_fd); + ::close(dst_fd); + + return (length > 0)? -1 : 0; +} diff --git a/src/modules/mavlink/mavlink_ftp.h b/src/modules/mavlink/mavlink_ftp.h index 1c873fe9f0..bde8d7ec57 100644 --- a/src/modules/mavlink/mavlink_ftp.h +++ b/src/modules/mavlink/mavlink_ftp.h @@ -97,6 +97,7 @@ public: kCmdCreateDirectory, ///< Creates directory at kCmdRemoveDirectory, ///< Removes Directory at , must be empty kCmdOpenFileWO, ///< Opens file at for writing, returns + kCmdTruncateFile, ///< Truncate file at to length kRspAck = 128, ///< Ack response kRspNak ///< Nak response @@ -139,6 +140,7 @@ private: static void _worker_trampoline(void *arg); void _process_request(Request *req); void _reply(Request *req); + int _copy_file(const char *src_path, const char *dst_path, ssize_t length); ErrorCode _workList(PayloadHeader *payload); ErrorCode _workOpen(PayloadHeader *payload, int oflag); @@ -149,6 +151,7 @@ private: ErrorCode _workRemoveDirectory(PayloadHeader *payload); ErrorCode _workCreateDirectory(PayloadHeader *payload); ErrorCode _workRemoveFile(PayloadHeader *payload); + ErrorCode _workTruncateFile(PayloadHeader *payload); static const unsigned kRequestQueueSize = 2; ///< Max number of queued requests Request _request_bufs[kRequestQueueSize]; ///< Request buffers which hold work From 407a4e0f0675f0b283eea0632208c8c3f8b9ca20 Mon Sep 17 00:00:00 2001 From: Vladimir Ermakov Date: Wed, 17 Sep 2014 20:02:32 +0400 Subject: [PATCH 213/411] FTP: fix truncate errors. Also correct errno reporting. Seems that warnx() everytime changes originak errno to EINVAL. --- src/modules/mavlink/mavlink_ftp.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/modules/mavlink/mavlink_ftp.cpp b/src/modules/mavlink/mavlink_ftp.cpp index 6865655da1..14a42ed724 100644 --- a/src/modules/mavlink/mavlink_ftp.cpp +++ b/src/modules/mavlink/mavlink_ftp.cpp @@ -230,6 +230,7 @@ out: warnx("FTP: ack\n"); #endif } else { + int r_errno = errno; warnx("FTP: nak %u", errorCode); payload->req_opcode = payload->opcode; payload->opcode = kRspNak; @@ -237,7 +238,7 @@ out: payload->data[0] = errorCode; if (errorCode == kErrFailErrno) { payload->size = 2; - payload->data[1] = errno; + payload->data[1] = r_errno; } } @@ -544,8 +545,7 @@ MavlinkFTP::_workTruncateFile(PayloadHeader* payload) } // check perms allow us to write (not romfs) - int mode = st.st_mode & (S_IRWXU | S_IRWXG | S_IRWXO); - if (!(mode & ~(S_IRUSR | S_IXUSR | S_IRGRP | S_IXGRP | S_IROTH | S_IXOTH))) { + if (!(st.st_mode & (S_IWUSR | S_IWGRP | S_IWOTH))) { errno = EROFS; return kErrFailErrno; } @@ -571,7 +571,7 @@ MavlinkFTP::_workTruncateFile(PayloadHeader* payload) return kErrFailErrno; } - if (lseek(fd, payload->offset - 1, SEEK_SET) != 0) { + if (lseek(fd, payload->offset - 1, SEEK_SET) < 0) { ::close(fd); return kErrFailErrno; } From 2148464a705edea20a0f0311b1904079f598eab8 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 20 Sep 2014 08:44:30 +0200 Subject: [PATCH 214/411] geofence: better usefeedback if loaded --- src/modules/navigator/geofence.cpp | 6 +++++- src/modules/navigator/geofence.h | 4 ++++ src/modules/navigator/navigator_main.cpp | 2 ++ 3 files changed, 11 insertions(+), 1 deletion(-) diff --git a/src/modules/navigator/geofence.cpp b/src/modules/navigator/geofence.cpp index 5504239c5e..0f431ded26 100644 --- a/src/modules/navigator/geofence.cpp +++ b/src/modules/navigator/geofence.cpp @@ -48,6 +48,7 @@ #include #include #include +#include /* Oddly, ERROR is not defined for C++ */ @@ -66,7 +67,8 @@ Geofence::Geofence() : _param_altitude_mode(this, "ALTMODE"), _param_source(this, "SOURCE"), _param_counter_threshold(this, "COUNT"), - _outside_counter(0) + _outside_counter(0), + _mavlinkFd(-1) { /* Load initial params */ updateParams(); @@ -330,8 +332,10 @@ Geofence::loadFromFile(const char *filename) { _verticesCount = pointCounter; warnx("Geofence: imported successfully"); + mavlink_log_info(_mavlinkFd, "Geofence imported"); } else { warnx("Geofence: import error"); + mavlink_log_critical(_mavlinkFd, "#audio: Geofence import error"); } return ERROR; diff --git a/src/modules/navigator/geofence.h b/src/modules/navigator/geofence.h index 65e5b4042b..9d647cb68a 100644 --- a/src/modules/navigator/geofence.h +++ b/src/modules/navigator/geofence.h @@ -98,6 +98,8 @@ public: int getSource() { return _param_source.get(); } + void setMavlinkFd(int value) { _mavlinkFd = value; } + private: orb_advert_t _fence_pub; /**< publish fence topic */ @@ -114,6 +116,8 @@ private: uint8_t _outside_counter; + int _mavlinkFd; + bool inside(double lat, double lon, float altitude); bool inside(const struct vehicle_global_position_s &global_position); bool inside(const struct vehicle_global_position_s &global_position, float baro_altitude_amsl); diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 9a8c54e7e6..6710247a7b 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -252,6 +252,7 @@ Navigator::task_main() warnx("Initializing.."); _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); + _geofence.setMavlinkFd(_mavlink_fd); /* Try to load the geofence: * if /fs/microsd/etc/geofence.txt load from this file @@ -263,6 +264,7 @@ Navigator::task_main() _geofence.loadFromFile(GEOFENCE_FILENAME); } else { + mavlink_log_critical(_mavlink_fd, "#audio: No geofence file"); if (_geofence.clearDm() > 0) warnx("Geofence cleared"); else From 54380e34b344108c72d2c46e9ca5e6b0b22390c8 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 20 Sep 2014 09:45:22 +0200 Subject: [PATCH 215/411] navigator: fix status information, remove fence_valid flag (this is handled by the geofence class) --- src/modules/navigator/navigator.h | 1 - src/modules/navigator/navigator_main.cpp | 5 ++--- 2 files changed, 2 insertions(+), 4 deletions(-) diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index 840b43f1b8..bf42acff98 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -191,7 +191,6 @@ private: Geofence _geofence; /**< class that handles the geofence */ bool _geofence_violation_warning_sent; /**< prevents spaming to mavlink */ - bool _fence_valid; /**< flag if fence is valid */ bool _inside_fence; /**< vehicle is inside fence */ NavigatorMode *_navigation_mode; /**< abstract pointer to current navigation mode class */ diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 6710247a7b..0e7f116ea4 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -127,7 +127,6 @@ Navigator::Navigator() : _loop_perf(perf_alloc(PC_ELAPSED, "navigator")), _geofence{}, _geofence_violation_warning_sent(false), - _fence_valid(false), _inside_fence(true), _navigation_mode(nullptr), _mission(this, "MIS"), @@ -532,7 +531,7 @@ Navigator::status() // warnx("Compass heading in degrees %5.5f", (double)(_global_pos.yaw * M_RAD_TO_DEG_F)); // } - if (_fence_valid) { + if (_geofence.valid()) { warnx("Geofence is valid"); /* TODO: needed? */ // warnx("Vertex longitude latitude"); @@ -540,7 +539,7 @@ Navigator::status() // warnx("%6u %9.5f %8.5f", i, (double)_fence.vertices[i].lon, (double)_fence.vertices[i].lat); } else { - warnx("Geofence not set"); + warnx("Geofence not set (no /etc/geofence.txt on microsd) or not valid"); } } From f681c6b5a3d1057434c1743496b3704d8244da2f Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sat, 20 Sep 2014 22:37:10 +1000 Subject: [PATCH 216/411] bottle_drop: don't talk about distance and error --- src/modules/bottle_drop/bottle_drop.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/bottle_drop/bottle_drop.cpp b/src/modules/bottle_drop/bottle_drop.cpp index 49919fd4a2..b16d9adda5 100644 --- a/src/modules/bottle_drop/bottle_drop.cpp +++ b/src/modules/bottle_drop/bottle_drop.cpp @@ -519,7 +519,7 @@ BottleDrop::task_main() approach_error = _wrap_pi(ground_direction - approach_direction); if (counter % 90 == 0) { - mavlink_log_critical(_mavlink_fd, "drop distance %u, heading error %u", (unsigned)distance_real, (unsigned)math::degrees(approach_error)); + mavlink_log_info(_mavlink_fd, "drop distance %u, heading error %u", (unsigned)distance_real, (unsigned)math::degrees(approach_error)); } } @@ -663,7 +663,7 @@ BottleDrop::task_main() _drop_state = DROP_STATE_DROPPED; mavlink_log_info(_mavlink_fd, "#audio: payload dropped"); } else { - + float distance_wp2 = get_distance_to_next_waypoint(_global_pos.lat, _global_pos.lon, flight_vector_e.lat, flight_vector_e.lon); if (distance_wp2 < distance_real) { From bdddcf338de5db7b1a7aef6ce124e4c180a27254 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sat, 20 Sep 2014 22:53:56 +1000 Subject: [PATCH 217/411] viper: changed some values for doors and bottle drop servos --- ROMFS/px4fmu_common/init.d/3035_viper | 3 ++- ROMFS/px4fmu_common/mixers/Viper.mix | 2 +- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/3035_viper b/ROMFS/px4fmu_common/init.d/3035_viper index 3714b612fc..ac4d919ce7 100644 --- a/ROMFS/px4fmu_common/init.d/3035_viper +++ b/ROMFS/px4fmu_common/init.d/3035_viper @@ -9,4 +9,5 @@ sh /etc/init.d/rc.fw_defaults set MIXER Viper -set FAILSAFE "-c567 -p 1000" +set FAILSAFE "-c56 -p 1000" +set FAILSAFE "-c7 -p 1200" diff --git a/ROMFS/px4fmu_common/mixers/Viper.mix b/ROMFS/px4fmu_common/mixers/Viper.mix index 5a0381bd87..5aa3828f29 100755 --- a/ROMFS/px4fmu_common/mixers/Viper.mix +++ b/ROMFS/px4fmu_common/mixers/Viper.mix @@ -66,6 +66,6 @@ S: 2 1 10000 10000 0 -10000 10000 M: 1 O: 10000 10000 0 -10000 10000 -S: 2 2 -10000 -10000 0 -10000 10000 +S: 2 2 -8000 -8000 0 -10000 10000 From 3e41945cc6b43e91d08082b1ce976d5382e0405a Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 21 Sep 2014 10:46:46 +0200 Subject: [PATCH 218/411] flare pitch limit: apply at throttle lim altitude --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 2 +- src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c | 2 ++ 2 files changed, 3 insertions(+), 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 c0c238ba9b..77eb78097b 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 @@ -1018,7 +1018,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi math::radians(_parameters.land_flare_pitch_min_deg), math::radians(_parameters.land_flare_pitch_max_deg), 0.0f, throttle_max, throttle_land, - false, math::radians(_parameters.land_flare_pitch_min_deg), + false, land_motor_lim ? math::radians(_parameters.land_flare_pitch_min_deg) : math::radians(_parameters.pitch_limit_min), _global_pos.alt, ground_speed, land_motor_lim ? TECS_MODE_LAND_THROTTLELIM : TECS_MODE_LAND); 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 f2e8c422f3..859cef8bdb 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 @@ -419,6 +419,7 @@ PARAM_DEFINE_FLOAT(FW_LND_RFRALT, -1.0f); * Flare, minimum pitch * * Minimum pitch during flare, a positive sign means nose up + * Applied once FW_LND_TLALT is reached * */ PARAM_DEFINE_FLOAT(FW_FLARE_PMIN, 2.5f); @@ -427,6 +428,7 @@ PARAM_DEFINE_FLOAT(FW_FLARE_PMIN, 2.5f); * Flare, maximum pitch * * Maximum pitch during flare, a positive sign means nose up + * Applied once FW_LND_TLALT is reached * */ PARAM_DEFINE_FLOAT(FW_FLARE_PMAX, 15.0f); From 980175bb60ba40d29091f92fa8d494aed79b9ef6 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 21 Sep 2014 13:32:47 +0200 Subject: [PATCH 219/411] commander: move position of gps failure check --- src/modules/commander/commander.cpp | 35 ++++++++++++++--------------- 1 file changed, 17 insertions(+), 18 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index c53b143491..03b73447f5 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1205,24 +1205,6 @@ int commander_thread_main(int argc, char *argv[]) check_valid(global_position.timestamp, POSITION_TIMEOUT, eph_good, &(status.condition_global_position_valid), &status_changed); - /* check if GPS fix is ok */ - if (gps_position.fix_type >= 3 && //XXX check eph and epv ? - hrt_elapsed_time(&gps_position.timestamp_position) < FAILSAFE_DEFAULT_TIMEOUT) { - /* handle the case where gps was regained */ - if (status.gps_failure) { - status.gps_failure = false; - status_changed = true; - mavlink_log_critical(mavlink_fd, "gps regained"); - } - } else { - if (!status.gps_failure) { - status.gps_failure = true; - status_changed = true; - mavlink_log_critical(mavlink_fd, "gps fix lost"); - } - } - - /* update home position */ if (!status.condition_home_position_valid && status.condition_global_position_valid && !armed.armed && (global_position.eph < eph_threshold) && (global_position.epv < epv_threshold)) { @@ -1434,6 +1416,23 @@ int commander_thread_main(int argc, char *argv[]) globallocalconverter_init((double)gps_position.lat * 1.0e-7, (double)gps_position.lon * 1.0e-7, (float)gps_position.alt * 1.0e-3f, hrt_absolute_time()); } + /* check if GPS fix is ok */ + if (gps_position.fix_type >= 3 && //XXX check eph and epv ? + hrt_elapsed_time(&gps_position.timestamp_position) < FAILSAFE_DEFAULT_TIMEOUT) { + /* handle the case where gps was regained */ + if (status.gps_failure) { + status.gps_failure = false; + status_changed = true; + mavlink_log_critical(mavlink_fd, "gps regained"); + } + } else { + if (!status.gps_failure) { + status.gps_failure = true; + status_changed = true; + mavlink_log_critical(mavlink_fd, "gps fix lost"); + } + } + /* start mission result check */ orb_check(mission_result_sub, &updated); From e65ec1b98b6f0ed4d526b2d9d1929d64fd7eceda Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 21 Sep 2014 13:50:43 +0200 Subject: [PATCH 220/411] bottle drop: Be less aggressive about scheduling --- src/modules/bottle_drop/bottle_drop.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/bottle_drop/bottle_drop.cpp b/src/modules/bottle_drop/bottle_drop.cpp index b16d9adda5..31c9157e11 100644 --- a/src/modules/bottle_drop/bottle_drop.cpp +++ b/src/modules/bottle_drop/bottle_drop.cpp @@ -222,7 +222,7 @@ BottleDrop::start() /* start the task */ _main_task = task_spawn_cmd("bottle_drop", SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 5, + SCHED_PRIORITY_DEFAULT + 15, 2048, (main_t)&BottleDrop::task_main_trampoline, nullptr); From 4b8a3856583d77e3f4c22d0f2999ccd3275e2520 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 21 Sep 2014 18:03:31 +0200 Subject: [PATCH 221/411] commander: improve output on gf violation --- src/modules/commander/commander.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index c53b143491..b43f08f673 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1449,8 +1449,12 @@ int commander_thread_main(int argc, char *argv[]) static bool flight_termination_printed = false; if (!flight_termination_printed) { warnx("Flight termination because of navigator request or geofence"); + mavlink_log_critical(mavlink_fd, "GF violation: flight termination"); flight_termination_printed = true; } + if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0 ) { + mavlink_log_critical(mavlink_fd, "GF violation: flight termination"); + } } // no reset is done here on purpose, on geofence violation we want to stay in flighttermination } From 885d3e8ca4a0f3c5bfad9936860d549b5b2f0674 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 21 Sep 2014 18:03:52 +0200 Subject: [PATCH 222/411] datman: reduce task priority --- src/modules/dataman/dataman.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/dataman/dataman.c b/src/modules/dataman/dataman.c index ca1fe9bbbd..b2355d4d89 100644 --- a/src/modules/dataman/dataman.c +++ b/src/modules/dataman/dataman.c @@ -797,7 +797,7 @@ start(void) sem_init(&g_init_sema, 1, 0); /* start the worker thread */ - if ((task = task_spawn_cmd("dataman", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 2000, task_main, NULL)) <= 0) { + if ((task = task_spawn_cmd("dataman", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, 2000, task_main, NULL)) <= 0) { warn("task start failed"); return -1; } From 808cc60cdad5c085d74e583d967cd86126ba1fca Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 21 Sep 2014 18:04:11 +0200 Subject: [PATCH 223/411] navigator: geofence with global pos: reduce update rate --- src/modules/navigator/navigator_main.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 0e7f116ea4..b633945445 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -387,9 +387,13 @@ Navigator::task_main() /* global position updated */ if (fds[0].revents & POLLIN) { global_position_update(); - if (_geofence.getSource() == Geofence::GF_SOURCE_GLOBALPOS) { + static int gposcounter = 0; + if (_geofence.getSource() == Geofence::GF_SOURCE_GLOBALPOS && + gposcounter % 10 == 0) { + /* Geofence is checked only every 10th gpos update */ have_geofence_position_data = true; } + gposcounter++; } /* Check geofence violation */ From d6b669f4971d8d7329f0bfd5ade64bed2bd3120f Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 21 Sep 2014 18:03:31 +0200 Subject: [PATCH 224/411] commander: improve output on gf violation --- src/modules/commander/commander.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 07fcb5d40e..0ada8e978b 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1442,8 +1442,12 @@ int commander_thread_main(int argc, char *argv[]) static bool flight_termination_printed = false; if (!flight_termination_printed) { warnx("Flight termination because of navigator request or geofence"); + mavlink_log_critical(mavlink_fd, "GF violation: flight termination"); flight_termination_printed = true; } + if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0 ) { + mavlink_log_critical(mavlink_fd, "GF violation: flight termination"); + } } // no reset is done here on purpose, on geofence violation we want to stay in flighttermination } From 31a17ce29a76a2f58e6feb92abe1de01eed5a8eb Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 21 Sep 2014 18:03:52 +0200 Subject: [PATCH 225/411] datman: reduce task priority --- src/modules/dataman/dataman.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/dataman/dataman.c b/src/modules/dataman/dataman.c index ca1fe9bbbd..b2355d4d89 100644 --- a/src/modules/dataman/dataman.c +++ b/src/modules/dataman/dataman.c @@ -797,7 +797,7 @@ start(void) sem_init(&g_init_sema, 1, 0); /* start the worker thread */ - if ((task = task_spawn_cmd("dataman", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 2000, task_main, NULL)) <= 0) { + if ((task = task_spawn_cmd("dataman", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, 2000, task_main, NULL)) <= 0) { warn("task start failed"); return -1; } From e3cac1999a0b4398e669f90cd28279ec2a0784a7 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 21 Sep 2014 18:04:11 +0200 Subject: [PATCH 226/411] navigator: geofence with global pos: reduce update rate --- src/modules/navigator/navigator_main.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 9a8c54e7e6..fc9120f137 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -386,9 +386,13 @@ Navigator::task_main() /* global position updated */ if (fds[0].revents & POLLIN) { global_position_update(); - if (_geofence.getSource() == Geofence::GF_SOURCE_GLOBALPOS) { + static int gposcounter = 0; + if (_geofence.getSource() == Geofence::GF_SOURCE_GLOBALPOS && + gposcounter % 10 == 0) { + /* Geofence is checked only every 10th gpos update */ have_geofence_position_data = true; } + gposcounter++; } /* Check geofence violation */ From 25b2d4b823d339c69365baecc7b9eada37e0a529 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Mon, 22 Sep 2014 10:57:17 +1000 Subject: [PATCH 227/411] change rates for mavlink streams --- src/modules/mavlink/mavlink_main.cpp | 45 +++++++++++++++++----------- 1 file changed, 28 insertions(+), 17 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index dd830071d9..69565e3ab3 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1328,7 +1328,7 @@ Mavlink::task_main(int argc, char *argv[]) * make space for two messages plus off-by-one space as we use the empty element * marker ring buffer approach. */ - if (OK != message_buffer_init(2 * sizeof(mavlink_message_t) + 1)) { + if (OK != message_buffer_init(10 * sizeof(mavlink_message_t) + 1)) { errx(1, "can't allocate message buffer, exiting"); } @@ -1374,39 +1374,50 @@ Mavlink::task_main(int argc, char *argv[]) _parameters_manager->set_interval(interval_from_rate(30.0f)); LL_APPEND(_streams, _parameters_manager); - /* MISSION_STREAM stream, actually sends all MISSION_XXX messages at some rate depending on - * remote requests rate. Rate specified here controls how much bandwidth we will reserve for - * mission messages. */ - _mission_manager = (MavlinkMissionManager *) MavlinkMissionManager::new_instance(this); - _mission_manager->set_interval(interval_from_rate(10.0f)); - _mission_manager->set_verbose(_verbose); - LL_APPEND(_streams, _mission_manager); + switch (_mode) { case MAVLINK_MODE_NORMAL: + /* MISSION_STREAM stream, actually sends all MISSION_XXX messages at some rate depending on + * remote requests rate. Rate specified here controls how much bandwidth we will reserve for + * mission messages. */ + _mission_manager = (MavlinkMissionManager *) MavlinkMissionManager::new_instance(this); + _mission_manager->set_interval(interval_from_rate(2.0f)); + _mission_manager->set_verbose(_verbose); + LL_APPEND(_streams, _mission_manager); + configure_stream("SYS_STATUS", 1.0f); configure_stream("GPS_GLOBAL_ORIGIN", 0.5f); configure_stream("HIGHRES_IMU", 1.0f); - configure_stream("ATTITUDE", 15.0f); - configure_stream("VFR_HUD", 8.0f); + configure_stream("ATTITUDE", 4.0f); + configure_stream("VFR_HUD", 4.0f); configure_stream("GPS_RAW_INT", 1.0f); - configure_stream("GLOBAL_POSITION_INT", 3.0f); - configure_stream("LOCAL_POSITION_NED", 3.0f); + configure_stream("GLOBAL_POSITION_INT", 1.0f); + configure_stream("LOCAL_POSITION_NED", 1.0f); configure_stream("RC_CHANNELS_RAW", 1.0f); - configure_stream("POSITION_TARGET_GLOBAL_INT", 3.0f); - configure_stream("ATTITUDE_TARGET", 15.0f); + configure_stream("POSITION_TARGET_GLOBAL_INT", 1.0f); + configure_stream("ATTITUDE_TARGET", 1.0f); configure_stream("DISTANCE_SENSOR", 0.5f); - configure_stream("OPTICAL_FLOW", 20.0f); + //configure_stream("OPTICAL_FLOW", 20.0f); break; case MAVLINK_MODE_ONBOARD: + /* MISSION_STREAM stream, actually sends all MISSION_XXX messages at some rate depending on + * remote requests rate. Rate specified here controls how much bandwidth we will reserve for + * mission messages. */ + _mission_manager = (MavlinkMissionManager *) MavlinkMissionManager::new_instance(this); + _mission_manager->set_interval(interval_from_rate(10.0f)); + _mission_manager->set_verbose(_verbose); + LL_APPEND(_streams, _mission_manager); + configure_stream("SYS_STATUS", 1.0f); // XXX OBC change back: We need to be bandwidth-efficient here too configure_stream("ATTITUDE", 50.0f); configure_stream("GLOBAL_POSITION_INT", 50.0f); configure_stream("CAMERA_CAPTURE", 2.0f); - configure_stream("ATTITUDE_TARGET", 50.0f); - configure_stream("POSITION_TARGET_GLOBAL_INT", 20.0f); + configure_stream("ATTITUDE_TARGET", 10.0f); + configure_stream("POSITION_TARGET_GLOBAL_INT", 10.0f); + configure_stream("VFR_HUD", 10.0f); break; default: From 98bc5ece10987c177be5035cfca6ecea841b8843 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Mon, 22 Sep 2014 11:03:36 +1000 Subject: [PATCH 228/411] switch mission manager back to what is was before --- src/modules/mavlink/mavlink_main.cpp | 24 ++++++++---------------- 1 file changed, 8 insertions(+), 16 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 69565e3ab3..114b4678b2 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1374,18 +1374,18 @@ Mavlink::task_main(int argc, char *argv[]) _parameters_manager->set_interval(interval_from_rate(30.0f)); LL_APPEND(_streams, _parameters_manager); + /* MISSION_STREAM stream, actually sends all MISSION_XXX messages at some rate depending on + * remote requests rate. Rate specified here controls how much bandwidth we will reserve for + * mission messages. */ + _mission_manager = (MavlinkMissionManager *) MavlinkMissionManager::new_instance(this); + _mission_manager->set_interval(interval_from_rate(10.0f)); + _mission_manager->set_verbose(_verbose); + LL_APPEND(_streams, _mission_manager); + switch (_mode) { case MAVLINK_MODE_NORMAL: - /* MISSION_STREAM stream, actually sends all MISSION_XXX messages at some rate depending on - * remote requests rate. Rate specified here controls how much bandwidth we will reserve for - * mission messages. */ - _mission_manager = (MavlinkMissionManager *) MavlinkMissionManager::new_instance(this); - _mission_manager->set_interval(interval_from_rate(2.0f)); - _mission_manager->set_verbose(_verbose); - LL_APPEND(_streams, _mission_manager); - configure_stream("SYS_STATUS", 1.0f); configure_stream("GPS_GLOBAL_ORIGIN", 0.5f); configure_stream("HIGHRES_IMU", 1.0f); @@ -1402,14 +1402,6 @@ Mavlink::task_main(int argc, char *argv[]) break; case MAVLINK_MODE_ONBOARD: - /* MISSION_STREAM stream, actually sends all MISSION_XXX messages at some rate depending on - * remote requests rate. Rate specified here controls how much bandwidth we will reserve for - * mission messages. */ - _mission_manager = (MavlinkMissionManager *) MavlinkMissionManager::new_instance(this); - _mission_manager->set_interval(interval_from_rate(10.0f)); - _mission_manager->set_verbose(_verbose); - LL_APPEND(_streams, _mission_manager); - configure_stream("SYS_STATUS", 1.0f); // XXX OBC change back: We need to be bandwidth-efficient here too configure_stream("ATTITUDE", 50.0f); From bc23b6239c50527aa550eed3bc6f17dec15c5c97 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Mon, 22 Sep 2014 11:03:49 +1000 Subject: [PATCH 229/411] increase ram --- src/modules/mavlink/mavlink_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 114b4678b2..b9f486c95e 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1630,7 +1630,7 @@ Mavlink::start(int argc, char *argv[]) task_spawn_cmd(buf, SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, - 2700, + 5000, (main_t)&Mavlink::start_helper, (const char **)argv); From 973c034d6ec502009a8fd92c14ef02c4d1769a64 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 5 Sep 2014 08:59:00 +0200 Subject: [PATCH 230/411] engine failure detection --- src/modules/commander/commander.cpp | 52 ++++++++++++++++++++++-- src/modules/commander/commander_params.c | 32 +++++++++++++++ src/modules/systemlib/circuit_breaker.c | 14 +++++++ src/modules/systemlib/circuit_breaker.h | 1 + 4 files changed, 95 insertions(+), 4 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 0ada8e978b..b9a0bd2cb7 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -686,6 +686,9 @@ int commander_thread_main(int argc, char *argv[]) param_t _param_datalink_loss_timeout = param_find("COM_DL_LOSS_T"); param_t _param_datalink_regain_timeout = param_find("COM_DL_REG_T"); param_t _param_onboard_sysid = param_find("COM_ONBSYSID"); + param_t _param_ef_throttle_thres = param_find("COM_EF_THROT"); + param_t _param_ef_current2throttle_thres = param_find("COM_EF_C2T"); + param_t _param_ef_time_thres = param_find("COM_EF_TIME"); /* welcome user */ warnx("starting"); @@ -974,8 +977,16 @@ int commander_thread_main(int argc, char *argv[]) int32_t datalink_loss_enabled = false; int32_t datalink_loss_timeout = 10; int32_t datalink_regain_timeout = 0; - uint8_t onboard_sysid = 42; /**< systemid of the onboard computer, telemetry from this sysid - is not validated for the datalink loss check */ + int32_t onboard_sysid = 42; /**< systemid of the onboard computer, + telemetry from this sysid is not + validated for the datalink loss check */ + + /* Thresholds for engine failure detection */ + int32_t ef_throttle_thres = 1.0f; + int32_t ef_current2throttle_thres = 0.0f; + int32_t ef_time_thres = 1000.0f; + uint64_t timestamp_engine_healthy = 0; /**< absolute time when engine + was healty*/ /* check which state machines for changes, clear "changed" flag */ bool arming_state_changed = false; @@ -1039,6 +1050,9 @@ int commander_thread_main(int argc, char *argv[]) param_get(_param_datalink_loss_timeout, &datalink_loss_timeout); param_get(_param_datalink_regain_timeout, &datalink_regain_timeout); param_get(_param_onboard_sysid, &onboard_sysid); + param_get(_param_ef_throttle_thres, &ef_throttle_thres); + param_get(_param_ef_current2throttle_thres, &ef_current2throttle_thres); + param_get(_param_ef_time_thres, &ef_time_thres); } orb_check(sp_man_sub, &updated); @@ -1576,10 +1590,11 @@ int commander_thread_main(int argc, char *argv[]) /* If this is not an onboard link/onboard computer: * set flag that we have a valid link */ - if (telemetry_sysid[i] != onboard_sysid) { + if (telemetry_sysid[i] != (uint8_t)onboard_sysid) { have_link = true; } - } else if (!telemetry_lost[i] && telemetry_sysid[i] != onboard_sysid) { + } else if (!telemetry_lost[i] && telemetry_sysid[i] != + (uint8_t)onboard_sysid) { /* telemetry was healthy also in last iteration * we don't have to check a timeout, * telemetry from onboard computers is not accepted as a valid datalink @@ -1612,6 +1627,35 @@ int commander_thread_main(int argc, char *argv[]) } } + /* Check engine failure + * only for fixed wing for now + */ + if (!circuit_breaker_enabled("CBRK_ENGINEFAIL", CBRK_ENGINEFAIL_KEY) && + status.is_rotary_wing == false && + armed.armed && + ((actuator_controls.control[3] > ef_throttle_thres && + battery.current_a/actuator_controls.control[3] < + ef_current2throttle_thres) || + (status.engine_failure))) { + /* potential failure, measure time */ + if (timestamp_engine_healthy > 0 && + hrt_elapsed_time(×tamp_engine_healthy) > + ef_time_thres * 1e6 && + !status.engine_failure) { + status.engine_failure = true; + status_changed = true; + mavlink_log_critical(mavlink_fd, "Engine Failure"); + } + } else { + /* no failure reset flag */ + timestamp_engine_healthy = hrt_absolute_time(); + if (status.engine_failure) { + status.engine_failure = false; + status_changed = true; + } + } + + /* handle commands last, as the system needs to be updated to handle them */ orb_check(cmd_sub, &updated); diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index 98c0982b21..15c299a8bd 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -138,3 +138,35 @@ PARAM_DEFINE_INT32(COM_DL_REG_T, 0); * @max 255 */ PARAM_DEFINE_INT32(COM_ONBSYSID, 42); + +/** Engine Failure Throttle Threshold + * + * Engine failure triggers only above this throttle value + * + * @group commander + * @min 0.0f + * @max 1.0f + */ +PARAM_DEFINE_FLOAT(COM_EF_THROT, 0.5f); + +/** Engine Failure Current/Throttle Threshold + * + * Engine failure triggers only below this current/throttle value + * + * @group commander + * @min 0.0f + * @max 7.0f + */ +PARAM_DEFINE_FLOAT(COM_EF_C2T, 5.0f); + +/** Engine Failure Time Threshold + * + * Engine failure triggers only if the throttle threshold and the + * current to throttle threshold are violated for this time + * + * @group commander + * @unit second + * @min 0.0f + * @max 7.0f + */ +PARAM_DEFINE_FLOAT(COM_EF_TIME, 5.0f); diff --git a/src/modules/systemlib/circuit_breaker.c b/src/modules/systemlib/circuit_breaker.c index b0f95aedf2..9e5429988b 100644 --- a/src/modules/systemlib/circuit_breaker.c +++ b/src/modules/systemlib/circuit_breaker.c @@ -108,6 +108,20 @@ PARAM_DEFINE_INT32(CBRK_AIRSPD_CHK, 0); */ PARAM_DEFINE_INT32(CBRK_FLIGHTTERM, 0); +/** + * Circuit breaker for engine failure detection + * + * Setting this parameter to 284953 will disable the engine failure detection. + * If the aircraft is in engine failure mode the enine failure flag will be + * set to healthy + * WARNING: ENABLING THIS CIRCUIT BREAKER IS AT OWN RISK + * + * @min 0 + * @max 284953 + * @group Circuit Breaker + */ +PARAM_DEFINE_INT32(CBRK_ENGINEFAIL, 0); + bool circuit_breaker_enabled(const char* breaker, int32_t magic) { int32_t val; diff --git a/src/modules/systemlib/circuit_breaker.h b/src/modules/systemlib/circuit_breaker.h index 445a89d3a7..6a55e4948d 100644 --- a/src/modules/systemlib/circuit_breaker.h +++ b/src/modules/systemlib/circuit_breaker.h @@ -54,6 +54,7 @@ #define CBRK_IO_SAFETY_KEY 22027 #define CBRK_AIRSPD_CHK_KEY 162128 #define CBRK_FLIGHTTERM_KEY 121212 +#define CBRK_ENGINEFAIL_KEY 284953 #include From d18f3ee70d5fbeb150c6b37ccafa4f622494ec19 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 5 Sep 2014 12:06:05 +0200 Subject: [PATCH 231/411] make rc loss timeout a param --- src/modules/commander/commander.cpp | 7 +++++-- src/modules/commander/commander_params.c | 11 +++++++++++ 2 files changed, 16 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index b9a0bd2cb7..b7a16e4e63 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -128,7 +128,6 @@ extern struct system_load_s system_load; #define POSITION_TIMEOUT (2 * 1000 * 1000) /**< consider the local or global position estimate invalid after 600ms */ #define FAILSAFE_DEFAULT_TIMEOUT (3 * 1000 * 1000) /**< hysteresis time - the failsafe will trigger after 3 seconds in this state */ -#define RC_TIMEOUT 500000 #define OFFBOARD_TIMEOUT 500000 #define DIFFPRESS_TIMEOUT 2000000 @@ -684,6 +683,7 @@ int commander_thread_main(int argc, char *argv[]) param_t _param_enable_parachute = param_find("NAV_PARACHUTE_EN"); param_t _param_enable_datalink_loss = param_find("COM_DL_LOSS_EN"); param_t _param_datalink_loss_timeout = param_find("COM_DL_LOSS_T"); + param_t _param_rc_loss_timeout = param_find("COM_RC_LOSS_T"); param_t _param_datalink_regain_timeout = param_find("COM_DL_REG_T"); param_t _param_onboard_sysid = param_find("COM_ONBSYSID"); param_t _param_ef_throttle_thres = param_find("COM_EF_THROT"); @@ -976,6 +976,7 @@ int commander_thread_main(int argc, char *argv[]) int32_t datalink_loss_enabled = false; int32_t datalink_loss_timeout = 10; + float rc_loss_timeout = 0.5; int32_t datalink_regain_timeout = 0; int32_t onboard_sysid = 42; /**< systemid of the onboard computer, telemetry from this sysid is not @@ -1048,6 +1049,7 @@ int commander_thread_main(int argc, char *argv[]) param_get(_param_enable_parachute, ¶chute_enabled); param_get(_param_enable_datalink_loss, &datalink_loss_enabled); param_get(_param_datalink_loss_timeout, &datalink_loss_timeout); + param_get(_param_rc_loss_timeout, &rc_loss_timeout); param_get(_param_datalink_regain_timeout, &datalink_regain_timeout); param_get(_param_onboard_sysid, &onboard_sysid); param_get(_param_ef_throttle_thres, &ef_throttle_thres); @@ -1466,7 +1468,8 @@ int commander_thread_main(int argc, char *argv[]) } /* RC input check */ - if (!status.rc_input_blocked && sp_man.timestamp != 0 && hrt_absolute_time() < sp_man.timestamp + RC_TIMEOUT) { + if (!status.rc_input_blocked && sp_man.timestamp != 0 && + hrt_absolute_time() < sp_man.timestamp + (uint64_t)(rc_loss_timeout * 1e6f)) { /* handle the case where RC signal was regained */ if (!status.rc_signal_found_once) { status.rc_signal_found_once = true; diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index 15c299a8bd..7d06003c91 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -170,3 +170,14 @@ PARAM_DEFINE_FLOAT(COM_EF_C2T, 5.0f); * @max 7.0f */ PARAM_DEFINE_FLOAT(COM_EF_TIME, 5.0f); + +/** RC loss time threshold + * + * After this amount of seconds without RC connection the rc lost flag is set to true + * + * @group commander + * @unit second + * @min 0 + * @max 35 + */ +PARAM_DEFINE_FLOAT(COM_RC_LOSS_T, 0.5); From 21009e89a4c748d8a61174058bb378c1d6306b8d Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 7 Sep 2014 15:29:53 +0200 Subject: [PATCH 232/411] flight termination mavlink outtput: limit rate --- src/modules/commander/commander.cpp | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index b7a16e4e63..134f23c0b7 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1688,9 +1688,12 @@ int commander_thread_main(int argc, char *argv[]) static bool flight_termination_printed = false; if (!flight_termination_printed) { warnx("Flight termination because of data link loss && gps failure"); + mavlink_log_critical(mavlink_fd, "DL and GPS lost: flight termination"); flight_termination_printed = true; } - mavlink_log_critical(mavlink_fd, "DL and GPS lost: flight termination"); + if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0 ) { + mavlink_log_critical(mavlink_fd, "DL and GPS lost: flight termination"); + } } /* At this point the rc signal and the gps system have been checked @@ -1709,7 +1712,9 @@ int commander_thread_main(int argc, char *argv[]) warnx("Flight termination because of RC signal loss && gps failure"); flight_termination_printed = true; } - mavlink_log_critical(mavlink_fd, "RC and GPS lost: flight termination"); + if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0 ) { + mavlink_log_critical(mavlink_fd, "RC and GPS lost: flight termination"); + } } } From a8239b2c4516c36d30767c0ae61b30f1e2dde096 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 7 Sep 2014 15:31:58 +0200 Subject: [PATCH 233/411] if V_RCL_LT < 0 go directly to termination --- src/modules/navigator/rcloss.cpp | 15 +++++++++++---- src/modules/navigator/rcloss_params.c | 3 ++- 2 files changed, 13 insertions(+), 5 deletions(-) diff --git a/src/modules/navigator/rcloss.cpp b/src/modules/navigator/rcloss.cpp index 651e311840..5564a1c42e 100644 --- a/src/modules/navigator/rcloss.cpp +++ b/src/modules/navigator/rcloss.cpp @@ -153,10 +153,17 @@ RCLoss::advance_rcl() { switch (_rcl_state) { case RCL_STATE_NONE: - /* Check the number of data link losses. If above home fly home directly */ - warnx("RC loss, OBC mode, loiter"); - mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: rc loss, loitering"); - _rcl_state = RCL_STATE_LOITER; + if (_param_loitertime.get() > 0.0f) { + warnx("RC loss, OBC mode, loiter"); + mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: rc loss, loitering"); + _rcl_state = RCL_STATE_LOITER; + } else { + warnx("RC loss, OBC mode, slip loiter, terminate"); + mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: rc loss, terminating"); + _rcl_state = RCL_STATE_TERMINATE; + _navigator->get_mission_result()->stay_in_failsafe = true; + _navigator->publish_mission_result(); + } break; case RCL_STATE_LOITER: _rcl_state = RCL_STATE_TERMINATE; diff --git a/src/modules/navigator/rcloss_params.c b/src/modules/navigator/rcloss_params.c index 83d23cf499..f1a01c73b5 100644 --- a/src/modules/navigator/rcloss_params.c +++ b/src/modules/navigator/rcloss_params.c @@ -51,9 +51,10 @@ * Loiter Time * * The amount of time in seconds the system should loiter at current position before termination + * Set to -1 to make the system skip loitering * * @unit seconds - * @min 0.0 + * @min -1.0 * @group RCL */ PARAM_DEFINE_FLOAT(NAV_RCL_LT, 120.0f); From 1fb8e76f0a4508cadd63b47e6f94a6638b699bcc Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 7 Sep 2014 15:32:28 +0200 Subject: [PATCH 234/411] fix typo in comment --- 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 ee08f3e981..f44985d505 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 @@ -1218,7 +1218,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } if (_vehicle_status.engine_failure || _vehicle_status.engine_failure_cmd) { - /* Set thrrust to 0 to minimize damage */ + /* Set thrust to 0 to minimize damage */ _att_sp.thrust = 0.0f; } else if (pos_sp_triplet.current.type == SETPOINT_TYPE_TAKEOFF && launch_detection_state != LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS) { From e17411769847f8681dc05da72ed4a06ff27a7a32 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 7 Sep 2014 15:33:11 +0200 Subject: [PATCH 235/411] gps failure has priority over engine falure, in case both fail make sure that the gps failure mode does not turn on the engine --- src/modules/commander/state_machine_helper.cpp | 4 ++-- src/modules/fw_att_control/fw_att_control_main.cpp | 8 ++++++-- 2 files changed, 8 insertions(+), 4 deletions(-) diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index e3b5d30e49..9568752aed 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -511,10 +511,10 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en } else if (status->rc_signal_lost_cmd) { status->nav_state = NAVIGATION_STATE_AUTO_RTGS; //XXX /* Finished handling commands which have priority , now handle failures */ - } else if (status->engine_failure) { - status->nav_state = NAVIGATION_STATE_AUTO_LANDENGFAIL; } else if (status->gps_failure) { status->nav_state = NAVIGATION_STATE_AUTO_LANDGPSFAIL; + } else if (status->engine_failure) { + status->nav_state = NAVIGATION_STATE_AUTO_LANDENGFAIL; } else if (((status->data_link_lost && data_link_loss_enabled) && status->rc_signal_lost) || (!data_link_loss_enabled && status->rc_signal_lost && mission_finished)) { status->failsafe = true; 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 517333c80a..7cf721a356 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -860,8 +860,12 @@ FixedwingAttitudeControl::task_main() } } - /* throttle passed through */ - _actuators.control[3] = (isfinite(throttle_sp)) ? throttle_sp : 0.0f; + /* throttle passed through if it is finite and if no engine failure was + * detected */ + _actuators.control[3] = (isfinite(throttle_sp) && + !(_vehicle_status.engine_failure || + _vehicle_status.engine_failure_cmd)) ? + throttle_sp : 0.0f; if (!isfinite(throttle_sp)) { if (_debug && loop_counter % 10 == 0) { warnx("throttle_sp %.4f", (double)throttle_sp); From b5ffcfe3d12a0e2e98cb5319e0286c8215150672 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 13 Sep 2014 16:23:28 +0200 Subject: [PATCH 236/411] Revert "datalink check: ignore onboard computer" This reverts commit 3f8793210b47bd8e09ed2adaabc2fab966db5df6. Conflicts: src/modules/commander/commander.cpp src/modules/commander/commander_params.c --- src/modules/commander/commander.cpp | 23 +++-------------------- src/modules/commander/commander_params.c | 10 ---------- 2 files changed, 3 insertions(+), 30 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 134f23c0b7..b43f08f673 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -685,7 +685,6 @@ int commander_thread_main(int argc, char *argv[]) param_t _param_datalink_loss_timeout = param_find("COM_DL_LOSS_T"); param_t _param_rc_loss_timeout = param_find("COM_RC_LOSS_T"); param_t _param_datalink_regain_timeout = param_find("COM_DL_REG_T"); - param_t _param_onboard_sysid = param_find("COM_ONBSYSID"); param_t _param_ef_throttle_thres = param_find("COM_EF_THROT"); param_t _param_ef_current2throttle_thres = param_find("COM_EF_C2T"); param_t _param_ef_time_thres = param_find("COM_EF_TIME"); @@ -883,14 +882,12 @@ int commander_thread_main(int argc, char *argv[]) uint64_t telemetry_last_heartbeat[TELEMETRY_STATUS_ORB_ID_NUM]; uint64_t telemetry_last_dl_loss[TELEMETRY_STATUS_ORB_ID_NUM]; bool telemetry_lost[TELEMETRY_STATUS_ORB_ID_NUM]; - uint8_t telemetry_sysid[TELEMETRY_STATUS_ORB_ID_NUM]; for (int i = 0; i < TELEMETRY_STATUS_ORB_ID_NUM; i++) { telemetry_subs[i] = orb_subscribe(telemetry_status_orb_id[i]); telemetry_last_heartbeat[i] = 0; telemetry_last_dl_loss[i] = 0; telemetry_lost[i] = true; - telemetry_sysid[i] = 0; } /* Subscribe to global position */ @@ -978,9 +975,6 @@ int commander_thread_main(int argc, char *argv[]) int32_t datalink_loss_timeout = 10; float rc_loss_timeout = 0.5; int32_t datalink_regain_timeout = 0; - int32_t onboard_sysid = 42; /**< systemid of the onboard computer, - telemetry from this sysid is not - validated for the datalink loss check */ /* Thresholds for engine failure detection */ int32_t ef_throttle_thres = 1.0f; @@ -988,7 +982,6 @@ int commander_thread_main(int argc, char *argv[]) int32_t ef_time_thres = 1000.0f; uint64_t timestamp_engine_healthy = 0; /**< absolute time when engine was healty*/ - /* check which state machines for changes, clear "changed" flag */ bool arming_state_changed = false; bool main_state_changed = false; @@ -1051,7 +1044,6 @@ int commander_thread_main(int argc, char *argv[]) param_get(_param_datalink_loss_timeout, &datalink_loss_timeout); param_get(_param_rc_loss_timeout, &rc_loss_timeout); param_get(_param_datalink_regain_timeout, &datalink_regain_timeout); - param_get(_param_onboard_sysid, &onboard_sysid); param_get(_param_ef_throttle_thres, &ef_throttle_thres); param_get(_param_ef_current2throttle_thres, &ef_current2throttle_thres); param_get(_param_ef_time_thres, &ef_time_thres); @@ -1101,7 +1093,6 @@ int commander_thread_main(int argc, char *argv[]) } telemetry_last_heartbeat[i] = telemetry.heartbeat_time; - telemetry_sysid[i] = telemetry.system_id; } } @@ -1590,18 +1581,10 @@ int commander_thread_main(int argc, char *argv[]) mavlink_log_critical(mavlink_fd, "data link %i regained", i); telemetry_lost[i] = false; - - /* If this is not an onboard link/onboard computer: - * set flag that we have a valid link */ - if (telemetry_sysid[i] != (uint8_t)onboard_sysid) { - have_link = true; - } - } else if (!telemetry_lost[i] && telemetry_sysid[i] != - (uint8_t)onboard_sysid) { + have_link = true; + } else if (!telemetry_lost[i]) { /* telemetry was healthy also in last iteration - * we don't have to check a timeout, - * telemetry from onboard computers is not accepted as a valid datalink - */ + * we don't have to check a timeout */ have_link = true; } diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index 7d06003c91..f4bc7fd7fb 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -129,16 +129,6 @@ PARAM_DEFINE_INT32(COM_DL_LOSS_T, 10); */ PARAM_DEFINE_INT32(COM_DL_REG_T, 0); -/** Onboard computer system id - * - * The system id of the onboard computer. Heartbeats from this system are ignored during the datalink check - * - * @group commander - * @min 0 - * @max 255 - */ -PARAM_DEFINE_INT32(COM_ONBSYSID, 42); - /** Engine Failure Throttle Threshold * * Engine failure triggers only above this throttle value From c7966d56f5457be2eab4d6ee9cfadb7c3f22674b Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 20 Sep 2014 08:44:30 +0200 Subject: [PATCH 237/411] geofence: better usefeedback if loaded --- src/modules/navigator/geofence.cpp | 6 +++++- src/modules/navigator/geofence.h | 4 ++++ src/modules/navigator/navigator_main.cpp | 2 ++ 3 files changed, 11 insertions(+), 1 deletion(-) diff --git a/src/modules/navigator/geofence.cpp b/src/modules/navigator/geofence.cpp index 5504239c5e..0f431ded26 100644 --- a/src/modules/navigator/geofence.cpp +++ b/src/modules/navigator/geofence.cpp @@ -48,6 +48,7 @@ #include #include #include +#include /* Oddly, ERROR is not defined for C++ */ @@ -66,7 +67,8 @@ Geofence::Geofence() : _param_altitude_mode(this, "ALTMODE"), _param_source(this, "SOURCE"), _param_counter_threshold(this, "COUNT"), - _outside_counter(0) + _outside_counter(0), + _mavlinkFd(-1) { /* Load initial params */ updateParams(); @@ -330,8 +332,10 @@ Geofence::loadFromFile(const char *filename) { _verticesCount = pointCounter; warnx("Geofence: imported successfully"); + mavlink_log_info(_mavlinkFd, "Geofence imported"); } else { warnx("Geofence: import error"); + mavlink_log_critical(_mavlinkFd, "#audio: Geofence import error"); } return ERROR; diff --git a/src/modules/navigator/geofence.h b/src/modules/navigator/geofence.h index 65e5b4042b..9d647cb68a 100644 --- a/src/modules/navigator/geofence.h +++ b/src/modules/navigator/geofence.h @@ -98,6 +98,8 @@ public: int getSource() { return _param_source.get(); } + void setMavlinkFd(int value) { _mavlinkFd = value; } + private: orb_advert_t _fence_pub; /**< publish fence topic */ @@ -114,6 +116,8 @@ private: uint8_t _outside_counter; + int _mavlinkFd; + bool inside(double lat, double lon, float altitude); bool inside(const struct vehicle_global_position_s &global_position); bool inside(const struct vehicle_global_position_s &global_position, float baro_altitude_amsl); diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index fc9120f137..4f92954fd9 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -252,6 +252,7 @@ Navigator::task_main() warnx("Initializing.."); _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); + _geofence.setMavlinkFd(_mavlink_fd); /* Try to load the geofence: * if /fs/microsd/etc/geofence.txt load from this file @@ -263,6 +264,7 @@ Navigator::task_main() _geofence.loadFromFile(GEOFENCE_FILENAME); } else { + mavlink_log_critical(_mavlink_fd, "#audio: No geofence file"); if (_geofence.clearDm() > 0) warnx("Geofence cleared"); else From 5a0e0d041229700d2ae9c14e4ab2798d34fff14c Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 20 Sep 2014 09:45:22 +0200 Subject: [PATCH 238/411] navigator: fix status information, remove fence_valid flag (this is handled by the geofence class) --- src/modules/navigator/navigator.h | 1 - src/modules/navigator/navigator_main.cpp | 5 ++--- 2 files changed, 2 insertions(+), 4 deletions(-) diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index 840b43f1b8..bf42acff98 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -191,7 +191,6 @@ private: Geofence _geofence; /**< class that handles the geofence */ bool _geofence_violation_warning_sent; /**< prevents spaming to mavlink */ - bool _fence_valid; /**< flag if fence is valid */ bool _inside_fence; /**< vehicle is inside fence */ NavigatorMode *_navigation_mode; /**< abstract pointer to current navigation mode class */ diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 4f92954fd9..b633945445 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -127,7 +127,6 @@ Navigator::Navigator() : _loop_perf(perf_alloc(PC_ELAPSED, "navigator")), _geofence{}, _geofence_violation_warning_sent(false), - _fence_valid(false), _inside_fence(true), _navigation_mode(nullptr), _mission(this, "MIS"), @@ -536,7 +535,7 @@ Navigator::status() // warnx("Compass heading in degrees %5.5f", (double)(_global_pos.yaw * M_RAD_TO_DEG_F)); // } - if (_fence_valid) { + if (_geofence.valid()) { warnx("Geofence is valid"); /* TODO: needed? */ // warnx("Vertex longitude latitude"); @@ -544,7 +543,7 @@ Navigator::status() // warnx("%6u %9.5f %8.5f", i, (double)_fence.vertices[i].lon, (double)_fence.vertices[i].lat); } else { - warnx("Geofence not set"); + warnx("Geofence not set (no /etc/geofence.txt on microsd) or not valid"); } } From d113fcfc54c246f3d5ac22ad5485f7103aecab41 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 21 Sep 2014 13:32:47 +0200 Subject: [PATCH 239/411] commander: move position of gps failure check --- src/modules/commander/commander.cpp | 35 ++++++++++++++--------------- 1 file changed, 17 insertions(+), 18 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index b43f08f673..bf15bbeb60 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1205,24 +1205,6 @@ int commander_thread_main(int argc, char *argv[]) check_valid(global_position.timestamp, POSITION_TIMEOUT, eph_good, &(status.condition_global_position_valid), &status_changed); - /* check if GPS fix is ok */ - if (gps_position.fix_type >= 3 && //XXX check eph and epv ? - hrt_elapsed_time(&gps_position.timestamp_position) < FAILSAFE_DEFAULT_TIMEOUT) { - /* handle the case where gps was regained */ - if (status.gps_failure) { - status.gps_failure = false; - status_changed = true; - mavlink_log_critical(mavlink_fd, "gps regained"); - } - } else { - if (!status.gps_failure) { - status.gps_failure = true; - status_changed = true; - mavlink_log_critical(mavlink_fd, "gps fix lost"); - } - } - - /* update home position */ if (!status.condition_home_position_valid && status.condition_global_position_valid && !armed.armed && (global_position.eph < eph_threshold) && (global_position.epv < epv_threshold)) { @@ -1434,6 +1416,23 @@ int commander_thread_main(int argc, char *argv[]) globallocalconverter_init((double)gps_position.lat * 1.0e-7, (double)gps_position.lon * 1.0e-7, (float)gps_position.alt * 1.0e-3f, hrt_absolute_time()); } + /* check if GPS fix is ok */ + if (gps_position.fix_type >= 3 && //XXX check eph and epv ? + hrt_elapsed_time(&gps_position.timestamp_position) < FAILSAFE_DEFAULT_TIMEOUT) { + /* handle the case where gps was regained */ + if (status.gps_failure) { + status.gps_failure = false; + status_changed = true; + mavlink_log_critical(mavlink_fd, "gps regained"); + } + } else { + if (!status.gps_failure) { + status.gps_failure = true; + status_changed = true; + mavlink_log_critical(mavlink_fd, "gps fix lost"); + } + } + /* start mission result check */ orb_check(mission_result_sub, &updated); From 8eb310061693c86a9c8b3c7f6e2bab1ec680c46a Mon Sep 17 00:00:00 2001 From: Vladimir Ermakov Date: Mon, 22 Sep 2014 13:48:43 +0400 Subject: [PATCH 240/411] FTP: Rename command. Payload: `\0` See: man 2 rename --- src/modules/mavlink/mavlink_ftp.cpp | 31 +++++++++++++++++++++++++++++ src/modules/mavlink/mavlink_ftp.h | 2 ++ 2 files changed, 33 insertions(+) diff --git a/src/modules/mavlink/mavlink_ftp.cpp b/src/modules/mavlink/mavlink_ftp.cpp index 14a42ed724..117cc4ec16 100644 --- a/src/modules/mavlink/mavlink_ftp.cpp +++ b/src/modules/mavlink/mavlink_ftp.cpp @@ -204,6 +204,10 @@ MavlinkFTP::_process_request(Request *req) errorCode = _workRemoveFile(payload); break; + case kCmdRename: + errorCode = _workRename(payload); + break; + case kCmdTruncateFile: errorCode = _workTruncateFile(payload); break; @@ -629,6 +633,33 @@ MavlinkFTP::_workReset(PayloadHeader* payload) return kErrNone; } +/// @brief Responds to a Rename command +MavlinkFTP::ErrorCode +MavlinkFTP::_workRename(PayloadHeader* payload) +{ + char oldpath[kMaxDataLength]; + char newpath[kMaxDataLength]; + + char *ptr = _data_as_cstring(payload); + size_t oldpath_sz = strlen(ptr); + + if (oldpath_sz == payload->size) { + // no newpath + errno = EINVAL; + return kErrFailErrno; + } + + strncpy(oldpath, ptr, kMaxDataLength); + strncpy(newpath, ptr + oldpath_sz + 1, kMaxDataLength); + + if (rename(oldpath, newpath) == 0) { + payload->size = 0; + return kErrNone; + } else { + return kErrFailErrno; + } +} + /// @brief Responds to a RemoveDirectory command MavlinkFTP::ErrorCode MavlinkFTP::_workRemoveDirectory(PayloadHeader* payload) diff --git a/src/modules/mavlink/mavlink_ftp.h b/src/modules/mavlink/mavlink_ftp.h index bde8d7ec57..0fbd010be2 100644 --- a/src/modules/mavlink/mavlink_ftp.h +++ b/src/modules/mavlink/mavlink_ftp.h @@ -98,6 +98,7 @@ public: kCmdRemoveDirectory, ///< Removes Directory at , must be empty kCmdOpenFileWO, ///< Opens file at for writing, returns kCmdTruncateFile, ///< Truncate file at to length + kCmdRename, ///< Rename to kRspAck = 128, ///< Ack response kRspNak ///< Nak response @@ -152,6 +153,7 @@ private: ErrorCode _workCreateDirectory(PayloadHeader *payload); ErrorCode _workRemoveFile(PayloadHeader *payload); ErrorCode _workTruncateFile(PayloadHeader *payload); + ErrorCode _workRename(PayloadHeader *payload); static const unsigned kRequestQueueSize = 2; ///< Max number of queued requests Request _request_bufs[kRequestQueueSize]; ///< Request buffers which hold work From ef5a93c09ce4bd08729cf3fbf87a6c1d453c646f Mon Sep 17 00:00:00 2001 From: Vladimir Ermakov Date: Mon, 22 Sep 2014 15:19:38 +0400 Subject: [PATCH 241/411] FTP: Add file checksum calculation command. --- src/modules/mavlink/mavlink_ftp.cpp | 39 +++++++++++++++++++++++++++++ src/modules/mavlink/mavlink_ftp.h | 2 ++ 2 files changed, 41 insertions(+) diff --git a/src/modules/mavlink/mavlink_ftp.cpp b/src/modules/mavlink/mavlink_ftp.cpp index 117cc4ec16..c26fe6b4bc 100644 --- a/src/modules/mavlink/mavlink_ftp.cpp +++ b/src/modules/mavlink/mavlink_ftp.cpp @@ -34,6 +34,7 @@ /// @file mavlink_ftp.cpp /// @author px4dev, Don Gagne +#include #include #include #include @@ -220,6 +221,11 @@ MavlinkFTP::_process_request(Request *req) errorCode = _workRemoveDirectory(payload); break; + + case kCmdCalcFileCRC32: + errorCode = _workCalcFileCRC32(payload); + break; + default: errorCode = kErrUnknownCommand; break; @@ -690,6 +696,39 @@ MavlinkFTP::_workCreateDirectory(PayloadHeader* payload) } } +/// @brief Responds to a CalcFileCRC32 command +MavlinkFTP::ErrorCode +MavlinkFTP::_workCalcFileCRC32(PayloadHeader* payload) +{ + char file_buf[256]; + uint32_t checksum = 0; + ssize_t bytes_read; + strncpy(file_buf, _data_as_cstring(payload), kMaxDataLength); + + int fd = ::open(file_buf, O_RDONLY); + if (fd < 0) { + return kErrFailErrno; + } + + do { + bytes_read = ::read(fd, file_buf, sizeof(file_buf)); + if (bytes_read < 0) { + int r_errno = errno; + ::close(fd); + errno = r_errno; + return kErrFailErrno; + } + + checksum = crc32part((uint8_t*)file_buf, bytes_read, checksum); + } while (bytes_read == sizeof(file_buf)); + + ::close(fd); + + payload->size = sizeof(uint32_t); + *((uint32_t*)payload->data) = checksum; + return kErrNone; +} + /// @brief Returns true if the specified session is a valid open session bool MavlinkFTP::_valid_session(unsigned index) diff --git a/src/modules/mavlink/mavlink_ftp.h b/src/modules/mavlink/mavlink_ftp.h index 0fbd010be2..bef6775a94 100644 --- a/src/modules/mavlink/mavlink_ftp.h +++ b/src/modules/mavlink/mavlink_ftp.h @@ -99,6 +99,7 @@ public: kCmdOpenFileWO, ///< Opens file at for writing, returns kCmdTruncateFile, ///< Truncate file at to length kCmdRename, ///< Rename to + kCmdCalcFileCRC32, ///< Calculate CRC32 for file at kRspAck = 128, ///< Ack response kRspNak ///< Nak response @@ -154,6 +155,7 @@ private: ErrorCode _workRemoveFile(PayloadHeader *payload); ErrorCode _workTruncateFile(PayloadHeader *payload); ErrorCode _workRename(PayloadHeader *payload); + ErrorCode _workCalcFileCRC32(PayloadHeader *payload); static const unsigned kRequestQueueSize = 2; ///< Max number of queued requests Request _request_bufs[kRequestQueueSize]; ///< Request buffers which hold work From 73aa7d81e31e18a6d1405697856e389a7a7b89af Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 22 Sep 2014 17:20:50 +0200 Subject: [PATCH 242/411] mf checker: fix landing check, ensure feedback from all checks is sent --- .../navigator/mission_feasibility_checker.cpp | 20 ++++++++++++++----- 1 file changed, 15 insertions(+), 5 deletions(-) diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp index 937e4fa5ad..389cdd0d2d 100644 --- a/src/modules/navigator/mission_feasibility_checker.cpp +++ b/src/modules/navigator/mission_feasibility_checker.cpp @@ -84,7 +84,12 @@ bool MissionFeasibilityChecker::checkMissionFeasible(bool isRotarywing, dm_item_ bool MissionFeasibilityChecker::checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt) { - return (checkGeofence(dm_current, nMissionItems, geofence) && checkHomePositionAltitude(dm_current, nMissionItems, home_alt)); + /* Perform checks and issue feedback to the user for all checks */ + bool resGeofence = checkGeofence(dm_current, nMissionItems, geofence); + bool resHomeAltitude = checkHomePositionAltitude(dm_current, nMissionItems, home_alt); + + /* Mission is only marked as feasible if all checks return true */ + return (resGeofence && resHomeAltitude); } bool MissionFeasibilityChecker::checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt) @@ -93,7 +98,13 @@ bool MissionFeasibilityChecker::checkMissionFeasibleFixedwing(dm_item_t dm_curre 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, geofence) && checkHomePositionAltitude(dm_current, nMissionItems, home_alt)); + /* Perform checks and issue feedback to the user for all checks */ + bool resLanding = checkFixedWingLanding(dm_current, nMissionItems); + bool resGeofence = checkGeofence(dm_current, nMissionItems, geofence); + bool resHomeAltitude = checkHomePositionAltitude(dm_current, nMissionItems, home_alt); + + /* Mission is only marked as feasible if all checks return true */ + return (resLanding && resGeofence && resHomeAltitude); } bool MissionFeasibilityChecker::checkGeofence(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence) @@ -216,9 +227,8 @@ bool MissionFeasibilityChecker::checkFixedWingLanding(dm_item_t dm_current, size } } - -// float slope_alt = wp_altitude + _H0 * expf(-math::max(0.0f, _flare_length - wp_distance)/_flare_constant) - _H1_virt; - return false; + /* No landing waypoints or no waypoints */ + return true; } void MissionFeasibilityChecker::updateNavigationCapabilities() From 69eb222d4e2cf3d0894992a50a7a5822489f757f Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 22 Sep 2014 19:04:24 +0200 Subject: [PATCH 243/411] ubx: add sbas config, default to disable --- src/drivers/gps/ubx.cpp | 12 ++++++++++++ src/drivers/gps/ubx.h | 17 +++++++++++++++++ 2 files changed, 29 insertions(+) diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp index d0854f5e9a..c54c474b3d 100644 --- a/src/drivers/gps/ubx.cpp +++ b/src/drivers/gps/ubx.cpp @@ -189,6 +189,16 @@ UBX::configure(unsigned &baudrate) return 1; } + /* send a SBAS message to set the SBAS options */ + memset(&_buf.payload_tx_cfg_sbas, 0, sizeof(_buf.payload_tx_cfg_sbas)); + _buf.payload_tx_cfg_sbas.mode = UBX_TX_CFG_SBAS_MODE; + + send_message(UBX_MSG_CFG_SBAS, _buf.raw, sizeof(_buf.payload_tx_cfg_sbas)); + + if (wait_for_ack(UBX_MSG_CFG_SBAS, UBX_CONFIG_TIMEOUT, true) < 0) { + return 1; + } + /* configure message rates */ /* the last argument is divisor for measurement rate (set by CFG RATE), i.e. 1 means 5Hz */ @@ -265,6 +275,8 @@ UBX::wait_for_ack(const uint16_t msg, const unsigned timeout, const bool report) } } + warnx("msg %u ret %d", msg, ret); + _ack_state = UBX_ACK_IDLE; return ret; } diff --git a/src/drivers/gps/ubx.h b/src/drivers/gps/ubx.h index 219a5762aa..c74eb9168f 100644 --- a/src/drivers/gps/ubx.h +++ b/src/drivers/gps/ubx.h @@ -73,6 +73,7 @@ #define UBX_ID_CFG_MSG 0x01 #define UBX_ID_CFG_RATE 0x08 #define UBX_ID_CFG_NAV5 0x24 +#define UBX_ID_CFG_SBAS 0x16 #define UBX_ID_MON_VER 0x04 #define UBX_ID_MON_HW 0x09 @@ -89,6 +90,7 @@ #define UBX_MSG_CFG_MSG ((UBX_CLASS_CFG) | UBX_ID_CFG_MSG << 8) #define UBX_MSG_CFG_RATE ((UBX_CLASS_CFG) | UBX_ID_CFG_RATE << 8) #define UBX_MSG_CFG_NAV5 ((UBX_CLASS_CFG) | UBX_ID_CFG_NAV5 << 8) +#define UBX_MSG_CFG_SBAS ((UBX_CLASS_CFG) | UBX_ID_CFG_SBAS << 8) #define UBX_MSG_MON_HW ((UBX_CLASS_MON) | UBX_ID_MON_HW << 8) #define UBX_MSG_MON_VER ((UBX_CLASS_MON) | UBX_ID_MON_VER << 8) @@ -128,6 +130,11 @@ #define UBX_TX_CFG_NAV5_DYNMODEL 7 /**< 0 Portable, 2 Stationary, 3 Pedestrian, 4 Automotive, 5 Sea, 6 Airborne <1g, 7 Airborne <2g, 8 Airborne <4g */ #define UBX_TX_CFG_NAV5_FIXMODE 2 /**< 1 2D only, 2 3D only, 3 Auto 2D/3D */ +/* TX CFG-SBAS message contents */ +#define UBX_TX_CFG_SBAS_MODE_ENABLED 1 /**< SBAS enabled */ +#define UBX_TX_CFG_SBAS_MODE_DISABLED 0 /**< SBAS disabled */ +#define UBX_TX_CFG_SBAS_MODE UBX_TX_CFG_SBAS_MODE_DISABLED /**< SBAS enabled or disabled */ + /* TX CFG-MSG message contents */ #define UBX_TX_CFG_MSG_RATE1_5HZ 0x01 /**< {0x00, 0x01, 0x00, 0x00, 0x00, 0x00} the second entry is for UART1 */ #define UBX_TX_CFG_MSG_RATE1_1HZ 0x05 /**< {0x00, 0x05, 0x00, 0x00, 0x00, 0x00} the second entry is for UART1 */ @@ -383,6 +390,15 @@ typedef struct { uint32_t reserved4; } ubx_payload_tx_cfg_nav5_t; +/* tx cfg-sbas */ +typedef struct { + uint8_t mode; + uint8_t usage; + uint8_t maxSBAS; + uint8_t scanmode2; + uint32_t scanmode1; +} ubx_payload_tx_cfg_sbas_t; + /* Tx CFG-MSG */ typedef struct { union { @@ -413,6 +429,7 @@ typedef union { ubx_payload_tx_cfg_prt_t payload_tx_cfg_prt; ubx_payload_tx_cfg_rate_t payload_tx_cfg_rate; ubx_payload_tx_cfg_nav5_t payload_tx_cfg_nav5; + ubx_payload_tx_cfg_sbas_t payload_tx_cfg_sbas; ubx_payload_tx_cfg_msg_t payload_tx_cfg_msg; uint8_t raw[]; } ubx_buf_t; From d9bb3a51124cb3006f7548ac6ee629b48a939336 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 23 Sep 2014 06:08:24 +1000 Subject: [PATCH 244/411] viper: removed wrong failsafe values --- ROMFS/px4fmu_common/init.d/3035_viper | 2 -- 1 file changed, 2 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/3035_viper b/ROMFS/px4fmu_common/init.d/3035_viper index ac4d919ce7..f3b0e84183 100644 --- a/ROMFS/px4fmu_common/init.d/3035_viper +++ b/ROMFS/px4fmu_common/init.d/3035_viper @@ -9,5 +9,3 @@ sh /etc/init.d/rc.fw_defaults set MIXER Viper -set FAILSAFE "-c56 -p 1000" -set FAILSAFE "-c7 -p 1200" From 32131a069eeb77637a67e10011b4b756bf4d718a Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 22 Sep 2014 09:44:38 +0200 Subject: [PATCH 245/411] inform in GCS when switching to laser --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 5 ++++- 1 file changed, 4 insertions(+), 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 eb4ffdabb3..ddfbc233b0 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 @@ -814,7 +814,10 @@ float FixedwingPositionControl::get_terrain_altitude_landing(float land_setpoint /* Decide if the terrain estimation can be used, once we switched to using the terrain we stick with it * for the whole landing */ if (global_pos.terrain_alt_valid || land_useterrain) { - land_useterrain = true; + if(!land_useterrain) { + mavlink_log_info(_mavlink_fd, "#audio: Landing, using terrain estimate"); + land_useterrain = true; + } return global_pos.terrain_alt; } else { return land_setpoint_alt; From 852159d9edaa85c3abee3059359264f3329841e8 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 22 Sep 2014 10:03:23 +0200 Subject: [PATCH 246/411] fw pos control: add param to enable/disable usage of terrain estimate during landing --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 6 +++++- src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c | 8 +++----- 2 files changed, 8 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 ddfbc233b0..a94311ccb3 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 @@ -240,6 +240,7 @@ private: float land_heading_hold_horizontal_distance; float land_flare_pitch_min_deg; float land_flare_pitch_max_deg; + int land_use_terrain_estimate; } _parameters; /**< local copies of interesting parameters */ @@ -287,6 +288,7 @@ private: param_t land_heading_hold_horizontal_distance; param_t land_flare_pitch_min_deg; param_t land_flare_pitch_max_deg; + param_t land_use_terrain_estimate; } _parameter_handles; /**< handles for interesting parameters */ @@ -474,6 +476,7 @@ FixedwingPositionControl::FixedwingPositionControl() : _parameter_handles.land_heading_hold_horizontal_distance = param_find("FW_LND_HHDIST"); _parameter_handles.land_flare_pitch_min_deg = param_find("FW_FLARE_PMIN"); _parameter_handles.land_flare_pitch_max_deg = param_find("FW_FLARE_PMAX"); + _parameter_handles.land_use_terrain_estimate= param_find("FW_LND_USETER"); _parameter_handles.time_const = param_find("FW_T_TIME_CONST"); _parameter_handles.time_const_throt = param_find("FW_T_THRO_CONST"); @@ -576,6 +579,7 @@ FixedwingPositionControl::parameters_update() param_get(_parameter_handles.land_heading_hold_horizontal_distance, &(_parameters.land_heading_hold_horizontal_distance)); param_get(_parameter_handles.land_flare_pitch_min_deg, &(_parameters.land_flare_pitch_min_deg)); param_get(_parameter_handles.land_flare_pitch_max_deg, &(_parameters.land_flare_pitch_max_deg)); + param_get(_parameter_handles.land_use_terrain_estimate, &(_parameters.land_use_terrain_estimate)); _l1_control.set_l1_damping(_parameters.l1_damping); _l1_control.set_l1_period(_parameters.l1_period); @@ -813,7 +817,7 @@ float FixedwingPositionControl::get_terrain_altitude_landing(float land_setpoint /* Decide if the terrain estimation can be used, once we switched to using the terrain we stick with it * for the whole landing */ - if (global_pos.terrain_alt_valid || land_useterrain) { + if (_parameters.land_use_terrain_estimate && (global_pos.terrain_alt_valid || land_useterrain)) { if(!land_useterrain) { mavlink_log_info(_mavlink_fd, "#audio: Landing, using terrain estimate"); land_useterrain = true; 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 41a3665381..c00d822327 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 @@ -412,15 +412,13 @@ PARAM_DEFINE_FLOAT(FW_LND_TLALT, -1.0f); PARAM_DEFINE_FLOAT(FW_LND_HHDIST, 15.0f); /** - * Relative altitude threshold for range finder measurements for use during landing + * Enable or disable usage of terrain estimate during landing * - * range finder measurements will only be used if the estimated relative altitude (gobal_pos.alt - landing_waypoint.alt) is < FW_LND_RFRALT - * set to < 0 to disable - * the correct value of this parameter depends on your range measuring device as well as on the terrain at the landing location + * 0: disabled, 1: enabled * * @group L1 Control */ -PARAM_DEFINE_FLOAT(FW_LND_RFRALT, -1.0f); +PARAM_DEFINE_INT32(FW_LND_USETER, 0); /** * Flare, minimum pitch From 7dd81c8cb2a05321bcb28819d6eb3aec67052517 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 27 Sep 2014 16:21:25 +0200 Subject: [PATCH 247/411] WIP on laser driver unit test --- Tools/tests-host/Makefile | 11 ++- Tools/tests-host/sf0x_data.txt | 33 +++++++ Tools/tests-host/sf0x_test | Bin 0 -> 9228 bytes Tools/tests-host/sf0x_test.cpp | 157 +++++++++++++++++++++++++++++++++ 4 files changed, 199 insertions(+), 2 deletions(-) create mode 100644 Tools/tests-host/sf0x_data.txt create mode 100755 Tools/tests-host/sf0x_test create mode 100644 Tools/tests-host/sf0x_test.cpp diff --git a/Tools/tests-host/Makefile b/Tools/tests-host/Makefile index f0737ef88c..b1547c23f0 100644 --- a/Tools/tests-host/Makefile +++ b/Tools/tests-host/Makefile @@ -3,7 +3,7 @@ CC=g++ CFLAGS=-I. -I../../src/modules -I ../../src/include -I../../src/drivers \ -I../../src -I../../src/lib -D__EXPORT="" -Dnullptr="0" -lm -all: mixer_test sbus2_test autodeclination_test +all: mixer_test sbus2_test autodeclination_test sf0x_test MIXER_FILES=../../src/systemcmds/tests/test_mixer.cpp \ ../../src/systemcmds/tests/test_conv.cpp \ @@ -20,6 +20,10 @@ SBUS2_FILES=../../src/modules/px4iofirmware/sbus.c \ hrt.cpp \ sbus2_test.cpp +SF0X_FILES= \ + hrt.cpp \ + sf0x_test.cpp + AUTODECLINATION_FILES= ../../src/lib/geo/geo_mag_declination.c \ hrt.cpp \ autodeclination_test.cpp @@ -30,10 +34,13 @@ mixer_test: $(MIXER_FILES) sbus2_test: $(SBUS2_FILES) $(CC) -o sbus2_test $(SBUS2_FILES) $(CFLAGS) +sf0x_test: $(SF0X_FILES) + $(CC) -o sf0x_test $(SF0X_FILES) $(CFLAGS) + autodeclination_test: $(SBUS2_FILES) $(CC) -o autodeclination_test $(AUTODECLINATION_FILES) $(CFLAGS) .PHONY: clean clean: - rm -f $(ODIR)/*.o *~ core $(INCDIR)/*~ mixer_test sbus2_test autodeclination_test \ No newline at end of file + rm -f $(ODIR)/*.o *~ core $(INCDIR)/*~ mixer_test sbus2_test autodeclination_test sf0x_test \ No newline at end of file diff --git a/Tools/tests-host/sf0x_data.txt b/Tools/tests-host/sf0x_data.txt new file mode 100644 index 0000000000..f61f59523e --- /dev/null +++ b/Tools/tests-host/sf0x_data.txt @@ -0,0 +1,33 @@ +0.02 +0.25 +0.03 +0.02 +0.25 +0.03 +0.02 +0.25 +0.03 +0.02 +0.25 +0.03 +0.25 +0.03 +0.02 +0.25 +0.03 +0.08 +0.40 +0.44 + + +0 +. +0 +1 + +0 +0. +.0 +.01 +01 +1 \ No newline at end of file diff --git a/Tools/tests-host/sf0x_test b/Tools/tests-host/sf0x_test new file mode 100755 index 0000000000000000000000000000000000000000..e56c35465423dc3abd2cccd3fdc118953c6a2cac GIT binary patch literal 9228 zcmeHNQEVJX8UEH66UTAcJ5hoW6gF;Lxk==l5k(=b;gV)yD&mmFDN3V8leKrxckTJ^ z*xREv>IFxxB~HhcD-={Age)GAr~sWH52-+@r9`pI3xPL86#1b=YY8OFMZ)Bvi~iZ! zcyHYV9*}t1Px8+E|IU2#|Fi#`XXoEP-1+3{@@O_t@ z0ff9`t2wX(b77J5ic_7a+*L*S)*tZsJ||lQa)2Jm_aUk9`FPGMlr3A>+v)Gnhkd^1 zWqXpPsXn&ZH(4rHwj7Sywj6^#4Y*Dv{k=Rk6pkNoWm@ola96HlG|ihlg5V9jcgdGmiJ&?keAc*4I2 z1HhbkRvLsaJsNO0G)_hXS^#cpzzqDg&=0`f>pzkw9tLg#!q-!wegyhmpug}uMBxqq zAB`#jP*{`c`#X$Q;tJ^(>+>4it@`HG*frSF61Z1XDg01J$!7~oGs)hd_tn3`3w5_ zv((vd^1y-N5!vYxX%E}$&-bHJKDF29I}nmzmo~?U5hF&7|2>{z^>^9)-?WjX`VH2s zt-Igv0@R`IeF@+__2SriR-IVMszcomCxEQ_+)7q`>Fo_QDdY|H*_91-zrMMlo?Rj3 zo&i|5$L`&o5(g|z=->al;adevo(s$19JVfA%3e_cC&P5pEx zuqVmtwN00Mp2r%sP1g7#dvue{Ur(OD*6IY=e}x92?#B`IFSBO%t6czVJn;Jj6}wn{ zY;#%v2Q{tN*2VDkVsmy+7fF# zv+C|7mQm6FC;E>Ydb^u^uj}o#D>U=X(Ki)} zf^+)iw&K!jNnL+^ZOrq$OSM~`=Xt(|tU3AipI31WlOf^;ELR<&MxkVw`Qnsj8jhi5?b7rSZLk8{LDt9@ zEK@6$ts>ng4)hLIFqodv&|5a_ie(PaA(w(fmUklfmA`@3$6b%OHE z(@TmEn}SHYQ)-leri8RMoIeJZ&di8A^(YBsrhR7fs8?oJ(g@1f>PL zKhl$qOwfX@r83;I?HQ^Mr7}D_WjPfR(j?G{3JHf(89rq>PJY@dWldw2beU9!m+gGf z$&$oU8D4Shg5Um>FF7TX?ww6#_=|>Jq(K5G9NkE8?8(^~A114!XLGiAt$J?Z_x_t6-EXmg!4o0;_wF!1~9Kz+yiV zXw&HzeKWDArKFR8~rfqU2iYTt&h9 R304l3v}WXL*|wy8{2QZP(|rH{ literal 0 HcmV?d00001 diff --git a/Tools/tests-host/sf0x_test.cpp b/Tools/tests-host/sf0x_test.cpp new file mode 100644 index 0000000000..d41f7e54a6 --- /dev/null +++ b/Tools/tests-host/sf0x_test.cpp @@ -0,0 +1,157 @@ + +#include +#include +#include +#include +#include +#include + +enum SF0X_PARSE_STATE { + SF0X_PARSE_STATE0_UNSYNC = 0, + SF0X_PARSE_STATE1_SYNC, + SF0X_PARSE_STATE2_GOT_DIGIT0, + SF0X_PARSE_STATE3_GOT_DOT, + SF0X_PARSE_STATE4_GOT_DIGIT1, + SF0X_PARSE_STATE5_GOT_DIGIT2, + SF0X_PARSE_STATE6_GOT_CARRIAGE_RETURN +}; + +int sf0x_parser(char c, char *parserbuf, unsigned *parserbuf_index, enum *SF0X_PARSE_STATE state, float *dist) +{ + int ret = -1; + + unsigned len = strlen(parserbuf); + switch (state) { + case SF0X_PARSE_STATE0_UNSYNC: + if (c == '\n') { + *state = SF0X_PARSE_STATE1_SYNC; + *parserbuf_index = 0; + } + break; + + case SF0X_PARSE_STATE1_SYNC: + if (c >= '0' && c <= '9') { + *state = SF0X_PARSE_STATE2_GOT_DIGIT0; + parserbuf[parserbuf_index] = c; + parserbuf_index++; + } + break; + + case SF0X_PARSE_STATE2_GOT_DIGIT0: + if (c >= '0' && c <= '9') { + *state = SF0X_PARSE_STATE2_GOT_DIGIT0; + parserbuf[parserbuf_index] = c; + parserbuf_index++; + } else if (c == '.') { + *state = SF0X_PARSE_STATE3_GOT_DOT; + parserbuf[parserbuf_index] = c; + parserbuf_index++; + } else { + *state = SF0X_PARSE_STATE0_UNSYNC; + } + break; + + case SF0X_PARSE_STATE3_GOT_DOT: + if (c >= '0' && c <= '9') { + *state = SF0X_PARSE_STATE4_GOT_DIGIT1; + parserbuf[parserbuf_index] = c; + parserbuf_index++; + } else { + *state = SF0X_PARSE_STATE0_UNSYNC; + } + break; + + case SF0X_PARSE_STATE4_GOT_DIGIT1: + if (c >= '0' && c <= '9') { + *state = SF0X_PARSE_STATE4_GOT_DIGIT2; + parserbuf[parserbuf_index] = c; + parserbuf_index++; + } else { + *state = SF0X_PARSE_STATE0_UNSYNC; + } + break; + + case SF0X_PARSE_STATE5_GOT_DIGIT2: + if (c == '\r') { + *state = SF0X_PARSE_STATE6_GOT_CARRIAGE_RETURN; + } else { + *state = SF0X_PARSE_STATE0_UNSYNC; + } + break; + + case SF0X_PARSE_STATE6_GOT_CARRIAGE_RETURN: + if (c == '\n') { + parserbuf[parserbuf_index] = '\0'; + *dist = strtod(linebuf, &end); + *state = SF0X_PARSE_STATE0_SYNC; + *parserbuf_index = 0; + ret = 0; + } else { + *state = SF0X_PARSE_STATE0_UNSYNC; + } + break; + } + + + + return ret; +} + +int main(int argc, char *argv[]) { + warnx("SF0X test started"); + + if (argc < 2) + errx(1, "Need a filename for the input file"); + + warnx("loading data from: %s", argv[1]); + + FILE *fp; + + fp = fopen(argv[1],"rt"); + + if (!fp) + errx(1, "failed opening file"); + + int ret = 0; + + const char LINE_MAX = 20; + char _linebuf[LINE_MAX]; + _linebuf[0] = '\0'; + + char *end; + + enum SF0X_PARSE_STATE state = SF0X_PARSE_STATE0_UNSYNC; + float dist_m; + char _parserbuf[LINE_MAX]; + unsigned _parsebuf_index = 0; + + while (fgets(_linebuf, LINE_MAX, fp) != NULL) { + + printf("\n%s", _linebuf); + + int parse_ret; + + for (int i = 0; i < strlen(_linebuf); i++) + { + printf("%0x ", _linebuf[i]); + parse_ret = sf0x_parser(_linebuf[i], _parserbuf, &_parsebuf_index, &state, &dist_m); + + if (parse_ret == 0) { + printf("PARSED!"); + } + } + + printf("\nparsed: %f %s\n", dist_m, (parse_ret == 0) ? "OK" : ""); + } + + // Init the parser + + + if (ret == EOF) { + warnx("Test finished, reached end of file"); + } else { + warnx("Test aborted, errno: %d", ret); + } + + return ret; +} From 8a18cfa3869555389e7e9ff8f104d83f9c54cb43 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 21 Sep 2014 18:03:52 +0200 Subject: [PATCH 248/411] datman: reduce task priority --- src/modules/dataman/dataman.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/dataman/dataman.c b/src/modules/dataman/dataman.c index ca1fe9bbbd..b2355d4d89 100644 --- a/src/modules/dataman/dataman.c +++ b/src/modules/dataman/dataman.c @@ -797,7 +797,7 @@ start(void) sem_init(&g_init_sema, 1, 0); /* start the worker thread */ - if ((task = task_spawn_cmd("dataman", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 2000, task_main, NULL)) <= 0) { + if ((task = task_spawn_cmd("dataman", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, 2000, task_main, NULL)) <= 0) { warn("task start failed"); return -1; } From cf4604f5c3614bc5cd1b82dfee693cbac36181ab Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 28 Sep 2014 13:34:44 +0400 Subject: [PATCH 249/411] navigator/mission.cpp: indentation fixed --- src/modules/navigator/mission.cpp | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 128dab7d77..719301a0ac 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -417,13 +417,13 @@ Mission::set_mission_items() if (_navigator->get_global_position()->alt < takeoff_alt - _navigator->get_acceptance_radius()) { mavlink_log_critical(_navigator->get_mavlink_fd(), "takeoff to %.1f meters above home", (double)(takeoff_alt - _navigator->get_home_position()->alt)); - _mission_item.nav_cmd = NAV_CMD_TAKEOFF; - _mission_item.lat = _navigator->get_global_position()->lat; - _mission_item.lon = _navigator->get_global_position()->lon; - _mission_item.altitude = takeoff_alt; - _mission_item.altitude_is_relative = false; - _mission_item.autocontinue = true; - _mission_item.time_inside = 0; + _mission_item.nav_cmd = NAV_CMD_TAKEOFF; + _mission_item.lat = _navigator->get_global_position()->lat; + _mission_item.lon = _navigator->get_global_position()->lon; + _mission_item.altitude = takeoff_alt; + _mission_item.altitude_is_relative = false; + _mission_item.autocontinue = true; + _mission_item.time_inside = 0; mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current); From 2d81c2cf466596b7c4f787a52836fd200af2a097 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 28 Sep 2014 13:43:42 +0400 Subject: [PATCH 250/411] mc_pos_control: commented code block removed --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 10 ---------- 1 file changed, 10 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 ce26f7dc25..99deb0d29a 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -1022,16 +1022,6 @@ MulticopterPositionControl::task_main() _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); From 964fddb387b8e635d2cd13fb2248643791304489 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 28 Sep 2014 16:08:25 +0200 Subject: [PATCH 251/411] make geofence update rate independent from positon update rate --- src/modules/navigator/navigator_main.cpp | 12 +++++++----- 1 file changed, 7 insertions(+), 5 deletions(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index b633945445..fad46998e1 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -87,6 +87,7 @@ */ extern "C" __EXPORT int navigator_main(int argc, char *argv[]); +#define GEOFENCE_CHECK_INTERVAL 200000 namespace navigator { @@ -343,7 +344,7 @@ Navigator::task_main() _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); } - bool have_geofence_position_data = false; + static bool have_geofence_position_data = false; /* gps updated */ if (fds[7].revents & POLLIN) { @@ -388,17 +389,18 @@ Navigator::task_main() if (fds[0].revents & POLLIN) { global_position_update(); static int gposcounter = 0; - if (_geofence.getSource() == Geofence::GF_SOURCE_GLOBALPOS && - gposcounter % 10 == 0) { - /* Geofence is checked only every 10th gpos update */ + if (_geofence.getSource() == Geofence::GF_SOURCE_GLOBALPOS) { have_geofence_position_data = true; } gposcounter++; } /* Check geofence violation */ - if (have_geofence_position_data) { + static hrt_abstime last_geofence_check = 0; + if (have_geofence_position_data && hrt_elapsed_time(&last_geofence_check) > GEOFENCE_CHECK_INTERVAL) { bool inside = _geofence.inside(_global_pos, _gps_pos, _sensor_combined.baro_alt_meter); + last_geofence_check = hrt_absolute_time(); + have_geofence_position_data = false; if (!inside) { /* inform other apps via the mission result */ _mission_result.geofence_violated = true; From 038e1cac03198259d6f7630c6bb7c65c35f44fae Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 28 Sep 2014 16:17:17 +0200 Subject: [PATCH 252/411] increase default engine failure threshold --- src/modules/commander/commander_params.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index f4bc7fd7fb..1b0c4258b2 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -159,7 +159,7 @@ PARAM_DEFINE_FLOAT(COM_EF_C2T, 5.0f); * @min 0.0f * @max 7.0f */ -PARAM_DEFINE_FLOAT(COM_EF_TIME, 5.0f); +PARAM_DEFINE_FLOAT(COM_EF_TIME, 10.0f); /** RC loss time threshold * From ab400089bc2a42f1f0ace569d8f0ee58f4338e1d Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 28 Sep 2014 16:17:40 +0200 Subject: [PATCH 253/411] disable flight termination as default for now --- src/modules/systemlib/circuit_breaker.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/systemlib/circuit_breaker.c b/src/modules/systemlib/circuit_breaker.c index 9e5429988b..64420be379 100644 --- a/src/modules/systemlib/circuit_breaker.c +++ b/src/modules/systemlib/circuit_breaker.c @@ -106,7 +106,7 @@ PARAM_DEFINE_INT32(CBRK_AIRSPD_CHK, 0); * @max 121212 * @group Circuit Breaker */ -PARAM_DEFINE_INT32(CBRK_FLIGHTTERM, 0); +PARAM_DEFINE_INT32(CBRK_FLIGHTTERM, 121212); /** * Circuit breaker for engine failure detection From 70e5d4027a3b1465d5128dbf9a04cbb6545e043d Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 30 Sep 2014 09:08:31 +0400 Subject: [PATCH 254/411] navigator: autocontinue fix --- src/modules/navigator/mission.cpp | 26 +++++++++++++++++--------- 1 file changed, 17 insertions(+), 9 deletions(-) diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 1040651320..8bacaa4254 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -150,8 +150,19 @@ Mission::on_active() /* lets check if we reached the current mission item */ if (_mission_type != MISSION_TYPE_NONE && is_mission_item_reached()) { - advance_mission(); - set_mission_items(); + if (_mission_item.autocontinue) { + /* switch to next waypoint if 'autocontinue' flag set */ + advance_mission(); + set_mission_items(); + + } else { + /* else just report that item reached */ + if (_mission_type == MISSION_TYPE_OFFBOARD) { + if (!(_mission_result.seq_reached == _current_offboard_mission_index && _mission_result.reached)) { + report_mission_item_reached(); + } + } + } } else if (_mission_type != MISSION_TYPE_NONE &&_param_altmode.get() == MISSION_ALTMODE_FOH) { altitude_sp_foh_update(); @@ -694,10 +705,8 @@ Mission::save_offboard_mission_state() void Mission::report_mission_item_reached() { - if (_mission_type == MISSION_TYPE_OFFBOARD) { - _mission_result.reached = true; - _mission_result.seq_reached = _current_offboard_mission_index; - } + _mission_result.reached = true; + _mission_result.seq_reached = _current_offboard_mission_index; publish_mission_result(); } @@ -705,6 +714,8 @@ void Mission::report_current_offboard_mission_item() { warnx("current offboard mission index: %d", _current_offboard_mission_index); + _mission_result.reached = false; + _mission_result.finished = false; _mission_result.seq_current = _current_offboard_mission_index; publish_mission_result(); @@ -730,7 +741,4 @@ Mission::publish_mission_result() /* advertise and publish */ _mission_result_pub = orb_advertise(ORB_ID(mission_result), &_mission_result); } - /* reset reached bool */ - _mission_result.reached = false; - _mission_result.finished = false; } From 3e1eec5906ea8df955bf6d0ac7ec182979f47c90 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Wed, 10 Sep 2014 14:42:32 +0900 Subject: [PATCH 255/411] mpu6k: set hardware filter during ACCELIOCLOWPASS also set from GYROIOCLOWPASS Conflicts: mavlink/include/mavlink/v1.0 --- src/drivers/mpu6000/mpu6000.cpp | 15 +++++---------- 1 file changed, 5 insertions(+), 10 deletions(-) diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index 6f5dae7adb..fb38c7e67b 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -968,11 +968,9 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg) return _accel_filter_x.get_cutoff_freq(); case ACCELIOCSLOWPASS: - if (arg == 0) { - // allow disabling of on-chip filter using - // zero as desired filter frequency - _set_dlpf_filter(0); - } + // set hardware filtering + _set_dlpf_filter(arg); + // set software filtering _accel_filter_x.set_cutoff_frequency(1.0e6f / _call_interval, arg); _accel_filter_y.set_cutoff_frequency(1.0e6f / _call_interval, arg); _accel_filter_z.set_cutoff_frequency(1.0e6f / _call_interval, arg); @@ -1056,11 +1054,8 @@ MPU6000::gyro_ioctl(struct file *filp, int cmd, unsigned long arg) _gyro_filter_x.set_cutoff_frequency(1.0e6f / _call_interval, arg); _gyro_filter_y.set_cutoff_frequency(1.0e6f / _call_interval, arg); _gyro_filter_z.set_cutoff_frequency(1.0e6f / _call_interval, arg); - if (arg == 0) { - // allow disabling of on-chip filter using 0 - // as desired frequency - _set_dlpf_filter(0); - } + // set hardware filtering + _set_dlpf_filter(arg); return OK; case GYROIOCSSCALE: From 8ced6bb49bf32128d2673e565cb112ca6d2f21eb Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 30 Sep 2014 10:05:39 +0200 Subject: [PATCH 256/411] Set filter frequency for hardware and software in parallel, always do so in the same order --- src/drivers/mpu6000/mpu6000.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index fb38c7e67b..b22bb2e070 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -910,12 +910,14 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg) // adjust filters float cutoff_freq_hz = _accel_filter_x.get_cutoff_freq(); float sample_rate = 1.0e6f/ticks; + _set_dlpf_filter(cutoff_freq_hz); _accel_filter_x.set_cutoff_frequency(sample_rate, cutoff_freq_hz); _accel_filter_y.set_cutoff_frequency(sample_rate, cutoff_freq_hz); _accel_filter_z.set_cutoff_frequency(sample_rate, cutoff_freq_hz); float cutoff_freq_hz_gyro = _gyro_filter_x.get_cutoff_freq(); + _set_dlpf_filter(cutoff_freq_hz_gyro); _gyro_filter_x.set_cutoff_frequency(sample_rate, cutoff_freq_hz_gyro); _gyro_filter_y.set_cutoff_frequency(sample_rate, cutoff_freq_hz_gyro); _gyro_filter_z.set_cutoff_frequency(sample_rate, cutoff_freq_hz_gyro); @@ -1051,11 +1053,11 @@ MPU6000::gyro_ioctl(struct file *filp, int cmd, unsigned long arg) case GYROIOCGLOWPASS: return _gyro_filter_x.get_cutoff_freq(); case GYROIOCSLOWPASS: + // set hardware filtering + _set_dlpf_filter(arg); _gyro_filter_x.set_cutoff_frequency(1.0e6f / _call_interval, arg); _gyro_filter_y.set_cutoff_frequency(1.0e6f / _call_interval, arg); _gyro_filter_z.set_cutoff_frequency(1.0e6f / _call_interval, arg); - // set hardware filtering - _set_dlpf_filter(arg); return OK; case GYROIOCSSCALE: From 70606d400bdceaa86331670938c1e0ae988ed97c Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 30 Sep 2014 11:11:30 +0200 Subject: [PATCH 257/411] remove wrong comments --- src/modules/navigator/navigator_main.cpp | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index fad46998e1..bf5e36d39e 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -432,9 +432,6 @@ Navigator::task_main() _can_loiter_at_sp = false; break; case NAVIGATION_STATE_AUTO_MISSION: - /* Some failsafe modes prohibit the fallback to mission - * usually this is done after some time to make sure - * that the full failsafe operation is performed */ _navigation_mode = &_mission; break; case NAVIGATION_STATE_AUTO_LOITER: @@ -450,7 +447,7 @@ Navigator::task_main() case NAVIGATION_STATE_AUTO_RTL: _navigation_mode = &_rtl; break; - case NAVIGATION_STATE_AUTO_RTGS: //XXX OBC: differentiate between rc loss and dl loss here + case NAVIGATION_STATE_AUTO_RTGS: /* Use complex data link loss mode only when enabled via param * otherwise use rtl */ if (_param_datalinkloss_obc.get() != 0) { From 1072a3380c2d6bdea010bb5091c6d0b23fe6f224 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 30 Sep 2014 11:11:46 +0200 Subject: [PATCH 258/411] enable engine failure circuit breaker --- src/modules/systemlib/circuit_breaker.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/systemlib/circuit_breaker.c b/src/modules/systemlib/circuit_breaker.c index 64420be379..83c8344d54 100644 --- a/src/modules/systemlib/circuit_breaker.c +++ b/src/modules/systemlib/circuit_breaker.c @@ -120,7 +120,7 @@ PARAM_DEFINE_INT32(CBRK_FLIGHTTERM, 121212); * @max 284953 * @group Circuit Breaker */ -PARAM_DEFINE_INT32(CBRK_ENGINEFAIL, 0); +PARAM_DEFINE_INT32(CBRK_ENGINEFAIL, 284953); bool circuit_breaker_enabled(const char* breaker, int32_t magic) { From d4c0dc2ba0271f4d9c8044fd2a3a178cbb9987e3 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 30 Sep 2014 11:20:30 +0200 Subject: [PATCH 259/411] add and activate circuit breaker for gps failure detection --- src/modules/commander/commander.cpp | 5 +++-- src/modules/systemlib/circuit_breaker.c | 14 ++++++++++++++ src/modules/systemlib/circuit_breaker.h | 1 + 3 files changed, 18 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index bf15bbeb60..b86f3678b0 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1417,8 +1417,9 @@ int commander_thread_main(int argc, char *argv[]) } /* check if GPS fix is ok */ - if (gps_position.fix_type >= 3 && //XXX check eph and epv ? - hrt_elapsed_time(&gps_position.timestamp_position) < FAILSAFE_DEFAULT_TIMEOUT) { + if (circuit_breaker_enabled("CBRK_GPSFAIL", CBRK_GPSFAIL_KEY) || + (gps_position.fix_type >= 3 && + hrt_elapsed_time(&gps_position.timestamp_position) < FAILSAFE_DEFAULT_TIMEOUT)) { /* handle the case where gps was regained */ if (status.gps_failure) { status.gps_failure = false; diff --git a/src/modules/systemlib/circuit_breaker.c b/src/modules/systemlib/circuit_breaker.c index 83c8344d54..12187d70ea 100644 --- a/src/modules/systemlib/circuit_breaker.c +++ b/src/modules/systemlib/circuit_breaker.c @@ -122,6 +122,20 @@ PARAM_DEFINE_INT32(CBRK_FLIGHTTERM, 121212); */ PARAM_DEFINE_INT32(CBRK_ENGINEFAIL, 284953); +/** + * Circuit breaker for gps failure detection + * + * Setting this parameter to 240024 will disable the gps failure detection. + * If the aircraft is in gps failure mode the gps failure flag will be + * set to healthy + * WARNING: ENABLING THIS CIRCUIT BREAKER IS AT OWN RISK + * + * @min 0 + * @max 240024 + * @group Circuit Breaker + */ +PARAM_DEFINE_INT32(CBRK_GPSFAIL, 240024); + bool circuit_breaker_enabled(const char* breaker, int32_t magic) { int32_t val; diff --git a/src/modules/systemlib/circuit_breaker.h b/src/modules/systemlib/circuit_breaker.h index 6a55e4948d..b3431722fc 100644 --- a/src/modules/systemlib/circuit_breaker.h +++ b/src/modules/systemlib/circuit_breaker.h @@ -55,6 +55,7 @@ #define CBRK_AIRSPD_CHK_KEY 162128 #define CBRK_FLIGHTTERM_KEY 121212 #define CBRK_ENGINEFAIL_KEY 284953 +#define CBRK_GPSFAIL_KEY 240024 #include From 8b7c57a0d050d51f45f8c66536e5450e7a494d73 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 30 Sep 2014 13:39:40 +0200 Subject: [PATCH 260/411] px4io driver: update cb only when changed --- src/drivers/px4io/px4io.cpp | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 6f8d6f5f6b..fbb5d4f2e6 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -295,6 +295,7 @@ private: float _battery_amp_bias; ///< current sensor bias float _battery_mamphour_total;///< amp hours consumed so far uint64_t _battery_last_timestamp;///< last amp hour calculation timestamp + bool _cb_flighttermination; ///< true if the flight termination circuit breaker is enabled #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 bool _dsm_vcc_ctl; ///< true if relay 1 controls DSM satellite RX power @@ -515,7 +516,8 @@ PX4IO::PX4IO(device::Device *interface) : _battery_amp_per_volt(90.0f / 5.0f), // this matches the 3DR current sensor _battery_amp_bias(0), _battery_mamphour_total(0), - _battery_last_timestamp(0) + _battery_last_timestamp(0), + _cb_flighttermination(true) #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 , _dsm_vcc_ctl(false) #endif @@ -1051,6 +1053,9 @@ PX4IO::task_main() } } + /* Update Circuit breakers */ + _cb_flighttermination = circuit_breaker_enabled("CBRK_FLIGHTTERM", CBRK_FLIGHTTERM_KEY); + } } @@ -1170,7 +1175,7 @@ PX4IO::io_set_arming_state() } /* Do not set failsafe if circuit breaker is enabled */ - if (armed.force_failsafe && !circuit_breaker_enabled("CBRK_FLIGHTTERM", CBRK_FLIGHTTERM_KEY)) { + if (armed.force_failsafe && !_cb_flighttermination) { set |= PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE; } else { clear |= PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE; From d11f05b12c0b0c56703ab14303f91efd123b9dc6 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 30 Sep 2014 13:40:03 +0200 Subject: [PATCH 261/411] commander: update gps and engine cb only when changed --- src/modules/commander/commander.cpp | 20 ++++++++++++++------ src/modules/uORB/topics/vehicle_status.h | 2 ++ 2 files changed, 16 insertions(+), 6 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index b86f3678b0..9ebe006f0d 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -775,6 +775,8 @@ int commander_thread_main(int argc, char *argv[]) // CIRCUIT BREAKERS status.circuit_breaker_engaged_power_check = false; status.circuit_breaker_engaged_airspd_check = false; + status.circuit_breaker_engaged_enginefailure_check = false; + status.circuit_breaker_engaged_gpsfailure_check = false; /* publish initial state */ status_pub = orb_advertise(ORB_ID(vehicle_status), &status); @@ -980,8 +982,8 @@ int commander_thread_main(int argc, char *argv[]) int32_t ef_throttle_thres = 1.0f; int32_t ef_current2throttle_thres = 0.0f; int32_t ef_time_thres = 1000.0f; - uint64_t timestamp_engine_healthy = 0; /**< absolute time when engine - was healty*/ + uint64_t timestamp_engine_healthy = 0; /**< absolute time when engine was healty */ + /* check which state machines for changes, clear "changed" flag */ bool arming_state_changed = false; bool main_state_changed = false; @@ -1028,8 +1030,14 @@ int commander_thread_main(int argc, char *argv[]) param_get(_param_system_id, &(status.system_id)); param_get(_param_component_id, &(status.component_id)); - status.circuit_breaker_engaged_power_check = circuit_breaker_enabled("CBRK_SUPPLY_CHK", CBRK_SUPPLY_CHK_KEY); - status.circuit_breaker_engaged_airspd_check = circuit_breaker_enabled("CBRK_AIRSPD_CHK", CBRK_AIRSPD_CHK_KEY); + status.circuit_breaker_engaged_power_check = + circuit_breaker_enabled("CBRK_SUPPLY_CHK", CBRK_SUPPLY_CHK_KEY); + status.circuit_breaker_engaged_airspd_check = + circuit_breaker_enabled("CBRK_AIRSPD_CHK", CBRK_AIRSPD_CHK_KEY); + status.circuit_breaker_engaged_enginefailure_check = + circuit_breaker_enabled("CBRK_ENGINEFAIL", CBRK_ENGINEFAIL_KEY); + status.circuit_breaker_engaged_gpsfailure_check = + circuit_breaker_enabled("CBRK_GPSFAIL", CBRK_GPSFAIL_KEY); status_changed = true; @@ -1417,7 +1425,7 @@ int commander_thread_main(int argc, char *argv[]) } /* check if GPS fix is ok */ - if (circuit_breaker_enabled("CBRK_GPSFAIL", CBRK_GPSFAIL_KEY) || + if (status.circuit_breaker_engaged_gpsfailure_check || (gps_position.fix_type >= 3 && hrt_elapsed_time(&gps_position.timestamp_position) < FAILSAFE_DEFAULT_TIMEOUT)) { /* handle the case where gps was regained */ @@ -1616,7 +1624,7 @@ int commander_thread_main(int argc, char *argv[]) /* Check engine failure * only for fixed wing for now */ - if (!circuit_breaker_enabled("CBRK_ENGINEFAIL", CBRK_ENGINEFAIL_KEY) && + if (!status.circuit_breaker_engaged_enginefailure_check && status.is_rotary_wing == false && armed.armed && ((actuator_controls.control[3] > ef_throttle_thres && diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index 9dccb2309d..505039d90d 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -239,6 +239,8 @@ struct vehicle_status_s { bool circuit_breaker_engaged_power_check; bool circuit_breaker_engaged_airspd_check; + bool circuit_breaker_engaged_enginefailure_check; + bool circuit_breaker_engaged_gpsfailure_check; }; /** From 213f4aadbdd089c3f793ba7ac13e0bc3b9b46fee Mon Sep 17 00:00:00 2001 From: Denis Yeldandi Date: Tue, 30 Sep 2014 15:44:47 +0400 Subject: [PATCH 262/411] Ashtech GPS driver --- src/drivers/drv_gps.h | 3 +- src/drivers/gps/ashtech.cpp | 616 ++++++++++++++++++ src/drivers/gps/ashtech.h | 96 +++ src/drivers/gps/gps.cpp | 18 + src/drivers/gps/module.mk | 1 + .../uORB/topics/vehicle_gps_position.h | 2 +- 6 files changed, 734 insertions(+), 2 deletions(-) create mode 100644 src/drivers/gps/ashtech.cpp create mode 100644 src/drivers/gps/ashtech.h diff --git a/src/drivers/drv_gps.h b/src/drivers/drv_gps.h index e14f4e00da..76a211000f 100644 --- a/src/drivers/drv_gps.h +++ b/src/drivers/drv_gps.h @@ -57,7 +57,8 @@ typedef enum { GPS_DRIVER_MODE_NONE = 0, GPS_DRIVER_MODE_UBX, - GPS_DRIVER_MODE_MTK + GPS_DRIVER_MODE_MTK, + GPS_DRIVER_MODE_ASHTECH } gps_driver_mode_t; diff --git a/src/drivers/gps/ashtech.cpp b/src/drivers/gps/ashtech.cpp new file mode 100644 index 0000000000..d338ff8e5e --- /dev/null +++ b/src/drivers/gps/ashtech.cpp @@ -0,0 +1,616 @@ +#include "ashtech.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +typedef double float64_t; +typedef float float32_t; + +char* str_scanDec(const char *pos, int8_t sign, int8_t n_max_digit, int32_t *result) +{ + int8_t n=0; + int32_t d=0; + int8_t neg = 0; + + if(*pos == '-') { neg = 1; pos++; } + else if(*pos == '+') { pos++; } + else if(sign) return(NULL); + while(*pos >= '0' && *pos <= '9') + { + d = d*10 + (*(pos++) - '0'); + n++; + if(n_max_digit>0 && n==n_max_digit) break; + } + if(n==0 || n>10) return(NULL); + if(neg) *result = -d; + else *result = d; + return((char *)pos); +} + +char* scanFloat64(const char *pos, int8_t sign, int8_t n_max_int, int8_t n_max_frac, + float64_t *result) +{ + float64_t f=0.0, div=1.0; + int32_t d_int; + int8_t n=0, isneg= 0; + + if( *pos == '-' ) isneg= 1; + if((pos = str_scanDec(pos, sign, n_max_int, &d_int)) == NULL) return(NULL); + if(*(pos) == '.') + { + pos++; + while(*pos >= '0' && *pos <= '9') + { + f = f*(10.0) + (float64_t)(*(pos++) - '0'); + div *= (0.1); + n++; + if(n_max_frac>0 && n==n_max_frac) break; + } + } + else if(n_max_frac > 0) return(NULL); + if( isneg ) *result = (float64_t)d_int - f*div; + else *result = (float64_t)d_int + f*div; + return((char *)pos); +} + + + +ASHTECH::ASHTECH(const int &fd, struct vehicle_gps_position_s *gps_position, struct satellite_info_s *satellite_info): +_fd(fd), +_satellite_info(satellite_info), +_gps_position(gps_position) +{ + decode_init(); + _decode_state = NME_DECODE_UNINIT; + _rx_buffer_bytes = 0; +} + +ASHTECH::~ASHTECH(){ +} +//All NMEA descriptions are taken from +//http://www.trimble.com/OEM_ReceiverHelp/V4.44/en/NMEA-0183messages_MessageOverview.html +int ASHTECH::handle_message(int len){ + if(len < 7) return 0; + int uiCalcComma = 0; + for(int i = 0 ; i < len; i++){ + if(_rx_buffer[i] == ',')uiCalcComma++; + } + char* bufptr = (char*)( _rx_buffer+6); + + if((memcmp(_rx_buffer+3, "ZDA,",3) == 0)&&(uiCalcComma == 6)){ + /* + UTC day, month, and year, and local time zone offset + An example of the ZDA message string is: + + $GPZDA,172809.456,12,07,1996,00,00*45 + + ZDA message fields + Field Meaning + 0 Message ID $GPZDA + 1 UTC + 2 Day, ranging between 01 and 31 + 3 Month, ranging between 01 and 12 + 4 Year + 5 Local time zone offset from GMT, ranging from 00 through �13 hours + 6 Local time zone offset from GMT, ranging from 00 through 59 minutes + 7 The checksum data, always begins with * + Fields 5 and 6 together yield the total offset. For example, if field 5 is -5 and field 6 is +15, local time is 5 hours and 15 minutes earlier than GMT. + */ + float64_t ashtech_time = 0.0; + int day = 0, month = 0,year = 0,local_time_off_hour = 0,local_time_off_min = 0; + if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&ashtech_time); + if(bufptr && *(++bufptr) != ',') bufptr = str_scanDec(bufptr, 0, 9, &day); + if(bufptr && *(++bufptr) != ',') bufptr = str_scanDec(bufptr, 0, 9, &month); + if(bufptr && *(++bufptr) != ',') bufptr = str_scanDec(bufptr, 0, 9, &year); + if(bufptr && *(++bufptr) != ',') bufptr = str_scanDec(bufptr, 0, 9, &local_time_off_hour); + if(bufptr && *(++bufptr) != ',') bufptr = str_scanDec(bufptr, 0, 9, &local_time_off_min); + + + int ashtech_hour = ashtech_time/10000; + int ashtech_minute = (ashtech_time - ashtech_hour*10000)/100; + float64_t ashtech_sec = ashtech_time - ashtech_hour*10000 - ashtech_minute*100; + //convert to unix timestamp + struct tm timeinfo; + timeinfo.tm_year = year - 1900; + timeinfo.tm_mon = month - 1; + timeinfo.tm_mday = day; + timeinfo.tm_hour = ashtech_hour; + timeinfo.tm_min = ashtech_minute; + timeinfo.tm_sec = int(ashtech_sec); + time_t epoch = mktime(&timeinfo); + + _gps_position->time_gps_usec = (uint64_t)epoch * 1000000; //TODO: test this + _gps_position->time_gps_usec += (uint64_t)((ashtech_sec - int(ashtech_sec)) * 1e6); + _gps_position->timestamp_time = hrt_absolute_time(); + } + + else if((memcmp(_rx_buffer+3, "GGA,",3) == 0)&&(uiCalcComma == 14)){ + /* + Time, position, and fix related data + An example of the GBS message string is: + + $GPGGA,172814.0,3723.46587704,N,12202.26957864,W,2,6,1.2,18.893,M,-25.669,M,2.0,0031*4F + + Note - The data string exceeds the ASHTECH standard length. + GGA message fields + Field Meaning + 0 Message ID $GPGGA + 1 UTC of position fix + 2 Latitude + 3 Direction of latitude: + N: North + S: South + 4 Longitude + 5 Direction of longitude: + E: East + W: West + 6 GPS Quality indicator: + 0: Fix not valid + 1: GPS fix + 2: Differential GPS fix, OmniSTAR VBS + 4: Real-Time Kinematic, fixed integers + 5: Real-Time Kinematic, float integers, OmniSTAR XP/HP or Location RTK + 7 Number of SVs in use, range from 00 through to 24+ + 8 HDOP + 9 Orthometric height (MSL reference) + 10 M: unit of measure for orthometric height is meters + 11 Geoid separation + 12 M: geoid separation measured in meters + 13 Age of differential GPS data record, Type 1 or Type 9. Null field when DGPS is not used. + 14 Reference station ID, range 0000-4095. A null field when any reference station ID is selected and no corrections are received1. + 15 + The checksum data, always begins with * + Note - If a user-defined geoid model, or an inclined + */ + float64_t ashtech_time = 0.0, lat = 0.0, lon = 0.0, alt = 0.0; + int num_of_sv = 0, fix_quality=0; + float64_t hdop = 99.9; + char ns = '?', ew = '?'; + + if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&ashtech_time); + if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&lat); + if(bufptr && *(++bufptr) != ',') ns = *(bufptr++); + if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&lon); + if(bufptr && *(++bufptr) != ',') ew = *(bufptr++); + if(bufptr && *(++bufptr) != ',') bufptr = str_scanDec(bufptr, 0, 9, &fix_quality); + if(bufptr && *(++bufptr) != ',') bufptr = str_scanDec(bufptr, 0, 9, &num_of_sv); + if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&hdop); + if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&alt); + + if(ns == 'S') + lat = -lat; + if(ew == 'W') + lon = -lon; + + _gps_position->lat = (int(lat*0.01) + (lat*0.01 - int(lat*0.01))*100.0/60.0)*10000000; + _gps_position->lon = (int(lon*0.01) + (lon*0.01 - int(lon*0.01))*100.0/60.0)*10000000; + _gps_position->alt = alt*1000; + _rate_count_lat_lon++; + + if((lat == 0.0) && (lon == 0.0) && (alt == 0.0)){ + _gps_position->fix_type = 0; + } + else + _gps_position->fix_type = 3 + fix_quality; + + _gps_position->timestamp_position = hrt_absolute_time(); + + _gps_position->vel_m_s = 0; /**< GPS ground speed (m/s) */ + _gps_position->vel_n_m_s = 0; /**< GPS ground speed in m/s */ + _gps_position->vel_e_m_s = 0; /**< GPS ground speed in m/s */ + _gps_position->vel_d_m_s = 0; /**< GPS ground speed in m/s */ + _gps_position->cog_rad = 0; /**< Course over ground (NOT heading, but direction of movement) in rad, -PI..PI */ + _gps_position->vel_ned_valid = true; /**< Flag to indicate if NED speed is valid */ + _gps_position->c_variance_rad = 0.1; + _gps_position->timestamp_velocity = hrt_absolute_time(); + return 1; + } + else if ((memcmp(_rx_buffer, "$PASHR,POS,", 11) == 0) && (uiCalcComma == 18)) { + /* + Example $PASHR,POS,2,10,125410.00,5525.8138702,N,03833.9587380,E,131.555,1.0,0.0,0.007,-0.001,2.0,1.0,1.7,1.0,*34 + + $PASHR,POS,d1,d2,m3,m4,c5,m6,c7,f8,f9,f10,f11,f12,f13,f14,f15,f16,s17*cc + Parameter Description Range + d1 Position mode 0: standalone + 1: differential + 2: RTK float + 3: RTK fixed + 5: Dead reckoning + 9: SBAS (see NPT setting) + d2 Number of satellite used in position fix 0-99 + m3 Current UTC time of position fix (hhmmss.ss) 000000.00-235959.99 + m4 Latitude of position (ddmm.mmmmmm) 0-90 degrees 00-59.9999999 minutes + c5 Latitude sector N, S + m6 Longitude of position (dddmm.mmmmmm) 0-180 degrees 00-59.9999999 minutes + c7 Longitude sector E,W + f8 Altitude above ellipsoid +9999.000 + f9 Differential age (data link age), seconds 0.0-600.0 + f10 True track/course over ground in degrees 0.0-359.9 + f11 Speed over ground in knots 0.0-999.9 + f12 Vertical velocity in decimeters per second +999.9 + f13 PDOP 0-99.9 + f14 HDOP 0-99.9 + f15 VDOP 0-99.9 + f16 TDOP 0-99.9 + s17 Reserved no data + *cc Checksum + */ + bufptr = (char*)( _rx_buffer+10); + float64_t ashtech_time = 0.0, lat = 0.0, lon = 0.0, alt = 0.0; + int num_of_sv = 0, fix_quality=0; + float64_t track_true = 0.0, ground_speed = 0.0 , age_of_corr = 0.0; + float64_t hdop = 99.9, vdop = 99.9, pdop = 99.9, tdop = 99.9,vertic_vel = 0.0; + char ns = '?', ew = '?'; + + if(bufptr && *(++bufptr) != ',') bufptr = str_scanDec(bufptr, 0, 9, &fix_quality); + if(bufptr && *(++bufptr) != ',') bufptr = str_scanDec(bufptr, 0, 9, &num_of_sv); + if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&ashtech_time); + if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&lat); + if(bufptr && *(++bufptr) != ',') ns = *(bufptr++); + if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&lon); + if(bufptr && *(++bufptr) != ',') ew = *(bufptr++); + if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&alt); + if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&age_of_corr); + if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&track_true); + if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&ground_speed); + if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&vertic_vel); + if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&pdop); + if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&hdop); + if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&vdop); + if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&tdop); + + if(ns == 'S') + lat = -lat; + if(ew == 'W') + lon = -lon; + + _gps_position->lat = (int(lat*0.01) + (lat*0.01 - int(lat*0.01))*100.0/60.0)*10000000; + _gps_position->lon = (int(lon*0.01) + (lon*0.01 - int(lon*0.01))*100.0/60.0)*10000000; + _gps_position->alt = alt*1000; + _rate_count_lat_lon++; + + if((lat == 0.0) && (lon == 0.0) && (alt == 0.0)){ + _gps_position->fix_type = 0; + } + else + _gps_position->fix_type = 3 + fix_quality; + + _gps_position->timestamp_position = hrt_absolute_time(); + + const float64_t m_pi = 3.14159265; + + double track_rad = track_true * m_pi / 180.0; + + double velocity_ms = ground_speed / 3.6; + double velocity_north = velocity_ms * cos(track_rad); + double velocity_east = velocity_ms * sin(track_rad); + + _gps_position->vel_m_s = velocity_ms; /**< GPS ground speed (m/s) */ + _gps_position->vel_n_m_s = velocity_north; /**< GPS ground speed in m/s */ + _gps_position->vel_e_m_s = velocity_east; /**< GPS ground speed in m/s */ + _gps_position->vel_d_m_s = -vertic_vel; /**< GPS ground speed in m/s */ + _gps_position->cog_rad = track_rad; /**< Course over ground (NOT heading, but direction of movement) in rad, -PI..PI */ + _gps_position->vel_ned_valid = true; /**< Flag to indicate if NED speed is valid */ + _gps_position->c_variance_rad = 0.1; + _gps_position->timestamp_velocity = hrt_absolute_time(); + return 1; + + } + else if((memcmp(_rx_buffer+3, "GST,",3) == 0)&&(uiCalcComma == 8)){ + /* + Position error statistics + An example of the GST message string is: + + $GPGST,172814.0,0.006,0.023,0.020,273.6,0.023,0.020,0.031*6A + + The Talker ID ($--) will vary depending on the satellite system used for the position solution: + + $GP - GPS only + $GL - GLONASS only + $GN - Combined + GST message fields + Field Meaning + 0 Message ID $GPGST + 1 UTC of position fix + 2 RMS value of the pseudorange residuals; includes carrier phase residuals during periods of RTK (float) and RTK (fixed) processing + 3 Error ellipse semi-major axis 1 sigma error, in meters + 4 Error ellipse semi-minor axis 1 sigma error, in meters + 5 Error ellipse orientation, degrees from true north + 6 Latitude 1 sigma error, in meters + 7 Longitude 1 sigma error, in meters + 8 Height 1 sigma error, in meters + 9 The checksum data, always begins with * + */ + float64_t ashtech_time = 0.0, lat_err = 0.0, lon_err = 0.0, alt_err = 0.0; + float64_t min_err = 0.0, maj_err = 0.0, deg_from_north = 0.0, rms_err = 0.0; + + if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&ashtech_time); + if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&rms_err); + if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&maj_err); + if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&min_err); + if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,°_from_north); + if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&lat_err); + if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&lon_err); + if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&alt_err); + + _gps_position->eph = sqrt(lat_err*lat_err +lon_err*lon_err); + _gps_position->epv = alt_err; + + _gps_position->s_variance_m_s = 0; + _gps_position->timestamp_variance = hrt_absolute_time(); + } + else if((memcmp(_rx_buffer+3, "VTG,",3) == 0)&&(uiCalcComma == 9)){ + /* + Track made good and speed over ground + An example of the VTG message string is: + + $GPVTG,054.7,T,034.4,M,005.5,N,010.2,K*48 + + VTG message fields + Field Meaning + 0 Message ID $GPVTG + 1 Track made good (degrees true) + 2 T: track made good is relative to true north + 3 Track made good (degrees magnetic) + 4 M: track made good is relative to magnetic north + 5 Speed, in knots + 6 N: speed is measured in knots + 7 Speed over ground in kilometers/hour (kph) + 8 K: speed over ground is measured in kph + 9 The checksum data, always begins with * + */ + /*float64_t track_true = 0.0, speed = 0.0, track_magnetic = 0.0, ground_speed = 0.0; + char true_north = '?', magnetic_north = '?', speed_in_knots = '?', speed_in_kph = '?'; + + if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&track_true); + if(bufptr && *(++bufptr) != ',') true_north = *(bufptr++); + if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&track_magnetic); + if(bufptr && *(++bufptr) != ',') magnetic_north = *(bufptr++); + if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&speed); + if(bufptr && *(++bufptr) != ',') speed_in_knots = *(bufptr++); + if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&ground_speed); + if(bufptr && *(++bufptr) != ',') speed_in_kph = *(bufptr++); + + const float64_t dPI = 3.14159265; + float64_t tan_C = tan(track_true * dPI / 180.0); + float64_t lat_ = sqrt(ground_speed*ground_speed/ (1+tan_C)); //km/hour + float64_t lon_ = lat_*tan_C; // //km/hour*/ + } + else if((memcmp(_rx_buffer+3, "GSV,",3) == 0)){ + /* + The GSV message string identifies the number of SVs in view, the PRN numbers, elevations, azimuths, and SNR values. An example of the GSV message string is: + + $GPGSV,4,1,13,02,02,213,,03,-3,000,,11,00,121,,14,13,172,05*67 + + GSV message fields + Field Meaning + 0 Message ID $GPGSV + 1 Total number of messages of this type in this cycle + 2 Message number + 3 Total number of SVs visible + 4 SV PRN number + 5 Elevation, in degrees, 90� maximum + 6 Azimuth, degrees from True North, 000� through 359� + 7 SNR, 00 through 99 dB (null when not tracking) + 8-11 Information about second SV, same format as fields 4 through 7 + 12-15 Information about third SV, same format as fields 4 through 7 + 16-19 Information about fourth SV, same format as fields 4 through 7 + 20 The checksum data, always begins with * + */ +// currently process only gps, because do not know what +// Global satellite ID I should use for non GPS sats + bool bGPS = false; + if(memcmp(_rx_buffer, "$GP",3) != 0) + return 0; + else + bGPS = true; + + int all_msg_num, this_msg_num, tot_sv_visible; + struct gsv_sat{ + int svid; + int elevation; + int azimuth; + int snr; + } sat[4]; + memset(sat, 0, sizeof(sat)); + + if(bufptr && *(++bufptr) != ',') bufptr = str_scanDec(bufptr, 0, 9, &all_msg_num); + if(bufptr && *(++bufptr) != ',') bufptr = str_scanDec(bufptr, 0, 9, &this_msg_num); + if(bufptr && *(++bufptr) != ',') bufptr = str_scanDec(bufptr, 0, 9, &tot_sv_visible); + if((this_msg_num<1) || (this_msg_num>all_msg_num)){ + return 0; + } + if((this_msg_num == 0)&&(bGPS == true)){ + memset(_satellite_info->svid, 0,sizeof(_satellite_info->svid)); + memset(_satellite_info->used, 0,sizeof(_satellite_info->used)); + memset(_satellite_info->snr, 0,sizeof(_satellite_info->snr)); + memset(_satellite_info->elevation,0,sizeof(_satellite_info->elevation)); + memset(_satellite_info->azimuth, 0,sizeof(_satellite_info->azimuth)); + } + + int end = 4; + if(this_msg_num == all_msg_num){ + end = tot_sv_visible - (this_msg_num-1)*4; + _gps_position->satellites_used = tot_sv_visible; + _satellite_info->count = SAT_INFO_MAX_SATELLITES; + _satellite_info->timestamp = hrt_absolute_time(); + } + for(int y = 0 ; y < end ;y++){ + if(bufptr && *(++bufptr) != ',') bufptr = str_scanDec(bufptr, 0, 9, &sat[y].svid); + if(bufptr && *(++bufptr) != ',') bufptr = str_scanDec(bufptr, 0, 9, &sat[y].elevation); + if(bufptr && *(++bufptr) != ',') bufptr = str_scanDec(bufptr, 0, 9, &sat[y].azimuth); + if(bufptr && *(++bufptr) != ',') bufptr = str_scanDec(bufptr, 0, 9, &sat[y].snr); + + _satellite_info->svid[y+(this_msg_num-1)*4] = sat[y].svid; + _satellite_info->used[y+(this_msg_num-1)*4] = ((sat[y].snr>0)? true: false); + _satellite_info->snr[y+(this_msg_num-1)*4] = sat[y].snr; + _satellite_info->elevation[y+(this_msg_num-1)*4] = sat[y].elevation; + _satellite_info->azimuth[y+(this_msg_num-1)*4] = sat[y].azimuth; + } + } + + return 0; +} + + +int ASHTECH::receive(unsigned timeout){ +{ + /* poll descriptor */ + pollfd fds[1]; + fds[0].fd = _fd; + fds[0].events = POLLIN; + + uint8_t buf[32]; + + /* timeout additional to poll */ + uint64_t time_started = hrt_absolute_time(); + + int j = 0; + ssize_t count = 0; + + while (true) { + + /* pass received bytes to the packet decoder */ + while (j < count) { + int l=0; + if ((l = parse_char(buf[j])) > 0) { + /* return to configure during configuration or to the gps driver during normal work + * if a packet has arrived */ + if (handle_message(l) > 0) + return 1; + } + /* in case we keep trying but only get crap from GPS */ + if (time_started + timeout*1000*2 < hrt_absolute_time() ) { + return -1; + } + j++; + } + + /* everything is read */ + j = count = 0; + + /* then poll for new data */ + int ret = ::poll(fds, sizeof(fds) / sizeof(fds[0]), timeout*2); + + if (ret < 0) { + /* something went wrong when polling */ + return -1; + + } else if (ret == 0) { + /* Timeout */ + return -1; + + } else if (ret > 0) { + /* if we have new data from GPS, go handle it */ + if (fds[0].revents & POLLIN) { + /* + * We are here because poll says there is some data, so this + * won't block even on a blocking device. If more bytes are + * available, we'll go back to poll() again... + */ + count = ::read(_fd, buf, sizeof(buf)); + } + } + } +} + +} +#define HEXDIGIT_CHAR(d) ((char)((d) + (((d) < 0xA) ? '0' : 'A'-0xA))) + +int ASHTECH::parse_char(uint8_t b) +{ + int iRet = 0; + + switch (_decode_state) { + /* First, look for sync1 */ + case NME_DECODE_UNINIT: + if (b == '$') { + _decode_state = NME_DECODE_GOT_SYNC1; + _rx_buffer_bytes = 0; + _rx_buffer[_rx_buffer_bytes++] = b; + } + + break; + + case NME_DECODE_GOT_SYNC1: + if (b == '$') { + _decode_state = NME_DECODE_GOT_SYNC1; + _rx_buffer_bytes = 0; + + } else if (b == '*') { + _decode_state = NME_DECODE_GOT_ASTERIKS; + } + + if (_rx_buffer_bytes >= (sizeof(_rx_buffer) - 5)) { + _decode_state = NME_DECODE_UNINIT; + _rx_buffer_bytes = 0; + + } else + _rx_buffer[_rx_buffer_bytes++] = b; + + break; + + case NME_DECODE_GOT_ASTERIKS: + _rx_buffer[_rx_buffer_bytes++] = b; + _decode_state = NME_DECODE_GOT_FIRST_CS_BYTE; + break; + + case NME_DECODE_GOT_FIRST_CS_BYTE: + _rx_buffer[_rx_buffer_bytes++] = b; + uint8_t checksum = 0; + uint8_t *buffer = _rx_buffer + 1; + uint8_t *bufend = _rx_buffer + _rx_buffer_bytes - 3; + + for (; buffer < bufend; buffer++) checksum ^= *buffer; + + if ((HEXDIGIT_CHAR(checksum >> 4) == *(_rx_buffer + _rx_buffer_bytes - 2)) && + (HEXDIGIT_CHAR(checksum & 0x0F) == *(_rx_buffer + _rx_buffer_bytes - 1))) { + iRet = _rx_buffer_bytes; + } + + _decode_state = NME_DECODE_UNINIT; + _rx_buffer_bytes = 0; + break; + } + + return iRet; +} + +void ASHTECH::decode_init(void){ + +} + +//ashtech boad configuration script +//char comm[] = "$PASHS,NME,GGA,A,ON,0.1\r\n" +char comm[] = "$PASHS,NME,ZDA,B,ON,3\r\n"\ + "$PASHS,NME,GGA,B,OFF\r\n"\ + "$PASHS,NME,GST,B,ON,3\r\n"\ + "$PASHS,NME,POS,B,ON,0.1\r\n"\ + "$PASHS,NME,GSV,B,ON,3\r\n"\ + "$PASHS,SPD,A,8\r\n"\ + "$PASHS,SPD,B,7\r\n"; // default baud is 7 + +int ASHTECH::configure(unsigned &baudrate){ + /* try different baudrates */ + const unsigned baudrates_to_try[] = {9600, 38400, 19200, 57600, 115200}; + + for (int baud_i = 0; baud_i < sizeof(baudrates_to_try)/sizeof(baudrates_to_try[0]); baud_i++) { + baudrate = baudrates_to_try[baud_i]; + set_baudrate(_fd, baudrate); + write(_fd, (uint8_t*)comm, sizeof(comm)); + } + set_baudrate(_fd, 38400); + return 0; +} diff --git a/src/drivers/gps/ashtech.h b/src/drivers/gps/ashtech.h new file mode 100644 index 0000000000..e8c5ffb4c4 --- /dev/null +++ b/src/drivers/gps/ashtech.h @@ -0,0 +1,96 @@ +/**************************************************************************** + * + * Copyright (C) 2013. All rights reserved. + * Author: Boriskin Aleksey + * Kistanov Alexander + * + * 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 ASHTECH protocol definitions */ + +#ifndef ASHTECH_H_ +#define ASHTECH_H_ + +#include "gps_helper.h" + +#ifndef RECV_BUFFER_SIZE +#define RECV_BUFFER_SIZE 512 + +#define SAT_INFO_MAX_SATELLITES 20 +#endif + + +class ASHTECH : public GPS_Helper +{ + enum ashtech_decode_state_t { + NME_DECODE_UNINIT, + NME_DECODE_GOT_SYNC1, + NME_DECODE_GOT_ASTERIKS, + NME_DECODE_GOT_FIRST_CS_BYTE + }; + + int _fd; + struct satellite_info_s *_satellite_info; + struct vehicle_gps_position_s *_gps_position; + int ashtechlog_fd;//miklm + + ashtech_decode_state_t _decode_state; + uint8_t _rx_buffer[RECV_BUFFER_SIZE]; + uint16_t _rx_buffer_bytes; + bool _parse_error; // parse error flag + char *_parse_pos; // parse position + + bool _gsv_in_progress; // Indicates that gsv data parsing is in progress + //int _satellites_count; // Number of satellites info parsed. + uint8_t count; /**< Number of satellites in satellite info */ + uint8_t svid[SAT_INFO_MAX_SATELLITES]; /**< Space vehicle ID [1..255], see scheme below */ + uint8_t used[SAT_INFO_MAX_SATELLITES]; /**< 0: Satellite not used, 1: used for navigation */ + uint8_t elevation[SAT_INFO_MAX_SATELLITES]; /**< Elevation (0: right on top of receiver, 90: on the horizon) of satellite */ + uint8_t azimuth[SAT_INFO_MAX_SATELLITES]; /**< Direction of satellite, 0: 0 deg, 255: 360 deg. */ + uint8_t snr[SAT_INFO_MAX_SATELLITES]; /**< dBHz, Signal to noise ratio of satellite C/N0, range 0..99, zero when not tracking this satellite. */ + +public: + ASHTECH(const int &fd, struct vehicle_gps_position_s *gps_position, struct satellite_info_s *satellite_info); + ~ASHTECH(); + int receive(unsigned timeout); + int configure(unsigned &baudrate); + void decode_init(void); + int handle_message(int len); + int parse_char(uint8_t b); + /** Read int ASHTECH parameter */ + int32_t read_int(); + /** Read float ASHTECH parameter */ + double read_float(); + /** Read char ASHTECH parameter */ + char read_char(); + +}; + +#endif /* ASHTECH_H_ */ diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index 34dd630866..2547cda97f 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -69,6 +69,7 @@ #include "ubx.h" #include "mtk.h" +#include "ashtech.h" #define TIMEOUT_5HZ 500 @@ -341,6 +342,11 @@ GPS::task_main() _Helper = new MTK(_serial_fd, &_report_gps_pos); break; + case GPS_DRIVER_MODE_ASHTECH: + + _Helper = new ASHTECH(_serial_fd, &_report_gps_pos, _p_report_sat_info); + break; + default: break; } @@ -402,6 +408,10 @@ GPS::task_main() mode_str = "MTK"; break; + case GPS_DRIVER_MODE_ASHTECH: + mode_str = "ASHTECH"; + break; + default: break; } @@ -429,6 +439,10 @@ GPS::task_main() break; case GPS_DRIVER_MODE_MTK: + _mode = GPS_DRIVER_MODE_ASHTECH; + break; + + case GPS_DRIVER_MODE_ASHTECH: _mode = GPS_DRIVER_MODE_UBX; break; @@ -475,6 +489,10 @@ GPS::print_info() warnx("protocol: MTK"); break; + case GPS_DRIVER_MODE_ASHTECH: + warnx("protocol: ASHTECH"); + break; + default: break; } diff --git a/src/drivers/gps/module.mk b/src/drivers/gps/module.mk index b008184244..4f99b0d3b5 100644 --- a/src/drivers/gps/module.mk +++ b/src/drivers/gps/module.mk @@ -40,6 +40,7 @@ MODULE_COMMAND = gps SRCS = gps.cpp \ gps_helper.cpp \ mtk.cpp \ + ashtech.cpp \ ubx.cpp MODULE_STACKSIZE = 1200 diff --git a/src/modules/uORB/topics/vehicle_gps_position.h b/src/modules/uORB/topics/vehicle_gps_position.h index 80d65cd697..43909e2faf 100644 --- a/src/modules/uORB/topics/vehicle_gps_position.h +++ b/src/modules/uORB/topics/vehicle_gps_position.h @@ -62,7 +62,7 @@ struct vehicle_gps_position_s { uint64_t timestamp_variance; float s_variance_m_s; /**< speed accuracy estimate m/s */ float c_variance_rad; /**< course accuracy estimate rad */ - uint8_t fix_type; /**< 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. */ + uint8_t fix_type; /**< 0-1: no fix, 2: 2D fix, 3: 3D fix, 4: Real-Time Kinematic, fixed integers, 5: Real-Time Kinematic, float integers, OmniSTAR XP/HP or Location RTK. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. */ float eph; /**< GPS HDOP horizontal dilution of position in m */ float epv; /**< GPS VDOP horizontal dilution of position in m */ From 47dcf88271b4b05f2007383e62a6dd239b7eb859 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 30 Sep 2014 15:18:30 +0200 Subject: [PATCH 263/411] Flash optimization --- makefiles/config_px4fmu-v1_default.mk | 6 +++--- src/lib/conversion/module.mk | 2 ++ 2 files changed, 5 insertions(+), 3 deletions(-) diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk index 6f39b56a13..cf631c2be5 100644 --- a/makefiles/config_px4fmu-v1_default.mk +++ b/makefiles/config_px4fmu-v1_default.mk @@ -24,14 +24,14 @@ MODULES += drivers/l3gd20 MODULES += drivers/mpu6000 MODULES += drivers/hmc5883 MODULES += drivers/ms5611 -MODULES += drivers/mb12xx +#MODULES += drivers/mb12xx MODULES += drivers/gps MODULES += drivers/hil -MODULES += drivers/blinkm +#MODULES += drivers/blinkm MODULES += drivers/rgbled MODULES += drivers/mkblctrl MODULES += drivers/airspeed -MODULES += drivers/ets_airspeed +#MODULES += drivers/ets_airspeed MODULES += drivers/meas_airspeed MODULES += drivers/frsky_telemetry MODULES += modules/sensors diff --git a/src/lib/conversion/module.mk b/src/lib/conversion/module.mk index f5f59a2dca..4593c48871 100644 --- a/src/lib/conversion/module.mk +++ b/src/lib/conversion/module.mk @@ -36,3 +36,5 @@ # SRCS = rotation.cpp + +MAXOPTIMIZATION = -Os From 47f151d4cefa5d7a9da34d6139aa493259f8b2fd Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 30 Sep 2014 15:19:06 +0200 Subject: [PATCH 264/411] Deactivate FrSky telem by default --- makefiles/config_px4fmu-v1_default.mk | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk index cf631c2be5..9fe16fbb6c 100644 --- a/makefiles/config_px4fmu-v1_default.mk +++ b/makefiles/config_px4fmu-v1_default.mk @@ -33,7 +33,7 @@ MODULES += drivers/mkblctrl MODULES += drivers/airspeed #MODULES += drivers/ets_airspeed MODULES += drivers/meas_airspeed -MODULES += drivers/frsky_telemetry +#MODULES += drivers/frsky_telemetry MODULES += modules/sensors # From 6da52c5543012a1391c5e5c4384434c1292b919b Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 30 Sep 2014 15:39:01 +0200 Subject: [PATCH 265/411] remove unnecessary variable --- 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 fad46998e1..b81155efe3 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -388,11 +388,9 @@ Navigator::task_main() /* global position updated */ if (fds[0].revents & POLLIN) { global_position_update(); - static int gposcounter = 0; if (_geofence.getSource() == Geofence::GF_SOURCE_GLOBALPOS) { have_geofence_position_data = true; } - gposcounter++; } /* Check geofence violation */ From c5a1ddd8f2dd32005c2581886e96b3419009fef4 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 30 Sep 2014 15:42:56 +0200 Subject: [PATCH 266/411] re-enable MC apps on FMUV1 --- makefiles/config_px4fmu-v1_default.mk | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk index 9aa8320ee8..9fe16fbb6c 100644 --- a/makefiles/config_px4fmu-v1_default.mk +++ b/makefiles/config_px4fmu-v1_default.mk @@ -64,17 +64,17 @@ MODULES += modules/gpio_led # # Estimation modules (EKF / other filters) # -#MODULES += modules/attitude_estimator_ekf +MODULES += modules/attitude_estimator_ekf MODULES += modules/ekf_att_pos_estimator -#MODULES += modules/position_estimator_inav +MODULES += modules/position_estimator_inav # # Vehicle Control # MODULES += modules/fw_pos_control_l1 MODULES += modules/fw_att_control -#MODULES += modules/mc_att_control -#MODULES += modules/mc_pos_control +MODULES += modules/mc_att_control +MODULES += modules/mc_pos_control # # Logging From 4fbeb73bda55f14d1ffd259d064e116047b20301 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 30 Sep 2014 15:48:48 +0200 Subject: [PATCH 267/411] fix merge fail, remove double declaration of circuit breaker --- src/modules/systemlib/circuit_breaker.c | 14 -------------- 1 file changed, 14 deletions(-) diff --git a/src/modules/systemlib/circuit_breaker.c b/src/modules/systemlib/circuit_breaker.c index 8ad79f94d6..12187d70ea 100644 --- a/src/modules/systemlib/circuit_breaker.c +++ b/src/modules/systemlib/circuit_breaker.c @@ -136,20 +136,6 @@ PARAM_DEFINE_INT32(CBRK_ENGINEFAIL, 284953); */ PARAM_DEFINE_INT32(CBRK_GPSFAIL, 240024); -/** - * Circuit breaker for gps failure detection - * - * Setting this parameter to 240024 will disable the gps failure detection. - * If the aircraft is in gps failure mode the gps failure flag will be - * set to healthy - * WARNING: ENABLING THIS CIRCUIT BREAKER IS AT OWN RISK - * - * @min 0 - * @max 240024 - * @group Circuit Breaker - */ -PARAM_DEFINE_INT32(CBRK_GPSFAIL, 240024); - bool circuit_breaker_enabled(const char* breaker, int32_t magic) { int32_t val; From f4851c1b148f21143c4a0446a843948af6793ed3 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 30 Sep 2014 15:50:58 +0200 Subject: [PATCH 268/411] ubx: disable sbas configuration per default --- src/drivers/gps/ubx.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp index c54c474b3d..45bd445c45 100644 --- a/src/drivers/gps/ubx.cpp +++ b/src/drivers/gps/ubx.cpp @@ -189,6 +189,7 @@ UBX::configure(unsigned &baudrate) return 1; } +#ifdef UBX_CONFIGURE_SBAS /* send a SBAS message to set the SBAS options */ memset(&_buf.payload_tx_cfg_sbas, 0, sizeof(_buf.payload_tx_cfg_sbas)); _buf.payload_tx_cfg_sbas.mode = UBX_TX_CFG_SBAS_MODE; @@ -198,6 +199,7 @@ UBX::configure(unsigned &baudrate) if (wait_for_ack(UBX_MSG_CFG_SBAS, UBX_CONFIG_TIMEOUT, true) < 0) { return 1; } +#endif /* configure message rates */ /* the last argument is divisor for measurement rate (set by CFG RATE), i.e. 1 means 5Hz */ From 5452732610bd1d5b795558fa8f9d9ffb2639dc0b Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 30 Sep 2014 15:54:27 +0200 Subject: [PATCH 269/411] switch back to common mavlink dialect --- src/modules/mavlink/mavlink_bridge_header.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_bridge_header.h b/src/modules/mavlink/mavlink_bridge_header.h index 00d427636a..0cd08769e1 100644 --- a/src/modules/mavlink/mavlink_bridge_header.h +++ b/src/modules/mavlink/mavlink_bridge_header.h @@ -84,7 +84,7 @@ void mavlink_send_uart_bytes(mavlink_channel_t chan, const uint8_t *ch, int leng extern mavlink_status_t *mavlink_get_channel_status(uint8_t chan); extern mavlink_message_t *mavlink_get_channel_buffer(uint8_t chan); -#include +#include __END_DECLS From 2766285d56cab6f9916efde535b979bc5475f6a5 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 30 Sep 2014 16:00:14 +0200 Subject: [PATCH 270/411] mavlink: change message buffer size to 4 --- src/modules/mavlink/mavlink_main.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index b9f486c95e..980362ce56 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1328,7 +1328,7 @@ Mavlink::task_main(int argc, char *argv[]) * make space for two messages plus off-by-one space as we use the empty element * marker ring buffer approach. */ - if (OK != message_buffer_init(10 * sizeof(mavlink_message_t) + 1)) { + if (OK != message_buffer_init(4 * sizeof(mavlink_message_t) + 1)) { errx(1, "can't allocate message buffer, exiting"); } @@ -1382,7 +1382,7 @@ Mavlink::task_main(int argc, char *argv[]) _mission_manager->set_verbose(_verbose); LL_APPEND(_streams, _mission_manager); - + switch (_mode) { case MAVLINK_MODE_NORMAL: From a960fcbdef475e5ab264760568b15867d55b7774 Mon Sep 17 00:00:00 2001 From: Denis Yeldandi Date: Thu, 2 Oct 2014 14:34:20 +0400 Subject: [PATCH 271/411] Increased ashtech POS frequency and increased baudrate --- src/drivers/gps/ashtech.cpp | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/src/drivers/gps/ashtech.cpp b/src/drivers/gps/ashtech.cpp index d338ff8e5e..614aade002 100644 --- a/src/drivers/gps/ashtech.cpp +++ b/src/drivers/gps/ashtech.cpp @@ -597,20 +597,22 @@ void ASHTECH::decode_init(void){ char comm[] = "$PASHS,NME,ZDA,B,ON,3\r\n"\ "$PASHS,NME,GGA,B,OFF\r\n"\ "$PASHS,NME,GST,B,ON,3\r\n"\ - "$PASHS,NME,POS,B,ON,0.1\r\n"\ + "$PASHS,NME,POS,B,ON,0.05\r\n"\ "$PASHS,NME,GSV,B,ON,3\r\n"\ "$PASHS,SPD,A,8\r\n"\ - "$PASHS,SPD,B,7\r\n"; // default baud is 7 + "$PASHS,SPD,B,9\r\n"; // default baud is 7 int ASHTECH::configure(unsigned &baudrate){ /* try different baudrates */ const unsigned baudrates_to_try[] = {9600, 38400, 19200, 57600, 115200}; + for (int baud_i = 0; baud_i < sizeof(baudrates_to_try)/sizeof(baudrates_to_try[0]); baud_i++) { baudrate = baudrates_to_try[baud_i]; set_baudrate(_fd, baudrate); write(_fd, (uint8_t*)comm, sizeof(comm)); } - set_baudrate(_fd, 38400); + + set_baudrate(_fd, 115200); return 0; } From 84908f8f3db5179ffff0d96d15756ab112535482 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 2 Oct 2014 15:45:02 +0400 Subject: [PATCH 272/411] mc_pos_control: AUTO speed limiting bug fixed --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 10 ++++++---- 1 file changed, 6 insertions(+), 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 99deb0d29a..ec7b2a78f5 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -829,10 +829,12 @@ MulticopterPositionControl::control_auto(float dt) /* move setpoint not faster than max allowed speed */ math::Vector<3> pos_sp_old_s = _pos_sp.emult(scale); - math::Vector<3> d_pos_s = pos_sp_s - pos_sp_old_s; - float d_pos_s_len = d_pos_s.length(); - if (d_pos_s_len > dt) { - pos_sp_s = pos_sp_old_s + d_pos_s / d_pos_s_len * dt; + + /* difference between current and desired position setpoints, 1 = max speed */ + math::Vector<3> d_pos_m = (pos_sp_s - pos_sp_old_s).edivide(_params.pos_p); + float d_pos_m_len = d_pos_m.length(); + if (d_pos_m_len > dt) { + pos_sp_s = pos_sp_old_s + (d_pos_m / d_pos_m_len * dt).emult(_params.pos_p); } /* scale result back to normal space */ From 5846f217d0e09ee4c1c64c0ebf0f2e621a14fa69 Mon Sep 17 00:00:00 2001 From: tstellanova Date: Fri, 3 Oct 2014 17:18:44 -0700 Subject: [PATCH 273/411] Use global position altitude to report HUD altitude, for consistency. Otherwise the HUD altitude jumps between two very different values. --- src/modules/mavlink/mavlink_messages.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index a2f3828ffa..941b45d881 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -820,7 +820,11 @@ protected: msg.groundspeed = sqrtf(pos.vel_n * pos.vel_n + pos.vel_e * pos.vel_e); msg.heading = _wrap_2pi(att.yaw) * M_RAD_TO_DEG_F; msg.throttle = armed.armed ? act.control[3] * 100.0f : 0.0f; - msg.alt = sensor_combined.baro_alt_meter; + /* + Do not use sensor_combined.baro_alt_meter here because + it is mismatched with WSG84 AMSL used elsewhere for reporting altitude. + */ + msg.alt = pos.alt; msg.climb = -pos.vel_d; _mavlink->send_message(MAVLINK_MSG_ID_VFR_HUD, &msg); From 04ceb3c95d6c713dfb4f673d3a4cafa7c27c13ca Mon Sep 17 00:00:00 2001 From: Denis Yeldandi Date: Sat, 4 Oct 2014 09:21:47 +0400 Subject: [PATCH 274/411] Fixed km/h -> knots in POS --- src/drivers/gps/ashtech.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/gps/ashtech.cpp b/src/drivers/gps/ashtech.cpp index d338ff8e5e..69c22ab281 100644 --- a/src/drivers/gps/ashtech.cpp +++ b/src/drivers/gps/ashtech.cpp @@ -293,7 +293,7 @@ int ASHTECH::handle_message(int len){ double track_rad = track_true * m_pi / 180.0; - double velocity_ms = ground_speed / 3.6; + double velocity_ms = ground_speed / 1.9438445; /** knots to m/s */ double velocity_north = velocity_ms * cos(track_rad); double velocity_east = velocity_ms * sin(track_rad); From 7bde4fa6342230ab37644f94d364cd6e9541773e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 4 Oct 2014 12:31:16 +0200 Subject: [PATCH 275/411] Revert "Use global position altitude to report HUD altitude, for consistency." --- src/modules/mavlink/mavlink_messages.cpp | 6 +----- 1 file changed, 1 insertion(+), 5 deletions(-) diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 941b45d881..a2f3828ffa 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -820,11 +820,7 @@ protected: msg.groundspeed = sqrtf(pos.vel_n * pos.vel_n + pos.vel_e * pos.vel_e); msg.heading = _wrap_2pi(att.yaw) * M_RAD_TO_DEG_F; msg.throttle = armed.armed ? act.control[3] * 100.0f : 0.0f; - /* - Do not use sensor_combined.baro_alt_meter here because - it is mismatched with WSG84 AMSL used elsewhere for reporting altitude. - */ - msg.alt = pos.alt; + msg.alt = sensor_combined.baro_alt_meter; msg.climb = -pos.vel_d; _mavlink->send_message(MAVLINK_MSG_ID_VFR_HUD, &msg); From 56a9d16fc476e99106bda0627cc20f69e987fc6a Mon Sep 17 00:00:00 2001 From: Vladimir Ermakov Date: Sun, 5 Oct 2014 13:19:13 +0400 Subject: [PATCH 276/411] FTP: Save errno in _copy_file(). --- src/modules/mavlink/mavlink_ftp.cpp | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/src/modules/mavlink/mavlink_ftp.cpp b/src/modules/mavlink/mavlink_ftp.cpp index c26fe6b4bc..4ed06561d3 100644 --- a/src/modules/mavlink/mavlink_ftp.cpp +++ b/src/modules/mavlink/mavlink_ftp.cpp @@ -560,7 +560,7 @@ MavlinkFTP::_workTruncateFile(PayloadHeader* payload) return kErrFailErrno; } - if (payload->offset == st.st_size) { + if (payload->offset == (unsigned)st.st_size) { // nothing to do return kErrNone; } @@ -574,7 +574,7 @@ MavlinkFTP::_workTruncateFile(PayloadHeader* payload) ::close(fd); return kErrNone; } - else if (payload->offset > st.st_size) { + else if (payload->offset > (unsigned)st.st_size) { // 2: extend file int fd = ::open(file, O_WRONLY); if (fd < 0) { @@ -808,6 +808,7 @@ MavlinkFTP::_copy_file(const char *src_path, const char *dst_path, ssize_t lengt { char buff[512]; int src_fd = -1, dst_fd = -1; + int op_errno = 0; src_fd = ::open(src_path, O_RDONLY); if (src_fd < 0) { @@ -816,7 +817,9 @@ MavlinkFTP::_copy_file(const char *src_path, const char *dst_path, ssize_t lengt dst_fd = ::open(dst_path, O_CREAT | O_TRUNC | O_WRONLY); if (dst_fd < 0) { + op_errno = errno; ::close(src_fd); + errno = op_errno; return -1; } @@ -831,12 +834,14 @@ MavlinkFTP::_copy_file(const char *src_path, const char *dst_path, ssize_t lengt } else if (bytes_read < 0) { warnx("cp: read"); + op_errno = errno; break; } bytes_written = ::write(dst_fd, buff, bytes_read); if (bytes_written != bytes_read) { warnx("cp: short write"); + op_errno = errno; break; } @@ -846,5 +851,6 @@ MavlinkFTP::_copy_file(const char *src_path, const char *dst_path, ssize_t lengt ::close(src_fd); ::close(dst_fd); + errno = op_errno; return (length > 0)? -1 : 0; } From 0fbdb8d326e3a764fde840e426e093bae32bcba6 Mon Sep 17 00:00:00 2001 From: Vladimir Ermakov Date: Sun, 5 Oct 2014 13:23:57 +0400 Subject: [PATCH 277/411] FTP: Return bytes written in payload. --- src/modules/mavlink/mavlink_ftp.cpp | 7 ++----- 1 file changed, 2 insertions(+), 5 deletions(-) diff --git a/src/modules/mavlink/mavlink_ftp.cpp b/src/modules/mavlink/mavlink_ftp.cpp index 4ed06561d3..f17497aa8f 100644 --- a/src/modules/mavlink/mavlink_ftp.cpp +++ b/src/modules/mavlink/mavlink_ftp.cpp @@ -508,11 +508,8 @@ MavlinkFTP::_workWrite(PayloadHeader* payload) return kErrFailErrno; } - // need to decide how to indicate how much data was written - // current: old code set hdr->size - payload->size = bytes_written; - //payload->size = sizeof(uint32_t); - //*((uint32_t*)payload->data) = bytes_written; + payload->size = sizeof(uint32_t); + *((uint32_t*)payload->data) = bytes_written; return kErrNone; } From 2722921b30a48fd3fb67ba25efe8c4e3ca1338c8 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 5 Oct 2014 12:30:29 +0200 Subject: [PATCH 278/411] Fixed function name of mission modification logic, attributed @DrTon --- src/modules/navigator/mission.cpp | 9 +++++---- src/modules/navigator/mission.h | 13 +++++++------ 2 files changed, 12 insertions(+), 10 deletions(-) diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 87d16f1e67..b5926df815 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -37,6 +37,7 @@ * * @author Julian Oes * @author Thomas Gubler + * @author Anton Babushkin */ #include @@ -157,7 +158,7 @@ Mission::on_active() /* else just report that item reached */ if (_mission_type == MISSION_TYPE_OFFBOARD) { if (!(_navigator->get_mission_result()->seq_reached == _current_offboard_mission_index && _navigator->get_mission_result()->reached)) { - report_mission_item_reached(); + set_mission_item_reached(); } } } @@ -701,7 +702,7 @@ Mission::save_offboard_mission_state() } void -Mission::report_mission_item_reached() +Mission::set_mission_item_reached() { _navigator->get_mission_result()->reached = true; _navigator->get_mission_result()->seq_reached = _current_offboard_mission_index; @@ -709,7 +710,7 @@ Mission::report_mission_item_reached() } void -Mission::report_current_offboard_mission_item() +Mission::set_current_offboard_mission_item() { warnx("current offboard mission index: %d", _current_offboard_mission_index); _navigator->get_mission_result()->reached = false; @@ -721,7 +722,7 @@ Mission::report_current_offboard_mission_item() } void -Mission::report_mission_finished() +Mission::set_mission_finished() { _navigator->get_mission_result()->finished = true; _navigator->publish_mission_result(); diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h index 5164737f33..ea7cc0927c 100644 --- a/src/modules/navigator/mission.h +++ b/src/modules/navigator/mission.h @@ -37,6 +37,7 @@ * * @author Julian Oes * @author Thomas Gubler + * @author Anton Babushkin */ #ifndef NAVIGATOR_MISSION_H @@ -130,19 +131,19 @@ private: void save_offboard_mission_state(); /** - * Report that a mission item has been reached + * Set a mission item as reached */ - void report_mission_item_reached(); + void set_mission_item_reached(); /** - * Rport the current mission item + * Set the current offboard mission item */ - void report_current_offboard_mission_item(); + void set_current_offboard_mission_item(); /** - * Report that the mission is finished if one exists or that none exists + * Set that the mission is finished if one exists or that none exists */ - void report_mission_finished(); + void set_mission_finished(); control::BlockParamInt _param_onboard_enabled; control::BlockParamFloat _param_takeoff_alt; From 61352b8e1af8d076ea9f6edb9932f3b498a72e7a Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 5 Oct 2014 12:52:04 +0200 Subject: [PATCH 279/411] remove warnx --- src/drivers/gps/ubx.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp index 45bd445c45..b0eb4ab662 100644 --- a/src/drivers/gps/ubx.cpp +++ b/src/drivers/gps/ubx.cpp @@ -277,8 +277,6 @@ UBX::wait_for_ack(const uint16_t msg, const unsigned timeout, const bool report) } } - warnx("msg %u ret %d", msg, ret); - _ack_state = UBX_ACK_IDLE; return ret; } From ebc84b9f44dd3d26bc8d26b18f8c059f900e5e1a Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 5 Oct 2014 13:02:13 +0200 Subject: [PATCH 280/411] reduce mavlink message buffer size --- src/modules/mavlink/mavlink_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 980362ce56..25dc018447 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1328,7 +1328,7 @@ Mavlink::task_main(int argc, char *argv[]) * make space for two messages plus off-by-one space as we use the empty element * marker ring buffer approach. */ - if (OK != message_buffer_init(4 * sizeof(mavlink_message_t) + 1)) { + if (OK != message_buffer_init(2 * sizeof(mavlink_message_t) + 1)) { errx(1, "can't allocate message buffer, exiting"); } From 2587074a07f67ed37c8d4f0b4d7f1b31bb2f6525 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 5 Oct 2014 13:07:56 +0200 Subject: [PATCH 281/411] Compile fixes in navigator --- src/modules/navigator/mission.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index b5926df815..7fac69a611 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -234,7 +234,7 @@ Mission::update_offboard_mission() _current_offboard_mission_index = 0; } - report_current_offboard_mission_item(); + set_current_offboard_mission_item(); } @@ -396,7 +396,7 @@ Mission::set_mission_items() _navigator->set_can_loiter_at_sp(pos_sp_triplet->current.type == SETPOINT_TYPE_LOITER); reset_mission_item_reached(); - report_mission_finished(); + set_mission_finished(); _navigator->set_position_setpoint_triplet_updated(); return; @@ -476,7 +476,7 @@ Mission::set_mission_items() reset_mission_item_reached(); if (_mission_type == MISSION_TYPE_OFFBOARD) { - report_current_offboard_mission_item(); + set_current_offboard_mission_item(); } // TODO: report onboard mission item somehow From ff17f31ccedc99bb9151e2f905d336345f2ed7d0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 5 Oct 2014 13:09:02 +0200 Subject: [PATCH 282/411] Dataman: Optimize for space --- src/modules/dataman/module.mk | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/dataman/module.mk b/src/modules/dataman/module.mk index 234607b3de..7ebe09fb7e 100644 --- a/src/modules/dataman/module.mk +++ b/src/modules/dataman/module.mk @@ -40,3 +40,5 @@ MODULE_COMMAND = dataman SRCS = dataman.c MODULE_STACKSIZE = 1200 + +MAXOPTIMIZATION = -Os From 850c630e11dca24bc160d00b580c7ff7d8b6ff6d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 5 Oct 2014 13:09:19 +0200 Subject: [PATCH 283/411] Navigator: Optimize for space --- src/modules/navigator/module.mk | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/navigator/module.mk b/src/modules/navigator/module.mk index c075903b75..fb8d079010 100644 --- a/src/modules/navigator/module.mk +++ b/src/modules/navigator/module.mk @@ -63,3 +63,5 @@ INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink MODULE_STACKSIZE = 1200 EXTRACXXFLAGS = -Weffc++ + +MAXOPTIMIZATION = -Os From c0eefc983a5bbe2faf05eb21daaf3514b767daa6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 5 Oct 2014 13:13:22 +0200 Subject: [PATCH 284/411] File test compile fixes --- src/modules/mavlink/mavlink_tests/mavlink_ftp_test.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.cpp b/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.cpp index a5fb2117ea..4caa820b6e 100644 --- a/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.cpp +++ b/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.cpp @@ -265,7 +265,7 @@ bool MavlinkFtpTest::_open_badfile_test(void) MavlinkFTP::PayloadHeader *reply; const char *dir = "/foo"; // non-existent file - payload.opcode = MavlinkFTP::kCmdOpenFile; + payload.opcode = MavlinkFTP::kCmdOpenFileRO; payload.offset = 0; bool success = _send_receive_msg(&payload, // FTP payload header @@ -295,7 +295,7 @@ bool MavlinkFtpTest::_open_terminate_test(void) struct stat st; const ReadTestCase *test = &_rgReadTestCases[i]; - payload.opcode = MavlinkFTP::kCmdOpenFile; + payload.opcode = MavlinkFTP::kCmdOpenFileRO; payload.offset = 0; bool success = _send_receive_msg(&payload, // FTP payload header @@ -342,7 +342,7 @@ bool MavlinkFtpTest::_terminate_badsession_test(void) MavlinkFTP::PayloadHeader *reply; const char *file = _rgReadTestCases[0].file; - payload.opcode = MavlinkFTP::kCmdOpenFile; + payload.opcode = MavlinkFTP::kCmdOpenFileRO; payload.offset = 0; bool success = _send_receive_msg(&payload, // FTP payload header @@ -400,7 +400,7 @@ bool MavlinkFtpTest::_read_test(void) // Test case data files are created for specific boundary conditions ut_compare("Test case data files are out of date", test->length, st.st_size); - payload.opcode = MavlinkFTP::kCmdOpenFile; + payload.opcode = MavlinkFTP::kCmdOpenFileRO; payload.offset = 0; bool success = _send_receive_msg(&payload, // FTP payload header @@ -463,7 +463,7 @@ bool MavlinkFtpTest::_read_badsession_test(void) MavlinkFTP::PayloadHeader *reply; const char *file = _rgReadTestCases[0].file; - payload.opcode = MavlinkFTP::kCmdOpenFile; + payload.opcode = MavlinkFTP::kCmdOpenFileRO; payload.offset = 0; bool success = _send_receive_msg(&payload, // FTP payload header From 72f6aaca96f7c7936f33cc3e7d7073ddb92d9bfa Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 6 Oct 2014 07:53:18 +0200 Subject: [PATCH 285/411] Add ST24 test harness --- Tools/tests-host/.gitignore | 1 + Tools/tests-host/Makefile | 11 +++++-- Tools/tests-host/st24_test.cpp | 58 ++++++++++++++++++++++++++++++++++ 3 files changed, 68 insertions(+), 2 deletions(-) create mode 100644 Tools/tests-host/st24_test.cpp diff --git a/Tools/tests-host/.gitignore b/Tools/tests-host/.gitignore index 87b314c614..b06f998157 100644 --- a/Tools/tests-host/.gitignore +++ b/Tools/tests-host/.gitignore @@ -2,3 +2,4 @@ mixer_test sbus2_test autodeclination_test +st24_test diff --git a/Tools/tests-host/Makefile b/Tools/tests-host/Makefile index f0737ef88c..5ee0c94326 100644 --- a/Tools/tests-host/Makefile +++ b/Tools/tests-host/Makefile @@ -3,7 +3,7 @@ CC=g++ CFLAGS=-I. -I../../src/modules -I ../../src/include -I../../src/drivers \ -I../../src -I../../src/lib -D__EXPORT="" -Dnullptr="0" -lm -all: mixer_test sbus2_test autodeclination_test +all: mixer_test sbus2_test autodeclination_test st24_test MIXER_FILES=../../src/systemcmds/tests/test_mixer.cpp \ ../../src/systemcmds/tests/test_conv.cpp \ @@ -20,6 +20,10 @@ SBUS2_FILES=../../src/modules/px4iofirmware/sbus.c \ hrt.cpp \ sbus2_test.cpp +ST24_FILES=../../src/lib/rc/st24.c \ + hrt.cpp \ + st24_test.cpp + AUTODECLINATION_FILES= ../../src/lib/geo/geo_mag_declination.c \ hrt.cpp \ autodeclination_test.cpp @@ -33,7 +37,10 @@ sbus2_test: $(SBUS2_FILES) autodeclination_test: $(SBUS2_FILES) $(CC) -o autodeclination_test $(AUTODECLINATION_FILES) $(CFLAGS) +st24_test: $(ST24_FILES) + $(CC) -o st24_test $(ST24_FILES) $(CFLAGS) + .PHONY: clean clean: - rm -f $(ODIR)/*.o *~ core $(INCDIR)/*~ mixer_test sbus2_test autodeclination_test \ No newline at end of file + rm -f $(ODIR)/*.o *~ core $(INCDIR)/*~ mixer_test sbus2_test autodeclination_test st24_test \ No newline at end of file diff --git a/Tools/tests-host/st24_test.cpp b/Tools/tests-host/st24_test.cpp new file mode 100644 index 0000000000..680791ca8a --- /dev/null +++ b/Tools/tests-host/st24_test.cpp @@ -0,0 +1,58 @@ + +#include +#include +#include +#include +#include +#include +#include "../../src/systemcmds/tests/tests.h" + +int main(int argc, char *argv[]) { + warnx("ST24 test started"); + + if (argc < 2) + errx(1, "Need a filename for the input file"); + + warnx("loading data from: %s", argv[1]); + + FILE *fp; + + fp = fopen(argv[1],"rt"); + + if (!fp) + errx(1, "failed opening file"); + + float f; + unsigned x; + int ret; + + // Trash the first 20 lines + for (unsigned i = 0; i < 20; i++) { + (void)fscanf(fp, "%f,%x,,", &f, &x); + } + + float last_time = 0; + + while (EOF != (ret = fscanf(fp, "%f,%x,,", &f, &x))) { + if (((f - last_time) * 1000 * 1000) > 3000) { + warnx("FRAME RESET\n\n"); + } + + warnx("%f: 0x%02x", (double)f, x); + + last_time = f; + + // Pipe the data into the parser + hrt_abstime now = hrt_absolute_time(); + + //if (partial_frame_count % 25 == 0) + //sbus_parse(now, frame, &partial_frame_count, rc_values, &num_values, &sbus_failsafe, &sbus_frame_drop, max_channels); + } + + if (ret == EOF) { + warnx("Test finished, reached end of file"); + } else { + warnx("Test aborted, errno: %d", ret); + } + +} From 7b0ac1db8583f3cd553e8dbcb4b338180ea5fb8b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 6 Oct 2014 07:53:32 +0200 Subject: [PATCH 286/411] Fix compile error in parser --- src/lib/rc/st24.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/rc/st24.c b/src/lib/rc/st24.c index 956a72bf35..2ae5765328 100644 --- a/src/lib/rc/st24.c +++ b/src/lib/rc/st24.c @@ -156,7 +156,7 @@ uint8_t st24_decode(uint8_t byte, uint8_t *rssi, uint8_t* rx_count, uint16_t *ch case ST24_PACKET_TYPE_CHANNELDATA24: { - ChannelData24* d = (ChannelData12*)&_rxpacket; + ChannelData24* d = (ChannelData24*)&_rxpacket; *rssi = d->rssi; *rx_count = d->packet_count; From b4c188cf19097a26c49c4b0235b11ae939554aa0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 6 Oct 2014 13:41:29 +0200 Subject: [PATCH 287/411] Fix file location for st24 test --- Tools/tests-host/Makefile | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Tools/tests-host/Makefile b/Tools/tests-host/Makefile index 5ee0c94326..f44f18be68 100644 --- a/Tools/tests-host/Makefile +++ b/Tools/tests-host/Makefile @@ -24,7 +24,7 @@ ST24_FILES=../../src/lib/rc/st24.c \ hrt.cpp \ st24_test.cpp -AUTODECLINATION_FILES= ../../src/lib/geo/geo_mag_declination.c \ +AUTODECLINATION_FILES= ../../src/lib/geo_lookup/geo_mag_declination.c \ hrt.cpp \ autodeclination_test.cpp From 6bf005e9019939d266da80f60aeeb126b66935de Mon Sep 17 00:00:00 2001 From: Denis Yeldandi Date: Mon, 6 Oct 2014 17:56:56 +0400 Subject: [PATCH 288/411] PASHS,POP,20 is needed for 20Hz configuration --- src/drivers/gps/ashtech.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/drivers/gps/ashtech.cpp b/src/drivers/gps/ashtech.cpp index b37f70ad1e..d40b12721f 100644 --- a/src/drivers/gps/ashtech.cpp +++ b/src/drivers/gps/ashtech.cpp @@ -594,7 +594,8 @@ void ASHTECH::decode_init(void){ //ashtech boad configuration script //char comm[] = "$PASHS,NME,GGA,A,ON,0.1\r\n" -char comm[] = "$PASHS,NME,ZDA,B,ON,3\r\n"\ +char comm[] = "$PASHS,POP,20\r\n"\ + "$PASHS,NME,ZDA,B,ON,3\r\n"\ "$PASHS,NME,GGA,B,OFF\r\n"\ "$PASHS,NME,GST,B,ON,3\r\n"\ "$PASHS,NME,POS,B,ON,0.05\r\n"\ From 34e75672bbf9a0e0e12d6a27abac5e1fcb3a0339 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 6 Oct 2014 19:20:02 +0200 Subject: [PATCH 289/411] SBUS2 fix --- Tools/tests-host/sbus2_test.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/Tools/tests-host/sbus2_test.cpp b/Tools/tests-host/sbus2_test.cpp index d8fcb695d8..e2c18369c0 100644 --- a/Tools/tests-host/sbus2_test.cpp +++ b/Tools/tests-host/sbus2_test.cpp @@ -29,7 +29,8 @@ int main(int argc, char *argv[]) { // Trash the first 20 lines for (unsigned i = 0; i < 20; i++) { - (void)fscanf(fp, "%f,%x,,", &f, &x); + char buf[200]; + (void)fgets(buf, sizeof(buf), fp); } // Init the parser From 966688d0921e397f7a19c8e9f3c9fd40b9561bba Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 6 Oct 2014 19:20:17 +0200 Subject: [PATCH 290/411] Fixed ST24 test --- Tools/tests-host/st24_test.cpp | 24 +++++++++++++++++++----- 1 file changed, 19 insertions(+), 5 deletions(-) diff --git a/Tools/tests-host/st24_test.cpp b/Tools/tests-host/st24_test.cpp index 680791ca8a..e1134bba63 100644 --- a/Tools/tests-host/st24_test.cpp +++ b/Tools/tests-host/st24_test.cpp @@ -4,7 +4,7 @@ #include #include #include -#include +#include #include "../../src/systemcmds/tests/tests.h" int main(int argc, char *argv[]) { @@ -28,7 +28,8 @@ int main(int argc, char *argv[]) { // Trash the first 20 lines for (unsigned i = 0; i < 20; i++) { - (void)fscanf(fp, "%f,%x,,", &f, &x); + char buf[200]; + (void)fgets(buf, sizeof(buf), fp); } float last_time = 0; @@ -38,15 +39,28 @@ int main(int argc, char *argv[]) { warnx("FRAME RESET\n\n"); } - warnx("%f: 0x%02x", (double)f, x); + uint8_t b = static_cast(x); last_time = f; // Pipe the data into the parser hrt_abstime now = hrt_absolute_time(); - //if (partial_frame_count % 25 == 0) - //sbus_parse(now, frame, &partial_frame_count, rc_values, &num_values, &sbus_failsafe, &sbus_frame_drop, max_channels); + uint8_t rssi; + uint8_t rx_count; + uint16_t channel_count; + int16_t channels[20]; + + + if (!st24_decode(b, &rssi, &rx_count, &channel_count, channels, sizeof(channels) / sizeof(channels[0]))) + { + //warnx("decoded: %u channels", (unsigned)channel_count); + for (unsigned i = 0; i < channel_count; i++) { + warnx("channel %u: %d", i, static_cast(channels[i])); + } + } + + //warnx("%f: 0x%02x >> RSSI: %u #: %u", (double)f, x, static_cast(rssi), static_cast(rx_count)); } if (ret == EOF) { From debff9e1796fe035975307dfc9d3e4ed081e3ded Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 6 Oct 2014 19:20:39 +0200 Subject: [PATCH 291/411] Updates to ST24 decoding library --- src/lib/rc/st24.c | 86 ++++++++++++++++++++++++++++++++++------------- src/lib/rc/st24.h | 2 +- 2 files changed, 64 insertions(+), 24 deletions(-) diff --git a/src/lib/rc/st24.c b/src/lib/rc/st24.c index 2ae5765328..f43f2de7ba 100644 --- a/src/lib/rc/st24.c +++ b/src/lib/rc/st24.c @@ -40,10 +40,11 @@ */ #include +#include #include "st24.h" enum ST24_DECODE_STATE { - ST24_DECODE_STATE_UNSYNCED, + ST24_DECODE_STATE_UNSYNCED = 0, ST24_DECODE_STATE_GOT_STX1, ST24_DECODE_STATE_GOT_STX2, ST24_DECODE_STATE_GOT_LEN, @@ -51,6 +52,24 @@ enum ST24_DECODE_STATE { ST24_DECODE_STATE_GOT_DATA }; +const char* decode_states[] = {"UNSYNCED", + "GOT_STX1", + "GOT_STX2", + "GOT_LEN", + "GOT_TYPE", + "GOT_DATA"}; + +/* define range mapping here, -+100% -> 1000..2000 */ +#define ST24_RANGE_MIN 0.0f +#define ST24_RANGE_MAX 8000.0f + +#define ST24_TARGET_MIN 1000.0f +#define ST24_TARGET_MAX 2000.0f + +/* pre-calculate the floating point stuff as far as possible at compile time */ +#define ST24_SCALE_FACTOR ((ST24_TARGET_MAX - ST24_TARGET_MIN) / (ST24_RANGE_MAX - ST24_RANGE_MIN)) +#define ST24_SCALE_OFFSET (int)(ST24_TARGET_MIN - (ST24_SCALE_FACTOR * ST24_RANGE_MIN + 0.5f)) + static enum ST24_DECODE_STATE _decode_state = ST24_DECODE_STATE_UNSYNCED; static unsigned _rxlen; @@ -83,7 +102,7 @@ uint8_t st24_common_crc8(uint8_t *ptr, uint8_t len) } -uint8_t st24_decode(uint8_t byte, uint8_t *rssi, uint8_t* rx_count, uint16_t *channels, uint16_t max_chan_count) +uint8_t st24_decode(uint8_t byte, uint8_t *rssi, uint8_t* rx_count, uint16_t *channel_count, int16_t *channels, uint16_t max_chan_count) { bool ret = false; @@ -104,9 +123,14 @@ uint8_t st24_decode(uint8_t byte, uint8_t *rssi, uint8_t* rx_count, uint16_t *ch break; case ST24_DECODE_STATE_GOT_STX2: - _rxpacket.length = byte; - _rxlen = 0; - _decode_state = ST24_DECODE_STATE_GOT_LEN; + /* ensure no data overflow failure or hack is possible */ + if ((unsigned)byte <= sizeof(_rxpacket.length) + sizeof(_rxpacket.type) + sizeof(_rxpacket.st24_data)) { + _rxpacket.length = byte; + _rxlen = 0; + _decode_state = ST24_DECODE_STATE_GOT_LEN; + } else { + _decode_state = ST24_DECODE_STATE_UNSYNCED; + } break; case ST24_DECODE_STATE_GOT_LEN: @@ -116,10 +140,9 @@ uint8_t st24_decode(uint8_t byte, uint8_t *rssi, uint8_t* rx_count, uint16_t *ch break; case ST24_DECODE_STATE_GOT_TYPE: - if (_rxlen < (_rxpacket.length - 1)) { - _rxpacket.st24_data[_rxlen] = byte; - _rxlen++; - } else { + _rxpacket.st24_data[_rxlen - 1] = byte; + _rxlen++; + if (_rxlen == (_rxpacket.length - 1)) { _decode_state = ST24_DECODE_STATE_GOT_DATA; } break; @@ -128,8 +151,7 @@ uint8_t st24_decode(uint8_t byte, uint8_t *rssi, uint8_t* rx_count, uint16_t *ch _rxpacket.crc8 = byte; _rxlen++; - if (st24_common_crc8((uint8_t*)&(_rxpacket.length), sizeof(_rxpacket.length) + - sizeof(_rxpacket.st24_data) + sizeof(_rxpacket.type)) == _rxpacket.crc8) { + if (st24_common_crc8((uint8_t*)&(_rxpacket.length), _rxlen) == _rxpacket.crc8) { ret = true; @@ -143,13 +165,18 @@ uint8_t st24_decode(uint8_t byte, uint8_t *rssi, uint8_t* rx_count, uint16_t *ch *rssi = d->rssi; *rx_count = d->packet_count; + *channel_count = 12; - for (unsigned i = 0; i < 1; i += 2) { - channels[i] = ((uint16_t)d->channel[i]) << 8; - channels[i] |= (0xF & d->channel[i+1]); + for (unsigned i = 0; i < *channel_count; i += 2) { + channels[i] = ((uint16_t)d->channel[i]) << 4; + channels[i] |= ((uint16_t)(0xF0 & d->channel[i+1]) >> 4); - channels[i+1] = ((uint16_t)(0xF & d->channel[i+1])) << 4; - channels[i+1] |= d->channel[i+2]; + channels[i+1] = ((uint16_t)d->channel[i+2] << 4); + channels[i+1] |= ((uint16_t)(0x0F & d->channel[i+1])); + + /* convert values to 1000-2000 ppm encoding in a not too sloppy fashion */ + // channels[i] = (uint16_t)(channels[i] * ST24_SCALE_FACTOR +.5f) + ST24_SCALE_OFFSET; + // channels[i+1] = (uint16_t)(channels[i+1] * ST24_SCALE_FACTOR +.5f) + ST24_SCALE_OFFSET; } } break; @@ -160,17 +187,29 @@ uint8_t st24_decode(uint8_t byte, uint8_t *rssi, uint8_t* rx_count, uint16_t *ch *rssi = d->rssi; *rx_count = d->packet_count; + *channel_count = 24; - for (unsigned i = 0; i < 1; i += 2) { - channels[i] = ((uint16_t)d->channel[i]) << 8; - channels[i] |= (0xF & d->channel[i+1]); + for (unsigned i = 0; i < *channel_count; i += 2) { + channels[i] = ((uint16_t)d->channel[i]) << 4; + channels[i] |= ((uint16_t)(0xF0 & d->channel[i+1]) >> 4); - channels[i+1] = ((uint16_t)(0xF & d->channel[i+1])) << 4; - channels[i+1] |= d->channel[i+2]; + channels[i+1] = ((uint16_t)d->channel[i+2] << 4); + channels[i+1] |= ((uint16_t)(0x0F & d->channel[i+1])); + + // XXX apply scaling } } break; + case ST24_PACKET_TYPE_TRANSMITTERGPSDATA: + { + + ReceiverFcPacket* d = (ReceiverFcPacket*)&_rxpacket; + /* we silently ignore this data for now, as its not classic TX data */ + ret = false; + } + break; + default: ret = false; break; @@ -178,10 +217,11 @@ uint8_t st24_decode(uint8_t byte, uint8_t *rssi, uint8_t* rx_count, uint16_t *ch } else { /* decoding failed */ - _decode_state = ST24_DECODE_STATE_UNSYNCED; + } + _decode_state = ST24_DECODE_STATE_UNSYNCED; break; } - return ret; + return !ret; } diff --git a/src/lib/rc/st24.h b/src/lib/rc/st24.h index 24dbf8e51a..45a835f77c 100644 --- a/src/lib/rc/st24.h +++ b/src/lib/rc/st24.h @@ -157,6 +157,6 @@ uint8_t st24_common_crc8(uint8_t *ptr, uint8_t len); * @param max_chan_count maximum channels to decode - if more channels are decoded, the last n are skipped and success (0) is returned * @return 0 for success (a decoded packet), 1 for no packet yet (accumulating), 3 for out of sync, 4 for checksum error */ -__EXPORT uint8_t st24_decode(uint8_t byte, uint8_t *rssi, uint8_t* rx_count, uint16_t *channels, uint16_t max_chan_count); +__EXPORT uint8_t st24_decode(uint8_t byte, uint8_t *rssi, uint8_t* rx_count, uint16_t* channel_count, int16_t *channels, uint16_t max_chan_count); __END_DECLS From 9c89499696baa15f709d125e46866e9f11ad5432 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 7 Oct 2014 07:14:28 +0200 Subject: [PATCH 292/411] Fix up ST24 lib --- Tools/tests-host/st24_test.cpp | 9 +++++-- src/lib/rc/st24.c | 46 +++++++++++++++++++++------------- 2 files changed, 36 insertions(+), 19 deletions(-) diff --git a/Tools/tests-host/st24_test.cpp b/Tools/tests-host/st24_test.cpp index e1134bba63..f143542bd0 100644 --- a/Tools/tests-host/st24_test.cpp +++ b/Tools/tests-host/st24_test.cpp @@ -36,7 +36,7 @@ int main(int argc, char *argv[]) { while (EOF != (ret = fscanf(fp, "%f,%x,,", &f, &x))) { if (((f - last_time) * 1000 * 1000) > 3000) { - warnx("FRAME RESET\n\n"); + //warnx("FRAME RESET\n\n"); } uint8_t b = static_cast(x); @@ -56,8 +56,13 @@ int main(int argc, char *argv[]) { { //warnx("decoded: %u channels", (unsigned)channel_count); for (unsigned i = 0; i < channel_count; i++) { - warnx("channel %u: %d", i, static_cast(channels[i])); + + int16_t val = channels[i]; + // if (i == 6) + warnx("channel %u: %d 0x%03X", i, static_cast(val), static_cast(val)); } + // unsigned chan = 1; + // warnx("channel %u: %d", chan, static_cast(channels[chan])); } //warnx("%f: 0x%02x >> RSSI: %u #: %u", (double)f, x, static_cast(rssi), static_cast(rx_count)); diff --git a/src/lib/rc/st24.c b/src/lib/rc/st24.c index f43f2de7ba..5a13649b5a 100644 --- a/src/lib/rc/st24.c +++ b/src/lib/rc/st24.c @@ -61,7 +61,7 @@ const char* decode_states[] = {"UNSYNCED", /* define range mapping here, -+100% -> 1000..2000 */ #define ST24_RANGE_MIN 0.0f -#define ST24_RANGE_MAX 8000.0f +#define ST24_RANGE_MAX 4096.0f #define ST24_TARGET_MIN 1000.0f #define ST24_TARGET_MAX 2000.0f @@ -161,42 +161,54 @@ uint8_t st24_decode(uint8_t byte, uint8_t *rssi, uint8_t* rx_count, uint16_t *ch case ST24_PACKET_TYPE_CHANNELDATA12: { - ChannelData12* d = (ChannelData12*)&_rxpacket; + ChannelData12* d = (ChannelData12*)_rxpacket.st24_data; *rssi = d->rssi; *rx_count = d->packet_count; *channel_count = 12; - for (unsigned i = 0; i < *channel_count; i += 2) { - channels[i] = ((uint16_t)d->channel[i]) << 4; - channels[i] |= ((uint16_t)(0xF0 & d->channel[i+1]) >> 4); - - channels[i+1] = ((uint16_t)d->channel[i+2] << 4); - channels[i+1] |= ((uint16_t)(0x0F & d->channel[i+1])); + unsigned stride_count = (*channel_count * 3) / 2; + unsigned chan_index = 0; + for (unsigned i = 0; i < stride_count; i += 3) { + channels[chan_index] = ((uint16_t)d->channel[i] << 4); + channels[chan_index] |= ((uint16_t)(0xF0 & d->channel[i+1]) >> 4); /* convert values to 1000-2000 ppm encoding in a not too sloppy fashion */ - // channels[i] = (uint16_t)(channels[i] * ST24_SCALE_FACTOR +.5f) + ST24_SCALE_OFFSET; - // channels[i+1] = (uint16_t)(channels[i+1] * ST24_SCALE_FACTOR +.5f) + ST24_SCALE_OFFSET; + channels[chan_index] = (uint16_t)(channels[chan_index] * ST24_SCALE_FACTOR +.5f) + ST24_SCALE_OFFSET; + chan_index++; + + channels[chan_index] = ((uint16_t)d->channel[i+2]); + channels[chan_index] |= (((uint16_t)(0x0F & d->channel[i+1])) << 8); + /* convert values to 1000-2000 ppm encoding in a not too sloppy fashion */ + channels[chan_index] = (uint16_t)(channels[chan_index] * ST24_SCALE_FACTOR +.5f) + ST24_SCALE_OFFSET; + chan_index++; } } break; case ST24_PACKET_TYPE_CHANNELDATA24: { - ChannelData24* d = (ChannelData24*)&_rxpacket; + ChannelData24* d = (ChannelData24*)&_rxpacket.st24_data; *rssi = d->rssi; *rx_count = d->packet_count; *channel_count = 24; - for (unsigned i = 0; i < *channel_count; i += 2) { - channels[i] = ((uint16_t)d->channel[i]) << 4; - channels[i] |= ((uint16_t)(0xF0 & d->channel[i+1]) >> 4); + unsigned stride_count = (*channel_count * 3) / 2; + unsigned chan_index = 0; - channels[i+1] = ((uint16_t)d->channel[i+2] << 4); - channels[i+1] |= ((uint16_t)(0x0F & d->channel[i+1])); + for (unsigned i = 0; i < stride_count; i += 3) { + channels[chan_index] = ((uint16_t)d->channel[i] << 4); + channels[chan_index] |= ((uint16_t)(0xF0 & d->channel[i+1]) >> 4); + /* convert values to 1000-2000 ppm encoding in a not too sloppy fashion */ + channels[chan_index] = (uint16_t)(channels[chan_index] * ST24_SCALE_FACTOR +.5f) + ST24_SCALE_OFFSET; + chan_index++; - // XXX apply scaling + channels[chan_index] = ((uint16_t)d->channel[i+2]); + channels[chan_index] |= (((uint16_t)(0x0F & d->channel[i+1])) << 8); + /* convert values to 1000-2000 ppm encoding in a not too sloppy fashion */ + channels[chan_index] = (uint16_t)(channels[chan_index] * ST24_SCALE_FACTOR +.5f) + ST24_SCALE_OFFSET; + chan_index++; } } break; From cb0cbe479ae4bdaab6b6570857d240940fea713d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 7 Oct 2014 08:05:32 +0200 Subject: [PATCH 293/411] Finalizing ST24 lib --- src/lib/rc/st24.c | 2 +- src/lib/rc/st24.h | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/lib/rc/st24.c b/src/lib/rc/st24.c index 5a13649b5a..31a1e3fd3d 100644 --- a/src/lib/rc/st24.c +++ b/src/lib/rc/st24.c @@ -102,7 +102,7 @@ uint8_t st24_common_crc8(uint8_t *ptr, uint8_t len) } -uint8_t st24_decode(uint8_t byte, uint8_t *rssi, uint8_t* rx_count, uint16_t *channel_count, int16_t *channels, uint16_t max_chan_count) +uint8_t st24_decode(uint8_t byte, uint8_t *rssi, uint8_t* rx_count, uint16_t *channel_count, uint16_t *channels, uint16_t max_chan_count) { bool ret = false; diff --git a/src/lib/rc/st24.h b/src/lib/rc/st24.h index 45a835f77c..6023a75076 100644 --- a/src/lib/rc/st24.h +++ b/src/lib/rc/st24.h @@ -157,6 +157,6 @@ uint8_t st24_common_crc8(uint8_t *ptr, uint8_t len); * @param max_chan_count maximum channels to decode - if more channels are decoded, the last n are skipped and success (0) is returned * @return 0 for success (a decoded packet), 1 for no packet yet (accumulating), 3 for out of sync, 4 for checksum error */ -__EXPORT uint8_t st24_decode(uint8_t byte, uint8_t *rssi, uint8_t* rx_count, uint16_t* channel_count, int16_t *channels, uint16_t max_chan_count); +__EXPORT uint8_t st24_decode(uint8_t byte, uint8_t *rssi, uint8_t* rx_count, uint16_t* channel_count, uint16_t *channels, uint16_t max_chan_count); __END_DECLS From c2687a777444f68e7a7685ec628848b950f02571 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 7 Oct 2014 08:05:45 +0200 Subject: [PATCH 294/411] ST24 integration in IO firmware --- src/modules/px4iofirmware/controls.c | 73 +++++++++++++++++----------- src/modules/px4iofirmware/dsm.c | 10 +++- src/modules/px4iofirmware/protocol.h | 1 + src/modules/px4iofirmware/px4io.h | 2 +- 4 files changed, 54 insertions(+), 32 deletions(-) diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c index 3aa3d28ff3..1b7be692ad 100644 --- a/src/modules/px4iofirmware/controls.c +++ b/src/modules/px4iofirmware/controls.c @@ -52,7 +52,7 @@ #define RC_CHANNEL_LOW_THRESH -8000 /* 10% threshold */ static bool ppm_input(uint16_t *values, uint16_t *num_values, uint16_t *frame_len); -static bool dsm_port_input(void); +static bool dsm_port_input(uint8_t *rssi); static perf_counter_t c_gather_dsm; static perf_counter_t c_gather_sbus; @@ -60,9 +60,49 @@ static perf_counter_t c_gather_ppm; static int _dsm_fd; -bool dsm_port_input() +bool dsm_port_input(uint8_t *rssi) { + perf_begin(c_gather_dsm); + uint16_t temp_count = r_raw_rc_count; + uint8_t n_bytes = 0; + uint8_t *bytes; + bool dsm_updated = dsm_input(r_raw_rc_values, &temp_count, &n_bytes, &bytes); + if (dsm_updated) { + r_raw_rc_flags |= PX4IO_P_STATUS_FLAGS_RC_DSM; + r_raw_rc_count = temp_count & 0x7fff; + if (temp_count & 0x8000) + r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_RC_DSM11; + else + r_raw_rc_flags &= ~PX4IO_P_RAW_RC_FLAGS_RC_DSM11; + + r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP); + r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE); + + } + perf_end(c_gather_dsm); + /* get data from FD and attempt to parse with DSM and ST24 libs */ + uint8_t st24_rssi, rx_count; + uint16_t st24_channel_count; + uint8_t st24_maxchans = 18; + + bool st24_updated = false; + + for (unsigned i = 0; i < n_bytes; i++) { + /* set updated flag if one complete packet was parsed */ + st24_updated |= st24_decode(bytes[i], &st24_rssi, &rx_count, + &st24_channel_count, r_raw_rc_values, st24_maxchans); + } + + if (st24_updated) { + + *rssi = st24_rssi; + r_raw_rc_count = st24_channel_count; + + r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_ST24; + r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP); + r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE); + } return false; } @@ -127,20 +167,7 @@ controls_tick() { #endif perf_begin(c_gather_dsm); - uint16_t temp_count = r_raw_rc_count; - bool dsm_updated = dsm_input(r_raw_rc_values, &temp_count); - if (dsm_updated) { - r_raw_rc_flags |= PX4IO_P_STATUS_FLAGS_RC_DSM; - r_raw_rc_count = temp_count & 0x7fff; - if (temp_count & 0x8000) - r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_RC_DSM11; - else - r_raw_rc_flags &= ~PX4IO_P_RAW_RC_FLAGS_RC_DSM11; - - r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP); - r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE); - - } + (void)dsm_port_input(&rssi); perf_end(c_gather_dsm); perf_begin(c_gather_sbus); @@ -186,18 +213,6 @@ controls_tick() { } perf_end(c_gather_ppm); - uint8_t st24_rssi, rx_count; - uint8_t st24_maxchans = 18; - bool st24_updated = st24_decode(0, &st24_rssi, &rx_count, r_raw_rc_values, st24_maxchans); - if (st24_updated) { - - rssi = st24_rssi; - - r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_PPM; - r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP); - r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE); - } - /* limit number of channels to allowable data size */ if (r_raw_rc_count > PX4IO_RC_INPUT_CHANNELS) r_raw_rc_count = PX4IO_RC_INPUT_CHANNELS; @@ -402,7 +417,7 @@ controls_tick() { r_status_flags |= PX4IO_P_STATUS_FLAGS_OVERRIDE; /* mix new RC input control values to servos */ - if (dsm_updated || sbus_updated || ppm_updated) + if (dsm_updated || sbus_updated || ppm_updated || st24_updated) mixer_tick(); } else { diff --git a/src/modules/px4iofirmware/dsm.c b/src/modules/px4iofirmware/dsm.c index d3f3658228..6e57c9ec75 100644 --- a/src/modules/px4iofirmware/dsm.c +++ b/src/modules/px4iofirmware/dsm.c @@ -452,10 +452,12 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values) * * @param[out] values pointer to per channel array of decoded values * @param[out] num_values pointer to number of raw channel values returned, high order bit 0:10 bit data, 1:11 bit data + * @param[out] n_butes number of bytes read + * @param[out] bytes pointer to the buffer of read bytes * @return true=decoded raw channel values updated, false=no update */ bool -dsm_input(uint16_t *values, uint16_t *num_values) +dsm_input(uint16_t *values, uint16_t *num_values, uint8_t *n_bytes, uint8_t **bytes) { ssize_t ret; hrt_abstime now; @@ -478,8 +480,12 @@ dsm_input(uint16_t *values, uint16_t *num_values) ret = read(dsm_fd, &dsm_frame[dsm_partial_frame_count], DSM_FRAME_SIZE - dsm_partial_frame_count); /* if the read failed for any reason, just give up here */ - if (ret < 1) + if (ret < 1) { return false; + } else { + *n_bytes = ret; + *bytes = &dsm_frame[dsm_partial_frame_count]; + } dsm_last_rx_time = now; diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h index 4739f6e400..e907d29593 100644 --- a/src/modules/px4iofirmware/protocol.h +++ b/src/modules/px4iofirmware/protocol.h @@ -112,6 +112,7 @@ #define PX4IO_P_STATUS_FLAGS_FAILSAFE (1 << 11) /* failsafe is active */ #define PX4IO_P_STATUS_FLAGS_SAFETY_OFF (1 << 12) /* safety is off */ #define PX4IO_P_STATUS_FLAGS_FMU_INITIALIZED (1 << 13) /* FMU was initialized and OK once */ +#define PX4IO_P_STATUS_FLAGS_RC_ST24 (1 << 14) /* SBUS input is valid */ #define PX4IO_P_STATUS_ALARMS 3 /* alarm flags - alarms latch, write 1 to a bit to clear it */ #define PX4IO_P_STATUS_ALARMS_VBATT_LOW (1 << 0) /* [1] VBatt is very close to regulator dropout */ diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h index b00a96717e..e32fcc72b6 100644 --- a/src/modules/px4iofirmware/px4io.h +++ b/src/modules/px4iofirmware/px4io.h @@ -215,7 +215,7 @@ extern uint16_t adc_measure(unsigned channel); extern void controls_init(void); extern void controls_tick(void); extern int dsm_init(const char *device); -extern bool dsm_input(uint16_t *values, uint16_t *num_values); +extern bool dsm_input(uint16_t *values, uint16_t *num_values, uint8_t *n_bytes, uint8_t **bytes); extern void dsm_bind(uint16_t cmd, int pulses); extern int sbus_init(const char *device); extern bool sbus_input(uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_channels); From 3fc064882f37919b21e0176a071a7a9430688987 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 7 Oct 2014 09:28:14 +0200 Subject: [PATCH 295/411] ST24 lib: formatting --- src/lib/rc/st24.c | 264 ++++++++++++++++++++++++---------------------- src/lib/rc/st24.h | 3 +- 2 files changed, 138 insertions(+), 129 deletions(-) diff --git a/src/lib/rc/st24.c b/src/lib/rc/st24.c index 31a1e3fd3d..bb42830db9 100644 --- a/src/lib/rc/st24.c +++ b/src/lib/rc/st24.c @@ -52,12 +52,13 @@ enum ST24_DECODE_STATE { ST24_DECODE_STATE_GOT_DATA }; -const char* decode_states[] = {"UNSYNCED", - "GOT_STX1", - "GOT_STX2", - "GOT_LEN", - "GOT_TYPE", - "GOT_DATA"}; +const char *decode_states[] = {"UNSYNCED", + "GOT_STX1", + "GOT_STX2", + "GOT_LEN", + "GOT_TYPE", + "GOT_DATA" + }; /* define range mapping here, -+100% -> 1000..2000 */ #define ST24_RANGE_MIN 0.0f @@ -102,137 +103,144 @@ uint8_t st24_common_crc8(uint8_t *ptr, uint8_t len) } -uint8_t st24_decode(uint8_t byte, uint8_t *rssi, uint8_t* rx_count, uint16_t *channel_count, uint16_t *channels, uint16_t max_chan_count) +uint8_t st24_decode(uint8_t byte, uint8_t *rssi, uint8_t *rx_count, uint16_t *channel_count, uint16_t *channels, + uint16_t max_chan_count) { bool ret = false; switch (_decode_state) { - case ST24_DECODE_STATE_UNSYNCED: - if (byte == ST24_STX1) { - _decode_state = ST24_DECODE_STATE_GOT_STX1; - } - break; + case ST24_DECODE_STATE_UNSYNCED: + if (byte == ST24_STX1) { + _decode_state = ST24_DECODE_STATE_GOT_STX1; + } - case ST24_DECODE_STATE_GOT_STX1: - if (byte == ST24_STX2) { - _decode_state = ST24_DECODE_STATE_GOT_STX2; - } else { - _decode_state = ST24_DECODE_STATE_UNSYNCED; - } - break; + break; - case ST24_DECODE_STATE_GOT_STX2: - /* ensure no data overflow failure or hack is possible */ - if ((unsigned)byte <= sizeof(_rxpacket.length) + sizeof(_rxpacket.type) + sizeof(_rxpacket.st24_data)) { - _rxpacket.length = byte; - _rxlen = 0; - _decode_state = ST24_DECODE_STATE_GOT_LEN; - } else { - _decode_state = ST24_DECODE_STATE_UNSYNCED; - } - break; + case ST24_DECODE_STATE_GOT_STX1: + if (byte == ST24_STX2) { + _decode_state = ST24_DECODE_STATE_GOT_STX2; - case ST24_DECODE_STATE_GOT_LEN: - _rxpacket.type = byte; - _rxlen++; - _decode_state = ST24_DECODE_STATE_GOT_TYPE; - break; - - case ST24_DECODE_STATE_GOT_TYPE: - _rxpacket.st24_data[_rxlen - 1] = byte; - _rxlen++; - if (_rxlen == (_rxpacket.length - 1)) { - _decode_state = ST24_DECODE_STATE_GOT_DATA; - } - break; - - case ST24_DECODE_STATE_GOT_DATA: - _rxpacket.crc8 = byte; - _rxlen++; - - if (st24_common_crc8((uint8_t*)&(_rxpacket.length), _rxlen) == _rxpacket.crc8) { - - ret = true; - - /* decode the actual packet */ - - switch (_rxpacket.type) { - - case ST24_PACKET_TYPE_CHANNELDATA12: - { - ChannelData12* d = (ChannelData12*)_rxpacket.st24_data; - - *rssi = d->rssi; - *rx_count = d->packet_count; - *channel_count = 12; - - unsigned stride_count = (*channel_count * 3) / 2; - unsigned chan_index = 0; - - for (unsigned i = 0; i < stride_count; i += 3) { - channels[chan_index] = ((uint16_t)d->channel[i] << 4); - channels[chan_index] |= ((uint16_t)(0xF0 & d->channel[i+1]) >> 4); - /* convert values to 1000-2000 ppm encoding in a not too sloppy fashion */ - channels[chan_index] = (uint16_t)(channels[chan_index] * ST24_SCALE_FACTOR +.5f) + ST24_SCALE_OFFSET; - chan_index++; - - channels[chan_index] = ((uint16_t)d->channel[i+2]); - channels[chan_index] |= (((uint16_t)(0x0F & d->channel[i+1])) << 8); - /* convert values to 1000-2000 ppm encoding in a not too sloppy fashion */ - channels[chan_index] = (uint16_t)(channels[chan_index] * ST24_SCALE_FACTOR +.5f) + ST24_SCALE_OFFSET; - chan_index++; - } - } - break; - - case ST24_PACKET_TYPE_CHANNELDATA24: - { - ChannelData24* d = (ChannelData24*)&_rxpacket.st24_data; - - *rssi = d->rssi; - *rx_count = d->packet_count; - *channel_count = 24; - - unsigned stride_count = (*channel_count * 3) / 2; - unsigned chan_index = 0; - - for (unsigned i = 0; i < stride_count; i += 3) { - channels[chan_index] = ((uint16_t)d->channel[i] << 4); - channels[chan_index] |= ((uint16_t)(0xF0 & d->channel[i+1]) >> 4); - /* convert values to 1000-2000 ppm encoding in a not too sloppy fashion */ - channels[chan_index] = (uint16_t)(channels[chan_index] * ST24_SCALE_FACTOR +.5f) + ST24_SCALE_OFFSET; - chan_index++; - - channels[chan_index] = ((uint16_t)d->channel[i+2]); - channels[chan_index] |= (((uint16_t)(0x0F & d->channel[i+1])) << 8); - /* convert values to 1000-2000 ppm encoding in a not too sloppy fashion */ - channels[chan_index] = (uint16_t)(channels[chan_index] * ST24_SCALE_FACTOR +.5f) + ST24_SCALE_OFFSET; - chan_index++; - } - } - break; - - case ST24_PACKET_TYPE_TRANSMITTERGPSDATA: - { - - ReceiverFcPacket* d = (ReceiverFcPacket*)&_rxpacket; - /* we silently ignore this data for now, as its not classic TX data */ - ret = false; - } - break; - - default: - ret = false; - break; - } - - } else { - /* decoding failed */ - - } + } else { _decode_state = ST24_DECODE_STATE_UNSYNCED; - break; + } + + break; + + case ST24_DECODE_STATE_GOT_STX2: + + /* ensure no data overflow failure or hack is possible */ + if ((unsigned)byte <= sizeof(_rxpacket.length) + sizeof(_rxpacket.type) + sizeof(_rxpacket.st24_data)) { + _rxpacket.length = byte; + _rxlen = 0; + _decode_state = ST24_DECODE_STATE_GOT_LEN; + + } else { + _decode_state = ST24_DECODE_STATE_UNSYNCED; + } + + break; + + case ST24_DECODE_STATE_GOT_LEN: + _rxpacket.type = byte; + _rxlen++; + _decode_state = ST24_DECODE_STATE_GOT_TYPE; + break; + + case ST24_DECODE_STATE_GOT_TYPE: + _rxpacket.st24_data[_rxlen - 1] = byte; + _rxlen++; + + if (_rxlen == (_rxpacket.length - 1)) { + _decode_state = ST24_DECODE_STATE_GOT_DATA; + } + + break; + + case ST24_DECODE_STATE_GOT_DATA: + _rxpacket.crc8 = byte; + _rxlen++; + + if (st24_common_crc8((uint8_t *) & (_rxpacket.length), _rxlen) == _rxpacket.crc8) { + + ret = true; + + /* decode the actual packet */ + + switch (_rxpacket.type) { + + case ST24_PACKET_TYPE_CHANNELDATA12: { + ChannelData12 *d = (ChannelData12 *)_rxpacket.st24_data; + + *rssi = d->rssi; + *rx_count = d->packet_count; + *channel_count = 12; + + unsigned stride_count = (*channel_count * 3) / 2; + unsigned chan_index = 0; + + for (unsigned i = 0; i < stride_count; i += 3) { + channels[chan_index] = ((uint16_t)d->channel[i] << 4); + channels[chan_index] |= ((uint16_t)(0xF0 & d->channel[i + 1]) >> 4); + /* convert values to 1000-2000 ppm encoding in a not too sloppy fashion */ + channels[chan_index] = (uint16_t)(channels[chan_index] * ST24_SCALE_FACTOR + .5f) + ST24_SCALE_OFFSET; + chan_index++; + + channels[chan_index] = ((uint16_t)d->channel[i + 2]); + channels[chan_index] |= (((uint16_t)(0x0F & d->channel[i + 1])) << 8); + /* convert values to 1000-2000 ppm encoding in a not too sloppy fashion */ + channels[chan_index] = (uint16_t)(channels[chan_index] * ST24_SCALE_FACTOR + .5f) + ST24_SCALE_OFFSET; + chan_index++; + } + } + break; + + case ST24_PACKET_TYPE_CHANNELDATA24: { + ChannelData24 *d = (ChannelData24 *)&_rxpacket.st24_data; + + *rssi = d->rssi; + *rx_count = d->packet_count; + *channel_count = 24; + + unsigned stride_count = (*channel_count * 3) / 2; + unsigned chan_index = 0; + + for (unsigned i = 0; i < stride_count; i += 3) { + channels[chan_index] = ((uint16_t)d->channel[i] << 4); + channels[chan_index] |= ((uint16_t)(0xF0 & d->channel[i + 1]) >> 4); + /* convert values to 1000-2000 ppm encoding in a not too sloppy fashion */ + channels[chan_index] = (uint16_t)(channels[chan_index] * ST24_SCALE_FACTOR + .5f) + ST24_SCALE_OFFSET; + chan_index++; + + channels[chan_index] = ((uint16_t)d->channel[i + 2]); + channels[chan_index] |= (((uint16_t)(0x0F & d->channel[i + 1])) << 8); + /* convert values to 1000-2000 ppm encoding in a not too sloppy fashion */ + channels[chan_index] = (uint16_t)(channels[chan_index] * ST24_SCALE_FACTOR + .5f) + ST24_SCALE_OFFSET; + chan_index++; + } + } + break; + + case ST24_PACKET_TYPE_TRANSMITTERGPSDATA: { + + // ReceiverFcPacket* d = (ReceiverFcPacket*)&_rxpacket.st24_data; + /* we silently ignore this data for now, as it is unused */ + ret = false; + } + break; + + default: + ret = false; + break; + } + + } else { + /* decoding failed */ + + } + + _decode_state = ST24_DECODE_STATE_UNSYNCED; + break; } return !ret; diff --git a/src/lib/rc/st24.h b/src/lib/rc/st24.h index 6023a75076..1876eabc5a 100644 --- a/src/lib/rc/st24.h +++ b/src/lib/rc/st24.h @@ -157,6 +157,7 @@ uint8_t st24_common_crc8(uint8_t *ptr, uint8_t len); * @param max_chan_count maximum channels to decode - if more channels are decoded, the last n are skipped and success (0) is returned * @return 0 for success (a decoded packet), 1 for no packet yet (accumulating), 3 for out of sync, 4 for checksum error */ -__EXPORT uint8_t st24_decode(uint8_t byte, uint8_t *rssi, uint8_t* rx_count, uint16_t* channel_count, uint16_t *channels, uint16_t max_chan_count); +__EXPORT uint8_t st24_decode(uint8_t byte, uint8_t *rssi, uint8_t *rx_count, uint16_t *channel_count, + uint16_t *channels, uint16_t max_chan_count); __END_DECLS From 35caa8bd996392924ab1d92eb46c3d40e0a91b28 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 7 Oct 2014 09:28:36 +0200 Subject: [PATCH 296/411] PX4IO Controls: compile fixes --- src/modules/px4iofirmware/controls.c | 19 ++++++++++--------- 1 file changed, 10 insertions(+), 9 deletions(-) diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c index 1b7be692ad..8670e30c2f 100644 --- a/src/modules/px4iofirmware/controls.c +++ b/src/modules/px4iofirmware/controls.c @@ -52,7 +52,7 @@ #define RC_CHANNEL_LOW_THRESH -8000 /* 10% threshold */ static bool ppm_input(uint16_t *values, uint16_t *num_values, uint16_t *frame_len); -static bool dsm_port_input(uint8_t *rssi); +static bool dsm_port_input(uint8_t *rssi, bool *dsm_updated, bool *st24_updated); static perf_counter_t c_gather_dsm; static perf_counter_t c_gather_sbus; @@ -60,14 +60,14 @@ static perf_counter_t c_gather_ppm; static int _dsm_fd; -bool dsm_port_input(uint8_t *rssi) +bool dsm_port_input(uint8_t *rssi, bool *dsm_updated, bool *st24_updated) { perf_begin(c_gather_dsm); uint16_t temp_count = r_raw_rc_count; uint8_t n_bytes = 0; uint8_t *bytes; - bool dsm_updated = dsm_input(r_raw_rc_values, &temp_count, &n_bytes, &bytes); - if (dsm_updated) { + *dsm_updated = dsm_input(r_raw_rc_values, &temp_count, &n_bytes, &bytes); + if (*dsm_updated) { r_raw_rc_flags |= PX4IO_P_STATUS_FLAGS_RC_DSM; r_raw_rc_count = temp_count & 0x7fff; if (temp_count & 0x8000) @@ -86,15 +86,15 @@ bool dsm_port_input(uint8_t *rssi) uint16_t st24_channel_count; uint8_t st24_maxchans = 18; - bool st24_updated = false; + *st24_updated = false; for (unsigned i = 0; i < n_bytes; i++) { /* set updated flag if one complete packet was parsed */ - st24_updated |= st24_decode(bytes[i], &st24_rssi, &rx_count, + *st24_updated |= st24_decode(bytes[i], &st24_rssi, &rx_count, &st24_channel_count, r_raw_rc_values, st24_maxchans); } - if (st24_updated) { + if (*st24_updated) { *rssi = st24_rssi; r_raw_rc_count = st24_channel_count; @@ -104,7 +104,7 @@ bool dsm_port_input(uint8_t *rssi) r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE); } - return false; + return (*dsm_updated | *st24_updated); } void @@ -167,7 +167,8 @@ controls_tick() { #endif perf_begin(c_gather_dsm); - (void)dsm_port_input(&rssi); + bool dsm_updated, st24_updated; + (void)dsm_port_input(&rssi, &dsm_updated, &st24_updated); perf_end(c_gather_dsm); perf_begin(c_gather_sbus); From c7e8570f8355cb96b4daf4f1d3283807ae0cbdc5 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 7 Oct 2014 09:36:24 +0200 Subject: [PATCH 297/411] PX4IO firmware: Fix comment --- src/modules/px4iofirmware/protocol.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h index e907d29593..89a470b44b 100644 --- a/src/modules/px4iofirmware/protocol.h +++ b/src/modules/px4iofirmware/protocol.h @@ -112,7 +112,7 @@ #define PX4IO_P_STATUS_FLAGS_FAILSAFE (1 << 11) /* failsafe is active */ #define PX4IO_P_STATUS_FLAGS_SAFETY_OFF (1 << 12) /* safety is off */ #define PX4IO_P_STATUS_FLAGS_FMU_INITIALIZED (1 << 13) /* FMU was initialized and OK once */ -#define PX4IO_P_STATUS_FLAGS_RC_ST24 (1 << 14) /* SBUS input is valid */ +#define PX4IO_P_STATUS_FLAGS_RC_ST24 (1 << 14) /* ST24 input is valid */ #define PX4IO_P_STATUS_ALARMS 3 /* alarm flags - alarms latch, write 1 to a bit to clear it */ #define PX4IO_P_STATUS_ALARMS_VBATT_LOW (1 << 0) /* [1] VBatt is very close to regulator dropout */ From 7921184e4081de18f92874df03a082c04510fa24 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 7 Oct 2014 09:36:42 +0200 Subject: [PATCH 298/411] IO input driver: Output ST24 as receiver type / status --- src/drivers/drv_rc_input.h | 3 ++- src/drivers/px4io/px4io.cpp | 10 +++++++++- 2 files changed, 11 insertions(+), 2 deletions(-) diff --git a/src/drivers/drv_rc_input.h b/src/drivers/drv_rc_input.h index 47fa8fa59a..b249c2a093 100644 --- a/src/drivers/drv_rc_input.h +++ b/src/drivers/drv_rc_input.h @@ -83,7 +83,8 @@ enum RC_INPUT_SOURCE { RC_INPUT_SOURCE_PX4FMU_PPM, RC_INPUT_SOURCE_PX4IO_PPM, RC_INPUT_SOURCE_PX4IO_SPEKTRUM, - RC_INPUT_SOURCE_PX4IO_SBUS + RC_INPUT_SOURCE_PX4IO_SBUS, + RC_INPUT_SOURCE_PX4IO_ST24 }; /** diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index fbb5d4f2e6..d212be7661 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1617,6 +1617,9 @@ PX4IO::io_publish_raw_rc() } else if (_status & PX4IO_P_STATUS_FLAGS_RC_SBUS) { rc_val.input_source = RC_INPUT_SOURCE_PX4IO_SBUS; + } else if (_status & PX4IO_P_STATUS_FLAGS_RC_ST24) { + rc_val.input_source = RC_INPUT_SOURCE_PX4IO_ST24; + } else { rc_val.input_source = RC_INPUT_SOURCE_UNKNOWN; @@ -1934,13 +1937,15 @@ PX4IO::print_status(bool extended_status) io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FREEMEM)); uint16_t flags = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS); uint16_t io_status_flags = flags; - printf("status 0x%04x%s%s%s%s%s%s%s%s%s%s%s%s\n", + printf("status 0x%04x%s%s%s%s%s%s%s%s%s%s%s%s%s%s\n", flags, ((flags & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED) ? " OUTPUTS_ARMED" : ""), ((flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) ? " SAFETY_OFF" : " SAFETY_SAFE"), ((flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) ? " OVERRIDE" : ""), ((flags & PX4IO_P_STATUS_FLAGS_RC_OK) ? " RC_OK" : " RC_FAIL"), ((flags & PX4IO_P_STATUS_FLAGS_RC_PPM) ? " PPM" : ""), + ((flags & PX4IO_P_STATUS_FLAGS_RC_DSM) ? " DSM" : ""), + ((flags & PX4IO_P_STATUS_FLAGS_RC_ST24) ? " ST24" : ""), ((flags & PX4IO_P_STATUS_FLAGS_RC_SBUS) ? " SBUS" : ""), ((flags & PX4IO_P_STATUS_FLAGS_FMU_OK) ? " FMU_OK" : " FMU_FAIL"), ((flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) ? " RAW_PWM_PASSTHROUGH" : ""), @@ -2465,6 +2470,9 @@ PX4IO::ioctl(file * filep, int cmd, unsigned long arg) } else if (status & PX4IO_P_STATUS_FLAGS_RC_SBUS) { rc_val->input_source = RC_INPUT_SOURCE_PX4IO_SBUS; + } else if (status & PX4IO_P_STATUS_FLAGS_RC_ST24) { + rc_val->input_source = RC_INPUT_SOURCE_PX4IO_ST24; + } else { rc_val->input_source = RC_INPUT_SOURCE_UNKNOWN; } From 7e5910bdbf10c5c00f438d17d1b0bcd00d6c020d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 7 Oct 2014 09:54:20 +0200 Subject: [PATCH 299/411] Formatted ST24 test code --- Tools/tests-host/st24_test.cpp | 31 +++++++++++++++---------------- 1 file changed, 15 insertions(+), 16 deletions(-) diff --git a/Tools/tests-host/st24_test.cpp b/Tools/tests-host/st24_test.cpp index f143542bd0..25a9355e2e 100644 --- a/Tools/tests-host/st24_test.cpp +++ b/Tools/tests-host/st24_test.cpp @@ -7,20 +7,23 @@ #include #include "../../src/systemcmds/tests/tests.h" -int main(int argc, char *argv[]) { +int main(int argc, char *argv[]) +{ warnx("ST24 test started"); - if (argc < 2) + if (argc < 2) { errx(1, "Need a filename for the input file"); + } warnx("loading data from: %s", argv[1]); FILE *fp; - - fp = fopen(argv[1],"rt"); - if (!fp) + fp = fopen(argv[1], "rt"); + + if (!fp) { errx(1, "failed opening file"); + } float f; unsigned x; @@ -36,7 +39,7 @@ int main(int argc, char *argv[]) { while (EOF != (ret = fscanf(fp, "%f,%x,,", &f, &x))) { if (((f - last_time) * 1000 * 1000) > 3000) { - //warnx("FRAME RESET\n\n"); + // warnx("FRAME RESET\n\n"); } uint8_t b = static_cast(x); @@ -49,27 +52,23 @@ int main(int argc, char *argv[]) { uint8_t rssi; uint8_t rx_count; uint16_t channel_count; - int16_t channels[20]; + uint16_t channels[20]; - if (!st24_decode(b, &rssi, &rx_count, &channel_count, channels, sizeof(channels) / sizeof(channels[0]))) - { - //warnx("decoded: %u channels", (unsigned)channel_count); + if (!st24_decode(b, &rssi, &rx_count, &channel_count, channels, sizeof(channels) / sizeof(channels[0]))) { + warnx("decoded: %u channels (converted to PPM range)", (unsigned)channel_count); + for (unsigned i = 0; i < channel_count; i++) { int16_t val = channels[i]; - // if (i == 6) - warnx("channel %u: %d 0x%03X", i, static_cast(val), static_cast(val)); + warnx("channel %u: %d 0x%03X", i, static_cast(val), static_cast(val)); } - // unsigned chan = 1; - // warnx("channel %u: %d", chan, static_cast(channels[chan])); } - - //warnx("%f: 0x%02x >> RSSI: %u #: %u", (double)f, x, static_cast(rssi), static_cast(rx_count)); } if (ret == EOF) { warnx("Test finished, reached end of file"); + } else { warnx("Test aborted, errno: %d", ret); } From ba2f55c3d7f5872aaf07e20b58b15df85417d43a Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 7 Oct 2014 10:09:03 +0200 Subject: [PATCH 300/411] Revert "increase ram" This reverts commit bc23b6239c50527aa550eed3bc6f17dec15c5c97. --- src/modules/mavlink/mavlink_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 25dc018447..34596320fe 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1630,7 +1630,7 @@ Mavlink::start(int argc, char *argv[]) task_spawn_cmd(buf, SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, - 5000, + 2700, (main_t)&Mavlink::start_helper, (const char **)argv); From c7f7de352d7e0f2921526e33077c6da6a46b404a Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 7 Oct 2014 10:12:56 +0200 Subject: [PATCH 301/411] revert some of the OBC rate changes --- src/modules/mavlink/mavlink_main.cpp | 17 +++++++---------- 1 file changed, 7 insertions(+), 10 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 34596320fe..4738d1ea71 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1382,28 +1382,25 @@ Mavlink::task_main(int argc, char *argv[]) _mission_manager->set_verbose(_verbose); LL_APPEND(_streams, _mission_manager); - - switch (_mode) { case MAVLINK_MODE_NORMAL: configure_stream("SYS_STATUS", 1.0f); configure_stream("GPS_GLOBAL_ORIGIN", 0.5f); configure_stream("HIGHRES_IMU", 1.0f); - configure_stream("ATTITUDE", 4.0f); - configure_stream("VFR_HUD", 4.0f); + configure_stream("ATTITUDE", 10.0f); + configure_stream("VFR_HUD", 8.0f); configure_stream("GPS_RAW_INT", 1.0f); - configure_stream("GLOBAL_POSITION_INT", 1.0f); - configure_stream("LOCAL_POSITION_NED", 1.0f); + configure_stream("GLOBAL_POSITION_INT", 3.0f); + configure_stream("LOCAL_POSITION_NED", 3.0f); configure_stream("RC_CHANNELS_RAW", 1.0f); - configure_stream("POSITION_TARGET_GLOBAL_INT", 1.0f); - configure_stream("ATTITUDE_TARGET", 1.0f); + configure_stream("POSITION_TARGET_GLOBAL_INT", 3.0f); + configure_stream("ATTITUDE_TARGET", 3.0f); configure_stream("DISTANCE_SENSOR", 0.5f); - //configure_stream("OPTICAL_FLOW", 20.0f); + configure_stream("OPTICAL_FLOW", 5.0f); break; case MAVLINK_MODE_ONBOARD: configure_stream("SYS_STATUS", 1.0f); - // XXX OBC change back: We need to be bandwidth-efficient here too configure_stream("ATTITUDE", 50.0f); configure_stream("GLOBAL_POSITION_INT", 50.0f); configure_stream("CAMERA_CAPTURE", 2.0f); From 72fbd76c84f8ecfc6844b3d23fa90ce0c3bc1227 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 7 Oct 2014 12:46:09 +0200 Subject: [PATCH 302/411] Updated and fixed parser for SF02/F laser sensor, test harness runs clean --- Tools/tests-host/.gitignore | 1 + Tools/tests-host/Makefile | 5 +- Tools/tests-host/sf0x_test | Bin 9228 -> 0 bytes Tools/tests-host/sf0x_test.cpp | 137 ++++++----------------------- src/drivers/sf0x/sf0x_parser.cpp | 143 +++++++++++++++++++++++++++++++ src/drivers/sf0x/sf0x_parser.h | 51 +++++++++++ 6 files changed, 222 insertions(+), 115 deletions(-) delete mode 100755 Tools/tests-host/sf0x_test create mode 100644 src/drivers/sf0x/sf0x_parser.cpp create mode 100644 src/drivers/sf0x/sf0x_parser.h diff --git a/Tools/tests-host/.gitignore b/Tools/tests-host/.gitignore index 87b314c614..ec11e0fec9 100644 --- a/Tools/tests-host/.gitignore +++ b/Tools/tests-host/.gitignore @@ -1,4 +1,5 @@ ./obj/* mixer_test +sf0x_test sbus2_test autodeclination_test diff --git a/Tools/tests-host/Makefile b/Tools/tests-host/Makefile index b1547c23f0..903c3a4ca1 100644 --- a/Tools/tests-host/Makefile +++ b/Tools/tests-host/Makefile @@ -22,9 +22,10 @@ SBUS2_FILES=../../src/modules/px4iofirmware/sbus.c \ SF0X_FILES= \ hrt.cpp \ - sf0x_test.cpp + sf0x_test.cpp \ + ../../src/drivers/sf0x/sf0x_parser.cpp -AUTODECLINATION_FILES= ../../src/lib/geo/geo_mag_declination.c \ +AUTODECLINATION_FILES= ../../src/lib/geo_lookup/geo_mag_declination.c \ hrt.cpp \ autodeclination_test.cpp diff --git a/Tools/tests-host/sf0x_test b/Tools/tests-host/sf0x_test deleted file mode 100755 index e56c35465423dc3abd2cccd3fdc118953c6a2cac..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 9228 zcmeHNQEVJX8UEH66UTAcJ5hoW6gF;Lxk==l5k(=b;gV)yD&mmFDN3V8leKrxckTJ^ z*xREv>IFxxB~HhcD-={Age)GAr~sWH52-+@r9`pI3xPL86#1b=YY8OFMZ)Bvi~iZ! zcyHYV9*}t1Px8+E|IU2#|Fi#`XXoEP-1+3{@@O_t@ z0ff9`t2wX(b77J5ic_7a+*L*S)*tZsJ||lQa)2Jm_aUk9`FPGMlr3A>+v)Gnhkd^1 zWqXpPsXn&ZH(4rHwj7Sywj6^#4Y*Dv{k=Rk6pkNoWm@ola96HlG|ihlg5V9jcgdGmiJ&?keAc*4I2 z1HhbkRvLsaJsNO0G)_hXS^#cpzzqDg&=0`f>pzkw9tLg#!q-!wegyhmpug}uMBxqq zAB`#jP*{`c`#X$Q;tJ^(>+>4it@`HG*frSF61Z1XDg01J$!7~oGs)hd_tn3`3w5_ zv((vd^1y-N5!vYxX%E}$&-bHJKDF29I}nmzmo~?U5hF&7|2>{z^>^9)-?WjX`VH2s zt-Igv0@R`IeF@+__2SriR-IVMszcomCxEQ_+)7q`>Fo_QDdY|H*_91-zrMMlo?Rj3 zo&i|5$L`&o5(g|z=->al;adevo(s$19JVfA%3e_cC&P5pEx zuqVmtwN00Mp2r%sP1g7#dvue{Ur(OD*6IY=e}x92?#B`IFSBO%t6czVJn;Jj6}wn{ zY;#%v2Q{tN*2VDkVsmy+7fF# zv+C|7mQm6FC;E>Ydb^u^uj}o#D>U=X(Ki)} zf^+)iw&K!jNnL+^ZOrq$OSM~`=Xt(|tU3AipI31WlOf^;ELR<&MxkVw`Qnsj8jhi5?b7rSZLk8{LDt9@ zEK@6$ts>ng4)hLIFqodv&|5a_ie(PaA(w(fmUklfmA`@3$6b%OHE z(@TmEn}SHYQ)-leri8RMoIeJZ&di8A^(YBsrhR7fs8?oJ(g@1f>PL zKhl$qOwfX@r83;I?HQ^Mr7}D_WjPfR(j?G{3JHf(89rq>PJY@dWldw2beU9!m+gGf z$&$oU8D4Shg5Um>FF7TX?ww6#_=|>Jq(K5G9NkE8?8(^~A114!XLGiAt$J?Z_x_t6-EXmg!4o0;_wF!1~9Kz+yiV zXw&HzeKWDArKFR8~rfqU2iYTt&h9 R304l3v}WXL*|wy8{2QZP(|rH{ diff --git a/Tools/tests-host/sf0x_test.cpp b/Tools/tests-host/sf0x_test.cpp index d41f7e54a6..ebc893b609 100644 --- a/Tools/tests-host/sf0x_test.cpp +++ b/Tools/tests-host/sf0x_test.cpp @@ -1,152 +1,63 @@ -#include #include #include #include +#include #include #include -enum SF0X_PARSE_STATE { - SF0X_PARSE_STATE0_UNSYNC = 0, - SF0X_PARSE_STATE1_SYNC, - SF0X_PARSE_STATE2_GOT_DIGIT0, - SF0X_PARSE_STATE3_GOT_DOT, - SF0X_PARSE_STATE4_GOT_DIGIT1, - SF0X_PARSE_STATE5_GOT_DIGIT2, - SF0X_PARSE_STATE6_GOT_CARRIAGE_RETURN -}; - -int sf0x_parser(char c, char *parserbuf, unsigned *parserbuf_index, enum *SF0X_PARSE_STATE state, float *dist) -{ - int ret = -1; - - unsigned len = strlen(parserbuf); - switch (state) { - case SF0X_PARSE_STATE0_UNSYNC: - if (c == '\n') { - *state = SF0X_PARSE_STATE1_SYNC; - *parserbuf_index = 0; - } - break; - - case SF0X_PARSE_STATE1_SYNC: - if (c >= '0' && c <= '9') { - *state = SF0X_PARSE_STATE2_GOT_DIGIT0; - parserbuf[parserbuf_index] = c; - parserbuf_index++; - } - break; - - case SF0X_PARSE_STATE2_GOT_DIGIT0: - if (c >= '0' && c <= '9') { - *state = SF0X_PARSE_STATE2_GOT_DIGIT0; - parserbuf[parserbuf_index] = c; - parserbuf_index++; - } else if (c == '.') { - *state = SF0X_PARSE_STATE3_GOT_DOT; - parserbuf[parserbuf_index] = c; - parserbuf_index++; - } else { - *state = SF0X_PARSE_STATE0_UNSYNC; - } - break; - - case SF0X_PARSE_STATE3_GOT_DOT: - if (c >= '0' && c <= '9') { - *state = SF0X_PARSE_STATE4_GOT_DIGIT1; - parserbuf[parserbuf_index] = c; - parserbuf_index++; - } else { - *state = SF0X_PARSE_STATE0_UNSYNC; - } - break; - - case SF0X_PARSE_STATE4_GOT_DIGIT1: - if (c >= '0' && c <= '9') { - *state = SF0X_PARSE_STATE4_GOT_DIGIT2; - parserbuf[parserbuf_index] = c; - parserbuf_index++; - } else { - *state = SF0X_PARSE_STATE0_UNSYNC; - } - break; - - case SF0X_PARSE_STATE5_GOT_DIGIT2: - if (c == '\r') { - *state = SF0X_PARSE_STATE6_GOT_CARRIAGE_RETURN; - } else { - *state = SF0X_PARSE_STATE0_UNSYNC; - } - break; - - case SF0X_PARSE_STATE6_GOT_CARRIAGE_RETURN: - if (c == '\n') { - parserbuf[parserbuf_index] = '\0'; - *dist = strtod(linebuf, &end); - *state = SF0X_PARSE_STATE0_SYNC; - *parserbuf_index = 0; - ret = 0; - } else { - *state = SF0X_PARSE_STATE0_UNSYNC; - } - break; - } - - - - return ret; -} +#include int main(int argc, char *argv[]) { warnx("SF0X test started"); - if (argc < 2) - errx(1, "Need a filename for the input file"); - - warnx("loading data from: %s", argv[1]); - - FILE *fp; - - fp = fopen(argv[1],"rt"); - - if (!fp) - errx(1, "failed opening file"); - int ret = 0; const char LINE_MAX = 20; char _linebuf[LINE_MAX]; _linebuf[0] = '\0'; - char *end; + const char *lines[] = {"0.01\r\n", + "0.02\r\n", + "0.03\r\n", + "0.04\r\n", + "0", + ".", + "0", + "5", + "\r", + "\n", + "0", + "3\r", + "\n" + "\r\n", + "0.06", + "\r\n" + }; enum SF0X_PARSE_STATE state = SF0X_PARSE_STATE0_UNSYNC; float dist_m; char _parserbuf[LINE_MAX]; unsigned _parsebuf_index = 0; - while (fgets(_linebuf, LINE_MAX, fp) != NULL) { + for (unsigned l = 0; l < sizeof(lines) / sizeof(lines[0]); l++) { printf("\n%s", _linebuf); int parse_ret; - for (int i = 0; i < strlen(_linebuf); i++) + for (int i = 0; i < strlen(lines[l]); i++) { - printf("%0x ", _linebuf[i]); - parse_ret = sf0x_parser(_linebuf[i], _parserbuf, &_parsebuf_index, &state, &dist_m); + parse_ret = sf0x_parser(lines[l][i], _parserbuf, &_parsebuf_index, &state, &dist_m); if (parse_ret == 0) { - printf("PARSED!"); + printf("\nparsed: %f %s\n", dist_m, (parse_ret == 0) ? "OK" : ""); } } + printf("%s", lines[l]); - printf("\nparsed: %f %s\n", dist_m, (parse_ret == 0) ? "OK" : ""); } - // Init the parser - - if (ret == EOF) { warnx("Test finished, reached end of file"); } else { diff --git a/src/drivers/sf0x/sf0x_parser.cpp b/src/drivers/sf0x/sf0x_parser.cpp new file mode 100644 index 0000000000..b9ceb52f53 --- /dev/null +++ b/src/drivers/sf0x/sf0x_parser.cpp @@ -0,0 +1,143 @@ +/**************************************************************************** + * + * Copyright (c) 2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file sf0x_parser.cpp + * @author Lorenz Meier + * + * Driver for the Lightware SF0x laser rangefinder series + */ + +#include "sf0x_parser.h" +#include +#include + +//#define SF0X_DEBUG + +#ifdef SF0X_DEBUG +#include + +const char* parser_state[] = { + "0_UNSYNC", + "1_SYNC", + "2_GOT_DIGIT0", + "3_GOT_DOT", + "4_GOT_DIGIT1", + "5_GOT_DIGIT2", + "6_GOT_CARRIAGE_RETURN" +}; +#endif + +int sf0x_parser(char c, char *parserbuf, unsigned *parserbuf_index, enum SF0X_PARSE_STATE *state, float *dist) +{ + int ret = -1; + char *end; + + unsigned len = strlen(parserbuf); + switch (*state) { + case SF0X_PARSE_STATE0_UNSYNC: + if (c == '\n') { + *state = SF0X_PARSE_STATE1_SYNC; + (*parserbuf_index) = 0; + } + break; + + case SF0X_PARSE_STATE1_SYNC: + if (c >= '0' && c <= '9') { + *state = SF0X_PARSE_STATE2_GOT_DIGIT0; + parserbuf[*parserbuf_index] = c; + (*parserbuf_index)++; + } + break; + + case SF0X_PARSE_STATE2_GOT_DIGIT0: + if (c >= '0' && c <= '9') { + *state = SF0X_PARSE_STATE2_GOT_DIGIT0; + parserbuf[*parserbuf_index] = c; + (*parserbuf_index)++; + } else if (c == '.') { + *state = SF0X_PARSE_STATE3_GOT_DOT; + parserbuf[*parserbuf_index] = c; + (*parserbuf_index)++; + } else { + *state = SF0X_PARSE_STATE0_UNSYNC; + } + break; + + case SF0X_PARSE_STATE3_GOT_DOT: + if (c >= '0' && c <= '9') { + *state = SF0X_PARSE_STATE4_GOT_DIGIT1; + parserbuf[*parserbuf_index] = c; + (*parserbuf_index)++; + } else { + *state = SF0X_PARSE_STATE0_UNSYNC; + } + break; + + case SF0X_PARSE_STATE4_GOT_DIGIT1: + if (c >= '0' && c <= '9') { + *state = SF0X_PARSE_STATE5_GOT_DIGIT2; + parserbuf[*parserbuf_index] = c; + (*parserbuf_index)++; + } else { + *state = SF0X_PARSE_STATE0_UNSYNC; + } + break; + + case SF0X_PARSE_STATE5_GOT_DIGIT2: + if (c == '\r') { + *state = SF0X_PARSE_STATE6_GOT_CARRIAGE_RETURN; + } else { + *state = SF0X_PARSE_STATE0_UNSYNC; + } + break; + + case SF0X_PARSE_STATE6_GOT_CARRIAGE_RETURN: + if (c == '\n') { + parserbuf[*parserbuf_index] = '\0'; + *dist = strtod(parserbuf, &end); + *state = SF0X_PARSE_STATE1_SYNC; + *parserbuf_index = 0; + ret = 0; + } else { + *state = SF0X_PARSE_STATE0_UNSYNC; + } + break; + } + +#ifdef SF0X_DEBUG + printf("state: SF0X_PARSE_STATE%s\n", parser_state[*state]); +#endif + + return ret; +} \ No newline at end of file diff --git a/src/drivers/sf0x/sf0x_parser.h b/src/drivers/sf0x/sf0x_parser.h new file mode 100644 index 0000000000..20892d50eb --- /dev/null +++ b/src/drivers/sf0x/sf0x_parser.h @@ -0,0 +1,51 @@ +/**************************************************************************** + * + * Copyright (c) 2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file sf0x_parser.cpp + * @author Lorenz Meier + * + * Declarations of parser for the Lightware SF0x laser rangefinder series + */ + +enum SF0X_PARSE_STATE { + SF0X_PARSE_STATE0_UNSYNC = 0, + SF0X_PARSE_STATE1_SYNC, + SF0X_PARSE_STATE2_GOT_DIGIT0, + SF0X_PARSE_STATE3_GOT_DOT, + SF0X_PARSE_STATE4_GOT_DIGIT1, + SF0X_PARSE_STATE5_GOT_DIGIT2, + SF0X_PARSE_STATE6_GOT_CARRIAGE_RETURN +}; + +int sf0x_parser(char c, char *parserbuf, unsigned *parserbuf_index, enum SF0X_PARSE_STATE *state, float *dist); \ No newline at end of file From 0078ba2a3bc224a30908fbcdf9c138101241dda5 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 7 Oct 2014 12:47:25 +0200 Subject: [PATCH 303/411] Removed bogus warnignn from test --- Tools/tests-host/sf0x_test.cpp | 6 +----- 1 file changed, 1 insertion(+), 5 deletions(-) diff --git a/Tools/tests-host/sf0x_test.cpp b/Tools/tests-host/sf0x_test.cpp index ebc893b609..6b98c2427f 100644 --- a/Tools/tests-host/sf0x_test.cpp +++ b/Tools/tests-host/sf0x_test.cpp @@ -58,11 +58,7 @@ int main(int argc, char *argv[]) { } - if (ret == EOF) { - warnx("Test finished, reached end of file"); - } else { - warnx("Test aborted, errno: %d", ret); - } + warnx("test finished"); return ret; } From 4183444de626698137ab3fbb413a96a806da3ade Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 7 Oct 2014 12:50:00 +0200 Subject: [PATCH 304/411] Changed to proper parser for SF02/F laser rangefinder --- src/drivers/sf0x/module.mk | 3 +- src/drivers/sf0x/sf0x.cpp | 89 ++++++-------------------------- src/drivers/sf0x/sf0x_parser.cpp | 1 - 3 files changed, 18 insertions(+), 75 deletions(-) diff --git a/src/drivers/sf0x/module.mk b/src/drivers/sf0x/module.mk index dc2c66d567..26e77d1cc6 100644 --- a/src/drivers/sf0x/module.mk +++ b/src/drivers/sf0x/module.mk @@ -37,6 +37,7 @@ MODULE_COMMAND = sf0x -SRCS = sf0x.cpp +SRCS = sf0x.cpp \ + sf0x_parser.cpp MAXOPTIMIZATION = -Os diff --git a/src/drivers/sf0x/sf0x.cpp b/src/drivers/sf0x/sf0x.cpp index d382d08d09..52f7413a90 100644 --- a/src/drivers/sf0x/sf0x.cpp +++ b/src/drivers/sf0x/sf0x.cpp @@ -72,6 +72,8 @@ #include +#include "sf0x_parser.h" + /* Configuration Constants */ /* oddly, ERROR is not defined for c++ */ @@ -120,6 +122,7 @@ private: int _fd; char _linebuf[10]; unsigned _linebuf_index; + enum SF0X_PARSE_STATE _parse_state; hrt_abstime _last_read; orb_advert_t _range_finder_topic; @@ -186,6 +189,7 @@ SF0X::SF0X(const char *port) : _collect_phase(false), _fd(-1), _linebuf_index(0), + _parse_state(SF0X_PARSE_STATE0_UNSYNC), _last_read(0), _range_finder_topic(-1), _consecutive_fail_count(0), @@ -526,14 +530,14 @@ SF0X::collect() } /* the buffer for read chars is buflen minus null termination */ - unsigned readlen = sizeof(_linebuf) - 1; + char readbuf[20]; + unsigned readlen = sizeof(readbuf) - 1; /* read from the sensor (uart buffer) */ - ret = ::read(_fd, &_linebuf[_linebuf_index], readlen - _linebuf_index); + ret = ::read(_fd, &readbuf[0], readlen); if (ret < 0) { - _linebuf[sizeof(_linebuf) - 1] = '\0'; - debug("read err: %d lbi: %d buf: %s", ret, (int)_linebuf_index, _linebuf); + debug("read err: %d", ret); perf_count(_comms_errors); perf_end(_sample_perf); @@ -548,84 +552,23 @@ SF0X::collect() return -EAGAIN; } - /* let the write pointer point to the next free entry */ - _linebuf_index += ret; - _last_read = hrt_absolute_time(); - /* require a reasonable amount of minimum bytes */ - if (_linebuf_index < 6) { - /* we need at this format: x.xx\r\n */ - return -EAGAIN; - - } else if (_linebuf[_linebuf_index - 2] != '\r' || _linebuf[_linebuf_index - 1] != '\n') { - - if (_linebuf_index == readlen) { - /* we have a full buffer, but no line ending - abort */ - _linebuf_index = 0; - perf_count(_comms_errors); - return -ENOMEM; - } else { - /* incomplete read, reschedule ourselves */ - return -EAGAIN; + float si_units; + bool valid = false; + + for (unsigned i = 0; i < ret; i++) { + if (OK == sf0x_parser(readbuf[i], _linebuf, &_linebuf_index, &_parse_state, &si_units)) { + valid = true; } } - char *end; - float si_units; - bool valid; - - /* enforce line ending */ - _linebuf[_linebuf_index] = '\0'; - - if (_linebuf[0] == '-' && _linebuf[1] == '-' && _linebuf[2] == '.') { - si_units = -1.0f; - valid = false; - - } else { - - /* we need to find a dot in the string, as we're missing the meters part else */ - valid = false; - - /* wipe out partially read content from last cycle(s), check for dot */ - for (unsigned i = 0; i < (_linebuf_index - 2); i++) { - if (_linebuf[i] == '\n') { - /* wipe out any partial measurements */ - for (unsigned j = 0; j <= i; j++) { - _linebuf[j] = ' '; - } - } - - /* we need a digit before the dot and a dot for a valid number */ - if (i > 0 && ((_linebuf[i - 1] >= '0') && (_linebuf[i - 1] <= '9')) && (_linebuf[i] == '.')) { - valid = true; - } - } - - if (valid) { - si_units = strtod(_linebuf, &end); - - /* we require at least four characters for a valid number */ - if (end > _linebuf + 3) { - valid = true; - } else { - si_units = -1.0f; - valid = false; - } - } + if (!valid) { + return -EAGAIN; } debug("val (float): %8.4f, raw: %s, valid: %s", (double)si_units, _linebuf, ((valid) ? "OK" : "NO")); - /* done with this chunk, resetting - even if invalid */ - _linebuf_index = 0; - - /* if its invalid, there is no reason to forward the value */ - if (!valid) { - perf_count(_comms_errors); - return -EINVAL; - } - struct range_finder_report report; /* this should be fairly close to the end of the measurement, so the best approximation of the time */ diff --git a/src/drivers/sf0x/sf0x_parser.cpp b/src/drivers/sf0x/sf0x_parser.cpp index b9ceb52f53..cddc3fe99c 100644 --- a/src/drivers/sf0x/sf0x_parser.cpp +++ b/src/drivers/sf0x/sf0x_parser.cpp @@ -63,7 +63,6 @@ int sf0x_parser(char c, char *parserbuf, unsigned *parserbuf_index, enum SF0X_PA int ret = -1; char *end; - unsigned len = strlen(parserbuf); switch (*state) { case SF0X_PARSE_STATE0_UNSYNC: if (c == '\n') { From 4d186e56eab98d79ca981a44ca099e172162d745 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 7 Oct 2014 12:51:19 +0200 Subject: [PATCH 305/411] Remove unused test data --- Tools/tests-host/sf0x_data.txt | 33 --------------------------------- 1 file changed, 33 deletions(-) delete mode 100644 Tools/tests-host/sf0x_data.txt diff --git a/Tools/tests-host/sf0x_data.txt b/Tools/tests-host/sf0x_data.txt deleted file mode 100644 index f61f59523e..0000000000 --- a/Tools/tests-host/sf0x_data.txt +++ /dev/null @@ -1,33 +0,0 @@ -0.02 -0.25 -0.03 -0.02 -0.25 -0.03 -0.02 -0.25 -0.03 -0.02 -0.25 -0.03 -0.25 -0.03 -0.02 -0.25 -0.03 -0.08 -0.40 -0.44 - - -0 -. -0 -1 - -0 -0. -.0 -.01 -01 -1 \ No newline at end of file From cebdae438d3f24075aab09275a537f02c5113b36 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 7 Oct 2014 12:51:50 +0200 Subject: [PATCH 306/411] Add missing newline --- Tools/tests-host/Makefile | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Tools/tests-host/Makefile b/Tools/tests-host/Makefile index 903c3a4ca1..5647de16d2 100644 --- a/Tools/tests-host/Makefile +++ b/Tools/tests-host/Makefile @@ -44,4 +44,4 @@ autodeclination_test: $(SBUS2_FILES) .PHONY: clean clean: - rm -f $(ODIR)/*.o *~ core $(INCDIR)/*~ mixer_test sbus2_test autodeclination_test sf0x_test \ No newline at end of file + rm -f $(ODIR)/*.o *~ core $(INCDIR)/*~ mixer_test sbus2_test autodeclination_test sf0x_test From 4ba4135c3b74f786b6ad795e2e9efd7271409b7f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 7 Oct 2014 12:52:48 +0200 Subject: [PATCH 307/411] Code style fixes, no code changes --- Tools/tests-host/sf0x_test.cpp | 39 ++++----- src/drivers/sf0x/sf0x_parser.cpp | 139 +++++++++++++++++-------------- 2 files changed, 96 insertions(+), 82 deletions(-) diff --git a/Tools/tests-host/sf0x_test.cpp b/Tools/tests-host/sf0x_test.cpp index 6b98c2427f..82d19fcbe1 100644 --- a/Tools/tests-host/sf0x_test.cpp +++ b/Tools/tests-host/sf0x_test.cpp @@ -8,7 +8,8 @@ #include -int main(int argc, char *argv[]) { +int main(int argc, char *argv[]) +{ warnx("SF0X test started"); int ret = 0; @@ -18,22 +19,22 @@ int main(int argc, char *argv[]) { _linebuf[0] = '\0'; const char *lines[] = {"0.01\r\n", - "0.02\r\n", - "0.03\r\n", - "0.04\r\n", - "0", - ".", - "0", - "5", - "\r", - "\n", - "0", - "3\r", - "\n" - "\r\n", - "0.06", - "\r\n" - }; + "0.02\r\n", + "0.03\r\n", + "0.04\r\n", + "0", + ".", + "0", + "5", + "\r", + "\n", + "0", + "3\r", + "\n" + "\r\n", + "0.06", + "\r\n" + }; enum SF0X_PARSE_STATE state = SF0X_PARSE_STATE0_UNSYNC; float dist_m; @@ -46,14 +47,14 @@ int main(int argc, char *argv[]) { int parse_ret; - for (int i = 0; i < strlen(lines[l]); i++) - { + for (int i = 0; i < strlen(lines[l]); i++) { parse_ret = sf0x_parser(lines[l][i], _parserbuf, &_parsebuf_index, &state, &dist_m); if (parse_ret == 0) { printf("\nparsed: %f %s\n", dist_m, (parse_ret == 0) ? "OK" : ""); } } + printf("%s", lines[l]); } diff --git a/src/drivers/sf0x/sf0x_parser.cpp b/src/drivers/sf0x/sf0x_parser.cpp index cddc3fe99c..8e73b0ad35 100644 --- a/src/drivers/sf0x/sf0x_parser.cpp +++ b/src/drivers/sf0x/sf0x_parser.cpp @@ -47,7 +47,7 @@ #ifdef SF0X_DEBUG #include -const char* parser_state[] = { +const char *parser_state[] = { "0_UNSYNC", "1_SYNC", "2_GOT_DIGIT0", @@ -64,74 +64,87 @@ int sf0x_parser(char c, char *parserbuf, unsigned *parserbuf_index, enum SF0X_PA char *end; switch (*state) { - case SF0X_PARSE_STATE0_UNSYNC: - if (c == '\n') { - *state = SF0X_PARSE_STATE1_SYNC; - (*parserbuf_index) = 0; - } - break; + case SF0X_PARSE_STATE0_UNSYNC: + if (c == '\n') { + *state = SF0X_PARSE_STATE1_SYNC; + (*parserbuf_index) = 0; + } - case SF0X_PARSE_STATE1_SYNC: - if (c >= '0' && c <= '9') { - *state = SF0X_PARSE_STATE2_GOT_DIGIT0; - parserbuf[*parserbuf_index] = c; - (*parserbuf_index)++; - } - break; + break; - case SF0X_PARSE_STATE2_GOT_DIGIT0: - if (c >= '0' && c <= '9') { - *state = SF0X_PARSE_STATE2_GOT_DIGIT0; - parserbuf[*parserbuf_index] = c; - (*parserbuf_index)++; - } else if (c == '.') { - *state = SF0X_PARSE_STATE3_GOT_DOT; - parserbuf[*parserbuf_index] = c; - (*parserbuf_index)++; - } else { - *state = SF0X_PARSE_STATE0_UNSYNC; - } - break; + case SF0X_PARSE_STATE1_SYNC: + if (c >= '0' && c <= '9') { + *state = SF0X_PARSE_STATE2_GOT_DIGIT0; + parserbuf[*parserbuf_index] = c; + (*parserbuf_index)++; + } - case SF0X_PARSE_STATE3_GOT_DOT: - if (c >= '0' && c <= '9') { - *state = SF0X_PARSE_STATE4_GOT_DIGIT1; - parserbuf[*parserbuf_index] = c; - (*parserbuf_index)++; - } else { - *state = SF0X_PARSE_STATE0_UNSYNC; - } - break; + break; - case SF0X_PARSE_STATE4_GOT_DIGIT1: - if (c >= '0' && c <= '9') { - *state = SF0X_PARSE_STATE5_GOT_DIGIT2; - parserbuf[*parserbuf_index] = c; - (*parserbuf_index)++; - } else { - *state = SF0X_PARSE_STATE0_UNSYNC; - } - break; + case SF0X_PARSE_STATE2_GOT_DIGIT0: + if (c >= '0' && c <= '9') { + *state = SF0X_PARSE_STATE2_GOT_DIGIT0; + parserbuf[*parserbuf_index] = c; + (*parserbuf_index)++; - case SF0X_PARSE_STATE5_GOT_DIGIT2: - if (c == '\r') { - *state = SF0X_PARSE_STATE6_GOT_CARRIAGE_RETURN; - } else { - *state = SF0X_PARSE_STATE0_UNSYNC; - } - break; + } else if (c == '.') { + *state = SF0X_PARSE_STATE3_GOT_DOT; + parserbuf[*parserbuf_index] = c; + (*parserbuf_index)++; - case SF0X_PARSE_STATE6_GOT_CARRIAGE_RETURN: - if (c == '\n') { - parserbuf[*parserbuf_index] = '\0'; - *dist = strtod(parserbuf, &end); - *state = SF0X_PARSE_STATE1_SYNC; - *parserbuf_index = 0; - ret = 0; - } else { - *state = SF0X_PARSE_STATE0_UNSYNC; - } - break; + } else { + *state = SF0X_PARSE_STATE0_UNSYNC; + } + + break; + + case SF0X_PARSE_STATE3_GOT_DOT: + if (c >= '0' && c <= '9') { + *state = SF0X_PARSE_STATE4_GOT_DIGIT1; + parserbuf[*parserbuf_index] = c; + (*parserbuf_index)++; + + } else { + *state = SF0X_PARSE_STATE0_UNSYNC; + } + + break; + + case SF0X_PARSE_STATE4_GOT_DIGIT1: + if (c >= '0' && c <= '9') { + *state = SF0X_PARSE_STATE5_GOT_DIGIT2; + parserbuf[*parserbuf_index] = c; + (*parserbuf_index)++; + + } else { + *state = SF0X_PARSE_STATE0_UNSYNC; + } + + break; + + case SF0X_PARSE_STATE5_GOT_DIGIT2: + if (c == '\r') { + *state = SF0X_PARSE_STATE6_GOT_CARRIAGE_RETURN; + + } else { + *state = SF0X_PARSE_STATE0_UNSYNC; + } + + break; + + case SF0X_PARSE_STATE6_GOT_CARRIAGE_RETURN: + if (c == '\n') { + parserbuf[*parserbuf_index] = '\0'; + *dist = strtod(parserbuf, &end); + *state = SF0X_PARSE_STATE1_SYNC; + *parserbuf_index = 0; + ret = 0; + + } else { + *state = SF0X_PARSE_STATE0_UNSYNC; + } + + break; } #ifdef SF0X_DEBUG From 760e72cc721c6a2b90515d37eba705c30be4928e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 7 Oct 2014 14:25:07 +0200 Subject: [PATCH 308/411] Remove unnecessary commands, unneeded output / resets --- src/drivers/sf0x/sf0x.cpp | 13 +------------ 1 file changed, 1 insertion(+), 12 deletions(-) diff --git a/src/drivers/sf0x/sf0x.cpp b/src/drivers/sf0x/sf0x.cpp index 52f7413a90..801bcf40a5 100644 --- a/src/drivers/sf0x/sf0x.cpp +++ b/src/drivers/sf0x/sf0x.cpp @@ -204,12 +204,6 @@ SF0X::SF0X(const char *port) : warnx("FAIL: laser fd"); } - /* tell it to stop auto-triggering */ - char stop_auto = ' '; - (void)::write(_fd, &stop_auto, 1); - usleep(100); - (void)::write(_fd, &stop_auto, 1); - struct termios uart_config; int termios_state; @@ -524,13 +518,8 @@ SF0X::collect() /* clear buffer if last read was too long ago */ uint64_t read_elapsed = hrt_elapsed_time(&_last_read); - /* timed out - retry */ - if (read_elapsed > (SF0X_CONVERSION_INTERVAL * 2)) { - _linebuf_index = 0; - } - /* the buffer for read chars is buflen minus null termination */ - char readbuf[20]; + char readbuf[sizeof(_linebuf)]; unsigned readlen = sizeof(readbuf) - 1; /* read from the sensor (uart buffer) */ From c58d845339c0d09fc703a1c730d89a7a00990906 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 7 Oct 2014 14:27:09 +0200 Subject: [PATCH 309/411] Remove range command --- src/drivers/sf0x/sf0x.cpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/src/drivers/sf0x/sf0x.cpp b/src/drivers/sf0x/sf0x.cpp index 801bcf40a5..11913b6cd7 100644 --- a/src/drivers/sf0x/sf0x.cpp +++ b/src/drivers/sf0x/sf0x.cpp @@ -494,6 +494,10 @@ SF0X::measure() /* * Send the command to begin a measurement. */ + + // XXX we probably should not try to talk to the sensor + // it seems to mind UART traffic. + #if 0 char cmd = SF0X_TAKE_RANGE_REG; ret = ::write(_fd, &cmd, 1); @@ -502,6 +506,7 @@ SF0X::measure() log("write fail %d", ret); return ret; } + #endif ret = OK; From 537991b83ca979c4a41465896cda789d9ebb6370 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 7 Oct 2014 14:48:41 +0200 Subject: [PATCH 310/411] Revert "Remove range command" This reverts commit c58d845339c0d09fc703a1c730d89a7a00990906. --- src/drivers/sf0x/sf0x.cpp | 5 ----- 1 file changed, 5 deletions(-) diff --git a/src/drivers/sf0x/sf0x.cpp b/src/drivers/sf0x/sf0x.cpp index 11913b6cd7..801bcf40a5 100644 --- a/src/drivers/sf0x/sf0x.cpp +++ b/src/drivers/sf0x/sf0x.cpp @@ -494,10 +494,6 @@ SF0X::measure() /* * Send the command to begin a measurement. */ - - // XXX we probably should not try to talk to the sensor - // it seems to mind UART traffic. - #if 0 char cmd = SF0X_TAKE_RANGE_REG; ret = ::write(_fd, &cmd, 1); @@ -506,7 +502,6 @@ SF0X::measure() log("write fail %d", ret); return ret; } - #endif ret = OK; From 6436db1a99888aff3802008c91f8a6c151f7da9c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 7 Oct 2014 22:24:32 +0200 Subject: [PATCH 311/411] Fix parser return type handling --- src/modules/px4iofirmware/controls.c | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c index 8670e30c2f..622f20c7d5 100644 --- a/src/modules/px4iofirmware/controls.c +++ b/src/modules/px4iofirmware/controls.c @@ -83,15 +83,14 @@ bool dsm_port_input(uint8_t *rssi, bool *dsm_updated, bool *st24_updated) /* get data from FD and attempt to parse with DSM and ST24 libs */ uint8_t st24_rssi, rx_count; - uint16_t st24_channel_count; - uint8_t st24_maxchans = 18; + uint16_t st24_channel_count = 0; *st24_updated = false; for (unsigned i = 0; i < n_bytes; i++) { /* set updated flag if one complete packet was parsed */ - *st24_updated |= st24_decode(bytes[i], &st24_rssi, &rx_count, - &st24_channel_count, r_raw_rc_values, st24_maxchans); + *st24_updated |= (OK == st24_decode(bytes[i], &st24_rssi, &rx_count, + &st24_channel_count, r_raw_rc_values, PX4IO_RC_INPUT_CHANNELS)); } if (*st24_updated) { From 726b10651aa38f1b542e6f3845da9aaf58af72a2 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 7 Oct 2014 22:25:03 +0200 Subject: [PATCH 312/411] ST24: Fix parser return values, update docs --- src/lib/rc/st24.c | 16 +++++++++------- src/lib/rc/st24.h | 4 ++-- 2 files changed, 11 insertions(+), 9 deletions(-) diff --git a/src/lib/rc/st24.c b/src/lib/rc/st24.c index bb42830db9..2b2178f97d 100644 --- a/src/lib/rc/st24.c +++ b/src/lib/rc/st24.c @@ -103,16 +103,18 @@ uint8_t st24_common_crc8(uint8_t *ptr, uint8_t len) } -uint8_t st24_decode(uint8_t byte, uint8_t *rssi, uint8_t *rx_count, uint16_t *channel_count, uint16_t *channels, +int st24_decode(uint8_t byte, uint8_t *rssi, uint8_t *rx_count, uint16_t *channel_count, uint16_t *channels, uint16_t max_chan_count) { - bool ret = false; + int ret = 1; switch (_decode_state) { case ST24_DECODE_STATE_UNSYNCED: if (byte == ST24_STX1) { _decode_state = ST24_DECODE_STATE_GOT_STX1; + } else { + ret = 3; } break; @@ -163,7 +165,7 @@ uint8_t st24_decode(uint8_t byte, uint8_t *rssi, uint8_t *rx_count, uint16_t *ch if (st24_common_crc8((uint8_t *) & (_rxpacket.length), _rxlen) == _rxpacket.crc8) { - ret = true; + ret = 0; /* decode the actual packet */ @@ -225,23 +227,23 @@ uint8_t st24_decode(uint8_t byte, uint8_t *rssi, uint8_t *rx_count, uint16_t *ch // ReceiverFcPacket* d = (ReceiverFcPacket*)&_rxpacket.st24_data; /* we silently ignore this data for now, as it is unused */ - ret = false; + ret = 2; } break; default: - ret = false; + ret = 2; break; } } else { /* decoding failed */ - + ret = 4; } _decode_state = ST24_DECODE_STATE_UNSYNCED; break; } - return !ret; + return ret; } diff --git a/src/lib/rc/st24.h b/src/lib/rc/st24.h index 1876eabc5a..454234601d 100644 --- a/src/lib/rc/st24.h +++ b/src/lib/rc/st24.h @@ -155,9 +155,9 @@ uint8_t st24_common_crc8(uint8_t *ptr, uint8_t len); * @param rx_count pointer to a byte where the receive count of packets signce last wireless frame is written back to * @param channels pointer to a datastructure of size max_chan_count where channel values (12 bit) are written back to * @param max_chan_count maximum channels to decode - if more channels are decoded, the last n are skipped and success (0) is returned - * @return 0 for success (a decoded packet), 1 for no packet yet (accumulating), 3 for out of sync, 4 for checksum error + * @return 0 for success (a decoded packet), 1 for no packet yet (accumulating), 2 for unknown packet, 3 for out of sync, 4 for checksum error */ -__EXPORT uint8_t st24_decode(uint8_t byte, uint8_t *rssi, uint8_t *rx_count, uint16_t *channel_count, +__EXPORT int st24_decode(uint8_t byte, uint8_t *rssi, uint8_t *rx_count, uint16_t *channel_count, uint16_t *channels, uint16_t max_chan_count); __END_DECLS From 8c6c08dcb5ce87e613cbf867571f219a60e1b813 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 7 Oct 2014 22:46:07 +0200 Subject: [PATCH 313/411] Limit channel count effectively --- src/lib/rc/st24.c | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/src/lib/rc/st24.c b/src/lib/rc/st24.c index 2b2178f97d..e8a791b8f6 100644 --- a/src/lib/rc/st24.c +++ b/src/lib/rc/st24.c @@ -176,7 +176,9 @@ int st24_decode(uint8_t byte, uint8_t *rssi, uint8_t *rx_count, uint16_t *channe *rssi = d->rssi; *rx_count = d->packet_count; - *channel_count = 12; + + /* this can lead to rounding of the strides */ + *channel_count = (max_chan_count < 12) ? max_chan_count : 12; unsigned stride_count = (*channel_count * 3) / 2; unsigned chan_index = 0; @@ -202,7 +204,9 @@ int st24_decode(uint8_t byte, uint8_t *rssi, uint8_t *rx_count, uint16_t *channe *rssi = d->rssi; *rx_count = d->packet_count; - *channel_count = 24; + + /* this can lead to rounding of the strides */ + *channel_count = (max_chan_count < 24) ? max_chan_count : 24; unsigned stride_count = (*channel_count * 3) / 2; unsigned chan_index = 0; From 9290e7b7f2e9b7e2f75b9d52c15a0c7f2064c146 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 8 Oct 2014 09:57:50 +0200 Subject: [PATCH 314/411] Added VTOL types --- mavlink/include/mavlink/v1.0 | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mavlink/include/mavlink/v1.0 b/mavlink/include/mavlink/v1.0 index 2423e47b4f..8587a007cf 160000 --- a/mavlink/include/mavlink/v1.0 +++ b/mavlink/include/mavlink/v1.0 @@ -1 +1 @@ -Subproject commit 2423e47b4f9169e77f7194b36fa2118e018c94e2 +Subproject commit 8587a007cf89cb13f146ac46be94b61276343ce1 From 94574f1a58735547a72c65556a683335fdf7ffbd Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 31 Jul 2014 10:40:50 +1000 Subject: [PATCH 315/411] lsm303d: don't use DRDY when not on internal SPI bus external SPI bus does not have accel DRDY connected Conflicts: mavlink/include/mavlink/v1.0 --- src/drivers/lsm303d/lsm303d.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 6880cf0f87..01c89192a7 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -1519,8 +1519,10 @@ LSM303D::measure() { // if the accel doesn't have any data ready then re-schedule // for 100 microseconds later. This ensures we don't double - // read a value and then miss the next value - if (stm32_gpioread(GPIO_EXTI_ACCEL_DRDY) == 0) { + // read a value and then miss the next value. + // Note that DRDY is not available when the lsm303d is + // connected on the external bus + if (_bus == PX4_SPI_BUS_SENSORS && stm32_gpioread(GPIO_EXTI_ACCEL_DRDY) == 0) { perf_count(_accel_reschedules); hrt_call_delay(&_accel_call, 100); return; From 0e51b99915ab5f69e723576d562a8b1c015bf14d Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Wed, 27 Aug 2014 21:34:48 +0900 Subject: [PATCH 316/411] device: initialise device id Conflicts: mavlink/include/mavlink/v1.0 --- src/drivers/device/device.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/drivers/device/device.cpp b/src/drivers/device/device.cpp index f1f777dce0..d46901683f 100644 --- a/src/drivers/device/device.cpp +++ b/src/drivers/device/device.cpp @@ -97,6 +97,7 @@ Device::Device(const char *name, /* setup a default device ID. When bus_type is UNKNOWN the other fields are invalid */ + _device_id.devid = 0; _device_id.devid_s.bus_type = DeviceBusType_UNKNOWN; _device_id.devid_s.bus = 0; _device_id.devid_s.address = 0; From b1887eee7910b2e85077fd99a9142fb9b1dfefcf Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 8 Oct 2014 12:28:03 +0200 Subject: [PATCH 317/411] Fix comment --- .../ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index 2c50e5c755..27c1f981e9 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -782,7 +782,7 @@ FixedwingEstimator::task_main() 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. */ From af783d4d98c596a4288ec9da7f3facfd6f281933 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Wed, 8 Oct 2014 20:52:38 +0400 Subject: [PATCH 318/411] UAVCAN update --- uavcan | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/uavcan b/uavcan index 286adbcc56..01d5bb242a 160000 --- a/uavcan +++ b/uavcan @@ -1 +1 @@ -Subproject commit 286adbcc56c4b093143b647ec7546abb149cd53b +Subproject commit 01d5bb242a6197e17e0ed466ed86e7ba42bd7d01 From 1bf1cec5672a7f30656e3fe44535ab0390bbd2d0 Mon Sep 17 00:00:00 2001 From: Denis Yeldandi Date: Wed, 8 Oct 2014 23:37:41 +0400 Subject: [PATCH 319/411] Corrected gps_fix values description --- src/modules/uORB/topics/vehicle_gps_position.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/uORB/topics/vehicle_gps_position.h b/src/modules/uORB/topics/vehicle_gps_position.h index 43909e2faf..31e616f4f7 100644 --- a/src/modules/uORB/topics/vehicle_gps_position.h +++ b/src/modules/uORB/topics/vehicle_gps_position.h @@ -62,7 +62,7 @@ struct vehicle_gps_position_s { uint64_t timestamp_variance; float s_variance_m_s; /**< speed accuracy estimate m/s */ float c_variance_rad; /**< course accuracy estimate rad */ - uint8_t fix_type; /**< 0-1: no fix, 2: 2D fix, 3: 3D fix, 4: Real-Time Kinematic, fixed integers, 5: Real-Time Kinematic, float integers, OmniSTAR XP/HP or Location RTK. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. */ + uint8_t fix_type; /**< 0-1: no fix, 2: 2D fix, 3: 3D fix, 4: RTCM code differential, 5: Real-Time Kinematic, float, 6: Real-Time Kinematic, fixed, 8: Extrapolated. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. */ float eph; /**< GPS HDOP horizontal dilution of position in m */ float epv; /**< GPS VDOP horizontal dilution of position in m */ From 09b5206404116b263feb50de18c54d53544c9114 Mon Sep 17 00:00:00 2001 From: Denis Yeldandi Date: Thu, 9 Oct 2014 11:15:11 +0400 Subject: [PATCH 320/411] Code style fix --- src/drivers/gps/ashtech.cpp | 995 ++++++++++++++++++++---------------- src/drivers/gps/ashtech.h | 4 +- src/drivers/gps/gps.cpp | 13 +- 3 files changed, 551 insertions(+), 461 deletions(-) diff --git a/src/drivers/gps/ashtech.cpp b/src/drivers/gps/ashtech.cpp index d40b12721f..7342d01d1b 100644 --- a/src/drivers/gps/ashtech.cpp +++ b/src/drivers/gps/ashtech.cpp @@ -18,282 +18,343 @@ typedef double float64_t; typedef float float32_t; -char* str_scanDec(const char *pos, int8_t sign, int8_t n_max_digit, int32_t *result) +char *str_scanDec(const char *pos, int8_t sign, int8_t n_max_digit, int32_t *result) { - int8_t n=0; - int32_t d=0; - int8_t neg = 0; + int8_t n = 0; + int32_t d = 0; + int8_t neg = 0; - if(*pos == '-') { neg = 1; pos++; } - else if(*pos == '+') { pos++; } - else if(sign) return(NULL); - while(*pos >= '0' && *pos <= '9') - { - d = d*10 + (*(pos++) - '0'); - n++; - if(n_max_digit>0 && n==n_max_digit) break; - } - if(n==0 || n>10) return(NULL); - if(neg) *result = -d; - else *result = d; - return((char *)pos); + if (*pos == '-') { neg = 1; pos++; } + + else if (*pos == '+') { pos++; } + + else if (sign) { return (NULL); } + + while (*pos >= '0' && *pos <= '9') { + d = d * 10 + (*(pos++) - '0'); + n++; + + if (n_max_digit > 0 && n == n_max_digit) { break; } + } + + if (n == 0 || n > 10) { return (NULL); } + + if (neg) { *result = -d; } + + else { *result = d; } + + return ((char *)pos); } -char* scanFloat64(const char *pos, int8_t sign, int8_t n_max_int, int8_t n_max_frac, - float64_t *result) +char *scanFloat64(const char *pos, int8_t sign, int8_t n_max_int, int8_t n_max_frac, + float64_t *result) { - float64_t f=0.0, div=1.0; - int32_t d_int; - int8_t n=0, isneg= 0; + float64_t f = 0.0, div = 1.0; + int32_t d_int; + int8_t n = 0, isneg = 0; - if( *pos == '-' ) isneg= 1; - if((pos = str_scanDec(pos, sign, n_max_int, &d_int)) == NULL) return(NULL); - if(*(pos) == '.') - { - pos++; - while(*pos >= '0' && *pos <= '9') - { - f = f*(10.0) + (float64_t)(*(pos++) - '0'); - div *= (0.1); - n++; - if(n_max_frac>0 && n==n_max_frac) break; - } - } - else if(n_max_frac > 0) return(NULL); - if( isneg ) *result = (float64_t)d_int - f*div; - else *result = (float64_t)d_int + f*div; - return((char *)pos); + if (*pos == '-') { isneg = 1; } + + if ((pos = str_scanDec(pos, sign, n_max_int, &d_int)) == NULL) { return (NULL); } + + if (*(pos) == '.') { + pos++; + + while (*pos >= '0' && *pos <= '9') { + f = f * (10.0) + (float64_t)(*(pos++) - '0'); + div *= (0.1); + n++; + + if (n_max_frac > 0 && n == n_max_frac) { break; } + } + + } else if (n_max_frac > 0) { return (NULL); } + + if (isneg) { *result = (float64_t)d_int - f * div; } + + else { *result = (float64_t)d_int + f * div; } + + return ((char *)pos); } ASHTECH::ASHTECH(const int &fd, struct vehicle_gps_position_s *gps_position, struct satellite_info_s *satellite_info): -_fd(fd), -_satellite_info(satellite_info), -_gps_position(gps_position) + _fd(fd), + _satellite_info(satellite_info), + _gps_position(gps_position) { - decode_init(); - _decode_state = NME_DECODE_UNINIT; - _rx_buffer_bytes = 0; + decode_init(); + _decode_state = NME_DECODE_UNINIT; + _rx_buffer_bytes = 0; } -ASHTECH::~ASHTECH(){ +ASHTECH::~ASHTECH() +{ } -//All NMEA descriptions are taken from -//http://www.trimble.com/OEM_ReceiverHelp/V4.44/en/NMEA-0183messages_MessageOverview.html -int ASHTECH::handle_message(int len){ - if(len < 7) return 0; - int uiCalcComma = 0; - for(int i = 0 ; i < len; i++){ - if(_rx_buffer[i] == ',')uiCalcComma++; - } - char* bufptr = (char*)( _rx_buffer+6); - if((memcmp(_rx_buffer+3, "ZDA,",3) == 0)&&(uiCalcComma == 6)){ - /* - UTC day, month, and year, and local time zone offset - An example of the ZDA message string is: +/* + * All NMEA descriptions are taken from + * http://www.trimble.com/OEM_ReceiverHelp/V4.44/en/NMEA-0183messages_MessageOverview.html + */ - $GPZDA,172809.456,12,07,1996,00,00*45 +int ASHTECH::handle_message(int len) +{ + if (len < 7) { return 0; } - ZDA message fields - Field Meaning - 0 Message ID $GPZDA - 1 UTC - 2 Day, ranging between 01 and 31 - 3 Month, ranging between 01 and 12 - 4 Year - 5 Local time zone offset from GMT, ranging from 00 through �13 hours - 6 Local time zone offset from GMT, ranging from 00 through 59 minutes - 7 The checksum data, always begins with * - Fields 5 and 6 together yield the total offset. For example, if field 5 is -5 and field 6 is +15, local time is 5 hours and 15 minutes earlier than GMT. - */ - float64_t ashtech_time = 0.0; - int day = 0, month = 0,year = 0,local_time_off_hour = 0,local_time_off_min = 0; - if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&ashtech_time); - if(bufptr && *(++bufptr) != ',') bufptr = str_scanDec(bufptr, 0, 9, &day); - if(bufptr && *(++bufptr) != ',') bufptr = str_scanDec(bufptr, 0, 9, &month); - if(bufptr && *(++bufptr) != ',') bufptr = str_scanDec(bufptr, 0, 9, &year); - if(bufptr && *(++bufptr) != ',') bufptr = str_scanDec(bufptr, 0, 9, &local_time_off_hour); - if(bufptr && *(++bufptr) != ',') bufptr = str_scanDec(bufptr, 0, 9, &local_time_off_min); + int uiCalcComma = 0; + + for (int i = 0 ; i < len; i++) { + if (_rx_buffer[i] == ',') { uiCalcComma++; } + } + + char *bufptr = (char *)(_rx_buffer + 6); + + if ((memcmp(_rx_buffer + 3, "ZDA,", 3) == 0) && (uiCalcComma == 6)) { + /* + UTC day, month, and year, and local time zone offset + An example of the ZDA message string is: + + $GPZDA,172809.456,12,07,1996,00,00*45 + + ZDA message fields + Field Meaning + 0 Message ID $GPZDA + 1 UTC + 2 Day, ranging between 01 and 31 + 3 Month, ranging between 01 and 12 + 4 Year + 5 Local time zone offset from GMT, ranging from 00 through �13 hours + 6 Local time zone offset from GMT, ranging from 00 through 59 minutes + 7 The checksum data, always begins with * + Fields 5 and 6 together yield the total offset. For example, if field 5 is -5 and field 6 is +15, local time is 5 hours and 15 minutes earlier than GMT. + */ + float64_t ashtech_time = 0.0; + int day = 0, month = 0, year = 0, local_time_off_hour = 0, local_time_off_min = 0; + + if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &ashtech_time); } + + if (bufptr && *(++bufptr) != ',') { bufptr = str_scanDec(bufptr, 0, 9, &day); } + + if (bufptr && *(++bufptr) != ',') { bufptr = str_scanDec(bufptr, 0, 9, &month); } + + if (bufptr && *(++bufptr) != ',') { bufptr = str_scanDec(bufptr, 0, 9, &year); } + + if (bufptr && *(++bufptr) != ',') { bufptr = str_scanDec(bufptr, 0, 9, &local_time_off_hour); } + + if (bufptr && *(++bufptr) != ',') { bufptr = str_scanDec(bufptr, 0, 9, &local_time_off_min); } - int ashtech_hour = ashtech_time/10000; - int ashtech_minute = (ashtech_time - ashtech_hour*10000)/100; - float64_t ashtech_sec = ashtech_time - ashtech_hour*10000 - ashtech_minute*100; - //convert to unix timestamp - struct tm timeinfo; - timeinfo.tm_year = year - 1900; - timeinfo.tm_mon = month - 1; - timeinfo.tm_mday = day; - timeinfo.tm_hour = ashtech_hour; - timeinfo.tm_min = ashtech_minute; - timeinfo.tm_sec = int(ashtech_sec); - time_t epoch = mktime(&timeinfo); + int ashtech_hour = ashtech_time / 10000; + int ashtech_minute = (ashtech_time - ashtech_hour * 10000) / 100; + float64_t ashtech_sec = ashtech_time - ashtech_hour * 10000 - ashtech_minute * 100; + /* + * convert to unix timestamp + */ + struct tm timeinfo; + timeinfo.tm_year = year - 1900; + timeinfo.tm_mon = month - 1; + timeinfo.tm_mday = day; + timeinfo.tm_hour = ashtech_hour; + timeinfo.tm_min = ashtech_minute; + timeinfo.tm_sec = int(ashtech_sec); + time_t epoch = mktime(&timeinfo); - _gps_position->time_gps_usec = (uint64_t)epoch * 1000000; //TODO: test this - _gps_position->time_gps_usec += (uint64_t)((ashtech_sec - int(ashtech_sec)) * 1e6); - _gps_position->timestamp_time = hrt_absolute_time(); - } + _gps_position->time_gps_usec = (uint64_t)epoch * 1000000; //TODO: test this + _gps_position->time_gps_usec += (uint64_t)((ashtech_sec - int(ashtech_sec)) * 1e6); + _gps_position->timestamp_time = hrt_absolute_time(); + } - else if((memcmp(_rx_buffer+3, "GGA,",3) == 0)&&(uiCalcComma == 14)){ - /* - Time, position, and fix related data - An example of the GBS message string is: + else if ((memcmp(_rx_buffer + 3, "GGA,", 3) == 0) && (uiCalcComma == 14)) { + /* + Time, position, and fix related data + An example of the GBS message string is: - $GPGGA,172814.0,3723.46587704,N,12202.26957864,W,2,6,1.2,18.893,M,-25.669,M,2.0,0031*4F + $GPGGA,172814.0,3723.46587704,N,12202.26957864,W,2,6,1.2,18.893,M,-25.669,M,2.0,0031*4F - Note - The data string exceeds the ASHTECH standard length. - GGA message fields - Field Meaning - 0 Message ID $GPGGA - 1 UTC of position fix - 2 Latitude - 3 Direction of latitude: - N: North - S: South - 4 Longitude - 5 Direction of longitude: - E: East - W: West - 6 GPS Quality indicator: - 0: Fix not valid - 1: GPS fix - 2: Differential GPS fix, OmniSTAR VBS - 4: Real-Time Kinematic, fixed integers - 5: Real-Time Kinematic, float integers, OmniSTAR XP/HP or Location RTK - 7 Number of SVs in use, range from 00 through to 24+ - 8 HDOP - 9 Orthometric height (MSL reference) - 10 M: unit of measure for orthometric height is meters - 11 Geoid separation - 12 M: geoid separation measured in meters - 13 Age of differential GPS data record, Type 1 or Type 9. Null field when DGPS is not used. - 14 Reference station ID, range 0000-4095. A null field when any reference station ID is selected and no corrections are received1. - 15 - The checksum data, always begins with * - Note - If a user-defined geoid model, or an inclined - */ - float64_t ashtech_time = 0.0, lat = 0.0, lon = 0.0, alt = 0.0; - int num_of_sv = 0, fix_quality=0; - float64_t hdop = 99.9; - char ns = '?', ew = '?'; + Note - The data string exceeds the ASHTECH standard length. + GGA message fields + Field Meaning + 0 Message ID $GPGGA + 1 UTC of position fix + 2 Latitude + 3 Direction of latitude: + N: North + S: South + 4 Longitude + 5 Direction of longitude: + E: East + W: West + 6 GPS Quality indicator: + 0: Fix not valid + 1: GPS fix + 2: Differential GPS fix, OmniSTAR VBS + 4: Real-Time Kinematic, fixed integers + 5: Real-Time Kinematic, float integers, OmniSTAR XP/HP or Location RTK + 7 Number of SVs in use, range from 00 through to 24+ + 8 HDOP + 9 Orthometric height (MSL reference) + 10 M: unit of measure for orthometric height is meters + 11 Geoid separation + 12 M: geoid separation measured in meters + 13 Age of differential GPS data record, Type 1 or Type 9. Null field when DGPS is not used. + 14 Reference station ID, range 0000-4095. A null field when any reference station ID is selected and no corrections are received1. + 15 + The checksum data, always begins with * + Note - If a user-defined geoid model, or an inclined + */ + float64_t ashtech_time = 0.0, lat = 0.0, lon = 0.0, alt = 0.0; + int num_of_sv = 0, fix_quality = 0; + float64_t hdop = 99.9; + char ns = '?', ew = '?'; - if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&ashtech_time); - if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&lat); - if(bufptr && *(++bufptr) != ',') ns = *(bufptr++); - if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&lon); - if(bufptr && *(++bufptr) != ',') ew = *(bufptr++); - if(bufptr && *(++bufptr) != ',') bufptr = str_scanDec(bufptr, 0, 9, &fix_quality); - if(bufptr && *(++bufptr) != ',') bufptr = str_scanDec(bufptr, 0, 9, &num_of_sv); - if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&hdop); - if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&alt); + if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &ashtech_time); } - if(ns == 'S') - lat = -lat; - if(ew == 'W') - lon = -lon; + if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &lat); } - _gps_position->lat = (int(lat*0.01) + (lat*0.01 - int(lat*0.01))*100.0/60.0)*10000000; - _gps_position->lon = (int(lon*0.01) + (lon*0.01 - int(lon*0.01))*100.0/60.0)*10000000; - _gps_position->alt = alt*1000; - _rate_count_lat_lon++; + if (bufptr && *(++bufptr) != ',') { ns = *(bufptr++); } - if((lat == 0.0) && (lon == 0.0) && (alt == 0.0)){ - _gps_position->fix_type = 0; - } - else - _gps_position->fix_type = 3 + fix_quality; + if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &lon); } - _gps_position->timestamp_position = hrt_absolute_time(); + if (bufptr && *(++bufptr) != ',') { ew = *(bufptr++); } - _gps_position->vel_m_s = 0; /**< GPS ground speed (m/s) */ - _gps_position->vel_n_m_s = 0; /**< GPS ground speed in m/s */ - _gps_position->vel_e_m_s = 0; /**< GPS ground speed in m/s */ - _gps_position->vel_d_m_s = 0; /**< GPS ground speed in m/s */ - _gps_position->cog_rad = 0; /**< Course over ground (NOT heading, but direction of movement) in rad, -PI..PI */ - _gps_position->vel_ned_valid = true; /**< Flag to indicate if NED speed is valid */ - _gps_position->c_variance_rad = 0.1; - _gps_position->timestamp_velocity = hrt_absolute_time(); - return 1; - } - else if ((memcmp(_rx_buffer, "$PASHR,POS,", 11) == 0) && (uiCalcComma == 18)) { - /* - Example $PASHR,POS,2,10,125410.00,5525.8138702,N,03833.9587380,E,131.555,1.0,0.0,0.007,-0.001,2.0,1.0,1.7,1.0,*34 + if (bufptr && *(++bufptr) != ',') { bufptr = str_scanDec(bufptr, 0, 9, &fix_quality); } - $PASHR,POS,d1,d2,m3,m4,c5,m6,c7,f8,f9,f10,f11,f12,f13,f14,f15,f16,s17*cc - Parameter Description Range - d1 Position mode 0: standalone - 1: differential - 2: RTK float - 3: RTK fixed - 5: Dead reckoning - 9: SBAS (see NPT setting) - d2 Number of satellite used in position fix 0-99 - m3 Current UTC time of position fix (hhmmss.ss) 000000.00-235959.99 - m4 Latitude of position (ddmm.mmmmmm) 0-90 degrees 00-59.9999999 minutes - c5 Latitude sector N, S - m6 Longitude of position (dddmm.mmmmmm) 0-180 degrees 00-59.9999999 minutes - c7 Longitude sector E,W - f8 Altitude above ellipsoid +9999.000 - f9 Differential age (data link age), seconds 0.0-600.0 - f10 True track/course over ground in degrees 0.0-359.9 - f11 Speed over ground in knots 0.0-999.9 - f12 Vertical velocity in decimeters per second +999.9 - f13 PDOP 0-99.9 - f14 HDOP 0-99.9 - f15 VDOP 0-99.9 - f16 TDOP 0-99.9 - s17 Reserved no data - *cc Checksum - */ - bufptr = (char*)( _rx_buffer+10); - float64_t ashtech_time = 0.0, lat = 0.0, lon = 0.0, alt = 0.0; - int num_of_sv = 0, fix_quality=0; - float64_t track_true = 0.0, ground_speed = 0.0 , age_of_corr = 0.0; - float64_t hdop = 99.9, vdop = 99.9, pdop = 99.9, tdop = 99.9,vertic_vel = 0.0; - char ns = '?', ew = '?'; + if (bufptr && *(++bufptr) != ',') { bufptr = str_scanDec(bufptr, 0, 9, &num_of_sv); } - if(bufptr && *(++bufptr) != ',') bufptr = str_scanDec(bufptr, 0, 9, &fix_quality); - if(bufptr && *(++bufptr) != ',') bufptr = str_scanDec(bufptr, 0, 9, &num_of_sv); - if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&ashtech_time); - if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&lat); - if(bufptr && *(++bufptr) != ',') ns = *(bufptr++); - if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&lon); - if(bufptr && *(++bufptr) != ',') ew = *(bufptr++); - if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&alt); - if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&age_of_corr); - if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&track_true); - if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&ground_speed); - if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&vertic_vel); - if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&pdop); - if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&hdop); - if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&vdop); - if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&tdop); + if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &hdop); } - if(ns == 'S') - lat = -lat; - if(ew == 'W') - lon = -lon; + if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &alt); } - _gps_position->lat = (int(lat*0.01) + (lat*0.01 - int(lat*0.01))*100.0/60.0)*10000000; - _gps_position->lon = (int(lon*0.01) + (lon*0.01 - int(lon*0.01))*100.0/60.0)*10000000; - _gps_position->alt = alt*1000; - _rate_count_lat_lon++; + if (ns == 'S') { + lat = -lat; + } - if((lat == 0.0) && (lon == 0.0) && (alt == 0.0)){ - _gps_position->fix_type = 0; - } - else - _gps_position->fix_type = 3 + fix_quality; + if (ew == 'W') { + lon = -lon; + } - _gps_position->timestamp_position = hrt_absolute_time(); - - const float64_t m_pi = 3.14159265; + _gps_position->lat = (int(lat * 0.01) + (lat * 0.01 - int(lat * 0.01)) * 100.0 / 60.0) * 10000000; + _gps_position->lon = (int(lon * 0.01) + (lon * 0.01 - int(lon * 0.01)) * 100.0 / 60.0) * 10000000; + _gps_position->alt = alt * 1000; + _rate_count_lat_lon++; - double track_rad = track_true * m_pi / 180.0; + if ((lat == 0.0) && (lon == 0.0) && (alt == 0.0)) { + _gps_position->fix_type = 0; - double velocity_ms = ground_speed / 1.9438445; /** knots to m/s */ + } else { + _gps_position->fix_type = 3 + fix_quality; + } + + _gps_position->timestamp_position = hrt_absolute_time(); + + _gps_position->vel_m_s = 0; /**< GPS ground speed (m/s) */ + _gps_position->vel_n_m_s = 0; /**< GPS ground speed in m/s */ + _gps_position->vel_e_m_s = 0; /**< GPS ground speed in m/s */ + _gps_position->vel_d_m_s = 0; /**< GPS ground speed in m/s */ + _gps_position->cog_rad = + 0; /**< Course over ground (NOT heading, but direction of movement) in rad, -PI..PI */ + _gps_position->vel_ned_valid = true; /**< Flag to indicate if NED speed is valid */ + _gps_position->c_variance_rad = 0.1; + _gps_position->timestamp_velocity = hrt_absolute_time(); + return 1; + + } else if ((memcmp(_rx_buffer, "$PASHR,POS,", 11) == 0) && (uiCalcComma == 18)) { + /* + Example $PASHR,POS,2,10,125410.00,5525.8138702,N,03833.9587380,E,131.555,1.0,0.0,0.007,-0.001,2.0,1.0,1.7,1.0,*34 + + $PASHR,POS,d1,d2,m3,m4,c5,m6,c7,f8,f9,f10,f11,f12,f13,f14,f15,f16,s17*cc + Parameter Description Range + d1 Position mode 0: standalone + 1: differential + 2: RTK float + 3: RTK fixed + 5: Dead reckoning + 9: SBAS (see NPT setting) + d2 Number of satellite used in position fix 0-99 + m3 Current UTC time of position fix (hhmmss.ss) 000000.00-235959.99 + m4 Latitude of position (ddmm.mmmmmm) 0-90 degrees 00-59.9999999 minutes + c5 Latitude sector N, S + m6 Longitude of position (dddmm.mmmmmm) 0-180 degrees 00-59.9999999 minutes + c7 Longitude sector E,W + f8 Altitude above ellipsoid +9999.000 + f9 Differential age (data link age), seconds 0.0-600.0 + f10 True track/course over ground in degrees 0.0-359.9 + f11 Speed over ground in knots 0.0-999.9 + f12 Vertical velocity in decimeters per second +999.9 + f13 PDOP 0-99.9 + f14 HDOP 0-99.9 + f15 VDOP 0-99.9 + f16 TDOP 0-99.9 + s17 Reserved no data + *cc Checksum + */ + bufptr = (char *)(_rx_buffer + 10); + float64_t ashtech_time = 0.0, lat = 0.0, lon = 0.0, alt = 0.0; + int num_of_sv = 0, fix_quality = 0; + float64_t track_true = 0.0, ground_speed = 0.0 , age_of_corr = 0.0; + float64_t hdop = 99.9, vdop = 99.9, pdop = 99.9, tdop = 99.9, vertic_vel = 0.0; + char ns = '?', ew = '?'; + + if (bufptr && *(++bufptr) != ',') { bufptr = str_scanDec(bufptr, 0, 9, &fix_quality); } + + if (bufptr && *(++bufptr) != ',') { bufptr = str_scanDec(bufptr, 0, 9, &num_of_sv); } + + if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &ashtech_time); } + + if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &lat); } + + if (bufptr && *(++bufptr) != ',') { ns = *(bufptr++); } + + if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &lon); } + + if (bufptr && *(++bufptr) != ',') { ew = *(bufptr++); } + + if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &alt); } + + if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &age_of_corr); } + + if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &track_true); } + + if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &ground_speed); } + + if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &vertic_vel); } + + if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &pdop); } + + if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &hdop); } + + if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &vdop); } + + if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &tdop); } + + if (ns == 'S') { + lat = -lat; + } + + if (ew == 'W') { + lon = -lon; + } + + _gps_position->lat = (int(lat * 0.01) + (lat * 0.01 - int(lat * 0.01)) * 100.0 / 60.0) * 10000000; + _gps_position->lon = (int(lon * 0.01) + (lon * 0.01 - int(lon * 0.01)) * 100.0 / 60.0) * 10000000; + _gps_position->alt = alt * 1000; + _rate_count_lat_lon++; + + if ((lat == 0.0) && (lon == 0.0) && (alt == 0.0)) { + _gps_position->fix_type = 0; + + } else { + _gps_position->fix_type = 3 + fix_quality; + } + + _gps_position->timestamp_position = hrt_absolute_time(); + + const float64_t m_pi = 3.14159265; + + double track_rad = track_true * m_pi / 180.0; + + double velocity_ms = ground_speed / 1.9438445; /** knots to m/s */ double velocity_north = velocity_ms * cos(track_rad); double velocity_east = velocity_ms * sin(track_rad); @@ -301,230 +362,255 @@ int ASHTECH::handle_message(int len){ _gps_position->vel_n_m_s = velocity_north; /**< GPS ground speed in m/s */ _gps_position->vel_e_m_s = velocity_east; /**< GPS ground speed in m/s */ _gps_position->vel_d_m_s = -vertic_vel; /**< GPS ground speed in m/s */ - _gps_position->cog_rad = track_rad; /**< Course over ground (NOT heading, but direction of movement) in rad, -PI..PI */ + _gps_position->cog_rad = + track_rad; /**< Course over ground (NOT heading, but direction of movement) in rad, -PI..PI */ _gps_position->vel_ned_valid = true; /**< Flag to indicate if NED speed is valid */ _gps_position->c_variance_rad = 0.1; _gps_position->timestamp_velocity = hrt_absolute_time(); - return 1; + return 1; - } - else if((memcmp(_rx_buffer+3, "GST,",3) == 0)&&(uiCalcComma == 8)){ - /* - Position error statistics - An example of the GST message string is: + } else if ((memcmp(_rx_buffer + 3, "GST,", 3) == 0) && (uiCalcComma == 8)) { + /* + Position error statistics + An example of the GST message string is: - $GPGST,172814.0,0.006,0.023,0.020,273.6,0.023,0.020,0.031*6A + $GPGST,172814.0,0.006,0.023,0.020,273.6,0.023,0.020,0.031*6A - The Talker ID ($--) will vary depending on the satellite system used for the position solution: + The Talker ID ($--) will vary depending on the satellite system used for the position solution: - $GP - GPS only - $GL - GLONASS only - $GN - Combined - GST message fields - Field Meaning - 0 Message ID $GPGST - 1 UTC of position fix - 2 RMS value of the pseudorange residuals; includes carrier phase residuals during periods of RTK (float) and RTK (fixed) processing - 3 Error ellipse semi-major axis 1 sigma error, in meters - 4 Error ellipse semi-minor axis 1 sigma error, in meters - 5 Error ellipse orientation, degrees from true north - 6 Latitude 1 sigma error, in meters - 7 Longitude 1 sigma error, in meters - 8 Height 1 sigma error, in meters - 9 The checksum data, always begins with * - */ - float64_t ashtech_time = 0.0, lat_err = 0.0, lon_err = 0.0, alt_err = 0.0; - float64_t min_err = 0.0, maj_err = 0.0, deg_from_north = 0.0, rms_err = 0.0; + $GP - GPS only + $GL - GLONASS only + $GN - Combined + GST message fields + Field Meaning + 0 Message ID $GPGST + 1 UTC of position fix + 2 RMS value of the pseudorange residuals; includes carrier phase residuals during periods of RTK (float) and RTK (fixed) processing + 3 Error ellipse semi-major axis 1 sigma error, in meters + 4 Error ellipse semi-minor axis 1 sigma error, in meters + 5 Error ellipse orientation, degrees from true north + 6 Latitude 1 sigma error, in meters + 7 Longitude 1 sigma error, in meters + 8 Height 1 sigma error, in meters + 9 The checksum data, always begins with * + */ + float64_t ashtech_time = 0.0, lat_err = 0.0, lon_err = 0.0, alt_err = 0.0; + float64_t min_err = 0.0, maj_err = 0.0, deg_from_north = 0.0, rms_err = 0.0; - if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&ashtech_time); - if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&rms_err); - if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&maj_err); - if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&min_err); - if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,°_from_north); - if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&lat_err); - if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&lon_err); - if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&alt_err); + if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &ashtech_time); } - _gps_position->eph = sqrt(lat_err*lat_err +lon_err*lon_err); - _gps_position->epv = alt_err; + if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &rms_err); } - _gps_position->s_variance_m_s = 0; - _gps_position->timestamp_variance = hrt_absolute_time(); - } - else if((memcmp(_rx_buffer+3, "VTG,",3) == 0)&&(uiCalcComma == 9)){ - /* - Track made good and speed over ground - An example of the VTG message string is: + if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &maj_err); } - $GPVTG,054.7,T,034.4,M,005.5,N,010.2,K*48 + if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &min_err); } - VTG message fields - Field Meaning - 0 Message ID $GPVTG - 1 Track made good (degrees true) - 2 T: track made good is relative to true north - 3 Track made good (degrees magnetic) - 4 M: track made good is relative to magnetic north - 5 Speed, in knots - 6 N: speed is measured in knots - 7 Speed over ground in kilometers/hour (kph) - 8 K: speed over ground is measured in kph - 9 The checksum data, always begins with * - */ - /*float64_t track_true = 0.0, speed = 0.0, track_magnetic = 0.0, ground_speed = 0.0; - char true_north = '?', magnetic_north = '?', speed_in_knots = '?', speed_in_kph = '?'; - - if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&track_true); - if(bufptr && *(++bufptr) != ',') true_north = *(bufptr++); - if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&track_magnetic); - if(bufptr && *(++bufptr) != ',') magnetic_north = *(bufptr++); - if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&speed); - if(bufptr && *(++bufptr) != ',') speed_in_knots = *(bufptr++); - if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&ground_speed); - if(bufptr && *(++bufptr) != ',') speed_in_kph = *(bufptr++); + if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, °_from_north); } - const float64_t dPI = 3.14159265; - float64_t tan_C = tan(track_true * dPI / 180.0); - float64_t lat_ = sqrt(ground_speed*ground_speed/ (1+tan_C)); //km/hour - float64_t lon_ = lat_*tan_C; // //km/hour*/ - } - else if((memcmp(_rx_buffer+3, "GSV,",3) == 0)){ - /* - The GSV message string identifies the number of SVs in view, the PRN numbers, elevations, azimuths, and SNR values. An example of the GSV message string is: + if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &lat_err); } - $GPGSV,4,1,13,02,02,213,,03,-3,000,,11,00,121,,14,13,172,05*67 + if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &lon_err); } - GSV message fields - Field Meaning - 0 Message ID $GPGSV - 1 Total number of messages of this type in this cycle - 2 Message number - 3 Total number of SVs visible - 4 SV PRN number - 5 Elevation, in degrees, 90� maximum - 6 Azimuth, degrees from True North, 000� through 359� - 7 SNR, 00 through 99 dB (null when not tracking) - 8-11 Information about second SV, same format as fields 4 through 7 - 12-15 Information about third SV, same format as fields 4 through 7 - 16-19 Information about fourth SV, same format as fields 4 through 7 - 20 The checksum data, always begins with * - */ -// currently process only gps, because do not know what -// Global satellite ID I should use for non GPS sats - bool bGPS = false; - if(memcmp(_rx_buffer, "$GP",3) != 0) - return 0; - else - bGPS = true; + if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &alt_err); } - int all_msg_num, this_msg_num, tot_sv_visible; - struct gsv_sat{ - int svid; - int elevation; - int azimuth; - int snr; - } sat[4]; - memset(sat, 0, sizeof(sat)); - - if(bufptr && *(++bufptr) != ',') bufptr = str_scanDec(bufptr, 0, 9, &all_msg_num); - if(bufptr && *(++bufptr) != ',') bufptr = str_scanDec(bufptr, 0, 9, &this_msg_num); - if(bufptr && *(++bufptr) != ',') bufptr = str_scanDec(bufptr, 0, 9, &tot_sv_visible); - if((this_msg_num<1) || (this_msg_num>all_msg_num)){ - return 0; - } - if((this_msg_num == 0)&&(bGPS == true)){ - memset(_satellite_info->svid, 0,sizeof(_satellite_info->svid)); - memset(_satellite_info->used, 0,sizeof(_satellite_info->used)); - memset(_satellite_info->snr, 0,sizeof(_satellite_info->snr)); - memset(_satellite_info->elevation,0,sizeof(_satellite_info->elevation)); - memset(_satellite_info->azimuth, 0,sizeof(_satellite_info->azimuth)); - } + _gps_position->eph = sqrt(lat_err * lat_err + lon_err * lon_err); + _gps_position->epv = alt_err; - int end = 4; - if(this_msg_num == all_msg_num){ - end = tot_sv_visible - (this_msg_num-1)*4; - _gps_position->satellites_used = tot_sv_visible; - _satellite_info->count = SAT_INFO_MAX_SATELLITES; - _satellite_info->timestamp = hrt_absolute_time(); - } - for(int y = 0 ; y < end ;y++){ - if(bufptr && *(++bufptr) != ',') bufptr = str_scanDec(bufptr, 0, 9, &sat[y].svid); - if(bufptr && *(++bufptr) != ',') bufptr = str_scanDec(bufptr, 0, 9, &sat[y].elevation); - if(bufptr && *(++bufptr) != ',') bufptr = str_scanDec(bufptr, 0, 9, &sat[y].azimuth); - if(bufptr && *(++bufptr) != ',') bufptr = str_scanDec(bufptr, 0, 9, &sat[y].snr); - - _satellite_info->svid[y+(this_msg_num-1)*4] = sat[y].svid; - _satellite_info->used[y+(this_msg_num-1)*4] = ((sat[y].snr>0)? true: false); - _satellite_info->snr[y+(this_msg_num-1)*4] = sat[y].snr; - _satellite_info->elevation[y+(this_msg_num-1)*4] = sat[y].elevation; - _satellite_info->azimuth[y+(this_msg_num-1)*4] = sat[y].azimuth; - } - } + _gps_position->s_variance_m_s = 0; + _gps_position->timestamp_variance = hrt_absolute_time(); - return 0; + } else if ((memcmp(_rx_buffer + 3, "VTG,", 3) == 0) && (uiCalcComma == 9)) { + /* + Track made good and speed over ground + An example of the VTG message string is: + + $GPVTG,054.7,T,034.4,M,005.5,N,010.2,K*48 + + VTG message fields + Field Meaning + 0 Message ID $GPVTG + 1 Track made good (degrees true) + 2 T: track made good is relative to true north + 3 Track made good (degrees magnetic) + 4 M: track made good is relative to magnetic north + 5 Speed, in knots + 6 N: speed is measured in knots + 7 Speed over ground in kilometers/hour (kph) + 8 K: speed over ground is measured in kph + 9 The checksum data, always begins with * + */ + /*float64_t track_true = 0.0, speed = 0.0, track_magnetic = 0.0, ground_speed = 0.0; + char true_north = '?', magnetic_north = '?', speed_in_knots = '?', speed_in_kph = '?'; + + if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&track_true); + if(bufptr && *(++bufptr) != ',') true_north = *(bufptr++); + if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&track_magnetic); + if(bufptr && *(++bufptr) != ',') magnetic_north = *(bufptr++); + if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&speed); + if(bufptr && *(++bufptr) != ',') speed_in_knots = *(bufptr++); + if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&ground_speed); + if(bufptr && *(++bufptr) != ',') speed_in_kph = *(bufptr++); + + const float64_t dPI = 3.14159265; + float64_t tan_C = tan(track_true * dPI / 180.0); + float64_t lat_ = sqrt(ground_speed*ground_speed/ (1+tan_C)); //km/hour + float64_t lon_ = lat_*tan_C; // //km/hour*/ + } else if ((memcmp(_rx_buffer + 3, "GSV,", 3) == 0)) { + /* + The GSV message string identifies the number of SVs in view, the PRN numbers, elevations, azimuths, and SNR values. An example of the GSV message string is: + + $GPGSV,4,1,13,02,02,213,,03,-3,000,,11,00,121,,14,13,172,05*67 + + GSV message fields + Field Meaning + 0 Message ID $GPGSV + 1 Total number of messages of this type in this cycle + 2 Message number + 3 Total number of SVs visible + 4 SV PRN number + 5 Elevation, in degrees, 90� maximum + 6 Azimuth, degrees from True North, 000� through 359� + 7 SNR, 00 through 99 dB (null when not tracking) + 8-11 Information about second SV, same format as fields 4 through 7 + 12-15 Information about third SV, same format as fields 4 through 7 + 16-19 Information about fourth SV, same format as fields 4 through 7 + 20 The checksum data, always begins with * + */ + /* + * currently process only gps, because do not know what + * Global satellite ID I should use for non GPS sats + */ + bool bGPS = false; + + if (memcmp(_rx_buffer, "$GP", 3) != 0) { + return 0; + + } else { + bGPS = true; + } + + int all_msg_num, this_msg_num, tot_sv_visible; + struct gsv_sat { + int svid; + int elevation; + int azimuth; + int snr; + } sat[4]; + memset(sat, 0, sizeof(sat)); + + if (bufptr && *(++bufptr) != ',') { bufptr = str_scanDec(bufptr, 0, 9, &all_msg_num); } + + if (bufptr && *(++bufptr) != ',') { bufptr = str_scanDec(bufptr, 0, 9, &this_msg_num); } + + if (bufptr && *(++bufptr) != ',') { bufptr = str_scanDec(bufptr, 0, 9, &tot_sv_visible); } + + if ((this_msg_num < 1) || (this_msg_num > all_msg_num)) { + return 0; + } + + if ((this_msg_num == 0) && (bGPS == true)) { + memset(_satellite_info->svid, 0, sizeof(_satellite_info->svid)); + memset(_satellite_info->used, 0, sizeof(_satellite_info->used)); + memset(_satellite_info->snr, 0, sizeof(_satellite_info->snr)); + memset(_satellite_info->elevation, 0, sizeof(_satellite_info->elevation)); + memset(_satellite_info->azimuth, 0, sizeof(_satellite_info->azimuth)); + } + + int end = 4; + + if (this_msg_num == all_msg_num) { + end = tot_sv_visible - (this_msg_num - 1) * 4; + _gps_position->satellites_used = tot_sv_visible; + _satellite_info->count = SAT_INFO_MAX_SATELLITES; + _satellite_info->timestamp = hrt_absolute_time(); + } + + for (int y = 0 ; y < end ; y++) { + if (bufptr && *(++bufptr) != ',') { bufptr = str_scanDec(bufptr, 0, 9, &sat[y].svid); } + + if (bufptr && *(++bufptr) != ',') { bufptr = str_scanDec(bufptr, 0, 9, &sat[y].elevation); } + + if (bufptr && *(++bufptr) != ',') { bufptr = str_scanDec(bufptr, 0, 9, &sat[y].azimuth); } + + if (bufptr && *(++bufptr) != ',') { bufptr = str_scanDec(bufptr, 0, 9, &sat[y].snr); } + + _satellite_info->svid[y + (this_msg_num - 1) * 4] = sat[y].svid; + _satellite_info->used[y + (this_msg_num - 1) * 4] = ((sat[y].snr > 0) ? true : false); + _satellite_info->snr[y + (this_msg_num - 1) * 4] = sat[y].snr; + _satellite_info->elevation[y + (this_msg_num - 1) * 4] = sat[y].elevation; + _satellite_info->azimuth[y + (this_msg_num - 1) * 4] = sat[y].azimuth; + } + } + + return 0; } -int ASHTECH::receive(unsigned timeout){ +int ASHTECH::receive(unsigned timeout) { - /* poll descriptor */ - pollfd fds[1]; - fds[0].fd = _fd; - fds[0].events = POLLIN; + { + /* poll descriptor */ + pollfd fds[1]; + fds[0].fd = _fd; + fds[0].events = POLLIN; - uint8_t buf[32]; + uint8_t buf[32]; - /* timeout additional to poll */ - uint64_t time_started = hrt_absolute_time(); + /* timeout additional to poll */ + uint64_t time_started = hrt_absolute_time(); - int j = 0; - ssize_t count = 0; + int j = 0; + ssize_t count = 0; - while (true) { + while (true) { - /* pass received bytes to the packet decoder */ - while (j < count) { - int l=0; - if ((l = parse_char(buf[j])) > 0) { - /* return to configure during configuration or to the gps driver during normal work - * if a packet has arrived */ - if (handle_message(l) > 0) - return 1; - } - /* in case we keep trying but only get crap from GPS */ - if (time_started + timeout*1000*2 < hrt_absolute_time() ) { - return -1; - } - j++; - } + /* pass received bytes to the packet decoder */ + while (j < count) { + int l = 0; - /* everything is read */ - j = count = 0; + if ((l = parse_char(buf[j])) > 0) { + /* return to configure during configuration or to the gps driver during normal work + * if a packet has arrived */ + if (handle_message(l) > 0) { + return 1; + } + } - /* then poll for new data */ - int ret = ::poll(fds, sizeof(fds) / sizeof(fds[0]), timeout*2); + /* in case we keep trying but only get crap from GPS */ + if (time_started + timeout * 1000 * 2 < hrt_absolute_time()) { + return -1; + } - if (ret < 0) { - /* something went wrong when polling */ - return -1; + j++; + } - } else if (ret == 0) { - /* Timeout */ - return -1; + /* everything is read */ + j = count = 0; - } else if (ret > 0) { - /* if we have new data from GPS, go handle it */ - if (fds[0].revents & POLLIN) { - /* - * We are here because poll says there is some data, so this - * won't block even on a blocking device. If more bytes are - * available, we'll go back to poll() again... - */ - count = ::read(_fd, buf, sizeof(buf)); - } - } - } -} + /* then poll for new data */ + int ret = ::poll(fds, sizeof(fds) / sizeof(fds[0]), timeout * 2); + + if (ret < 0) { + /* something went wrong when polling */ + return -1; + + } else if (ret == 0) { + /* Timeout */ + return -1; + + } else if (ret > 0) { + /* if we have new data from GPS, go handle it */ + if (fds[0].revents & POLLIN) { + /* + * We are here because poll says there is some data, so this + * won't block even on a blocking device. If more bytes are + * available, we'll go back to poll() again... + */ + count = ::read(_fd, buf, sizeof(buf)); + } + } + } + } } #define HEXDIGIT_CHAR(d) ((char)((d) + (((d) < 0xA) ? '0' : 'A'-0xA))) @@ -557,8 +643,9 @@ int ASHTECH::parse_char(uint8_t b) _decode_state = NME_DECODE_UNINIT; _rx_buffer_bytes = 0; - } else + } else { _rx_buffer[_rx_buffer_bytes++] = b; + } break; @@ -573,7 +660,7 @@ int ASHTECH::parse_char(uint8_t b) uint8_t *buffer = _rx_buffer + 1; uint8_t *bufend = _rx_buffer + _rx_buffer_bytes - 3; - for (; buffer < bufend; buffer++) checksum ^= *buffer; + for (; buffer < bufend; buffer++) { checksum ^= *buffer; } if ((HEXDIGIT_CHAR(checksum >> 4) == *(_rx_buffer + _rx_buffer_bytes - 2)) && (HEXDIGIT_CHAR(checksum & 0x0F) == *(_rx_buffer + _rx_buffer_bytes - 1))) { @@ -588,32 +675,36 @@ int ASHTECH::parse_char(uint8_t b) return iRet; } -void ASHTECH::decode_init(void){ +void ASHTECH::decode_init(void) +{ } -//ashtech boad configuration script -//char comm[] = "$PASHS,NME,GGA,A,ON,0.1\r\n" +/* + * ashtech boad configuration script + */ + char comm[] = "$PASHS,POP,20\r\n"\ - "$PASHS,NME,ZDA,B,ON,3\r\n"\ - "$PASHS,NME,GGA,B,OFF\r\n"\ - "$PASHS,NME,GST,B,ON,3\r\n"\ - "$PASHS,NME,POS,B,ON,0.05\r\n"\ - "$PASHS,NME,GSV,B,ON,3\r\n"\ - "$PASHS,SPD,A,8\r\n"\ - "$PASHS,SPD,B,9\r\n"; // default baud is 7 + "$PASHS,NME,ZDA,B,ON,3\r\n"\ + "$PASHS,NME,GGA,B,OFF\r\n"\ + "$PASHS,NME,GST,B,ON,3\r\n"\ + "$PASHS,NME,POS,B,ON,0.05\r\n"\ + "$PASHS,NME,GSV,B,ON,3\r\n"\ + "$PASHS,SPD,A,8\r\n"\ + "$PASHS,SPD,B,9\r\n"; -int ASHTECH::configure(unsigned &baudrate){ - /* try different baudrates */ - const unsigned baudrates_to_try[] = {9600, 38400, 19200, 57600, 115200}; +int ASHTECH::configure(unsigned &baudrate) +{ + /* try different baudrates */ + const unsigned baudrates_to_try[] = {9600, 38400, 19200, 57600, 115200}; - for (int baud_i = 0; baud_i < sizeof(baudrates_to_try)/sizeof(baudrates_to_try[0]); baud_i++) { - baudrate = baudrates_to_try[baud_i]; - set_baudrate(_fd, baudrate); - write(_fd, (uint8_t*)comm, sizeof(comm)); - } + for (int baud_i = 0; baud_i < sizeof(baudrates_to_try) / sizeof(baudrates_to_try[0]); baud_i++) { + baudrate = baudrates_to_try[baud_i]; + set_baudrate(_fd, baudrate); + write(_fd, (uint8_t *)comm, sizeof(comm)); + } - set_baudrate(_fd, 115200); - return 0; + set_baudrate(_fd, 115200); + return 0; } diff --git a/src/drivers/gps/ashtech.h b/src/drivers/gps/ashtech.h index e8c5ffb4c4..56f8571a49 100644 --- a/src/drivers/gps/ashtech.h +++ b/src/drivers/gps/ashtech.h @@ -67,8 +67,8 @@ class ASHTECH : public GPS_Helper bool _parse_error; // parse error flag char *_parse_pos; // parse position - bool _gsv_in_progress; // Indicates that gsv data parsing is in progress - //int _satellites_count; // Number of satellites info parsed. + bool _gsv_in_progress; /**< Indicates that gsv data parsing is in progress */ + //int _satellites_count; /**< Number of satellites info parsed. */ uint8_t count; /**< Number of satellites in satellite info */ uint8_t svid[SAT_INFO_MAX_SATELLITES]; /**< Space vehicle ID [1..255], see scheme below */ uint8_t used[SAT_INFO_MAX_SATELLITES]; /**< 0: Satellite not used, 1: used for navigation */ diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index 2547cda97f..5fb4b9ff8d 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -343,7 +343,6 @@ GPS::task_main() break; case GPS_DRIVER_MODE_ASHTECH: - _Helper = new ASHTECH(_serial_fd, &_report_gps_pos, _p_report_sat_info); break; @@ -408,9 +407,9 @@ GPS::task_main() mode_str = "MTK"; break; - case GPS_DRIVER_MODE_ASHTECH: - mode_str = "ASHTECH"; - break; + case GPS_DRIVER_MODE_ASHTECH: + mode_str = "ASHTECH"; + break; default: break; @@ -439,10 +438,10 @@ GPS::task_main() break; case GPS_DRIVER_MODE_MTK: - _mode = GPS_DRIVER_MODE_ASHTECH; - break; + _mode = GPS_DRIVER_MODE_ASHTECH; + break; - case GPS_DRIVER_MODE_ASHTECH: + case GPS_DRIVER_MODE_ASHTECH: _mode = GPS_DRIVER_MODE_UBX; break; From cfc6c234b83a457e4288c39f778341f6c8489e85 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 9 Oct 2014 09:22:34 +0200 Subject: [PATCH 321/411] att pos estimator: Initialize distance --- src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index 27c1f981e9..03ae1d8479 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -382,6 +382,7 @@ FixedwingEstimator::FixedwingEstimator() : _local_pos({}), _gps({}), _wind({}), + _distance{}, _gyro_offsets({}), _accel_offsets({}), From aca9b138b7e92bd2a32e8673c81f3fe2d088a427 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 9 Oct 2014 09:23:00 +0200 Subject: [PATCH 322/411] att pos estimator: Use float constant where it should be float. --- .../ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index 03ae1d8479..a61346c2ed 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -1085,7 +1085,7 @@ FixedwingEstimator::task_main() float baro_elapsed = (_baro.timestamp - baro_last) / 1e6f; - _ekf->updateDtHgtFilt(math::constrain(baro_elapsed, 0.001f, 0.1)); + _ekf->updateDtHgtFilt(math::constrain(baro_elapsed, 0.001f, 0.1f)); _ekf->baroHgt = _baro.altitude; From e0719f21417d3ba572df18460073faa923d5ff29 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 9 Oct 2014 09:23:28 +0200 Subject: [PATCH 323/411] Initialize inhibition variable. --- src/modules/ekf_att_pos_estimator/estimator_23states.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp index 94a6d165fe..e629d33455 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp @@ -119,6 +119,7 @@ AttPosEKF::AttPosEKF() : inhibitWindStates(true), inhibitMagStates(true), inhibitGndState(true), + inhibitScaleState(true), onGround(true), staticMode(true), From d524b17164125415c831e38ec1e7db11a6ea97e1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 9 Oct 2014 09:24:08 +0200 Subject: [PATCH 324/411] Disambiguate local variable names. --- .../ekf_att_pos_estimator/estimator_23states.cpp | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp index e629d33455..c17e034add 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp @@ -2561,15 +2561,15 @@ void AttPosEKF::CovarianceInit() P[22][22] = sq(0.5f); } -float AttPosEKF::ConstrainFloat(float val, float min, float max) +float AttPosEKF::ConstrainFloat(float val, float min_val, float max_val) { float ret; - if (val > max) { - ret = max; - ekf_debug("> max: %8.4f, val: %8.4f", (double)max, (double)val); - } else if (val < min) { - ret = min; - ekf_debug("< min: %8.4f, val: %8.4f", (double)min, (double)val); + if (val > max_val) { + ret = max_val; + ekf_debug("> max: %8.4f, val: %8.4f", (double)max_val, (double)val); + } else if (val < min_val) { + ret = min_val; + ekf_debug("< min: %8.4f, val: %8.4f", (double)min_val, (double)val); } else { ret = val; } From fa194832ce136116b37ba8ce787b40efce53bc3d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 9 Oct 2014 09:24:47 +0200 Subject: [PATCH 325/411] mavlink: Handle new auto sub states. --- src/modules/mavlink/mavlink_messages.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index a2f3828ffa..c182cfdb95 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -162,6 +162,8 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set break; case NAVIGATION_STATE_AUTO_RTL: + /* fallthrough */ + case NAVIGATION_STATE_AUTO_RCRECOVER: *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; @@ -170,6 +172,8 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set break; case NAVIGATION_STATE_LAND: + case NAVIGATION_STATE_AUTO_LANDENGFAIL: + case NAVIGATION_STATE_AUTO_LANDGPSFAIL: /* fallthrough */ case NAVIGATION_STATE_DESCEND: *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED From 5ec6cb0789917aa49c629b8d9cb354df1e3c0f0f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 9 Oct 2014 09:26:46 +0200 Subject: [PATCH 326/411] navigator: Remove excessive C++ check flags, which enable debatable warnings. --- src/modules/navigator/module.mk | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/modules/navigator/module.mk b/src/modules/navigator/module.mk index 72dc2815ce..c44d4c35e6 100644 --- a/src/modules/navigator/module.mk +++ b/src/modules/navigator/module.mk @@ -61,6 +61,4 @@ INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink MODULE_STACKSIZE = 1200 -EXTRACXXFLAGS = -Weffc++ - MAXOPTIMIZATION = -Os From 7bb9b3efa71107c8988f6933720365b42c8a4046 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 9 Oct 2014 09:27:25 +0200 Subject: [PATCH 327/411] IO firmware hot fix: Use right pointer type for RSSI value. --- src/modules/px4iofirmware/controls.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c index 622f20c7d5..7b127759ad 100644 --- a/src/modules/px4iofirmware/controls.c +++ b/src/modules/px4iofirmware/controls.c @@ -52,7 +52,7 @@ #define RC_CHANNEL_LOW_THRESH -8000 /* 10% threshold */ static bool ppm_input(uint16_t *values, uint16_t *num_values, uint16_t *frame_len); -static bool dsm_port_input(uint8_t *rssi, bool *dsm_updated, bool *st24_updated); +static bool dsm_port_input(uint16_t *rssi, bool *dsm_updated, bool *st24_updated); static perf_counter_t c_gather_dsm; static perf_counter_t c_gather_sbus; @@ -60,7 +60,7 @@ static perf_counter_t c_gather_ppm; static int _dsm_fd; -bool dsm_port_input(uint8_t *rssi, bool *dsm_updated, bool *st24_updated) +bool dsm_port_input(uint16_t *rssi, bool *dsm_updated, bool *st24_updated) { perf_begin(c_gather_dsm); uint16_t temp_count = r_raw_rc_count; From 1d50af272f5f3727faad911a69e1b3807f28ca2d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 9 Oct 2014 09:29:05 +0200 Subject: [PATCH 328/411] EEPROM driver: Do not issue warnings that result from our special case handling - this driver is only used for this one particular eeprom and out of the NuttX tree. --- src/systemcmds/mtd/24xxxx_mtd.c | 15 ++++++++++++--- 1 file changed, 12 insertions(+), 3 deletions(-) diff --git a/src/systemcmds/mtd/24xxxx_mtd.c b/src/systemcmds/mtd/24xxxx_mtd.c index 9913637970..f85ed8e2d0 100644 --- a/src/systemcmds/mtd/24xxxx_mtd.c +++ b/src/systemcmds/mtd/24xxxx_mtd.c @@ -80,11 +80,17 @@ /* As a minimum, the size of the AT24 part and its 7-bit I2C address are required. */ #ifndef CONFIG_AT24XX_SIZE -# warning "Assuming AT24 size 64" +/* XXX this is a well vetted special case, + * do not issue a warning any more + * # warning "Assuming AT24 size 64" + */ # define CONFIG_AT24XX_SIZE 64 #endif #ifndef CONFIG_AT24XX_ADDR -# warning "Assuming AT24 address of 0x50" +/* XXX this is a well vetted special case, + * do not issue a warning any more + * # warning "Assuming AT24 address of 0x50" + */ # define CONFIG_AT24XX_ADDR 0x50 #endif @@ -115,7 +121,10 @@ */ #ifndef CONFIG_AT24XX_MTD_BLOCKSIZE -# warning "Assuming driver block size is the same as the FLASH page size" +/* XXX this is a well vetted special case, + * do not issue a warning any more + * # warning "Assuming driver block size is the same as the FLASH page size" + */ # define CONFIG_AT24XX_MTD_BLOCKSIZE AT24XX_PAGESIZE #endif From 9f33555f53d253a04de1ca97d1306b763c9d2b6e Mon Sep 17 00:00:00 2001 From: Denis Yeldandi Date: Thu, 9 Oct 2014 11:36:04 +0400 Subject: [PATCH 329/411] More code style fixes --- src/drivers/gps/ashtech.h | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/drivers/gps/ashtech.h b/src/drivers/gps/ashtech.h index 56f8571a49..6e65d0b2b8 100644 --- a/src/drivers/gps/ashtech.h +++ b/src/drivers/gps/ashtech.h @@ -59,16 +59,16 @@ class ASHTECH : public GPS_Helper int _fd; struct satellite_info_s *_satellite_info; struct vehicle_gps_position_s *_gps_position; - int ashtechlog_fd;//miklm + int ashtechlog_fd; ashtech_decode_state_t _decode_state; uint8_t _rx_buffer[RECV_BUFFER_SIZE]; uint16_t _rx_buffer_bytes; - bool _parse_error; // parse error flag - char *_parse_pos; // parse position + bool _parse_error; /** parse error flag */ + char *_parse_pos; /** parse position */ bool _gsv_in_progress; /**< Indicates that gsv data parsing is in progress */ - //int _satellites_count; /**< Number of satellites info parsed. */ + /* int _satellites_count; /**< Number of satellites info parsed. */ uint8_t count; /**< Number of satellites in satellite info */ uint8_t svid[SAT_INFO_MAX_SATELLITES]; /**< Space vehicle ID [1..255], see scheme below */ uint8_t used[SAT_INFO_MAX_SATELLITES]; /**< 0: Satellite not used, 1: used for navigation */ From d3875eabf2ce0ac538954b0d5747f1155cf2ef33 Mon Sep 17 00:00:00 2001 From: Denis Yeldandi Date: Thu, 9 Oct 2014 11:37:22 +0400 Subject: [PATCH 330/411] Removed VTG message parsing --- src/drivers/gps/ashtech.cpp | 36 ------------------------------------ 1 file changed, 36 deletions(-) diff --git a/src/drivers/gps/ashtech.cpp b/src/drivers/gps/ashtech.cpp index 7342d01d1b..077a6ddf44 100644 --- a/src/drivers/gps/ashtech.cpp +++ b/src/drivers/gps/ashtech.cpp @@ -419,42 +419,6 @@ int ASHTECH::handle_message(int len) _gps_position->s_variance_m_s = 0; _gps_position->timestamp_variance = hrt_absolute_time(); - } else if ((memcmp(_rx_buffer + 3, "VTG,", 3) == 0) && (uiCalcComma == 9)) { - /* - Track made good and speed over ground - An example of the VTG message string is: - - $GPVTG,054.7,T,034.4,M,005.5,N,010.2,K*48 - - VTG message fields - Field Meaning - 0 Message ID $GPVTG - 1 Track made good (degrees true) - 2 T: track made good is relative to true north - 3 Track made good (degrees magnetic) - 4 M: track made good is relative to magnetic north - 5 Speed, in knots - 6 N: speed is measured in knots - 7 Speed over ground in kilometers/hour (kph) - 8 K: speed over ground is measured in kph - 9 The checksum data, always begins with * - */ - /*float64_t track_true = 0.0, speed = 0.0, track_magnetic = 0.0, ground_speed = 0.0; - char true_north = '?', magnetic_north = '?', speed_in_knots = '?', speed_in_kph = '?'; - - if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&track_true); - if(bufptr && *(++bufptr) != ',') true_north = *(bufptr++); - if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&track_magnetic); - if(bufptr && *(++bufptr) != ',') magnetic_north = *(bufptr++); - if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&speed); - if(bufptr && *(++bufptr) != ',') speed_in_knots = *(bufptr++); - if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&ground_speed); - if(bufptr && *(++bufptr) != ',') speed_in_kph = *(bufptr++); - - const float64_t dPI = 3.14159265; - float64_t tan_C = tan(track_true * dPI / 180.0); - float64_t lat_ = sqrt(ground_speed*ground_speed/ (1+tan_C)); //km/hour - float64_t lon_ = lat_*tan_C; // //km/hour*/ } else if ((memcmp(_rx_buffer + 3, "GSV,", 3) == 0)) { /* The GSV message string identifies the number of SVs in view, the PRN numbers, elevations, azimuths, and SNR values. An example of the GSV message string is: From e37b25fd5868d00a3a6b39c1112f30bfe37a8a47 Mon Sep 17 00:00:00 2001 From: Denis Yeldandi Date: Thu, 9 Oct 2014 11:47:20 +0400 Subject: [PATCH 331/411] Non-ascii characters cleanup --- src/drivers/gps/ashtech.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/drivers/gps/ashtech.cpp b/src/drivers/gps/ashtech.cpp index 077a6ddf44..71ddb28cc4 100644 --- a/src/drivers/gps/ashtech.cpp +++ b/src/drivers/gps/ashtech.cpp @@ -124,7 +124,7 @@ int ASHTECH::handle_message(int len) 2 Day, ranging between 01 and 31 3 Month, ranging between 01 and 12 4 Year - 5 Local time zone offset from GMT, ranging from 00 through �13 hours + 5 Local time zone offset from GMT, ranging from 00 through 13 hours 6 Local time zone offset from GMT, ranging from 00 through 59 minutes 7 The checksum data, always begins with * Fields 5 and 6 together yield the total offset. For example, if field 5 is -5 and field 6 is +15, local time is 5 hours and 15 minutes earlier than GMT. @@ -432,8 +432,8 @@ int ASHTECH::handle_message(int len) 2 Message number 3 Total number of SVs visible 4 SV PRN number - 5 Elevation, in degrees, 90� maximum - 6 Azimuth, degrees from True North, 000� through 359� + 5 Elevation, in degrees, 90 maximum + 6 Azimuth, degrees from True North, 000 through 359 7 SNR, 00 through 99 dB (null when not tracking) 8-11 Information about second SV, same format as fields 4 through 7 12-15 Information about third SV, same format as fields 4 through 7 From c4e934c13311fac2eb85e5e3f1a161af78be7118 Mon Sep 17 00:00:00 2001 From: Denis Yeldandi Date: Thu, 9 Oct 2014 12:01:09 +0400 Subject: [PATCH 332/411] Multiple fixes: - Fixed boad - board typo - Ashtech initialization string is const char* now - Using standard M_PI constant instead of locally defined one - Removed float32_t and float64_t in favor of standard float and double --- src/drivers/gps/ashtech.cpp | 39 ++++++++++++++++--------------------- 1 file changed, 17 insertions(+), 22 deletions(-) diff --git a/src/drivers/gps/ashtech.cpp b/src/drivers/gps/ashtech.cpp index 71ddb28cc4..503e56dae8 100644 --- a/src/drivers/gps/ashtech.cpp +++ b/src/drivers/gps/ashtech.cpp @@ -15,9 +15,6 @@ #include #include -typedef double float64_t; -typedef float float32_t; - char *str_scanDec(const char *pos, int8_t sign, int8_t n_max_digit, int32_t *result) { int8_t n = 0; @@ -47,9 +44,9 @@ char *str_scanDec(const char *pos, int8_t sign, int8_t n_max_digit, int32_t *re } char *scanFloat64(const char *pos, int8_t sign, int8_t n_max_int, int8_t n_max_frac, - float64_t *result) + double *result) { - float64_t f = 0.0, div = 1.0; + double f = 0.0, div = 1.0; int32_t d_int; int8_t n = 0, isneg = 0; @@ -61,7 +58,7 @@ char *scanFloat64(const char *pos, int8_t sign, int8_t n_max_int, int8_t n_max_ pos++; while (*pos >= '0' && *pos <= '9') { - f = f * (10.0) + (float64_t)(*(pos++) - '0'); + f = f * (10.0) + (double)(*(pos++) - '0'); div *= (0.1); n++; @@ -70,9 +67,9 @@ char *scanFloat64(const char *pos, int8_t sign, int8_t n_max_int, int8_t n_max_ } else if (n_max_frac > 0) { return (NULL); } - if (isneg) { *result = (float64_t)d_int - f * div; } + if (isneg) { *result = (double)d_int - f * div; } - else { *result = (float64_t)d_int + f * div; } + else { *result = (double)d_int + f * div; } return ((char *)pos); } @@ -129,7 +126,7 @@ int ASHTECH::handle_message(int len) 7 The checksum data, always begins with * Fields 5 and 6 together yield the total offset. For example, if field 5 is -5 and field 6 is +15, local time is 5 hours and 15 minutes earlier than GMT. */ - float64_t ashtech_time = 0.0; + double ashtech_time = 0.0; int day = 0, month = 0, year = 0, local_time_off_hour = 0, local_time_off_min = 0; if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &ashtech_time); } @@ -147,7 +144,7 @@ int ASHTECH::handle_message(int len) int ashtech_hour = ashtech_time / 10000; int ashtech_minute = (ashtech_time - ashtech_hour * 10000) / 100; - float64_t ashtech_sec = ashtech_time - ashtech_hour * 10000 - ashtech_minute * 100; + double ashtech_sec = ashtech_time - ashtech_hour * 10000 - ashtech_minute * 100; /* * convert to unix timestamp */ @@ -203,9 +200,9 @@ int ASHTECH::handle_message(int len) The checksum data, always begins with * Note - If a user-defined geoid model, or an inclined */ - float64_t ashtech_time = 0.0, lat = 0.0, lon = 0.0, alt = 0.0; + double ashtech_time = 0.0, lat = 0.0, lon = 0.0, alt = 0.0; int num_of_sv = 0, fix_quality = 0; - float64_t hdop = 99.9; + double hdop = 99.9; char ns = '?', ew = '?'; if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &ashtech_time); } @@ -290,10 +287,10 @@ int ASHTECH::handle_message(int len) *cc Checksum */ bufptr = (char *)(_rx_buffer + 10); - float64_t ashtech_time = 0.0, lat = 0.0, lon = 0.0, alt = 0.0; + double ashtech_time = 0.0, lat = 0.0, lon = 0.0, alt = 0.0; int num_of_sv = 0, fix_quality = 0; - float64_t track_true = 0.0, ground_speed = 0.0 , age_of_corr = 0.0; - float64_t hdop = 99.9, vdop = 99.9, pdop = 99.9, tdop = 99.9, vertic_vel = 0.0; + double track_true = 0.0, ground_speed = 0.0 , age_of_corr = 0.0; + double hdop = 99.9, vdop = 99.9, pdop = 99.9, tdop = 99.9, vertic_vel = 0.0; char ns = '?', ew = '?'; if (bufptr && *(++bufptr) != ',') { bufptr = str_scanDec(bufptr, 0, 9, &fix_quality); } @@ -350,9 +347,7 @@ int ASHTECH::handle_message(int len) _gps_position->timestamp_position = hrt_absolute_time(); - const float64_t m_pi = 3.14159265; - - double track_rad = track_true * m_pi / 180.0; + double track_rad = track_true * M_PI / 180.0; double velocity_ms = ground_speed / 1.9438445; /** knots to m/s */ double velocity_north = velocity_ms * cos(track_rad); @@ -394,8 +389,8 @@ int ASHTECH::handle_message(int len) 8 Height 1 sigma error, in meters 9 The checksum data, always begins with * */ - float64_t ashtech_time = 0.0, lat_err = 0.0, lon_err = 0.0, alt_err = 0.0; - float64_t min_err = 0.0, maj_err = 0.0, deg_from_north = 0.0, rms_err = 0.0; + double ashtech_time = 0.0, lat_err = 0.0, lon_err = 0.0, alt_err = 0.0; + double min_err = 0.0, maj_err = 0.0, deg_from_north = 0.0, rms_err = 0.0; if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &ashtech_time); } @@ -645,10 +640,10 @@ void ASHTECH::decode_init(void) } /* - * ashtech boad configuration script + * ashtech board configuration script */ -char comm[] = "$PASHS,POP,20\r\n"\ +const char comm[] = "$PASHS,POP,20\r\n"\ "$PASHS,NME,ZDA,B,ON,3\r\n"\ "$PASHS,NME,GGA,B,OFF\r\n"\ "$PASHS,NME,GST,B,ON,3\r\n"\ From 676cb91a1d973313e15597280121c52cc2d84e43 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 9 Oct 2014 10:55:48 +0200 Subject: [PATCH 333/411] Hotfix for PX4IO comms: Raise timeout to 10 ms. --- nuttx-configs/px4fmu-v1/nsh/defconfig | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nuttx-configs/px4fmu-v1/nsh/defconfig b/nuttx-configs/px4fmu-v1/nsh/defconfig index cd9c05b136..edf49b26e8 100644 --- a/nuttx-configs/px4fmu-v1/nsh/defconfig +++ b/nuttx-configs/px4fmu-v1/nsh/defconfig @@ -287,7 +287,7 @@ CONFIG_STM32_USART_SINGLEWIRE=y # # CONFIG_STM32_I2C_DYNTIMEO is not set CONFIG_STM32_I2CTIMEOSEC=0 -CONFIG_STM32_I2CTIMEOMS=1 +CONFIG_STM32_I2CTIMEOMS=10 # CONFIG_STM32_I2C_DUTY16_9 is not set # From bc5d1648fe298102eb90b0a6f4ce7cedf364b165 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 9 Oct 2014 11:13:34 +0200 Subject: [PATCH 334/411] Aerocore: Add missing declarations --- src/drivers/boards/aerocore/aerocore_init.c | 13 +++++++++++++ src/drivers/boards/aerocore/board_config.h | 19 +++++++++++++++++++ 2 files changed, 32 insertions(+) diff --git a/src/drivers/boards/aerocore/aerocore_init.c b/src/drivers/boards/aerocore/aerocore_init.c index 4e3ba2d7e4..1ce235da8f 100644 --- a/src/drivers/boards/aerocore/aerocore_init.c +++ b/src/drivers/boards/aerocore/aerocore_init.c @@ -93,6 +93,19 @@ # endif #endif +/* + * Ideally we'd be able to get these from up_internal.h, + * but since we want to be able to disable the NuttX use + * of leds for system indication at will and there is no + * separate switch, we need to build independent of the + * CONFIG_ARCH_LEDS configuration switch. + */ +__BEGIN_DECLS +extern void led_init(void); +extern void led_on(int led); +extern void led_off(int led); +__END_DECLS + /**************************************************************************** * Protected Functions ****************************************************************************/ diff --git a/src/drivers/boards/aerocore/board_config.h b/src/drivers/boards/aerocore/board_config.h index 70142a3147..776a2071ed 100644 --- a/src/drivers/boards/aerocore/board_config.h +++ b/src/drivers/boards/aerocore/board_config.h @@ -171,6 +171,25 @@ __BEGIN_DECLS extern void stm32_spiinitialize(void); +/**************************************************************************** + * Name: nsh_archinitialize + * + * Description: + * Perform architecture specific initialization for NSH. + * + * CONFIG_NSH_ARCHINIT=y : + * Called from the NSH library + * + * CONFIG_BOARD_INITIALIZE=y, CONFIG_NSH_LIBRARY=y, && + * CONFIG_NSH_ARCHINIT=n : + * Called from board_initialize(). + * + ****************************************************************************/ + +#ifdef CONFIG_NSH_LIBRARY +int nsh_archinitialize(void); +#endif + #endif /* __ASSEMBLY__ */ __END_DECLS From c3e25377d7d86277b338020d7efd986770a2429c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 9 Oct 2014 11:13:55 +0200 Subject: [PATCH 335/411] FMUv1: Add missing declarations. --- src/drivers/boards/px4fmu-v1/board_config.h | 21 +++++++++++++++++++++ 1 file changed, 21 insertions(+) diff --git a/src/drivers/boards/px4fmu-v1/board_config.h b/src/drivers/boards/px4fmu-v1/board_config.h index a70a6240cb..188885be2d 100644 --- a/src/drivers/boards/px4fmu-v1/board_config.h +++ b/src/drivers/boards/px4fmu-v1/board_config.h @@ -209,6 +209,27 @@ __BEGIN_DECLS extern void stm32_spiinitialize(void); +extern void stm32_usbinitialize(void); + +/**************************************************************************** + * Name: nsh_archinitialize + * + * Description: + * Perform architecture specific initialization for NSH. + * + * CONFIG_NSH_ARCHINIT=y : + * Called from the NSH library + * + * CONFIG_BOARD_INITIALIZE=y, CONFIG_NSH_LIBRARY=y, && + * CONFIG_NSH_ARCHINIT=n : + * Called from board_initialize(). + * + ****************************************************************************/ + +#ifdef CONFIG_NSH_LIBRARY +int nsh_archinitialize(void); +#endif + #endif /* __ASSEMBLY__ */ __END_DECLS From 4bfa30bfd6151c63c4cd8e2ece51e5568f4f3d16 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 9 Oct 2014 11:14:13 +0200 Subject: [PATCH 336/411] FMUv2: Add missing declarations. --- src/drivers/boards/px4fmu-v2/board_config.h | 21 +++++++++++++++++++++ src/drivers/boards/px4fmu-v2/px4fmu2_init.c | 13 +++++++++++++ 2 files changed, 34 insertions(+) diff --git a/src/drivers/boards/px4fmu-v2/board_config.h b/src/drivers/boards/px4fmu-v2/board_config.h index 0190a5b5b0..ee365e47c0 100644 --- a/src/drivers/boards/px4fmu-v2/board_config.h +++ b/src/drivers/boards/px4fmu-v2/board_config.h @@ -229,6 +229,27 @@ __BEGIN_DECLS extern void stm32_spiinitialize(void); +extern void stm32_usbinitialize(void); + +/**************************************************************************** + * Name: nsh_archinitialize + * + * Description: + * Perform architecture specific initialization for NSH. + * + * CONFIG_NSH_ARCHINIT=y : + * Called from the NSH library + * + * CONFIG_BOARD_INITIALIZE=y, CONFIG_NSH_LIBRARY=y, && + * CONFIG_NSH_ARCHINIT=n : + * Called from board_initialize(). + * + ****************************************************************************/ + +#ifdef CONFIG_NSH_LIBRARY +int nsh_archinitialize(void); +#endif + #endif /* __ASSEMBLY__ */ __END_DECLS diff --git a/src/drivers/boards/px4fmu-v2/px4fmu2_init.c b/src/drivers/boards/px4fmu-v2/px4fmu2_init.c index bf41bb1fed..9b25c574a8 100644 --- a/src/drivers/boards/px4fmu-v2/px4fmu2_init.c +++ b/src/drivers/boards/px4fmu-v2/px4fmu2_init.c @@ -94,6 +94,19 @@ # endif #endif +/* + * Ideally we'd be able to get these from up_internal.h, + * but since we want to be able to disable the NuttX use + * of leds for system indication at will and there is no + * separate switch, we need to build independent of the + * CONFIG_ARCH_LEDS configuration switch. + */ +__BEGIN_DECLS +extern void led_init(void); +extern void led_on(int led); +extern void led_off(int led); +__END_DECLS + /**************************************************************************** * Protected Functions ****************************************************************************/ From 5bbca777961e0e0b109d954772ff0176c65277f5 Mon Sep 17 00:00:00 2001 From: Denis Yeldandi Date: Thu, 9 Oct 2014 14:11:18 +0400 Subject: [PATCH 337/411] Got rid of str_scanDec and scanFloat64. - Replaced str_scanDec and scanFloat64 with strtol and strtod. - Added __attribute__ ((unused)) to yet unused variables - Added initialization for a few variables --- src/drivers/gps/ashtech.cpp | 170 ++++++++++++------------------------ 1 file changed, 56 insertions(+), 114 deletions(-) diff --git a/src/drivers/gps/ashtech.cpp b/src/drivers/gps/ashtech.cpp index 503e56dae8..1b706b5b27 100644 --- a/src/drivers/gps/ashtech.cpp +++ b/src/drivers/gps/ashtech.cpp @@ -1,6 +1,7 @@ #include "ashtech.h" #include +#include #include #include #include @@ -15,67 +16,6 @@ #include #include -char *str_scanDec(const char *pos, int8_t sign, int8_t n_max_digit, int32_t *result) -{ - int8_t n = 0; - int32_t d = 0; - int8_t neg = 0; - - if (*pos == '-') { neg = 1; pos++; } - - else if (*pos == '+') { pos++; } - - else if (sign) { return (NULL); } - - while (*pos >= '0' && *pos <= '9') { - d = d * 10 + (*(pos++) - '0'); - n++; - - if (n_max_digit > 0 && n == n_max_digit) { break; } - } - - if (n == 0 || n > 10) { return (NULL); } - - if (neg) { *result = -d; } - - else { *result = d; } - - return ((char *)pos); -} - -char *scanFloat64(const char *pos, int8_t sign, int8_t n_max_int, int8_t n_max_frac, - double *result) -{ - double f = 0.0, div = 1.0; - int32_t d_int; - int8_t n = 0, isneg = 0; - - if (*pos == '-') { isneg = 1; } - - if ((pos = str_scanDec(pos, sign, n_max_int, &d_int)) == NULL) { return (NULL); } - - if (*(pos) == '.') { - pos++; - - while (*pos >= '0' && *pos <= '9') { - f = f * (10.0) + (double)(*(pos++) - '0'); - div *= (0.1); - n++; - - if (n_max_frac > 0 && n == n_max_frac) { break; } - } - - } else if (n_max_frac > 0) { return (NULL); } - - if (isneg) { *result = (double)d_int - f * div; } - - else { *result = (double)d_int + f * div; } - - return ((char *)pos); -} - - - ASHTECH::ASHTECH(const int &fd, struct vehicle_gps_position_s *gps_position, struct satellite_info_s *satellite_info): _fd(fd), _satellite_info(satellite_info), @@ -97,6 +37,8 @@ ASHTECH::~ASHTECH() int ASHTECH::handle_message(int len) { + char * endp; + if (len < 7) { return 0; } int uiCalcComma = 0; @@ -127,19 +69,19 @@ int ASHTECH::handle_message(int len) Fields 5 and 6 together yield the total offset. For example, if field 5 is -5 and field 6 is +15, local time is 5 hours and 15 minutes earlier than GMT. */ double ashtech_time = 0.0; - int day = 0, month = 0, year = 0, local_time_off_hour = 0, local_time_off_min = 0; + int day = 0, month = 0, year = 0, local_time_off_hour __attribute__((unused)) = 0, local_time_off_min __attribute__((unused)) = 0; - if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &ashtech_time); } + if (bufptr && *(++bufptr) != ',') { ashtech_time = strtod(bufptr, &endp); bufptr = endp; } - if (bufptr && *(++bufptr) != ',') { bufptr = str_scanDec(bufptr, 0, 9, &day); } + if (bufptr && *(++bufptr) != ',') { day = strtol(bufptr, &endp, 10); bufptr = endp; } - if (bufptr && *(++bufptr) != ',') { bufptr = str_scanDec(bufptr, 0, 9, &month); } + if (bufptr && *(++bufptr) != ',') { month = strtol(bufptr, &endp, 10); bufptr = endp; } - if (bufptr && *(++bufptr) != ',') { bufptr = str_scanDec(bufptr, 0, 9, &year); } + if (bufptr && *(++bufptr) != ',') { year = strtol(bufptr, &endp, 10); bufptr = endp; } - if (bufptr && *(++bufptr) != ',') { bufptr = str_scanDec(bufptr, 0, 9, &local_time_off_hour); } + if (bufptr && *(++bufptr) != ',') { local_time_off_hour = strtol(bufptr, &endp, 10); bufptr = endp; } - if (bufptr && *(++bufptr) != ',') { bufptr = str_scanDec(bufptr, 0, 9, &local_time_off_min); } + if (bufptr && *(++bufptr) != ',') { local_time_off_min = strtol(bufptr, &endp, 10); bufptr = endp; } int ashtech_hour = ashtech_time / 10000; @@ -200,28 +142,28 @@ int ASHTECH::handle_message(int len) The checksum data, always begins with * Note - If a user-defined geoid model, or an inclined */ - double ashtech_time = 0.0, lat = 0.0, lon = 0.0, alt = 0.0; - int num_of_sv = 0, fix_quality = 0; - double hdop = 99.9; + double ashtech_time __attribute__((unused)) = 0.0, lat = 0.0, lon = 0.0, alt = 0.0; + int num_of_sv __attribute__((unused)) = 0, fix_quality = 0; + double hdop __attribute__((unused)) = 99.9; char ns = '?', ew = '?'; - if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &ashtech_time); } + if (bufptr && *(++bufptr) != ',') { ashtech_time = strtod(bufptr, &endp); bufptr = endp; } - if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &lat); } + if (bufptr && *(++bufptr) != ',') { lat = strtod(bufptr, &endp); bufptr = endp; } if (bufptr && *(++bufptr) != ',') { ns = *(bufptr++); } - if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &lon); } + if (bufptr && *(++bufptr) != ',') { lon = strtod(bufptr, &endp); bufptr = endp; } if (bufptr && *(++bufptr) != ',') { ew = *(bufptr++); } - if (bufptr && *(++bufptr) != ',') { bufptr = str_scanDec(bufptr, 0, 9, &fix_quality); } + if (bufptr && *(++bufptr) != ',') { fix_quality = strtol(bufptr, &endp, 10); bufptr = endp; } - if (bufptr && *(++bufptr) != ',') { bufptr = str_scanDec(bufptr, 0, 9, &num_of_sv); } + if (bufptr && *(++bufptr) != ',') { num_of_sv = strtol(bufptr, &endp, 10); bufptr = endp; } - if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &hdop); } + if (bufptr && *(++bufptr) != ',') { hdop = strtod(bufptr, &endp); bufptr = endp; } - if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &alt); } + if (bufptr && *(++bufptr) != ',') { alt = strtod(bufptr, &endp); bufptr = endp; } if (ns == 'S') { lat = -lat; @@ -287,43 +229,43 @@ int ASHTECH::handle_message(int len) *cc Checksum */ bufptr = (char *)(_rx_buffer + 10); - double ashtech_time = 0.0, lat = 0.0, lon = 0.0, alt = 0.0; - int num_of_sv = 0, fix_quality = 0; - double track_true = 0.0, ground_speed = 0.0 , age_of_corr = 0.0; - double hdop = 99.9, vdop = 99.9, pdop = 99.9, tdop = 99.9, vertic_vel = 0.0; + double ashtech_time __attribute__((unused)) = 0.0, lat = 0.0, lon = 0.0, alt = 0.0; + int num_of_sv __attribute__((unused)) = 0, fix_quality = 0; + double track_true = 0.0, ground_speed = 0.0 , age_of_corr __attribute__((unused)) = 0.0; + double hdop __attribute__((unused)) = 99.9, vdop __attribute__((unused)) = 99.9, pdop __attribute__((unused)) = 99.9, tdop __attribute__((unused)) = 99.9, vertic_vel = 0.0; char ns = '?', ew = '?'; - if (bufptr && *(++bufptr) != ',') { bufptr = str_scanDec(bufptr, 0, 9, &fix_quality); } + if (bufptr && *(++bufptr) != ',') { fix_quality = strtol(bufptr, &endp, 10); bufptr = endp; } - if (bufptr && *(++bufptr) != ',') { bufptr = str_scanDec(bufptr, 0, 9, &num_of_sv); } + if (bufptr && *(++bufptr) != ',') { num_of_sv = strtol(bufptr, &endp, 10); bufptr = endp; } - if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &ashtech_time); } + if (bufptr && *(++bufptr) != ',') { ashtech_time = strtod(bufptr, &endp); bufptr = endp; } - if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &lat); } + if (bufptr && *(++bufptr) != ',') { lat = strtod(bufptr, &endp); bufptr = endp; } if (bufptr && *(++bufptr) != ',') { ns = *(bufptr++); } - if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &lon); } + if (bufptr && *(++bufptr) != ',') { lon = strtod(bufptr, &endp); bufptr = endp; } if (bufptr && *(++bufptr) != ',') { ew = *(bufptr++); } - if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &alt); } + if (bufptr && *(++bufptr) != ',') { alt = strtod(bufptr, &endp); bufptr = endp; } - if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &age_of_corr); } + if (bufptr && *(++bufptr) != ',') { age_of_corr = strtod(bufptr, &endp); bufptr = endp; } - if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &track_true); } + if (bufptr && *(++bufptr) != ',') { track_true = strtod(bufptr, &endp); bufptr = endp; } - if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &ground_speed); } + if (bufptr && *(++bufptr) != ',') { ground_speed = strtod(bufptr, &endp); bufptr = endp; } - if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &vertic_vel); } + if (bufptr && *(++bufptr) != ',') { vertic_vel = strtod(bufptr, &endp); bufptr = endp; } - if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &pdop); } + if (bufptr && *(++bufptr) != ',') { pdop = strtod(bufptr, &endp); bufptr = endp; } - if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &hdop); } + if (bufptr && *(++bufptr) != ',') { hdop = strtod(bufptr, &endp); bufptr = endp; } - if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &vdop); } + if (bufptr && *(++bufptr) != ',') { vdop = strtod(bufptr, &endp); bufptr = endp; } - if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &tdop); } + if (bufptr && *(++bufptr) != ',') { tdop = strtod(bufptr, &endp); bufptr = endp; } if (ns == 'S') { lat = -lat; @@ -389,24 +331,24 @@ int ASHTECH::handle_message(int len) 8 Height 1 sigma error, in meters 9 The checksum data, always begins with * */ - double ashtech_time = 0.0, lat_err = 0.0, lon_err = 0.0, alt_err = 0.0; - double min_err = 0.0, maj_err = 0.0, deg_from_north = 0.0, rms_err = 0.0; + double ashtech_time __attribute__((unused)) = 0.0, lat_err = 0.0, lon_err = 0.0, alt_err = 0.0; + double min_err __attribute__((unused)) = 0.0, maj_err __attribute__((unused)) = 0.0, deg_from_north __attribute__((unused)) = 0.0, rms_err __attribute__((unused)) = 0.0; - if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &ashtech_time); } + if (bufptr && *(++bufptr) != ',') { ashtech_time = strtod(bufptr, &endp); bufptr = endp; } - if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &rms_err); } + if (bufptr && *(++bufptr) != ',') { rms_err = strtod(bufptr, &endp); bufptr = endp; } - if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &maj_err); } + if (bufptr && *(++bufptr) != ',') { maj_err = strtod(bufptr, &endp); bufptr = endp; } - if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &min_err); } + if (bufptr && *(++bufptr) != ',') { min_err = strtod(bufptr, &endp); bufptr = endp; } - if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, °_from_north); } + if (bufptr && *(++bufptr) != ',') { deg_from_north = strtod(bufptr, &endp); bufptr = endp; } - if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &lat_err); } + if (bufptr && *(++bufptr) != ',') { lat_err = strtod(bufptr, &endp); bufptr = endp; } - if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &lon_err); } + if (bufptr && *(++bufptr) != ',') { lon_err = strtod(bufptr, &endp); bufptr = endp; } - if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &alt_err); } + if (bufptr && *(++bufptr) != ',') { alt_err = strtod(bufptr, &endp); bufptr = endp; } _gps_position->eph = sqrt(lat_err * lat_err + lon_err * lon_err); _gps_position->epv = alt_err; @@ -448,7 +390,7 @@ int ASHTECH::handle_message(int len) bGPS = true; } - int all_msg_num, this_msg_num, tot_sv_visible; + int all_msg_num = 0, this_msg_num = 0, tot_sv_visible = 0; struct gsv_sat { int svid; int elevation; @@ -457,11 +399,11 @@ int ASHTECH::handle_message(int len) } sat[4]; memset(sat, 0, sizeof(sat)); - if (bufptr && *(++bufptr) != ',') { bufptr = str_scanDec(bufptr, 0, 9, &all_msg_num); } + if (bufptr && *(++bufptr) != ',') { all_msg_num = strtol(bufptr, &endp, 10); bufptr = endp; } - if (bufptr && *(++bufptr) != ',') { bufptr = str_scanDec(bufptr, 0, 9, &this_msg_num); } + if (bufptr && *(++bufptr) != ',') { this_msg_num = strtol(bufptr, &endp, 10); bufptr = endp; } - if (bufptr && *(++bufptr) != ',') { bufptr = str_scanDec(bufptr, 0, 9, &tot_sv_visible); } + if (bufptr && *(++bufptr) != ',') { tot_sv_visible = strtol(bufptr, &endp, 10); bufptr = endp; } if ((this_msg_num < 1) || (this_msg_num > all_msg_num)) { return 0; @@ -485,13 +427,13 @@ int ASHTECH::handle_message(int len) } for (int y = 0 ; y < end ; y++) { - if (bufptr && *(++bufptr) != ',') { bufptr = str_scanDec(bufptr, 0, 9, &sat[y].svid); } + if (bufptr && *(++bufptr) != ',') { sat[y].svid = strtol(bufptr, &endp, 10); bufptr = endp; } - if (bufptr && *(++bufptr) != ',') { bufptr = str_scanDec(bufptr, 0, 9, &sat[y].elevation); } + if (bufptr && *(++bufptr) != ',') { sat[y].elevation = strtol(bufptr, &endp, 10); bufptr = endp; } - if (bufptr && *(++bufptr) != ',') { bufptr = str_scanDec(bufptr, 0, 9, &sat[y].azimuth); } + if (bufptr && *(++bufptr) != ',') { sat[y].azimuth = strtol(bufptr, &endp, 10); bufptr = endp; } - if (bufptr && *(++bufptr) != ',') { bufptr = str_scanDec(bufptr, 0, 9, &sat[y].snr); } + if (bufptr && *(++bufptr) != ',') { sat[y].snr = strtol(bufptr, &endp, 10); bufptr = endp; } _satellite_info->svid[y + (this_msg_num - 1) * 4] = sat[y].svid; _satellite_info->used[y + (this_msg_num - 1) * 4] = ((sat[y].snr > 0) ? true : false); From fb6a68af70b280c7e5c2db9949c88cadcd1921ba Mon Sep 17 00:00:00 2001 From: Denis Yeldandi Date: Fri, 10 Oct 2014 17:51:18 +0400 Subject: [PATCH 338/411] Fixed issue #1382 - Ashtech driver is no longer checking fix status from comparing coordinates to 0.0;0.0;0.0, instead it's checking fix type in GGA or checking coordinate exsistance in POS. This removes compiler warning about float euqality comparison. - Fixed compiler warning about comparison between signed and unsigned int - Fixed compiler warning about class property masking --- src/drivers/gps/ashtech.cpp | 52 +++++++++++++++++++++++++++++-------- src/drivers/gps/ashtech.h | 2 +- 2 files changed, 42 insertions(+), 12 deletions(-) diff --git a/src/drivers/gps/ashtech.cpp b/src/drivers/gps/ashtech.cpp index 1b706b5b27..a61c7196b5 100644 --- a/src/drivers/gps/ashtech.cpp +++ b/src/drivers/gps/ashtech.cpp @@ -178,11 +178,20 @@ int ASHTECH::handle_message(int len) _gps_position->alt = alt * 1000; _rate_count_lat_lon++; - if ((lat == 0.0) && (lon == 0.0) && (alt == 0.0)) { + if (fix_quality == 0) { _gps_position->fix_type = 0; } else { - _gps_position->fix_type = 3 + fix_quality; + /* + * in this NMEA message float integers (value 5) mode has higher value than fixed integers (value 4), whereas it provides lower quality, + * and since value 3 is not being used, I "moved" value 5 to 3 to add it to _gps_position->fix_type + */ + if (fix_quality == 5) { fix_quality = 3; } + + /* + * fix quality 1 means just a normal 3D fix, so I'm subtracting 1 here. This way we'll have 3 for auto, 4 for DGPS, 5 for floats, 6 for fixed. + */ + _gps_position->fix_type = 3 + fix_quality - 1; } _gps_position->timestamp_position = hrt_absolute_time(); @@ -229,6 +238,11 @@ int ASHTECH::handle_message(int len) *cc Checksum */ bufptr = (char *)(_rx_buffer + 10); + + /* + * Ashtech would return empty space as coordinate (lat, lon or alt) if it doesn't have a fix yet + */ + int coordinatesFound = 0; double ashtech_time __attribute__((unused)) = 0.0, lat = 0.0, lon = 0.0, alt = 0.0; int num_of_sv __attribute__((unused)) = 0, fix_quality = 0; double track_true = 0.0, ground_speed = 0.0 , age_of_corr __attribute__((unused)) = 0.0; @@ -241,15 +255,31 @@ int ASHTECH::handle_message(int len) if (bufptr && *(++bufptr) != ',') { ashtech_time = strtod(bufptr, &endp); bufptr = endp; } - if (bufptr && *(++bufptr) != ',') { lat = strtod(bufptr, &endp); bufptr = endp; } + if (bufptr && *(++bufptr) != ',') { + /* + * if a coordinate is skipped (i.e. no fix), it either won't get into this block (two commas in a row) + * or strtod won't find anything and endp will point exactly where bufptr is. The same is for lon and alt. + */ + lat = strtod(bufptr, &endp); + if (bufptr != endp) {coordinatesFound++;} + bufptr = endp; + } if (bufptr && *(++bufptr) != ',') { ns = *(bufptr++); } - if (bufptr && *(++bufptr) != ',') { lon = strtod(bufptr, &endp); bufptr = endp; } + if (bufptr && *(++bufptr) != ',') { + lon = strtod(bufptr, &endp); + if (bufptr != endp) {coordinatesFound++;} + bufptr = endp; + } if (bufptr && *(++bufptr) != ',') { ew = *(bufptr++); } - if (bufptr && *(++bufptr) != ',') { alt = strtod(bufptr, &endp); bufptr = endp; } + if (bufptr && *(++bufptr) != ',') { + alt = strtod(bufptr, &endp); + if (bufptr != endp) {coordinatesFound++;} + bufptr = endp; + } if (bufptr && *(++bufptr) != ',') { age_of_corr = strtod(bufptr, &endp); bufptr = endp; } @@ -280,7 +310,7 @@ int ASHTECH::handle_message(int len) _gps_position->alt = alt * 1000; _rate_count_lat_lon++; - if ((lat == 0.0) && (lon == 0.0) && (alt == 0.0)) { + if (coordinatesFound < 3) { _gps_position->fix_type = 0; } else { @@ -461,12 +491,12 @@ int ASHTECH::receive(unsigned timeout) uint64_t time_started = hrt_absolute_time(); int j = 0; - ssize_t count = 0; + ssize_t bytes_count = 0; while (true) { /* pass received bytes to the packet decoder */ - while (j < count) { + while (j < bytes_count) { int l = 0; if ((l = parse_char(buf[j])) > 0) { @@ -486,7 +516,7 @@ int ASHTECH::receive(unsigned timeout) } /* everything is read */ - j = count = 0; + j = bytes_count = 0; /* then poll for new data */ int ret = ::poll(fds, sizeof(fds) / sizeof(fds[0]), timeout * 2); @@ -507,7 +537,7 @@ int ASHTECH::receive(unsigned timeout) * won't block even on a blocking device. If more bytes are * available, we'll go back to poll() again... */ - count = ::read(_fd, buf, sizeof(buf)); + bytes_count = ::read(_fd, buf, sizeof(buf)); } } } @@ -600,7 +630,7 @@ int ASHTECH::configure(unsigned &baudrate) const unsigned baudrates_to_try[] = {9600, 38400, 19200, 57600, 115200}; - for (int baud_i = 0; baud_i < sizeof(baudrates_to_try) / sizeof(baudrates_to_try[0]); baud_i++) { + for (unsigned int baud_i = 0; baud_i < sizeof(baudrates_to_try) / sizeof(baudrates_to_try[0]); baud_i++) { baudrate = baudrates_to_try[baud_i]; set_baudrate(_fd, baudrate); write(_fd, (uint8_t *)comm, sizeof(comm)); diff --git a/src/drivers/gps/ashtech.h b/src/drivers/gps/ashtech.h index 6e65d0b2b8..6ba522b9c5 100644 --- a/src/drivers/gps/ashtech.h +++ b/src/drivers/gps/ashtech.h @@ -68,7 +68,7 @@ class ASHTECH : public GPS_Helper char *_parse_pos; /** parse position */ bool _gsv_in_progress; /**< Indicates that gsv data parsing is in progress */ - /* int _satellites_count; /**< Number of satellites info parsed. */ + /* int _satellites_count; **< Number of satellites info parsed. */ uint8_t count; /**< Number of satellites in satellite info */ uint8_t svid[SAT_INFO_MAX_SATELLITES]; /**< Space vehicle ID [1..255], see scheme below */ uint8_t used[SAT_INFO_MAX_SATELLITES]; /**< 0: Satellite not used, 1: used for navigation */ From 686d3f4c7989f9b883b54fc26bb1974d10df98d3 Mon Sep 17 00:00:00 2001 From: Denis Yeldandi Date: Fri, 10 Oct 2014 18:42:24 +0400 Subject: [PATCH 339/411] Checking if fix status is less or equal to 0 rather than just equal --- src/drivers/gps/ashtech.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/gps/ashtech.cpp b/src/drivers/gps/ashtech.cpp index a61c7196b5..a2e292de2a 100644 --- a/src/drivers/gps/ashtech.cpp +++ b/src/drivers/gps/ashtech.cpp @@ -178,7 +178,7 @@ int ASHTECH::handle_message(int len) _gps_position->alt = alt * 1000; _rate_count_lat_lon++; - if (fix_quality == 0) { + if (fix_quality <= 0) { _gps_position->fix_type = 0; } else { From 73e9137865942b1c5970838ade92f9abb965f15d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 11 Oct 2014 00:28:14 +0200 Subject: [PATCH 340/411] Fix unsigned comparison --- src/drivers/sf0x/sf0x.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/sf0x/sf0x.cpp b/src/drivers/sf0x/sf0x.cpp index 801bcf40a5..fdd524189c 100644 --- a/src/drivers/sf0x/sf0x.cpp +++ b/src/drivers/sf0x/sf0x.cpp @@ -546,7 +546,7 @@ SF0X::collect() float si_units; bool valid = false; - for (unsigned i = 0; i < ret; i++) { + for (int i = 0; i < ret; i++) { if (OK == sf0x_parser(readbuf[i], _linebuf, &_linebuf_index, &_parse_state, &si_units)) { valid = true; } From b7b48047910d0e25d3349b7dade0b8d34649fdc3 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 11 Oct 2014 00:28:48 +0200 Subject: [PATCH 341/411] GEO: fix compile warnings --- src/lib/geo/geo.c | 17 ++++++++++------- 1 file changed, 10 insertions(+), 7 deletions(-) diff --git a/src/lib/geo/geo.c b/src/lib/geo/geo.c index 1c8d2a2a75..f595467b36 100644 --- a/src/lib/geo/geo.c +++ b/src/lib/geo/geo.c @@ -362,8 +362,12 @@ __EXPORT int get_distance_to_line(struct crosstrack_error_s *crosstrack_error, d crosstrack_error->distance = 0.0f; crosstrack_error->bearing = 0.0f; + dist_to_end = get_distance_to_next_waypoint(lat_now, lon_now, lat_end, lon_end); + // Return error if arguments are bad - if (lat_now == 0.0 || lon_now == 0.0 || lat_start == 0.0 || lon_start == 0.0 || lat_end == 0.0d || lon_end == 0.0d) { return return_value; } + if (dist_to_end < 0.1f) { + return ERROR; + } bearing_end = get_bearing_to_next_waypoint(lat_now, lon_now, lat_end, lon_end); bearing_track = get_bearing_to_next_waypoint(lat_start, lon_start, lat_end, lon_end); @@ -377,7 +381,6 @@ __EXPORT int get_distance_to_line(struct crosstrack_error_s *crosstrack_error, d return return_value; } - dist_to_end = get_distance_to_next_waypoint(lat_now, lon_now, lat_end, lon_end); crosstrack_error->distance = (dist_to_end) * sinf(bearing_diff); if (sin(bearing_diff) >= 0) { @@ -414,10 +417,10 @@ __EXPORT int get_distance_to_arc(struct crosstrack_error_s *crosstrack_error, do crosstrack_error->bearing = 0.0f; // Return error if arguments are bad - if (lat_now == 0.0 || lon_now == 0.0 || lat_center == 0.0 || lon_center == 0.0 || radius == 0.0f) { return return_value; } + if (radius < 0.1f) { return return_value; } - if (arc_sweep >= 0) { + if (arc_sweep >= 0.0f) { bearing_sector_start = arc_start_bearing; bearing_sector_end = arc_start_bearing + arc_sweep; @@ -463,8 +466,8 @@ __EXPORT int get_distance_to_arc(struct crosstrack_error_s *crosstrack_error, do double start_disp_x = (double)radius * sin(arc_start_bearing); double start_disp_y = (double)radius * cos(arc_start_bearing); - double end_disp_x = (double)radius * sin(_wrapPI((double)(arc_start_bearing + arc_sweep))); - double end_disp_y = (double)radius * cos(_wrapPI((double)(arc_start_bearing + arc_sweep))); + double end_disp_x = (double)radius * sin(_wrap_pi((double)(arc_start_bearing + arc_sweep))); + double end_disp_y = (double)radius * cos(_wrap_pi((double)(arc_start_bearing + arc_sweep))); double lon_start = lon_now + start_disp_x / 111111.0; double lat_start = lat_now + start_disp_y * cos(lat_now) / 111111.0; double lon_end = lon_now + end_disp_x / 111111.0; @@ -484,7 +487,7 @@ __EXPORT int get_distance_to_arc(struct crosstrack_error_s *crosstrack_error, do } - crosstrack_error->bearing = _wrapPI((double)crosstrack_error->bearing); + crosstrack_error->bearing = _wrap_pi((double)crosstrack_error->bearing); return_value = OK; return return_value; } From 835bab9115b49d90326a992917e8bd3e088e3fcd Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 12 Oct 2014 15:20:22 +0400 Subject: [PATCH 342/411] UAVCAN update --- uavcan | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/uavcan b/uavcan index 01d5bb242a..4de0338824 160000 --- a/uavcan +++ b/uavcan @@ -1 +1 @@ -Subproject commit 01d5bb242a6197e17e0ed466ed86e7ba42bd7d01 +Subproject commit 4de0338824972de4d3a8e29697ea1a2d95a926c0 From ced75deebe527581930419155fb0d73f18b7a5fe Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 12 Oct 2014 15:34:28 +0400 Subject: [PATCH 343/411] ESC scaling fix --- src/modules/uavcan/actuators/esc.cpp | 15 ++++++--------- src/modules/uavcan/actuators/esc.hpp | 2 +- 2 files changed, 7 insertions(+), 10 deletions(-) diff --git a/src/modules/uavcan/actuators/esc.cpp b/src/modules/uavcan/actuators/esc.cpp index 223d947312..b516536c75 100644 --- a/src/modules/uavcan/actuators/esc.cpp +++ b/src/modules/uavcan/actuators/esc.cpp @@ -93,25 +93,22 @@ void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs) */ uavcan::equipment::esc::RawCommand msg; + static const int cmd_max = uavcan::equipment::esc::RawCommand::FieldTypes::cmd::RawValueType::max(); + if (_armed) { for (unsigned i = 0; i < num_outputs; i++) { - float scaled = (outputs[i] + 1.0F) * 0.5F * uavcan::equipment::esc::RawCommand::CMD_MAX; + float scaled = (outputs[i] + 1.0F) * 0.5F * cmd_max; if (scaled < 1.0F) { scaled = 1.0F; // Since we're armed, we don't want to stop it completely } - if (scaled < uavcan::equipment::esc::RawCommand::CMD_MIN) { - scaled = uavcan::equipment::esc::RawCommand::CMD_MIN; + if (scaled > cmd_max) { + scaled = cmd_max; perf_count(_perfcnt_scaling_error); - } else if (scaled > uavcan::equipment::esc::RawCommand::CMD_MAX) { - scaled = uavcan::equipment::esc::RawCommand::CMD_MAX; - perf_count(_perfcnt_scaling_error); - } else { - ; // Correct value } - msg.cmd.push_back(static_cast(scaled)); + msg.cmd.push_back(static_cast(scaled)); } } diff --git a/src/modules/uavcan/actuators/esc.hpp b/src/modules/uavcan/actuators/esc.hpp index cf09882104..0add7e0ad4 100644 --- a/src/modules/uavcan/actuators/esc.hpp +++ b/src/modules/uavcan/actuators/esc.hpp @@ -73,7 +73,7 @@ private: void orb_pub_timer_cb(const uavcan::TimerEvent &event); - static constexpr unsigned MAX_RATE_HZ = 100; ///< XXX make this configurable + static constexpr unsigned MAX_RATE_HZ = 200; ///< XXX make this configurable static constexpr unsigned ESC_STATUS_UPDATE_RATE_HZ = 5; static constexpr unsigned MAX_ESCS = uavcan::equipment::esc::RawCommand::FieldTypes::cmd::MaxSize; From e5a77a638a53aa9baec2ffe9d8ad96fb095b0966 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 12 Oct 2014 20:56:45 +0400 Subject: [PATCH 344/411] ESC status feedback --- src/modules/uavcan/actuators/esc.cpp | 29 +++++++++++++++++++++++++--- src/modules/uavcan/actuators/esc.hpp | 15 +++++++------- 2 files changed, 33 insertions(+), 11 deletions(-) diff --git a/src/modules/uavcan/actuators/esc.cpp b/src/modules/uavcan/actuators/esc.cpp index b516536c75..0601d9fa25 100644 --- a/src/modules/uavcan/actuators/esc.cpp +++ b/src/modules/uavcan/actuators/esc.cpp @@ -73,7 +73,7 @@ int UavcanEscController::init() void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs) { - if ((outputs == nullptr) || (num_outputs > MAX_ESCS)) { + if ((outputs == nullptr) || (num_outputs > uavcan::equipment::esc::RawCommand::FieldTypes::cmd::MaxSize)) { perf_count(_perfcnt_invalid_input); return; } @@ -126,10 +126,33 @@ void UavcanEscController::arm_esc(bool arm) void UavcanEscController::esc_status_sub_cb(const uavcan::ReceivedDataStructure &msg) { - // TODO save status into a local storage; publish to ORB later from orb_pub_timer_cb() + if (msg.esc_index < CONNECTED_ESC_MAX) { + _esc_status.esc_count = uavcan::max(_esc_status.esc_count, msg.esc_index + 1); + _esc_status.timestamp = msg.getMonotonicTimestamp().toUSec(); + + auto &ref = _esc_status.esc[msg.esc_index]; + + ref.esc_address = msg.getSrcNodeID().get(); + + // >0 checks allow to weed out NaNs and negative values that aren't supported. + ref.esc_voltage = (msg.voltage > 0) ? msg.voltage * 10.0F : 0; + ref.esc_current = (msg.current > 0) ? msg.current * 10.0F : 0; + ref.esc_temperature = (msg.temperature > 0) ? msg.temperature * 10.0F : 0; + + ref.esc_setpoint = msg.power_rating_pct; + ref.esc_rpm = abs(msg.rpm); + ref.esc_errorcount = msg.error_count; + } } void UavcanEscController::orb_pub_timer_cb(const uavcan::TimerEvent&) { - // TODO publish to ORB + _esc_status.counter += 1; + _esc_status.esc_connectiontype = ESC_CONNECTION_TYPE_CAN; + + if (_esc_status_pub > 0) { + (void)orb_publish(ORB_ID(esc_status), _esc_status_pub, &_esc_status); + } else { + _esc_status_pub = orb_advertise(ORB_ID(esc_status), &_esc_status); + } } diff --git a/src/modules/uavcan/actuators/esc.hpp b/src/modules/uavcan/actuators/esc.hpp index 0add7e0ad4..f4a3877e62 100644 --- a/src/modules/uavcan/actuators/esc.hpp +++ b/src/modules/uavcan/actuators/esc.hpp @@ -48,6 +48,8 @@ #include #include #include +#include + class UavcanEscController { @@ -74,8 +76,7 @@ private: static constexpr unsigned MAX_RATE_HZ = 200; ///< XXX make this configurable - static constexpr unsigned ESC_STATUS_UPDATE_RATE_HZ = 5; - static constexpr unsigned MAX_ESCS = uavcan::equipment::esc::RawCommand::FieldTypes::cmd::MaxSize; + static constexpr unsigned ESC_STATUS_UPDATE_RATE_HZ = 10; typedef uavcan::MethodBinder&)> @@ -84,6 +85,10 @@ private: typedef uavcan::MethodBinder TimerCbBinder; + bool _armed = false; + esc_status_s _esc_status = {}; + orb_advert_t _esc_status_pub = -1; + /* * libuavcan related things */ @@ -93,12 +98,6 @@ private: uavcan::Subscriber _uavcan_sub_status; uavcan::TimerEventForwarder _orb_timer; - /* - * ESC states - */ - bool _armed = false; - uavcan::equipment::esc::Status _states[MAX_ESCS]; - /* * Perf counters */ From db0b892c23d56d6363ea91988d2a25e6b4d48bce Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Sun, 12 Oct 2014 11:38:34 -0700 Subject: [PATCH 345/411] Fix 8+ channel binding --- src/drivers/drv_pwm_output.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/drv_pwm_output.h b/src/drivers/drv_pwm_output.h index bc586f3950..f66ec7c95d 100644 --- a/src/drivers/drv_pwm_output.h +++ b/src/drivers/drv_pwm_output.h @@ -165,7 +165,7 @@ ORB_DECLARE(output_pwm); #define DSM2_BIND_PULSES 3 /* DSM_BIND_START ioctl parameter, pulses required to start dsm2 pairing */ #define DSMX_BIND_PULSES 7 /* DSM_BIND_START ioctl parameter, pulses required to start dsmx pairing */ -#define DSMX8_BIND_PULSES 10 /* DSM_BIND_START ioctl parameter, pulses required to start 8 or more channel dsmx pairing */ +#define DSMX8_BIND_PULSES 9 /* DSM_BIND_START ioctl parameter, pulses required to start 8 or more channel dsmx pairing */ /** power up DSM receiver */ #define DSM_BIND_POWER_UP _IOC(_PWM_SERVO_BASE, 11) From 1bf4270e3ee6f33f8adf0027c1a59f3fc0b35263 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Mon, 13 Oct 2014 17:01:34 +0400 Subject: [PATCH 346/411] Update ORB topic 'esc_status' --- src/drivers/hott/messages.cpp | 14 +++++++------- src/drivers/mkblctrl/mkblctrl.cpp | 6 +++--- src/modules/sdlog2/sdlog2_messages.h | 14 +++++++------- src/modules/uORB/topics/esc_status.h | 19 ++++++++++--------- src/modules/uavcan/actuators/esc.cpp | 14 ++++++-------- 5 files changed, 33 insertions(+), 34 deletions(-) diff --git a/src/drivers/hott/messages.cpp b/src/drivers/hott/messages.cpp index 0861325739..f1b12b0678 100644 --- a/src/drivers/hott/messages.cpp +++ b/src/drivers/hott/messages.cpp @@ -115,9 +115,9 @@ publish_gam_message(const uint8_t *buffer) esc.esc[0].esc_vendor = ESC_VENDOR_GRAUPNER_HOTT; esc.esc[0].esc_rpm = (uint16_t)((msg.rpm_H << 8) | (msg.rpm_L & 0xff)) * 10; - esc.esc[0].esc_temperature = msg.temperature1 - 20; - esc.esc[0].esc_voltage = (uint16_t)((msg.main_voltage_H << 8) | (msg.main_voltage_L & 0xff)); - esc.esc[0].esc_current = (uint16_t)((msg.current_H << 8) | (msg.current_L & 0xff)); + esc.esc[0].esc_temperature = static_cast(msg.temperature1) - 20.0F; + esc.esc[0].esc_voltage = static_cast((msg.main_voltage_H << 8) | (msg.main_voltage_L & 0xff)) * 0.1F; + esc.esc[0].esc_current = static_cast((msg.current_H << 8) | (msg.current_L & 0xff)) * 0.1F; /* announce the esc if needed, just publish else */ if (_esc_pub > 0) { @@ -186,18 +186,18 @@ build_gam_response(uint8_t *buffer, size_t *size) msg.gam_sensor_id = GAM_SENSOR_ID; msg.sensor_text_id = GAM_SENSOR_TEXT_ID; - msg.temperature1 = (uint8_t)(esc.esc[0].esc_temperature + 20); + msg.temperature1 = (uint8_t)(esc.esc[0].esc_temperature + 20.0F); msg.temperature2 = 20; // 0 deg. C. - uint16_t voltage = (uint16_t)(esc.esc[0].esc_voltage); + const uint16_t voltage = (uint16_t)(esc.esc[0].esc_voltage * 10.0F); msg.main_voltage_L = (uint8_t)voltage & 0xff; msg.main_voltage_H = (uint8_t)(voltage >> 8) & 0xff; - uint16_t current = (uint16_t)(esc.esc[0].esc_current); + const uint16_t current = (uint16_t)(esc.esc[0].esc_current * 10.0F); msg.current_L = (uint8_t)current & 0xff; msg.current_H = (uint8_t)(current >> 8) & 0xff; - uint16_t rpm = (uint16_t)(esc.esc[0].esc_rpm * 0.1f); + const uint16_t rpm = (uint16_t)(esc.esc[0].esc_rpm * 0.1f); msg.rpm_L = (uint8_t)rpm & 0xff; msg.rpm_H = (uint8_t)(rpm >> 8) & 0xff; diff --git a/src/drivers/mkblctrl/mkblctrl.cpp b/src/drivers/mkblctrl/mkblctrl.cpp index 3996b76a64..1055487cb9 100644 --- a/src/drivers/mkblctrl/mkblctrl.cpp +++ b/src/drivers/mkblctrl/mkblctrl.cpp @@ -600,8 +600,8 @@ MK::task_main() esc.esc[i].esc_address = (uint8_t) BLCTRL_BASE_ADDR + i; esc.esc[i].esc_vendor = ESC_VENDOR_MIKROKOPTER; esc.esc[i].esc_version = (uint16_t) Motor[i].Version; - esc.esc[i].esc_voltage = (uint16_t) 0; - esc.esc[i].esc_current = (uint16_t) Motor[i].Current; + esc.esc[i].esc_voltage = 0.0F; + esc.esc[i].esc_current = static_cast(Motor[i].Current) * 0.1F; esc.esc[i].esc_rpm = (uint16_t) 0; esc.esc[i].esc_setpoint = (float) Motor[i].SetPoint_PX4; @@ -614,7 +614,7 @@ MK::task_main() esc.esc[i].esc_setpoint_raw = (uint16_t) Motor[i].SetPoint; } - esc.esc[i].esc_temperature = (uint16_t) Motor[i].Temperature; + esc.esc[i].esc_temperature = static_cast(Motor[i].Temperature); esc.esc[i].esc_state = (uint16_t) Motor[i].State; esc.esc[i].esc_errorcount = (uint16_t) 0; diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index d2845eb753..aeff4fd305 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -241,15 +241,15 @@ struct log_GPSP_s { #define LOG_ESC_MSG 18 struct log_ESC_s { uint16_t counter; - uint8_t esc_count; - uint8_t esc_connectiontype; - uint8_t esc_num; + uint8_t esc_count; + uint8_t esc_connectiontype; + uint8_t esc_num; uint16_t esc_address; uint16_t esc_version; - uint16_t esc_voltage; - uint16_t esc_current; + float esc_voltage; + float esc_current; uint16_t esc_rpm; - uint16_t esc_temperature; + float esc_temperature; float esc_setpoint; uint16_t esc_setpoint_raw; }; @@ -452,7 +452,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(FLOW, "hhfffBB", "RawX,RawY,CompX,CompY,Dist,Q,SensID"), LOG_FORMAT(GPOS, "LLfffffff", "Lat,Lon,Alt,VelN,VelE,VelD,EPH,EPV,TALT"), LOG_FORMAT(GPSP, "BLLffBfbf", "NavState,Lat,Lon,Alt,Yaw,Type,LoitR,LoitDir,PitMin"), - LOG_FORMAT(ESC, "HBBBHHHHHHfH", "count,nESC,Conn,N,Ver,Adr,Volt,Amp,RPM,Temp,SetP,SetPRAW"), + LOG_FORMAT(ESC, "HBBBHHffHffH", "count,nESC,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"), LOG_FORMAT(DIST, "ffB", "Bottom,BottomRate,Flags"), diff --git a/src/modules/uORB/topics/esc_status.h b/src/modules/uORB/topics/esc_status.h index 628824efa8..b82796908e 100644 --- a/src/modules/uORB/topics/esc_status.h +++ b/src/modules/uORB/topics/esc_status.h @@ -78,6 +78,7 @@ enum ESC_CONNECTION_TYPE { /** * Electronic speed controller status. + * Unsupported float fields should be assigned NaN. */ struct esc_status_s { /* use of a counter and timestamp recommended (but not necessary) */ @@ -89,17 +90,17 @@ struct esc_status_s { enum ESC_CONNECTION_TYPE esc_connectiontype; /**< how ESCs connected to the system */ struct { - uint16_t esc_address; /**< Address of current ESC (in most cases 1-8 / must be set by driver) */ enum ESC_VENDOR esc_vendor; /**< Vendor of current ESC */ - uint16_t esc_version; /**< Version of current ESC - if supported */ - uint16_t esc_voltage; /**< Voltage measured from current ESC - if supported */ - uint16_t esc_current; /**< Current measured from current ESC (100mA steps) - if supported */ - uint16_t esc_rpm; /**< RPM measured from current ESC - if supported */ - uint16_t esc_temperature; /**< Temperature measured from current ESC - if supported */ - float esc_setpoint; /**< setpoint of current ESC */ + uint32_t esc_errorcount; /**< Number of reported errors by ESC - if supported */ + float esc_voltage; /**< Voltage measured from current ESC [V] - if supported */ + float esc_current; /**< Current measured from current ESC [A] - if supported */ + float esc_temperature; /**< Temperature measured from current ESC [degC] - if supported */ + float esc_setpoint; /**< setpoint of current ESC */ uint16_t esc_setpoint_raw; /**< setpoint of current ESC (Value sent to ESC) */ - uint16_t esc_state; /**< State of ESC - depend on Vendor */ - uint16_t esc_errorcount; /**< Number of reported errors by ESC - if supported */ + uint16_t esc_address; /**< Address of current ESC (in most cases 1-8 / must be set by driver) */ + uint16_t esc_version; /**< Version of current ESC - if supported */ + uint16_t esc_rpm; /**< RPM measured from current ESC [RPM] - if supported */ + uint16_t esc_state; /**< State of ESC - depend on Vendor */ } esc[CONNECTED_ESC_MAX]; }; diff --git a/src/modules/uavcan/actuators/esc.cpp b/src/modules/uavcan/actuators/esc.cpp index 0601d9fa25..249ffba07c 100644 --- a/src/modules/uavcan/actuators/esc.cpp +++ b/src/modules/uavcan/actuators/esc.cpp @@ -134,14 +134,12 @@ void UavcanEscController::esc_status_sub_cb(const uavcan::ReceivedDataStructure< ref.esc_address = msg.getSrcNodeID().get(); - // >0 checks allow to weed out NaNs and negative values that aren't supported. - ref.esc_voltage = (msg.voltage > 0) ? msg.voltage * 10.0F : 0; - ref.esc_current = (msg.current > 0) ? msg.current * 10.0F : 0; - ref.esc_temperature = (msg.temperature > 0) ? msg.temperature * 10.0F : 0; - - ref.esc_setpoint = msg.power_rating_pct; - ref.esc_rpm = abs(msg.rpm); - ref.esc_errorcount = msg.error_count; + ref.esc_voltage = msg.voltage; + ref.esc_current = msg.current; + ref.esc_temperature = msg.temperature; + ref.esc_setpoint = msg.power_rating_pct; + ref.esc_rpm = abs(msg.rpm); + ref.esc_errorcount = msg.error_count; } } From ecd144b8b4c334f4eb7a0e39e47065d6958fefaf Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Mon, 13 Oct 2014 17:07:14 +0400 Subject: [PATCH 347/411] Publishing esc_setpoint_raw from the UAVCAN driver --- src/modules/uavcan/actuators/esc.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/uavcan/actuators/esc.cpp b/src/modules/uavcan/actuators/esc.cpp index 249ffba07c..08e5049477 100644 --- a/src/modules/uavcan/actuators/esc.cpp +++ b/src/modules/uavcan/actuators/esc.cpp @@ -109,6 +109,8 @@ void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs) } msg.cmd.push_back(static_cast(scaled)); + + _esc_status.esc[i].esc_setpoint_raw = abs(static_cast(scaled)); } } From 6ec338c23f0b97d998401346ed9fa35cccecf0ae Mon Sep 17 00:00:00 2001 From: James Harrison Date: Tue, 14 Oct 2014 22:18:51 +0100 Subject: [PATCH 348/411] Fix px_romfs_pruner.py to skip .data files The .data extension was missing but used in some places; this would cause the pruner to fail with a UnicodeDecodeError. --- Tools/px_romfs_pruner.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Tools/px_romfs_pruner.py b/Tools/px_romfs_pruner.py index fcc40b09e1..aef1cc7a3b 100644 --- a/Tools/px_romfs_pruner.py +++ b/Tools/px_romfs_pruner.py @@ -57,7 +57,7 @@ def main(): for (root, dirs, files) in os.walk(args.folder): for file in files: # only prune text files - if ".zip" in file or ".bin" in file or ".swp" in file: + if ".zip" in file or ".bin" in file or ".swp" in file or ".data" in file: continue file_path = os.path.join(root, file) From f9856c6228396ca2fe8eb4c86e12c60df14d6b4c Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Wed, 15 Oct 2014 13:46:16 +0400 Subject: [PATCH 349/411] ESC status - supporting negative RPM --- src/modules/sdlog2/sdlog2_messages.h | 4 ++-- src/modules/uORB/topics/esc_status.h | 2 +- src/modules/uavcan/actuators/esc.cpp | 2 +- 3 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index aeff4fd305..d49fc0c799 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -248,7 +248,7 @@ struct log_ESC_s { uint16_t esc_version; float esc_voltage; float esc_current; - uint16_t esc_rpm; + int32_t esc_rpm; float esc_temperature; float esc_setpoint; uint16_t esc_setpoint_raw; @@ -452,7 +452,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(FLOW, "hhfffBB", "RawX,RawY,CompX,CompY,Dist,Q,SensID"), LOG_FORMAT(GPOS, "LLfffffff", "Lat,Lon,Alt,VelN,VelE,VelD,EPH,EPV,TALT"), LOG_FORMAT(GPSP, "BLLffBfbf", "NavState,Lat,Lon,Alt,Yaw,Type,LoitR,LoitDir,PitMin"), - LOG_FORMAT(ESC, "HBBBHHffHffH", "count,nESC,Conn,N,Ver,Adr,Volt,Amp,RPM,Temp,SetP,SetPRAW"), + LOG_FORMAT(ESC, "HBBBHHffiffH", "count,nESC,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"), LOG_FORMAT(DIST, "ffB", "Bottom,BottomRate,Flags"), diff --git a/src/modules/uORB/topics/esc_status.h b/src/modules/uORB/topics/esc_status.h index b82796908e..98cdad3d52 100644 --- a/src/modules/uORB/topics/esc_status.h +++ b/src/modules/uORB/topics/esc_status.h @@ -99,7 +99,7 @@ struct esc_status_s { uint16_t esc_setpoint_raw; /**< setpoint of current ESC (Value sent to ESC) */ uint16_t esc_address; /**< Address of current ESC (in most cases 1-8 / must be set by driver) */ uint16_t esc_version; /**< Version of current ESC - if supported */ - uint16_t esc_rpm; /**< RPM measured from current ESC [RPM] - if supported */ + int32_t esc_rpm; /**< Motor RPM, negative for reverse rotation [RPM] - if supported */ uint16_t esc_state; /**< State of ESC - depend on Vendor */ } esc[CONNECTED_ESC_MAX]; diff --git a/src/modules/uavcan/actuators/esc.cpp b/src/modules/uavcan/actuators/esc.cpp index 08e5049477..1990651ef2 100644 --- a/src/modules/uavcan/actuators/esc.cpp +++ b/src/modules/uavcan/actuators/esc.cpp @@ -140,7 +140,7 @@ void UavcanEscController::esc_status_sub_cb(const uavcan::ReceivedDataStructure< ref.esc_current = msg.current; ref.esc_temperature = msg.temperature; ref.esc_setpoint = msg.power_rating_pct; - ref.esc_rpm = abs(msg.rpm); + ref.esc_rpm = msg.rpm; ref.esc_errorcount = msg.error_count; } } From 7fd3bd9af8ee150911a08e75a1e30fa4cb2b8e2d Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Wed, 15 Oct 2014 14:01:17 +0400 Subject: [PATCH 350/411] esc_status layout optimization --- src/modules/uORB/topics/esc_status.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/uORB/topics/esc_status.h b/src/modules/uORB/topics/esc_status.h index 98cdad3d52..b45c49788d 100644 --- a/src/modules/uORB/topics/esc_status.h +++ b/src/modules/uORB/topics/esc_status.h @@ -92,6 +92,7 @@ struct esc_status_s { struct { enum ESC_VENDOR esc_vendor; /**< Vendor of current ESC */ uint32_t esc_errorcount; /**< Number of reported errors by ESC - if supported */ + int32_t esc_rpm; /**< Motor RPM, negative for reverse rotation [RPM] - if supported */ float esc_voltage; /**< Voltage measured from current ESC [V] - if supported */ float esc_current; /**< Current measured from current ESC [A] - if supported */ float esc_temperature; /**< Temperature measured from current ESC [degC] - if supported */ @@ -99,7 +100,6 @@ struct esc_status_s { uint16_t esc_setpoint_raw; /**< setpoint of current ESC (Value sent to ESC) */ uint16_t esc_address; /**< Address of current ESC (in most cases 1-8 / must be set by driver) */ uint16_t esc_version; /**< Version of current ESC - if supported */ - int32_t esc_rpm; /**< Motor RPM, negative for reverse rotation [RPM] - if supported */ uint16_t esc_state; /**< State of ESC - depend on Vendor */ } esc[CONNECTED_ESC_MAX]; From 0d917576d484d2e2dc0233c5545a16e36f6e2f41 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 15 Oct 2014 22:19:06 +0200 Subject: [PATCH 351/411] Enable flaps in manual override --- src/drivers/px4io/px4io.cpp | 9 ++++++++- src/modules/px4iofirmware/controls.c | 14 +++++++++++--- 2 files changed, 19 insertions(+), 4 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index d212be7661..73160b2d9d 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1265,11 +1265,18 @@ PX4IO::io_set_rc_config() if ((ichan >= 0) && (ichan < (int)_max_rc_input)) input_map[ichan - 1] = 3; - param_get(param_find("RC_MAP_MODE_SW"), &ichan); + param_get(param_find("RC_MAP_FLAPS"), &ichan); if ((ichan >= 0) && (ichan < (int)_max_rc_input)) input_map[ichan - 1] = 4; + param_get(param_find("RC_MAP_MODE_SW"), &ichan); + + if ((ichan >= 0) && (ichan < (int)_max_rc_input)) { + /* use out of normal bounds index to indicate special channel */ + input_map[ichan - 1] = 8; + } + /* * Iterate all possible RC inputs. */ diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c index 7b127759ad..0b0832d557 100644 --- a/src/modules/px4iofirmware/controls.c +++ b/src/modules/px4iofirmware/controls.c @@ -60,6 +60,8 @@ static perf_counter_t c_gather_ppm; static int _dsm_fd; +static uint16_t rc_value_override = 0; + bool dsm_port_input(uint16_t *rssi, bool *dsm_updated, bool *st24_updated) { perf_begin(c_gather_dsm); @@ -313,8 +315,14 @@ controls_tick() { } } - r_rc_values[mapped] = SIGNED_TO_REG(scaled); - assigned_channels |= (1 << mapped); + /* pick out override channel, indicated by mapping 8 (9 th channel, virtual) */ + if (mapped == 8) { + rc_value_override = SIGNED_TO_REG(scaled); + } else { + /* normal channel */ + r_rc_values[mapped] = SIGNED_TO_REG(scaled); + assigned_channels |= (1 << mapped); + } } } @@ -409,7 +417,7 @@ controls_tick() { * requested override. * */ - if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) && (REG_TO_SIGNED(r_rc_values[4]) < RC_CHANNEL_LOW_THRESH)) + if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) && (REG_TO_SIGNED(rc_value_override) < RC_CHANNEL_LOW_THRESH)) override = true; if (override) { From f4903316329ca91b9e872327f701de5b6a2cc13c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 16 Oct 2014 22:57:27 +0200 Subject: [PATCH 352/411] Enable flaps, avoid mode switch position --- src/drivers/px4io/px4io.cpp | 2 +- src/modules/px4iofirmware/controls.c | 13 +++++-------- src/modules/px4iofirmware/mixer.cpp | 7 ------- src/modules/px4iofirmware/protocol.h | 1 + src/modules/px4iofirmware/registers.c | 3 ++- 5 files changed, 9 insertions(+), 17 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 73160b2d9d..3871b4a2cb 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1274,7 +1274,7 @@ PX4IO::io_set_rc_config() if ((ichan >= 0) && (ichan < (int)_max_rc_input)) { /* use out of normal bounds index to indicate special channel */ - input_map[ichan - 1] = 8; + input_map[ichan - 1] = PX4IO_P_RC_CONFIG_ASSIGNMENT_MODESWITCH; } /* diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c index 0b0832d557..ad60ee03e0 100644 --- a/src/modules/px4iofirmware/controls.c +++ b/src/modules/px4iofirmware/controls.c @@ -315,15 +315,12 @@ controls_tick() { } } - /* pick out override channel, indicated by mapping 8 (9 th channel, virtual) */ - if (mapped == 8) { - rc_value_override = SIGNED_TO_REG(scaled); - } else { - /* normal channel */ - r_rc_values[mapped] = SIGNED_TO_REG(scaled); - assigned_channels |= (1 << mapped); - } + r_rc_values[mapped] = SIGNED_TO_REG(scaled); + assigned_channels |= (1 << mapped); + } else if (mapped == PX4IO_P_RC_CONFIG_ASSIGNMENT_MODESWITCH) { + /* pick out override channel, indicated by special mapping */ + rc_value_override = SIGNED_TO_REG(scaled); } } } diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index 3e19333d8e..c0b8ac358c 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -60,13 +60,6 @@ extern "C" { */ #define FMU_INPUT_DROP_LIMIT_US 500000 -/* XXX need to move the RC_CHANNEL_FUNCTION out of rc_channels.h and into systemlib */ -#define ROLL 0 -#define PITCH 1 -#define YAW 2 -#define THROTTLE 3 -#define OVERRIDE 4 - /* current servo arm/disarm state */ static bool mixer_servos_armed = false; static bool should_arm = false; diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h index 89a470b44b..a3e8a58d31 100644 --- a/src/modules/px4iofirmware/protocol.h +++ b/src/modules/px4iofirmware/protocol.h @@ -246,6 +246,7 @@ enum { /* DSM bind states */ #define PX4IO_P_RC_CONFIG_MAX 2 /**< highest input value */ #define PX4IO_P_RC_CONFIG_DEADZONE 3 /**< band around center that is ignored */ #define PX4IO_P_RC_CONFIG_ASSIGNMENT 4 /**< mapped input value */ +#define PX4IO_P_RC_CONFIG_ASSIGNMENT_MODESWITCH 100 /**< magic value for mode switch */ #define PX4IO_P_RC_CONFIG_OPTIONS 5 /**< channel options bitmask */ #define PX4IO_P_RC_CONFIG_OPTIONS_ENABLED (1 << 0) #define PX4IO_P_RC_CONFIG_OPTIONS_REVERSE (1 << 1) diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index 7a5a5e4846..7f19e983f4 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -686,7 +686,8 @@ 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 (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] >= PX4IO_RC_MAPPED_CONTROL_CHANNELS) { + } else if ((conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] >= PX4IO_RC_MAPPED_CONTROL_CHANNELS) && + (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] != PX4IO_P_RC_CONFIG_ASSIGNMENT_MODESWITCH)) { count++; } From 5bc2b34e482fe8c4b0cab8f9748bd97dc3e17291 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 17 Oct 2014 09:37:21 +0200 Subject: [PATCH 353/411] Reset performance counters on arming to allow better resolution during flight --- src/modules/sdlog2/sdlog2.c | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index e135930778..1282bc0ea6 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -628,6 +628,9 @@ void sdlog2_start_log() perf_print_all(perf_fd); close(perf_fd); + /* reset performance counters to get in-flight min and max values in post flight log */ + perf_reset_all(); + logging_enabled = true; } From f500ad4699da9b56ecfd0e413532119577df837d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 17 Oct 2014 09:38:04 +0200 Subject: [PATCH 354/411] Log messages sent via MAVLink --- src/modules/mavlink/mavlink_messages.cpp | 29 ++++++++++++++++++++++++ 1 file changed, 29 insertions(+) diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index c182cfdb95..a4ed14914d 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -341,11 +341,18 @@ private: /* do not allow top copying this class */ MavlinkStreamStatustext(MavlinkStreamStatustext &); MavlinkStreamStatustext& operator = (const MavlinkStreamStatustext &); + FILE *fp = nullptr; protected: explicit MavlinkStreamStatustext(Mavlink *mavlink) : MavlinkStream(mavlink) {} + ~MavlinkStreamStatustext() { + if (fp) { + fclose(fp); + } + } + void send(const hrt_abstime t) { if (!mavlink_logbuffer_is_empty(_mavlink->get_logbuffer())) { @@ -359,6 +366,28 @@ protected: strncpy(msg.text, logmsg.text, sizeof(msg.text)); _mavlink->send_message(MAVLINK_MSG_ID_STATUSTEXT, &msg); + + if (fp) { + fputs(msg.text, fp); + fputs("\n", fp); + fsync(fileno(fp)); + } else { + /* string to hold the path to the log */ + char log_file_name[32] = ""; + char log_file_path[64] = ""; + + timespec ts; + clock_gettime(CLOCK_REALTIME, &ts); + /* use GPS time for log file naming, e.g. /fs/microsd/2014-01-19/19_37_52.bin */ + time_t gps_time_sec = ts.tv_sec + (ts.tv_nsec / 1e9); + struct tm t; + gmtime_r(&gps_time_sec, &t); + + // XXX we do not want to interfere here with the SD log app + strftime(log_file_name, sizeof(log_file_name), "msgs_%Y_%m_%d_%H_%M_%S.txt", &t); + snprintf(log_file_path, sizeof(log_file_path), "/fs/microsd/%s", log_file_name); + fp = fopen(log_file_path, "ab"); + } } } } From a1ea16f7947bedb258ebc198cde52e5976ed0fdd Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 17 Oct 2014 09:39:50 +0200 Subject: [PATCH 355/411] Log text messages only in first instance --- src/modules/mavlink/mavlink_messages.cpp | 39 +++++++++++++----------- 1 file changed, 21 insertions(+), 18 deletions(-) diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index a4ed14914d..cccb698bf9 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -367,26 +367,29 @@ protected: _mavlink->send_message(MAVLINK_MSG_ID_STATUSTEXT, &msg); - if (fp) { - fputs(msg.text, fp); - fputs("\n", fp); - fsync(fileno(fp)); - } else { - /* string to hold the path to the log */ - char log_file_name[32] = ""; - char log_file_path[64] = ""; + /* write log messages in first instance to disk */ + if (_mavlink->get_instance_id() == 0) { + if (fp) { + fputs(msg.text, fp); + fputs("\n", fp); + fsync(fileno(fp)); + } else { + /* string to hold the path to the log */ + char log_file_name[32] = ""; + char log_file_path[64] = ""; - timespec ts; - clock_gettime(CLOCK_REALTIME, &ts); - /* use GPS time for log file naming, e.g. /fs/microsd/2014-01-19/19_37_52.bin */ - time_t gps_time_sec = ts.tv_sec + (ts.tv_nsec / 1e9); - struct tm t; - gmtime_r(&gps_time_sec, &t); + timespec ts; + clock_gettime(CLOCK_REALTIME, &ts); + /* use GPS time for log file naming, e.g. /fs/microsd/2014-01-19/19_37_52.bin */ + time_t gps_time_sec = ts.tv_sec + (ts.tv_nsec / 1e9); + struct tm t; + gmtime_r(&gps_time_sec, &t); - // XXX we do not want to interfere here with the SD log app - strftime(log_file_name, sizeof(log_file_name), "msgs_%Y_%m_%d_%H_%M_%S.txt", &t); - snprintf(log_file_path, sizeof(log_file_path), "/fs/microsd/%s", log_file_name); - fp = fopen(log_file_path, "ab"); + // XXX we do not want to interfere here with the SD log app + strftime(log_file_name, sizeof(log_file_name), "msgs_%Y_%m_%d_%H_%M_%S.txt", &t); + snprintf(log_file_path, sizeof(log_file_path), "/fs/microsd/%s", log_file_name); + fp = fopen(log_file_path, "ab"); + } } } } From 99bfbb6dc3c2d8f60b356945026e586397d66170 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 14 Oct 2014 10:07:40 +1100 Subject: [PATCH 356/411] ll40ls: add last distance in "ll40ls info" output --- src/drivers/ll40ls/ll40ls.cpp | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/src/drivers/ll40ls/ll40ls.cpp b/src/drivers/ll40ls/ll40ls.cpp index a69e6ee556..7fd023ec45 100644 --- a/src/drivers/ll40ls/ll40ls.cpp +++ b/src/drivers/ll40ls/ll40ls.cpp @@ -132,6 +132,7 @@ private: perf_counter_t _sample_perf; perf_counter_t _comms_errors; perf_counter_t _buffer_overflows; + uint16_t _last_distance; /** * Test whether the device supported by the driver is present at a @@ -200,7 +201,8 @@ LL40LS::LL40LS(int bus, int address) : _range_finder_topic(-1), _sample_perf(perf_alloc(PC_ELAPSED, "ll40ls_read")), _comms_errors(perf_alloc(PC_COUNT, "ll40ls_comms_errors")), - _buffer_overflows(perf_alloc(PC_COUNT, "ll40ls_buffer_overflows")) + _buffer_overflows(perf_alloc(PC_COUNT, "ll40ls_buffer_overflows")), + _last_distance(0) { // up the retries since the device misses the first measure attempts I2C::_retries = 3; @@ -521,6 +523,8 @@ LL40LS::collect() float si_units = distance * 0.01f; /* cm to m */ struct range_finder_report report; + _last_distance = distance; + /* this should be fairly close to the end of the measurement, so the best approximation of the time */ report.timestamp = hrt_absolute_time(); report.error_count = perf_event_count(_comms_errors); @@ -648,6 +652,8 @@ LL40LS::print_info() perf_print_counter(_buffer_overflows); printf("poll interval: %u ticks\n", _measure_ticks); _reports->print_info("report queue"); + printf("distance: %ucm (0x%04x)\n", + (unsigned)_last_distance, (unsigned)_last_distance); } /** From c6ada17f685fc16c0882d890be089c23a49b7390 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 20 Oct 2014 07:05:58 +1100 Subject: [PATCH 357/411] ll40ls: support either internal or external I2C bus --- src/drivers/ll40ls/ll40ls.cpp | 187 ++++++++++++++++++++++++---------- 1 file changed, 131 insertions(+), 56 deletions(-) diff --git a/src/drivers/ll40ls/ll40ls.cpp b/src/drivers/ll40ls/ll40ls.cpp index 7fd023ec45..8b156f7ba6 100644 --- a/src/drivers/ll40ls/ll40ls.cpp +++ b/src/drivers/ll40ls/ll40ls.cpp @@ -74,7 +74,8 @@ /* Configuration Constants */ #define LL40LS_BUS PX4_I2C_BUS_EXPANSION #define LL40LS_BASEADDR 0x42 /* 7-bit address */ -#define LL40LS_DEVICE_PATH "/dev/ll40ls" +#define LL40LS_DEVICE_PATH_INT "/dev/ll40ls_int" +#define LL40LS_DEVICE_PATH_EXT "/dev/ll40ls_ext" /* LL40LS Registers addresses */ @@ -101,7 +102,7 @@ static const int ERROR = -1; class LL40LS : public device::I2C { public: - LL40LS(int bus = LL40LS_BUS, int address = LL40LS_BASEADDR); + LL40LS(int bus, const char *path, int address = LL40LS_BASEADDR); virtual ~LL40LS(); virtual int init(); @@ -134,6 +135,9 @@ private: perf_counter_t _buffer_overflows; uint16_t _last_distance; + /**< the bus the device is connected to */ + int _bus; + /** * Test whether the device supported by the driver is present at a * specific address. @@ -189,8 +193,8 @@ private: */ extern "C" __EXPORT int ll40ls_main(int argc, char *argv[]); -LL40LS::LL40LS(int bus, int address) : - I2C("LL40LS", LL40LS_DEVICE_PATH, bus, address, 100000), +LL40LS::LL40LS(int bus, const char *path, int address) : + I2C("LL40LS", path, bus, address, 100000), _min_distance(LL40LS_MIN_DISTANCE), _max_distance(LL40LS_MAX_DISTANCE), _reports(nullptr), @@ -202,7 +206,8 @@ LL40LS::LL40LS(int bus, int address) : _sample_perf(perf_alloc(PC_ELAPSED, "ll40ls_read")), _comms_errors(perf_alloc(PC_COUNT, "ll40ls_comms_errors")), _buffer_overflows(perf_alloc(PC_COUNT, "ll40ls_buffer_overflows")), - _last_distance(0) + _last_distance(0), + _bus(bus) { // up the retries since the device misses the first measure attempts I2C::_retries = 3; @@ -668,55 +673,89 @@ namespace ll40ls #endif const int ERROR = -1; -LL40LS *g_dev; +LL40LS *g_dev_int; +LL40LS *g_dev_ext; -void start(); -void stop(); -void test(); -void reset(); -void info(); +void start(int bus); +void stop(int bus); +void test(int bus); +void reset(int bus); +void info(int bus); +void usage(); /** * Start the driver. */ void -start() +start(int bus) { - int fd; - - if (g_dev != nullptr) { - errx(1, "already started"); + /* create the driver, attempt expansion bus first */ + if (bus == -1 || bus == PX4_I2C_BUS_EXPANSION) { + if (g_dev_ext != nullptr) + errx(0, "already started external"); + g_dev_ext = new LL40LS(PX4_I2C_BUS_EXPANSION, LL40LS_DEVICE_PATH_EXT); + if (g_dev_ext != nullptr && OK != g_dev_ext->init()) { + delete g_dev_ext; + g_dev_ext = nullptr; + } } - /* create the driver */ - g_dev = new LL40LS(LL40LS_BUS); +#ifdef PX4_I2C_BUS_ONBOARD + /* if this failed, attempt onboard sensor */ + if (bus == -1 || bus == PX4_I2C_BUS_ONBOARD) { + if (g_dev_int != nullptr) + errx(0, "already started internal"); + g_dev_int = new LL40LS(PX4_I2C_BUS_ONBOARD, LL40LS_DEVICE_PATH_INT); + if (g_dev_int != nullptr && OK != g_dev_int->init()) { + /* tear down the failing onboard instance */ + delete g_dev_int; + g_dev_int = nullptr; - if (g_dev == nullptr) { - goto fail; - } - - if (OK != g_dev->init()) { - goto fail; + if (bus == PX4_I2C_BUS_ONBOARD) { + goto fail; + } + } + if (g_dev_int == nullptr && bus == PX4_I2C_BUS_ONBOARD) { + goto fail; + } } +#endif /* set the poll rate to default, starts automatic data collection */ - fd = open(LL40LS_DEVICE_PATH, O_RDONLY); + if (g_dev_int != nullptr) { + int fd = open(LL40LS_DEVICE_PATH_INT, O_RDONLY); + if (fd == -1) { + goto fail; + } + int ret = ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT); + close(fd); + if (ret < 0) { + goto fail; + } + } - if (fd < 0) { - goto fail; - } - - if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { - goto fail; - } + if (g_dev_ext != nullptr) { + int fd = open(LL40LS_DEVICE_PATH_EXT, O_RDONLY); + if (fd == -1) { + goto fail; + } + int ret = ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT); + close(fd); + if (ret < 0) { + goto fail; + } + } exit(0); fail: - - if (g_dev != nullptr) { - delete g_dev; - g_dev = nullptr; + if (g_dev_int != nullptr && (bus == -1 || bus == PX4_I2C_BUS_ONBOARD)) { + delete g_dev_int; + g_dev_int = nullptr; + } + if (g_dev_ext != nullptr && (bus == -1 || bus == PX4_I2C_BUS_EXPANSION)) { + delete g_dev_ext; + g_dev_ext = nullptr; } errx(1, "driver start failed"); @@ -725,11 +764,12 @@ fail: /** * Stop the driver */ -void stop() +void stop(int bus) { - if (g_dev != nullptr) { - delete g_dev; - g_dev = nullptr; + LL40LS **g_dev = (bus == PX4_I2C_BUS_ONBOARD?&g_dev_int:&g_dev_ext); + if (*g_dev != nullptr) { + delete *g_dev; + *g_dev = nullptr; } else { errx(1, "driver not running"); @@ -744,16 +784,17 @@ void stop() * and automatic modes. */ void -test() +test(int bus) { struct range_finder_report report; ssize_t sz; int ret; + const char *path = (bus==PX4_I2C_BUS_ONBOARD?LL40LS_DEVICE_PATH_INT:LL40LS_DEVICE_PATH_EXT); - int fd = open(LL40LS_DEVICE_PATH, O_RDONLY); + int fd = open(path, O_RDONLY); if (fd < 0) { - err(1, "%s open failed (try 'll40ls start' if the driver is not running", LL40LS_DEVICE_PATH); + err(1, "%s open failed (try 'll40ls start' if the driver is not running", path); } /* do a simple demand read */ @@ -809,9 +850,10 @@ test() * Reset the driver. */ void -reset() +reset(int bus) { - int fd = open(LL40LS_DEVICE_PATH, O_RDONLY); + const char *path = (bus==PX4_I2C_BUS_ONBOARD?LL40LS_DEVICE_PATH_INT:LL40LS_DEVICE_PATH_EXT); + int fd = open(path, O_RDONLY); if (fd < 0) { err(1, "failed "); @@ -832,8 +874,9 @@ reset() * Print a little info about the driver. */ void -info() +info(int bus) { + LL40LS *g_dev = (bus == PX4_I2C_BUS_ONBOARD?g_dev_int:g_dev_ext); if (g_dev == nullptr) { errx(1, "driver not running"); } @@ -844,44 +887,76 @@ info() exit(0); } +void +usage() +{ + warnx("missing command: try 'start', 'stop', 'info', 'test', 'reset', 'info'"); + warnx("options:"); + warnx(" -X only external bus"); +#ifdef PX4_I2C_BUS_ONBOARD + warnx(" -I only internal bus"); +#endif +} + } // namespace int ll40ls_main(int argc, char *argv[]) { + int ch; + int bus = -1; + + while ((ch = getopt(argc, argv, "XI")) != EOF) { + switch (ch) { +#ifdef PX4_I2C_BUS_ONBOARD + case 'I': + bus = PX4_I2C_BUS_ONBOARD; + break; +#endif + case 'X': + bus = PX4_I2C_BUS_EXPANSION; + break; + default: + ll40ls::usage(); + exit(0); + } + } + + const char *verb = argv[optind]; + /* * Start/load the driver. */ - if (!strcmp(argv[1], "start")) { - ll40ls::start(); + if (!strcmp(verb, "start")) { + ll40ls::start(bus); } /* * Stop the driver */ - if (!strcmp(argv[1], "stop")) { - ll40ls::stop(); + if (!strcmp(verb, "stop")) { + ll40ls::stop(bus); } /* * Test the driver/device. */ - if (!strcmp(argv[1], "test")) { - ll40ls::test(); + if (!strcmp(verb, "test")) { + ll40ls::test(bus); } /* * Reset the driver. */ - if (!strcmp(argv[1], "reset")) { - ll40ls::reset(); + if (!strcmp(verb, "reset")) { + ll40ls::reset(bus); } /* * Print driver information. */ - if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status")) { - ll40ls::info(); + if (!strcmp(verb, "info") || !strcmp(verb, "status")) { + ll40ls::info(bus); } errx(1, "unrecognized command, try 'start', 'test', 'reset' or 'info'"); From 1ccb56de92719f36c8b4d80e3dae144dbb8fb913 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 20 Oct 2014 15:21:53 +0200 Subject: [PATCH 358/411] Better error feedback --- .../ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index a61346c2ed..4309338606 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -600,7 +600,7 @@ FixedwingEstimator::check_filter_state() "stale IMU data, resetting", "got initial position lock", "excessive gyro offsets", - "GPS velocity divergence", + "velocity diverted, check accel config", "excessive covariances", "unknown condition"}; @@ -614,7 +614,7 @@ FixedwingEstimator::check_filter_state() } warnx("reset: %s", feedback[warn_index]); - mavlink_log_critical(_mavlink_fd, "[ekf] re-init: %s", feedback[warn_index]); + mavlink_log_critical(_mavlink_fd, "[ekf check] %s", feedback[warn_index]); } struct estimator_status_report rep; From 3f4516810b149d1f8054f1b113508f9b1491b786 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 20 Oct 2014 15:22:33 +0200 Subject: [PATCH 359/411] Improved EKF check feedback --- .../ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index 4309338606..685f5e12f1 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -597,12 +597,12 @@ FixedwingEstimator::check_filter_state() const char* const feedback[] = { 0, "NaN in states, resetting", - "stale IMU data, resetting", + "stale sensor data, resetting", "got initial position lock", "excessive gyro offsets", "velocity diverted, check accel config", "excessive covariances", - "unknown condition"}; + "unknown condition, resetting"}; // Print out error condition if (check) { From 99f839b03861a9868f366c63cd214aa5898414aa Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Thu, 23 Oct 2014 10:57:31 +0200 Subject: [PATCH 360/411] Add a min PWM output so motors idle on arming. --- ROMFS/px4fmu_common/init.d/10015_tbs_discovery | 1 + 1 file changed, 1 insertion(+) diff --git a/ROMFS/px4fmu_common/init.d/10015_tbs_discovery b/ROMFS/px4fmu_common/init.d/10015_tbs_discovery index c4be169436..b1aa8c00b3 100644 --- a/ROMFS/px4fmu_common/init.d/10015_tbs_discovery +++ b/ROMFS/px4fmu_common/init.d/10015_tbs_discovery @@ -27,3 +27,4 @@ fi set MIXER FMU_quad_w set PWM_OUTPUTS 1234 +set PWM_MIN 1200 From f3cda1839624b22bca8b030a43b72e8c0e354ab3 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 23 Oct 2014 16:17:20 +0200 Subject: [PATCH 361/411] Formatting commander.cpp to simplify later rework by ensuring formatting match. NO CODE CHANGES --- src/modules/commander/commander.cpp | 247 ++++++++++++++++++---------- 1 file changed, 159 insertions(+), 88 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 616b855df4..cb507c9bac 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -207,7 +207,9 @@ void usage(const char *reason); /** * React to commands that are sent e.g. from the mavlink module. */ -bool handle_command(struct vehicle_status_s *status, const struct safety_s *safety, struct vehicle_command_s *cmd, struct actuator_armed_s *armed, struct home_position_s *home, struct vehicle_global_position_s *global_pos, orb_advert_t *home_pub); +bool handle_command(struct vehicle_status_s *status, const struct safety_s *safety, struct vehicle_command_s *cmd, + struct actuator_armed_s *armed, struct home_position_s *home, struct vehicle_global_position_s *global_pos, + orb_advert_t *home_pub); /** * Mainloop of commander. @@ -230,7 +232,8 @@ 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); +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 arm_disarm(bool arm, const int mavlink_fd, const char *armedBy); @@ -393,7 +396,8 @@ transition_result_t arm_disarm(bool arm, const int mavlink_fd_local, const char // Transition the armed state. By passing mavlink_fd to arming_state_transition it will // output appropriate error messages if the state cannot transition. - arming_res = arming_state_transition(&status, &safety, arm ? ARMING_STATE_ARMED : ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */, mavlink_fd_local); + arming_res = arming_state_transition(&status, &safety, arm ? ARMING_STATE_ARMED : ARMING_STATE_STANDBY, &armed, + true /* fRunPreArmChecks */, mavlink_fd_local); if (arming_res == TRANSITION_CHANGED && mavlink_fd) { mavlink_log_info(mavlink_fd_local, "[cmd] %s by %s", arm ? "ARMED" : "DISARMED", armedBy); @@ -406,11 +410,12 @@ transition_result_t arm_disarm(bool arm, const int mavlink_fd_local, const char } bool handle_command(struct vehicle_status_s *status_local, const struct safety_s *safety_local, - struct vehicle_command_s *cmd, struct actuator_armed_s *armed_local, - struct home_position_s *home, struct vehicle_global_position_s *global_pos, orb_advert_t *home_pub) + struct vehicle_command_s *cmd, struct actuator_armed_s *armed_local, + struct home_position_s *home, struct vehicle_global_position_s *global_pos, orb_advert_t *home_pub) { /* only handle commands that are meant to be handled by this system and component */ - if (cmd->target_system != status_local->system_id || ((cmd->target_component != status_local->component_id) && (cmd->target_component != 0))) { // component_id 0: valid for all components + if (cmd->target_system != status_local->system_id || ((cmd->target_component != status_local->component_id) + && (cmd->target_component != 0))) { // component_id 0: valid for all components return false; } @@ -537,13 +542,13 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s } else { mavlink_log_critical(mavlink_fd, "REJ CMD: %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f", - (double)cmd->param1, - (double)cmd->param2, - (double)cmd->param3, - (double)cmd->param4, - (double)cmd->param5, - (double)cmd->param6, - (double)cmd->param7); + (double)cmd->param1, + (double)cmd->param2, + (double)cmd->param3, + (double)cmd->param4, + (double)cmd->param5, + (double)cmd->param6, + (double)cmd->param7); } } break; @@ -554,35 +559,43 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s //XXX update state machine? armed_local->force_failsafe = true; warnx("forcing failsafe (termination)"); + } else { armed_local->force_failsafe = false; warnx("disabling failsafe (termination)"); } + /* param2 is currently used for other failsafe modes */ status_local->engine_failure_cmd = false; status_local->data_link_lost_cmd = false; status_local->gps_failure_cmd = false; status_local->rc_signal_lost_cmd = false; + if ((int)cmd->param2 <= 0) { /* reset all commanded failure modes */ warnx("reset all non-flighttermination failsafe commands"); + } else if ((int)cmd->param2 == 1) { /* trigger engine failure mode */ status_local->engine_failure_cmd = true; warnx("engine failure mode commanded"); + } else if ((int)cmd->param2 == 2) { /* trigger data link loss mode */ status_local->data_link_lost_cmd = true; warnx("data link loss mode commanded"); + } else if ((int)cmd->param2 == 3) { /* trigger gps loss mode */ status_local->gps_failure_cmd = true; warnx("gps loss mode commanded"); + } else if ((int)cmd->param2 == 4) { /* trigger rc loss mode */ status_local->rc_signal_lost_cmd = true; warnx("rc loss mode commanded"); } + cmd_result = VEHICLE_CMD_RESULT_ACCEPTED; } break; @@ -633,21 +646,27 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s } } break; + case VEHICLE_CMD_NAV_GUIDED_ENABLE: { transition_result_t res = TRANSITION_DENIED; static main_state_t main_state_pre_offboard = MAIN_STATE_MANUAL; + if (status_local->main_state != MAIN_STATE_OFFBOARD) { main_state_pre_offboard = status_local->main_state; } + if (cmd->param1 > 0.5f) { res = main_state_transition(status_local, MAIN_STATE_OFFBOARD); + if (res == TRANSITION_DENIED) { print_reject_mode(status_local, "OFFBOARD"); status_local->offboard_control_set_by_command = false; + } else { /* Set flag that offboard was set via command, main state is not overridden by rc */ status_local->offboard_control_set_by_command = true; } + } else { /* If the mavlink command is used to enable or disable offboard control: * switch back to previous mode when disabling */ @@ -656,6 +675,7 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s } } break; + case VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN: case VEHICLE_CMD_PREFLIGHT_CALIBRATION: case VEHICLE_CMD_PREFLIGHT_SET_SENSOR_OFFSETS: @@ -802,6 +822,7 @@ int commander_thread_main(int argc, char *argv[]) /* publish initial state */ status_pub = orb_advertise(ORB_ID(vehicle_status), &status); + if (status_pub < 0) { warnx("ERROR: orb_advertise for topic vehicle_status failed (uorb app running?).\n"); warnx("exiting."); @@ -827,11 +848,14 @@ int commander_thread_main(int argc, char *argv[]) /* init mission state, do it here to allow navigator to use stored mission even if mavlink failed to start */ orb_advert_t mission_pub = -1; mission_s mission; + if (dm_read(DM_KEY_MISSION_STATE, 0, &mission, sizeof(mission_s)) == sizeof(mission_s)) { if (mission.dataman_id >= 0 && mission.dataman_id <= 1) { - warnx("loaded mission state: dataman_id=%d, count=%u, current=%d", mission.dataman_id, mission.count, mission.current_seq); + warnx("loaded mission state: dataman_id=%d, count=%u, current=%d", mission.dataman_id, mission.count, + mission.current_seq); mavlink_log_info(mavlink_fd, "[cmd] dataman_id=%d, count=%u, current=%d", - mission.dataman_id, mission.count, mission.current_seq); + mission.dataman_id, mission.count, mission.current_seq); + } else { const char *missionfail = "reading mission state failed"; warnx("%s", missionfail); @@ -1097,6 +1121,7 @@ int commander_thread_main(int argc, char *argv[]) status.offboard_control_signal_lost = false; status_changed = true; } + } else { if (!status.offboard_control_signal_lost) { status.offboard_control_signal_lost = true; @@ -1115,9 +1140,9 @@ int commander_thread_main(int argc, char *argv[]) /* perform system checks when new telemetry link connected */ if (mavlink_fd && - telemetry_last_heartbeat[i] == 0 && - telemetry.heartbeat_time > 0 && - hrt_elapsed_time(&telemetry.heartbeat_time) < datalink_loss_timeout * 1e6) { + telemetry_last_heartbeat[i] == 0 && + telemetry.heartbeat_time > 0 && + hrt_elapsed_time(&telemetry.heartbeat_time) < datalink_loss_timeout * 1e6) { (void)rc_calibration_check(mavlink_fd); } @@ -1130,6 +1155,7 @@ int commander_thread_main(int argc, char *argv[]) if (updated) { orb_copy(ORB_ID(sensor_combined), sensor_sub, &sensors); + /* Check if the barometer is healthy and issue a warning in the GCS if not so. * Because the barometer is used for calculating AMSL altitude which is used to ensure * vertical separation from other airtraffic the operator has to know when the @@ -1142,6 +1168,7 @@ int commander_thread_main(int argc, char *argv[]) status_changed = true; mavlink_log_critical(mavlink_fd, "baro healthy"); } + } else { if (!status.barometer_failure) { status.barometer_failure = true; @@ -1164,10 +1191,11 @@ int commander_thread_main(int argc, char *argv[]) if (hrt_elapsed_time(&system_power.timestamp) < 200000) { if (system_power.servo_valid && - !system_power.brick_valid && - !system_power.usb_connected) { + !system_power.brick_valid && + !system_power.usb_connected) { /* flying only on servo rail, this is unsafe */ status.condition_power_input_valid = false; + } else { status.condition_power_input_valid = true; } @@ -1187,9 +1215,11 @@ int commander_thread_main(int argc, char *argv[]) /* disarm if safety is now on and still armed */ if (status.hil_state == HIL_STATE_OFF && safety.safety_switch_available && !safety.safety_off && armed.armed) { - arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR); + arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : + ARMING_STATE_STANDBY_ERROR); - if (TRANSITION_CHANGED == arming_state_transition(&status, &safety, new_arming_state, &armed, true /* fRunPreArmChecks */, mavlink_fd)) { + if (TRANSITION_CHANGED == arming_state_transition(&status, &safety, new_arming_state, &armed, + true /* fRunPreArmChecks */, mavlink_fd)) { mavlink_log_info(mavlink_fd, "DISARMED by safety switch"); arming_state_changed = true; } @@ -1233,7 +1263,8 @@ int commander_thread_main(int argc, char *argv[]) } } - check_valid(global_position.timestamp, POSITION_TIMEOUT, eph_good, &(status.condition_global_position_valid), &status_changed); + check_valid(global_position.timestamp, POSITION_TIMEOUT, eph_good, &(status.condition_global_position_valid), + &status_changed); /* update home position */ if (!status.condition_home_position_valid && status.condition_global_position_valid && !armed.armed && @@ -1283,8 +1314,11 @@ int commander_thread_main(int argc, char *argv[]) local_eph_good = false; } } - check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.xy_valid && local_eph_good, &(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.xy_valid + && local_eph_good, &(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.condition_landed != local_position.landed) { @@ -1315,7 +1349,8 @@ int commander_thread_main(int argc, char *argv[]) /* get throttle (if armed), as we only care about energy negative throttle also counts */ float throttle = (armed.armed) ? fabsf(actuator_controls.control[3]) : 0.0f; - status.battery_remaining = battery_remaining_estimate_voltage(battery.voltage_filtered_v, battery.discharged_mah, throttle); + status.battery_remaining = battery_remaining_estimate_voltage(battery.voltage_filtered_v, battery.discharged_mah, + throttle); } } @@ -1383,26 +1418,30 @@ int commander_thread_main(int argc, char *argv[]) status.battery_warning = VEHICLE_BATTERY_WARNING_LOW; status_changed = true; - } else if (status.condition_battery_voltage_valid && status.battery_remaining < 0.09f && !critical_battery_voltage_actions_done && low_battery_voltage_actions_done) { + } else if (status.condition_battery_voltage_valid && status.battery_remaining < 0.09f + && !critical_battery_voltage_actions_done && low_battery_voltage_actions_done) { /* critical battery voltage, this is rather an emergency, change state machine */ critical_battery_voltage_actions_done = true; mavlink_log_emergency(mavlink_fd, "CRITICAL BATTERY, LAND IMMEDIATELY"); status.battery_warning = VEHICLE_BATTERY_WARNING_CRITICAL; if (armed.armed) { - arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed, true /* fRunPreArmChecks */, mavlink_fd); + arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed, true /* fRunPreArmChecks */, + mavlink_fd); if (arming_ret == TRANSITION_CHANGED) { arming_state_changed = true; } } else { - arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed, true /* fRunPreArmChecks */, mavlink_fd); + arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed, true /* fRunPreArmChecks */, + mavlink_fd); if (arming_ret == TRANSITION_CHANGED) { arming_state_changed = true; } } + status_changed = true; } @@ -1411,7 +1450,8 @@ 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) { /* TODO: check for sensors */ - arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */, mavlink_fd); + arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */, + mavlink_fd); if (arming_ret == TRANSITION_CHANGED) { arming_state_changed = true; @@ -1439,23 +1479,25 @@ int commander_thread_main(int argc, char *argv[]) /* Initialize map projection if gps is valid */ if (!map_projection_global_initialized() - && (gps_position.eph < eph_threshold) - && (gps_position.epv < epv_threshold) - && hrt_elapsed_time((hrt_abstime*)&gps_position.timestamp_position) < 1e6) { + && (gps_position.eph < eph_threshold) + && (gps_position.epv < epv_threshold) + && hrt_elapsed_time((hrt_abstime *)&gps_position.timestamp_position) < 1e6) { /* set reference for global coordinates <--> local coordiantes conversion and map_projection */ - globallocalconverter_init((double)gps_position.lat * 1.0e-7, (double)gps_position.lon * 1.0e-7, (float)gps_position.alt * 1.0e-3f, hrt_absolute_time()); + globallocalconverter_init((double)gps_position.lat * 1.0e-7, (double)gps_position.lon * 1.0e-7, + (float)gps_position.alt * 1.0e-3f, hrt_absolute_time()); } /* check if GPS fix is ok */ if (status.circuit_breaker_engaged_gpsfailure_check || - (gps_position.fix_type >= 3 && - hrt_elapsed_time(&gps_position.timestamp_position) < FAILSAFE_DEFAULT_TIMEOUT)) { + (gps_position.fix_type >= 3 && + hrt_elapsed_time(&gps_position.timestamp_position) < FAILSAFE_DEFAULT_TIMEOUT)) { /* handle the case where gps was regained */ if (status.gps_failure) { status.gps_failure = false; status_changed = true; mavlink_log_critical(mavlink_fd, "gps regained"); } + } else { if (!status.gps_failure) { status.gps_failure = true; @@ -1477,12 +1519,14 @@ int commander_thread_main(int argc, char *argv[]) armed.force_failsafe = true; status_changed = true; static bool flight_termination_printed = false; + if (!flight_termination_printed) { warnx("Flight termination because of navigator request or geofence"); mavlink_log_critical(mavlink_fd, "GF violation: flight termination"); flight_termination_printed = true; } - if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0 ) { + + if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) { mavlink_log_critical(mavlink_fd, "GF violation: flight termination"); } } // no reset is done here on purpose, on geofence violation we want to stay in flighttermination @@ -1490,7 +1534,7 @@ int commander_thread_main(int argc, char *argv[]) /* RC input check */ if (!status.rc_input_blocked && sp_man.timestamp != 0 && - hrt_absolute_time() < sp_man.timestamp + (uint64_t)(rc_loss_timeout * 1e6f)) { + hrt_absolute_time() < sp_man.timestamp + (uint64_t)(rc_loss_timeout * 1e6f)) { /* handle the case where RC signal was regained */ if (!status.rc_signal_found_once) { status.rc_signal_found_once = true; @@ -1515,11 +1559,15 @@ int commander_thread_main(int argc, char *argv[]) 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); - arming_ret = arming_state_transition(&status, &safety, new_arming_state, &armed, true /* fRunPreArmChecks */, mavlink_fd); + arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : + ARMING_STATE_STANDBY_ERROR); + arming_ret = arming_state_transition(&status, &safety, new_arming_state, &armed, true /* fRunPreArmChecks */, + mavlink_fd); + if (arming_ret == TRANSITION_CHANGED) { arming_state_changed = true; } + stick_off_counter = 0; } else { @@ -1541,8 +1589,11 @@ int commander_thread_main(int argc, char *argv[]) */ if (status.main_state != MAIN_STATE_MANUAL) { print_reject_arm("NOT ARMING: Switch to MANUAL mode first."); + } else { - arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed, true /* fRunPreArmChecks */, mavlink_fd); + arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed, true /* fRunPreArmChecks */, + mavlink_fd); + if (arming_ret == TRANSITION_CHANGED) { arming_state_changed = true; } @@ -1565,6 +1616,7 @@ int commander_thread_main(int argc, char *argv[]) } else { mavlink_log_info(mavlink_fd, "DISARMED by RC"); } + arming_state_changed = true; } else if (arming_ret == TRANSITION_DENIED) { @@ -1600,18 +1652,20 @@ int commander_thread_main(int argc, char *argv[]) /* data links check */ bool have_link = false; + for (int i = 0; i < TELEMETRY_STATUS_ORB_ID_NUM; i++) { if (telemetry_last_heartbeat[i] != 0 && - hrt_elapsed_time(&telemetry_last_heartbeat[i]) < datalink_loss_timeout * 1e6) { + hrt_elapsed_time(&telemetry_last_heartbeat[i]) < datalink_loss_timeout * 1e6) { /* handle the case where data link was regained, * accept datalink as healthy only after datalink_regain_timeout seconds * */ if (telemetry_lost[i] && - hrt_elapsed_time(&telemetry_last_dl_loss[i]) > datalink_regain_timeout * 1e6) { + hrt_elapsed_time(&telemetry_last_dl_loss[i]) > datalink_regain_timeout * 1e6) { mavlink_log_critical(mavlink_fd, "data link %i regained", i); telemetry_lost[i] = false; have_link = true; + } else if (!telemetry_lost[i]) { /* telemetry was healthy also in last iteration * we don't have to check a timeout */ @@ -1620,6 +1674,7 @@ int commander_thread_main(int argc, char *argv[]) } else { telemetry_last_dl_loss[i] = hrt_absolute_time(); + if (!telemetry_lost[i]) { mavlink_log_critical(mavlink_fd, "data link %i lost", i); telemetry_lost[i] = true; @@ -1647,24 +1702,26 @@ int commander_thread_main(int argc, char *argv[]) * only for fixed wing for now */ if (!status.circuit_breaker_engaged_enginefailure_check && - status.is_rotary_wing == false && - armed.armed && - ((actuator_controls.control[3] > ef_throttle_thres && - battery.current_a/actuator_controls.control[3] < - ef_current2throttle_thres) || - (status.engine_failure))) { + status.is_rotary_wing == false && + armed.armed && + ((actuator_controls.control[3] > ef_throttle_thres && + battery.current_a / actuator_controls.control[3] < + ef_current2throttle_thres) || + (status.engine_failure))) { /* potential failure, measure time */ if (timestamp_engine_healthy > 0 && - hrt_elapsed_time(×tamp_engine_healthy) > - ef_time_thres * 1e6 && - !status.engine_failure) { - status.engine_failure = true; - status_changed = true; - mavlink_log_critical(mavlink_fd, "Engine Failure"); + hrt_elapsed_time(×tamp_engine_healthy) > + ef_time_thres * 1e6 && + !status.engine_failure) { + status.engine_failure = true; + status_changed = true; + mavlink_log_critical(mavlink_fd, "Engine Failure"); } + } else { /* no failure reset flag */ timestamp_engine_healthy = hrt_absolute_time(); + if (status.engine_failure) { status.engine_failure = false; status_changed = true; @@ -1691,20 +1748,22 @@ int commander_thread_main(int argc, char *argv[]) * If we are not in a manual (RC stick controlled mode) * and both failed we want to terminate the flight */ if (status.main_state != MAIN_STATE_MANUAL && - status.main_state != MAIN_STATE_ACRO && - status.main_state != MAIN_STATE_ALTCTL && - status.main_state != MAIN_STATE_POSCTL && - ((status.data_link_lost && status.gps_failure) || - (status.data_link_lost_cmd && status.gps_failure_cmd))) { + status.main_state != MAIN_STATE_ACRO && + status.main_state != MAIN_STATE_ALTCTL && + status.main_state != MAIN_STATE_POSCTL && + ((status.data_link_lost && status.gps_failure) || + (status.data_link_lost_cmd && status.gps_failure_cmd))) { armed.force_failsafe = true; status_changed = true; static bool flight_termination_printed = false; + if (!flight_termination_printed) { warnx("Flight termination because of data link loss && gps failure"); mavlink_log_critical(mavlink_fd, "DL and GPS lost: flight termination"); flight_termination_printed = true; } - if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0 ) { + + if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) { mavlink_log_critical(mavlink_fd, "DL and GPS lost: flight termination"); } } @@ -1713,19 +1772,21 @@ int commander_thread_main(int argc, char *argv[]) * If we are in manual (controlled with RC): * if both failed we want to terminate the flight */ if ((status.main_state == MAIN_STATE_ACRO || - status.main_state == MAIN_STATE_MANUAL || - status.main_state == MAIN_STATE_ALTCTL || - status.main_state == MAIN_STATE_POSCTL) && - ((status.rc_signal_lost && status.gps_failure) || - (status.rc_signal_lost_cmd && status.gps_failure_cmd))) { + status.main_state == MAIN_STATE_MANUAL || + status.main_state == MAIN_STATE_ALTCTL || + status.main_state == MAIN_STATE_POSCTL) && + ((status.rc_signal_lost && status.gps_failure) || + (status.rc_signal_lost_cmd && status.gps_failure_cmd))) { armed.force_failsafe = true; status_changed = true; static bool flight_termination_printed = false; + if (!flight_termination_printed) { warnx("Flight termination because of RC signal loss && gps failure"); flight_termination_printed = true; } - if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0 ) { + + if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) { mavlink_log_critical(mavlink_fd, "RC and GPS lost: flight termination"); } } @@ -1766,6 +1827,7 @@ int commander_thread_main(int argc, char *argv[]) /* mark home position as set */ status.condition_home_position_valid = true; } + arming_state_changed = false; } @@ -1810,7 +1872,8 @@ int commander_thread_main(int argc, char *argv[]) } /* play arming and battery warning tunes */ - if (!arm_tune_played && armed.armed && (!safety.safety_switch_available || (safety.safety_switch_available && safety.safety_off))) { + if (!arm_tune_played && armed.armed && (!safety.safety_switch_available || (safety.safety_switch_available + && safety.safety_off))) { /* play tune when armed */ set_tune(TONE_ARMING_WARNING_TUNE); arm_tune_played = true; @@ -1988,6 +2051,7 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s /* offboard switch overrides main switch */ if (sp_man->offboard_switch == SWITCH_POS_ON) { res = main_state_transition(status_local, MAIN_STATE_OFFBOARD); + if (res == TRANSITION_DENIED) { print_reject_mode(status_local, "OFFBOARD"); @@ -2009,6 +2073,7 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s } else { res = main_state_transition(status_local, MAIN_STATE_MANUAL); } + // TRANSITION_DENIED is not possible here break; @@ -2023,7 +2088,7 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s print_reject_mode(status_local, "POSCTL"); } - // fallback to ALTCTL + // fallback to ALTCTL res = main_state_transition(status_local, MAIN_STATE_ALTCTL); if (res != TRANSITION_DENIED) { @@ -2047,14 +2112,14 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s break; // changed successfully or already in this state } - print_reject_mode(status_local, "AUTO_RTL"); + print_reject_mode(status_local, "AUTO_RTL"); - // fallback to LOITER if home position not set - res = main_state_transition(status_local, MAIN_STATE_AUTO_LOITER); + // fallback to LOITER if home position not set + res = main_state_transition(status_local, MAIN_STATE_AUTO_LOITER); - if (res != TRANSITION_DENIED) { - break; // changed successfully or already in this state - } + if (res != TRANSITION_DENIED) { + break; // changed successfully or already in this state + } } else if (sp_man->loiter_switch == SWITCH_POS_ON) { res = main_state_transition(status_local, MAIN_STATE_AUTO_LOITER); @@ -2063,7 +2128,7 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s break; // changed successfully or already in this state } - print_reject_mode(status_local, "AUTO_LOITER"); + print_reject_mode(status_local, "AUTO_LOITER"); } else { res = main_state_transition(status_local, MAIN_STATE_AUTO_MISSION); @@ -2072,22 +2137,22 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s break; // changed successfully or already in this state } - print_reject_mode(status_local, "AUTO_MISSION"); + print_reject_mode(status_local, "AUTO_MISSION"); - // fallback to LOITER if home position not set - res = main_state_transition(status_local, MAIN_STATE_AUTO_LOITER); + // fallback to LOITER if home position not set + res = main_state_transition(status_local, MAIN_STATE_AUTO_LOITER); - if (res != TRANSITION_DENIED) { - break; // changed successfully or already in this state - } + if (res != TRANSITION_DENIED) { + break; // changed successfully or already in this state + } } - // fallback to POSCTL - res = main_state_transition(status_local, MAIN_STATE_POSCTL); + // fallback to POSCTL + res = main_state_transition(status_local, MAIN_STATE_POSCTL); - if (res != TRANSITION_DENIED) { - break; // changed successfully or already in this state - } + if (res != TRANSITION_DENIED) { + break; // changed successfully or already in this state + } // fallback to ALTCTL res = main_state_transition(status_local, MAIN_STATE_ALTCTL); @@ -2169,6 +2234,7 @@ set_control_mode() control_mode.flag_control_position_enabled = false; control_mode.flag_control_velocity_enabled = false; break; + case OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE: control_mode.flag_control_rates_enabled = true; control_mode.flag_control_attitude_enabled = true; @@ -2177,6 +2243,7 @@ set_control_mode() control_mode.flag_control_position_enabled = false; control_mode.flag_control_velocity_enabled = false; break; + case OFFBOARD_CONTROL_MODE_DIRECT_FORCE: control_mode.flag_control_rates_enabled = true; control_mode.flag_control_attitude_enabled = false; @@ -2186,6 +2253,7 @@ set_control_mode() control_mode.flag_control_position_enabled = false; control_mode.flag_control_velocity_enabled = false; break; + case OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED: case OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_OFFSET_NED: case OFFBOARD_CONTROL_MODE_DIRECT_BODY_NED: @@ -2198,6 +2266,7 @@ set_control_mode() control_mode.flag_control_velocity_enabled = true; //XXX: the flags could depend on sp_offboard.ignore break; + default: control_mode.flag_control_rates_enabled = false; control_mode.flag_control_attitude_enabled = false; @@ -2206,6 +2275,7 @@ set_control_mode() control_mode.flag_control_position_enabled = false; control_mode.flag_control_velocity_enabled = false; } + break; case NAVIGATION_STATE_POSCTL: @@ -2415,7 +2485,8 @@ void *commander_low_prio_loop(void *arg) int calib_ret = ERROR; /* try to go to INIT/PREFLIGHT arming state */ - if (TRANSITION_DENIED == arming_state_transition(&status, &safety, ARMING_STATE_INIT, &armed, true /* fRunPreArmChecks */, mavlink_fd)) { + if (TRANSITION_DENIED == arming_state_transition(&status, &safety, ARMING_STATE_INIT, &armed, + true /* fRunPreArmChecks */, mavlink_fd)) { answer_command(cmd, VEHICLE_CMD_RESULT_DENIED); break; } From 33dcb687e82ea5e0f6bdba37419361ca923703df Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 24 Oct 2014 08:53:10 +0200 Subject: [PATCH 362/411] Made some space for FDs - needs proper fix, but will give hackers some relief --- src/modules/sdlog2/sdlog2.c | 23 +++++++++++++++++++---- 1 file changed, 19 insertions(+), 4 deletions(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 1282bc0ea6..9bde374324 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1038,7 +1038,6 @@ int sdlog2_thread_main(int argc, char *argv[]) subs.cmd_sub = orb_subscribe(ORB_ID(vehicle_command)); subs.status_sub = orb_subscribe(ORB_ID(vehicle_status)); subs.gps_pos_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); - subs.sat_info_sub = orb_subscribe(ORB_ID(satellite_info)); subs.sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); subs.att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); subs.att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); @@ -1057,9 +1056,6 @@ int sdlog2_thread_main(int argc, char *argv[]) subs.esc_sub = orb_subscribe(ORB_ID(esc_status)); subs.global_vel_sp_sub = orb_subscribe(ORB_ID(vehicle_global_velocity_setpoint)); subs.battery_sub = orb_subscribe(ORB_ID(battery_status)); - for (int i = 0; i < TELEMETRY_STATUS_ORB_ID_NUM; i++) { - subs.telemetry_subs[i] = orb_subscribe(telemetry_status_orb_id[i]); - } subs.range_finder_sub = orb_subscribe(ORB_ID(sensor_range_finder)); subs.estimator_status_sub = orb_subscribe(ORB_ID(estimator_status)); subs.tecs_status_sub = orb_subscribe(ORB_ID(tecs_status)); @@ -1069,6 +1065,25 @@ int sdlog2_thread_main(int argc, char *argv[]) /* we need to rate-limit wind, as we do not need the full update rate */ orb_set_interval(subs.wind_sub, 90); + + /* add new topics HERE */ + + + for (int i = 0; i < TELEMETRY_STATUS_ORB_ID_NUM; i++) { + subs.telemetry_subs[i] = orb_subscribe(telemetry_status_orb_id[i]); + } + + if (_extended_logging) { + subs.sat_info_sub = orb_subscribe(ORB_ID(satellite_info)); + } + + /* close non-needed fd's */ + + /* close stdin */ + close(0); + /* close stdout */ + close(1); + thread_running = true; /* initialize thread synchronization */ From cb79ef4df3c7514fa279f3b3259fb0ddc05f3b12 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 24 Oct 2014 15:27:43 +1100 Subject: [PATCH 363/411] ll40ls: auto-detect ll40ls on either 0x42 or 0x62 I2C address --- src/drivers/ll40ls/ll40ls.cpp | 53 ++++++++++++++++++++++++++++++++--- 1 file changed, 49 insertions(+), 4 deletions(-) diff --git a/src/drivers/ll40ls/ll40ls.cpp b/src/drivers/ll40ls/ll40ls.cpp index 8b156f7ba6..ce0363dbb5 100644 --- a/src/drivers/ll40ls/ll40ls.cpp +++ b/src/drivers/ll40ls/ll40ls.cpp @@ -73,15 +73,19 @@ /* Configuration Constants */ #define LL40LS_BUS PX4_I2C_BUS_EXPANSION -#define LL40LS_BASEADDR 0x42 /* 7-bit address */ +#define LL40LS_BASEADDR 0x62 /* 7-bit address */ +#define LL40LS_BASEADDR_OLD 0x42 /* previous 7-bit address */ #define LL40LS_DEVICE_PATH_INT "/dev/ll40ls_int" #define LL40LS_DEVICE_PATH_EXT "/dev/ll40ls_ext" /* LL40LS Registers addresses */ #define LL40LS_MEASURE_REG 0x00 /* Measure range register */ -#define LL40LS_MSRREG_ACQUIRE 0x04 /* Value to initiate a measurement, varies based on sensor revision */ +#define LL40LS_MSRREG_ACQUIRE 0x04 /* Value to initiate a measurement, varies based on sensor revision */ #define LL40LS_DISTHIGH_REG 0x8F /* High byte of distance register, auto increment */ +#define LL40LS_WHO_AM_I_REG 0x11 +#define LL40LS_WHO_AM_I_REG_VAL 0xCA +#define LL40LS_SIGNAL_STRENGTH_REG 0x5b /* Device limits */ #define LL40LS_MIN_DISTANCE (0.00f) @@ -117,6 +121,7 @@ public: protected: virtual int probe(); + virtual int read_reg(uint8_t reg, uint8_t &val); private: float _min_distance; @@ -210,7 +215,7 @@ LL40LS::LL40LS(int bus, const char *path, int address) : _bus(bus) { // up the retries since the device misses the first measure attempts - I2C::_retries = 3; + _retries = 3; // enable debug() calls _debug_enabled = false; @@ -277,10 +282,50 @@ out: return ret; } +int +LL40LS::read_reg(uint8_t reg, uint8_t &val) +{ + return transfer(®, 1, &val, 1); +} + int LL40LS::probe() { - return measure(); + // cope with both old and new I2C bus address + const uint8_t addresses[2] = {LL40LS_BASEADDR, LL40LS_BASEADDR_OLD}; + + // more retries for detection + _retries = 10; + + for (uint8_t i=0; i Date: Fri, 24 Oct 2014 15:53:46 +1100 Subject: [PATCH 364/411] ll40ls: start a measurement after a probe this ensures register 0 also works --- src/drivers/ll40ls/ll40ls.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/drivers/ll40ls/ll40ls.cpp b/src/drivers/ll40ls/ll40ls.cpp index ce0363dbb5..6793acd810 100644 --- a/src/drivers/ll40ls/ll40ls.cpp +++ b/src/drivers/ll40ls/ll40ls.cpp @@ -325,7 +325,9 @@ LL40LS::probe() ok: _retries = 3; - return OK; + + // start a measurement + return measure(); } void From ae29d04ff5dc1d3d9c48b081846f04ad34e44373 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 24 Oct 2014 15:57:47 +1100 Subject: [PATCH 365/411] px4flow: try a 22 byte transfer in probe() this allows us to distinguish between a ll40ls and px4flow on I2C address 0x42 --- src/drivers/px4flow/px4flow.cpp | 11 +++++++++++ 1 file changed, 11 insertions(+) diff --git a/src/drivers/px4flow/px4flow.cpp b/src/drivers/px4flow/px4flow.cpp index 60ad3c1aff..04aba9eae4 100644 --- a/src/drivers/px4flow/px4flow.cpp +++ b/src/drivers/px4flow/px4flow.cpp @@ -249,6 +249,17 @@ out: int PX4FLOW::probe() { + uint8_t val[22]; + + // to be sure this is not a ll40ls Lidar (which can also be on + // 0x42) we check if a 22 byte transfer works from address + // 0. The ll40ls gives an error for that, whereas the flow + // happily returns some data + if (transfer(nullptr, 0, &val[0], 22) != OK) { + return -EIO; + } + + // that worked, so start a measurement cycle return measure(); } From 1d29093f3da209d0ae48ace66d48dfbe72b0cd48 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 28 Oct 2014 12:53:24 +0100 Subject: [PATCH 366/411] Flow driver: auto-format fixes --- src/drivers/px4flow/px4flow.cpp | 172 +++++++++++++++++++------------- 1 file changed, 102 insertions(+), 70 deletions(-) diff --git a/src/drivers/px4flow/px4flow.cpp b/src/drivers/px4flow/px4flow.cpp index 04aba9eae4..6c68636c6d 100644 --- a/src/drivers/px4flow/px4flow.cpp +++ b/src/drivers/px4flow/px4flow.cpp @@ -76,7 +76,7 @@ #define PX4FLOW_BUS PX4_I2C_BUS_EXPANSION #define I2C_FLOW_ADDRESS 0x42 //* 7-bit address. 8-bit address is 0x84 //range 0x42 - 0x49 - + /* PX4FLOW Registers addresses */ #define PX4FLOW_REG 0x00 /* Measure Register */ @@ -212,8 +212,9 @@ PX4FLOW::~PX4FLOW() stop(); /* free any existing reports */ - if (_reports != nullptr) + if (_reports != nullptr) { delete _reports; + } } int @@ -222,22 +223,25 @@ PX4FLOW::init() int ret = ERROR; /* do I2C init (and probe) first */ - if (I2C::init() != OK) + if (I2C::init() != OK) { goto out; + } /* allocate basic report buffers */ _reports = new RingBuffer(2, sizeof(struct optical_flow_s)); - if (_reports == nullptr) + if (_reports == nullptr) { goto out; + } /* get a publish handle on the px4flow topic */ struct optical_flow_s zero_report; memset(&zero_report, 0, sizeof(zero_report)); _px4flow_topic = orb_advertise(ORB_ID(optical_flow), &zero_report); - if (_px4flow_topic < 0) + if (_px4flow_topic < 0) { debug("failed to create px4flow object. Did you start uOrb?"); + } ret = OK; /* sensor is ok, but we don't really know if it is within range */ @@ -258,7 +262,7 @@ PX4FLOW::probe() if (transfer(nullptr, 0, &val[0], 22) != OK) { return -EIO; } - + // that worked, so start a measurement cycle return measure(); } @@ -271,20 +275,20 @@ PX4FLOW::ioctl(struct file *filp, int cmd, unsigned long arg) case SENSORIOCSPOLLRATE: { switch (arg) { - /* switching to manual polling */ + /* switching to manual polling */ case SENSOR_POLLRATE_MANUAL: stop(); _measure_ticks = 0; return OK; - /* external signalling (DRDY) not supported */ + /* external signalling (DRDY) not supported */ case SENSOR_POLLRATE_EXTERNAL: - /* zero would be bad */ + /* zero would be bad */ case 0: return -EINVAL; - /* set default/max polling rate */ + /* set default/max polling rate */ case SENSOR_POLLRATE_MAX: case SENSOR_POLLRATE_DEFAULT: { /* do we need to start internal polling? */ @@ -294,13 +298,14 @@ PX4FLOW::ioctl(struct file *filp, int cmd, unsigned long arg) _measure_ticks = USEC2TICK(PX4FLOW_CONVERSION_INTERVAL); /* if we need to start the poll state machine, do it */ - if (want_start) + if (want_start) { start(); + } return OK; } - /* adjust to a legal polling interval in Hz */ + /* adjust to a legal polling interval in Hz */ default: { /* do we need to start internal polling? */ bool want_start = (_measure_ticks == 0); @@ -309,15 +314,17 @@ PX4FLOW::ioctl(struct file *filp, int cmd, unsigned long arg) unsigned ticks = USEC2TICK(1000000 / arg); /* check against maximum rate */ - if (ticks < USEC2TICK(PX4FLOW_CONVERSION_INTERVAL)) + if (ticks < USEC2TICK(PX4FLOW_CONVERSION_INTERVAL)) { return -EINVAL; + } /* update interval for next measurement */ _measure_ticks = ticks; /* if we need to start the poll state machine, do it */ - if (want_start) + if (want_start) { start(); + } return OK; } @@ -325,25 +332,29 @@ PX4FLOW::ioctl(struct file *filp, int cmd, unsigned long arg) } case SENSORIOCGPOLLRATE: - if (_measure_ticks == 0) + if (_measure_ticks == 0) { return SENSOR_POLLRATE_MANUAL; + } return (1000 / _measure_ticks); case SENSORIOCSQUEUEDEPTH: { - /* lower bound is mandatory, upper bound is a sanity check */ - if ((arg < 1) || (arg > 100)) - return -EINVAL; + /* lower bound is mandatory, upper bound is a sanity check */ + if ((arg < 1) || (arg > 100)) { + return -EINVAL; + } + + irqstate_t flags = irqsave(); + + if (!_reports->resize(arg)) { + irqrestore(flags); + return -ENOMEM; + } - irqstate_t flags = irqsave(); - if (!_reports->resize(arg)) { irqrestore(flags); - return -ENOMEM; - } - irqrestore(flags); - return OK; - } + return OK; + } case SENSORIOCGQUEUEDEPTH: return _reports->size(); @@ -366,8 +377,9 @@ PX4FLOW::read(struct file *filp, char *buffer, size_t buflen) int ret = 0; /* buffer must be large enough */ - if (count < 1) + if (count < 1) { return -ENOSPC; + } /* if automatic measurement is enabled */ if (_measure_ticks > 0) { @@ -428,13 +440,13 @@ PX4FLOW::measure() uint8_t cmd = PX4FLOW_REG; ret = transfer(&cmd, 1, nullptr, 0); - if (OK != ret) - { + if (OK != ret) { perf_count(_comms_errors); log("i2c::transfer returned %d", ret); - printf("i2c::transfer flow returned %d"); + printf("i2c::transfer flow returned %d"); return ret; } + ret = OK; return ret; @@ -446,14 +458,13 @@ PX4FLOW::collect() int ret = -EIO; /* read from the sensor */ - uint8_t val[22] = {0, 0,0, 0,0, 0,0, 0,0, 0,0, 0,0, 0,0, 0,0, 0,0, 0,0, 0}; + uint8_t val[22] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}; perf_begin(_sample_perf); ret = transfer(nullptr, 0, &val[0], 22); - if (ret < 0) - { + if (ret < 0) { log("error reading from sensor: %d", ret); perf_count(_comms_errors); perf_end(_sample_perf); @@ -478,12 +489,12 @@ PX4FLOW::collect() int16_t gdist = val[21] << 8 | val[20]; struct optical_flow_s report; - report.flow_comp_x_m = float(flowcx)/1000.0f; - report.flow_comp_y_m = float(flowcy)/1000.0f; - report.flow_raw_x= val[3] << 8 | val[2]; - report.flow_raw_y= val[5] << 8 | val[4]; - report.ground_distance_m =float(gdist)/1000.0f; - report.quality= val[10]; + report.flow_comp_x_m = float(flowcx) / 1000.0f; + report.flow_comp_y_m = float(flowcy) / 1000.0f; + report.flow_raw_x = val[3] << 8 | val[2]; + report.flow_raw_y = val[5] << 8 | val[4]; + report.ground_distance_m = float(gdist) / 1000.0f; + report.quality = val[10]; report.sensor_id = 0; report.timestamp = hrt_absolute_time(); @@ -520,11 +531,13 @@ PX4FLOW::start() true, true, true, - SUBSYSTEM_TYPE_OPTICALFLOW}; + SUBSYSTEM_TYPE_OPTICALFLOW + }; static orb_advert_t pub = -1; if (pub > 0) { orb_publish(ORB_ID(subsystem_info), pub, &info); + } else { pub = orb_advertise(ORB_ID(subsystem_info), &info); } @@ -578,8 +591,9 @@ PX4FLOW::cycle() } /* measurement phase */ - if (OK != measure()) + if (OK != measure()) { log("measure error"); + } /* next phase is collection */ _collect_phase = true; @@ -630,33 +644,37 @@ start() { int fd; - if (g_dev != nullptr) + if (g_dev != nullptr) { errx(1, "already started"); + } /* create the driver */ g_dev = new PX4FLOW(PX4FLOW_BUS); - if (g_dev == nullptr) + if (g_dev == nullptr) { goto fail; + } - if (OK != g_dev->init()) + if (OK != g_dev->init()) { goto fail; + } /* set the poll rate to default, starts automatic data collection */ fd = open(PX4FLOW_DEVICE_PATH, O_RDONLY); - if (fd < 0) + if (fd < 0) { goto fail; + } - if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MAX) < 0) + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MAX) < 0) { goto fail; + } exit(0); fail: - if (g_dev != nullptr) - { + if (g_dev != nullptr) { delete g_dev; g_dev = nullptr; } @@ -669,15 +687,14 @@ fail: */ void stop() { - if (g_dev != nullptr) - { + if (g_dev != nullptr) { delete g_dev; g_dev = nullptr; - } - else - { + + } else { errx(1, "driver not running"); } + exit(0); } @@ -695,14 +712,17 @@ test() int fd = open(PX4FLOW_DEVICE_PATH, O_RDONLY); - if (fd < 0) + if (fd < 0) { err(1, "%s open failed (try 'px4flow start' if the driver is not running", PX4FLOW_DEVICE_PATH); + } /* do a simple demand read */ sz = read(fd, &report, sizeof(report)); if (sz != sizeof(report)) - // err(1, "immediate read failed"); + { + warnx("immediate read failed"); + } warnx("single read"); warnx("flowx: %0.2f m/s", (double)report.flow_comp_x_m); @@ -711,8 +731,9 @@ test() /* start the sensor polling at 2Hz */ - if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) + if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) { errx(1, "failed to set 2Hz poll rate"); + } /* read the sensor 5x and report each value */ for (unsigned i = 0; i < 5; i++) { @@ -723,14 +744,16 @@ test() fds.events = POLLIN; ret = poll(&fds, 1, 2000); - if (ret != 1) + if (ret != 1) { errx(1, "timed out waiting for sensor data"); + } /* now go get it */ sz = read(fd, &report, sizeof(report)); - if (sz != sizeof(report)) + if (sz != sizeof(report)) { err(1, "periodic read failed"); + } warnx("periodic read %u", i); warnx("flowx: %0.2f m/s", (double)report.flow_comp_x_m); @@ -751,14 +774,17 @@ reset() { int fd = open(PX4FLOW_DEVICE_PATH, O_RDONLY); - if (fd < 0) + if (fd < 0) { err(1, "failed "); + } - if (ioctl(fd, SENSORIOCRESET, 0) < 0) + if (ioctl(fd, SENSORIOCRESET, 0) < 0) { err(1, "driver reset failed"); + } - if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { err(1, "driver poll restart failed"); + } exit(0); } @@ -769,8 +795,9 @@ reset() void info() { - if (g_dev == nullptr) + if (g_dev == nullptr) { errx(1, "driver not running"); + } printf("state @ %p\n", g_dev); g_dev->print_info(); @@ -786,32 +813,37 @@ px4flow_main(int argc, char *argv[]) /* * Start/load the driver. */ - if (!strcmp(argv[1], "start")) + if (!strcmp(argv[1], "start")) { px4flow::start(); + } - /* - * Stop the driver - */ - if (!strcmp(argv[1], "stop")) - px4flow::stop(); + /* + * Stop the driver + */ + if (!strcmp(argv[1], "stop")) { + px4flow::stop(); + } /* * Test the driver/device. */ - if (!strcmp(argv[1], "test")) + if (!strcmp(argv[1], "test")) { px4flow::test(); + } /* * Reset the driver. */ - if (!strcmp(argv[1], "reset")) + if (!strcmp(argv[1], "reset")) { px4flow::reset(); + } /* * Print driver information. */ - if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status")) + if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status")) { px4flow::info(); + } errx(1, "unrecognized command, try 'start', 'test', 'reset' or 'info'"); } From aa7b00f8199e167b94a7c5e9e4e004bbd50b2f68 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 29 Oct 2014 09:58:44 +0100 Subject: [PATCH 367/411] Add define to cull flash-intense mathlib tests --- src/systemcmds/tests/tests_main.c | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/systemcmds/tests/tests_main.c b/src/systemcmds/tests/tests_main.c index e3f26924ff..0f56704e6f 100644 --- a/src/systemcmds/tests/tests_main.c +++ b/src/systemcmds/tests/tests_main.c @@ -110,7 +110,9 @@ const struct { {"conv", test_conv, OPT_NOJIGTEST | OPT_NOALLTEST}, {"mount", test_mount, OPT_NOJIGTEST | OPT_NOALLTEST}, {"mtd", test_mtd, 0}, +#ifndef TESTS_MATHLIB_DISABLE {"mathlib", test_mathlib, 0}, +#endif {"help", test_help, OPT_NOALLTEST | OPT_NOHELP | OPT_NOJIGTEST}, {NULL, NULL, 0} }; From a6c6ae023de74f87ee593e8c914d66e7a8be7011 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 29 Oct 2014 14:04:33 +0100 Subject: [PATCH 368/411] Autoformatting (reviewed) IO sbus handler. No functionality changes. --- src/modules/px4iofirmware/sbus.c | 55 +++++++++++++++++++------------- 1 file changed, 33 insertions(+), 22 deletions(-) diff --git a/src/modules/px4iofirmware/sbus.c b/src/modules/px4iofirmware/sbus.c index 0f0414148c..6ead38d617 100644 --- a/src/modules/px4iofirmware/sbus.c +++ b/src/modules/px4iofirmware/sbus.c @@ -61,7 +61,7 @@ /* Measured values with Futaba FX-30/R6108SB: -+100% on TX: PCM 1.100/1.520/1.950ms -> SBus raw values: 350/1024/1700 (100% ATV) - -+140% on TX: PCM 0.930/1.520/2.112ms -> SBus raw values: 78/1024/1964 (140% ATV) + -+140% on TX: PCM 0.930/1.520/2.112ms -> SBus raw values: 78/1024/1964 (140% ATV) -+152% on TX: PCM 0.884/1.520/2.160ms -> SBus raw values: 1/1024/2047 (140% ATV plus dirty tricks) */ @@ -87,13 +87,15 @@ static unsigned partial_frame_count; unsigned sbus_frame_drops; -static bool sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_channels); +static bool sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, + bool *sbus_frame_drop, uint16_t max_channels); int sbus_init(const char *device) { - if (sbus_fd < 0) + if (sbus_fd < 0) { sbus_fd = open(device, O_RDWR | O_NONBLOCK); + } if (sbus_fd >= 0) { struct termios t; @@ -113,6 +115,7 @@ sbus_init(const char *device) } else { debug("S.Bus: open failed"); } + return sbus_fd; } @@ -167,8 +170,9 @@ sbus_input(uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sb ret = read(sbus_fd, &frame[partial_frame_count], SBUS_FRAME_SIZE - partial_frame_count); /* if the read failed for any reason, just give up here */ - if (ret < 1) + if (ret < 1) { return false; + } last_rx_time = now; @@ -180,8 +184,9 @@ sbus_input(uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sb /* * If we don't have a full frame, return */ - if (partial_frame_count < SBUS_FRAME_SIZE) + if (partial_frame_count < SBUS_FRAME_SIZE) { return false; + } /* * Great, it looks like we might have a frame. Go ahead and @@ -228,7 +233,8 @@ static const struct sbus_bit_pick sbus_decoder[SBUS_INPUT_CHANNELS][3] = { }; static bool -sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_values) +sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, + uint16_t max_values) { /* check frame boundary markers to avoid out-of-sync cases */ if ((frame[0] != 0x0f)) { @@ -237,23 +243,27 @@ sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, bool } switch (frame[24]) { - case 0x00: + case 0x00: /* this is S.BUS 1 */ break; - case 0x03: + + case 0x03: /* S.BUS 2 SLOT0: RX battery and external voltage */ break; - case 0x83: + + case 0x83: /* S.BUS 2 SLOT1 */ break; - case 0x43: - case 0xC3: - case 0x23: - case 0xA3: - case 0x63: - case 0xE3: + + case 0x43: + case 0xC3: + case 0x23: + case 0xA3: + case 0x63: + case 0xE3: break; - default: + + default: /* we expect one of the bits above, but there are some we don't know yet */ break; } @@ -283,7 +293,7 @@ sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, bool /* convert 0-2048 values to 1000-2000 ppm encoding in a not too sloppy fashion */ - values[channel] = (uint16_t)(value * SBUS_SCALE_FACTOR +.5f) + SBUS_SCALE_OFFSET; + values[channel] = (uint16_t)(value * SBUS_SCALE_FACTOR + .5f) + SBUS_SCALE_OFFSET; } /* decode switch channels if data fields are wide enough */ @@ -304,16 +314,17 @@ sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, bool /* report that we failed to read anything valid off the receiver */ *sbus_failsafe = true; *sbus_frame_drop = true; - } - else if (frame[SBUS_FLAGS_BYTE] & (1 << SBUS_FRAMELOST_BIT)) { /* a frame was lost */ + + } else if (frame[SBUS_FLAGS_BYTE] & (1 << SBUS_FRAMELOST_BIT)) { /* a frame was lost */ /* set a special warning flag - * - * Attention! This flag indicates a skipped frame only, not a total link loss! Handling this - * condition as fail-safe greatly reduces the reliability and range of the radio link, + * + * Attention! This flag indicates a skipped frame only, not a total link loss! Handling this + * condition as fail-safe greatly reduces the reliability and range of the radio link, * e.g. by prematurely issueing return-to-launch!!! */ *sbus_failsafe = false; *sbus_frame_drop = true; + } else { *sbus_failsafe = false; *sbus_frame_drop = false; From 4839ed915c68ffc60700fe170e5ba3766a6b34f1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 29 Oct 2014 17:00:00 +0100 Subject: [PATCH 369/411] Update MAVLink submodule --- mavlink/include/mavlink/v1.0 | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mavlink/include/mavlink/v1.0 b/mavlink/include/mavlink/v1.0 index 8587a007cf..76a9dd7563 160000 --- a/mavlink/include/mavlink/v1.0 +++ b/mavlink/include/mavlink/v1.0 @@ -1 +1 @@ -Subproject commit 8587a007cf89cb13f146ac46be94b61276343ce1 +Subproject commit 76a9dd75632ecf0f622b8b0af098fa51033796a7 From d5b8385a13f4d09453bc00406ec96da9345addcf Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 29 Oct 2014 17:55:53 +0100 Subject: [PATCH 370/411] Fix low stack space on commander - relevant in HIL --- 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 cb507c9bac..b72ebcc50e 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -263,7 +263,7 @@ int commander_main(int argc, char *argv[]) daemon_task = task_spawn_cmd("commander", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 40, - 2950, + 3200, commander_thread_main, (argv) ? (const char **)&argv[2] : (const char **)NULL); From 2e33683630002aef5881734c96383685b7e8443f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 27 Oct 2014 13:10:58 +0100 Subject: [PATCH 371/411] Abort on large packets which do not fit in buffer - not just if the gap is not provided any more. --- src/modules/mavlink/mavlink_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 0d932d20a0..4448f8d6f3 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -763,7 +763,7 @@ Mavlink::send_message(const uint8_t msgid, const void *msg) _last_write_try_time = hrt_absolute_time(); /* check if there is space in the buffer, let it overflow else */ - if (buf_free < TX_BUFFER_GAP) { + if ((buf_free < TX_BUFFER_GAP) || (buf_free < packet_len)) { /* no enough space in buffer to send */ count_txerr(); count_txerrbytes(packet_len); From dd23d0acbcce9f4c79e120ec782a522164a25d83 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 9 Oct 2014 15:35:18 +1100 Subject: [PATCH 372/411] drivers: allow forcing the safety switch on This allows forcing the safety switch to the on position from software which stops the pwm outputs --- src/drivers/drv_pwm_output.h | 3 +++ src/drivers/px4fmu/fmu.cpp | 1 + src/drivers/px4io/px4io.cpp | 5 +++++ src/modules/px4iofirmware/protocol.h | 1 + src/modules/px4iofirmware/registers.c | 6 ++++++ 5 files changed, 16 insertions(+) diff --git a/src/drivers/drv_pwm_output.h b/src/drivers/drv_pwm_output.h index f66ec7c95d..6873f24b60 100644 --- a/src/drivers/drv_pwm_output.h +++ b/src/drivers/drv_pwm_output.h @@ -213,6 +213,9 @@ ORB_DECLARE(output_pwm); /** make failsafe non-recoverable (termination) if it occurs */ #define PWM_SERVO_SET_TERMINATION_FAILSAFE _IOC(_PWM_SERVO_BASE, 25) +/** force safety switch on (to enable use of safety switch) */ +#define PWM_SERVO_SET_FORCE_SAFETY_ON _IOC(_PWM_SERVO_BASE, 26) + /* * * diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 122a3cd174..3d3e1b0ebb 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -829,6 +829,7 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) case PWM_SERVO_SET_ARM_OK: case PWM_SERVO_CLEAR_ARM_OK: case PWM_SERVO_SET_FORCE_SAFETY_OFF: + case PWM_SERVO_SET_FORCE_SAFETY_ON: // these are no-ops, as no safety switch break; diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 3871b4a2cb..fd9eb41709 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -2278,6 +2278,11 @@ PX4IO::ioctl(file * filep, int cmd, unsigned long arg) ret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FORCE_SAFETY_OFF, PX4IO_FORCE_SAFETY_MAGIC); break; + case PWM_SERVO_SET_FORCE_SAFETY_ON: + /* force safety switch on */ + ret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FORCE_SAFETY_ON, PX4IO_FORCE_SAFETY_MAGIC); + break; + case PWM_SERVO_SET_FORCE_FAILSAFE: /* force failsafe mode instantly */ if (arg == 0) { diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h index a3e8a58d31..9b2e047cbb 100644 --- a/src/modules/px4iofirmware/protocol.h +++ b/src/modules/px4iofirmware/protocol.h @@ -221,6 +221,7 @@ enum { /* DSM bind states */ hence index 12 can safely be used. */ #define PX4IO_P_SETUP_RC_THR_FAILSAFE_US 13 /**< the throttle failsafe pulse length in microseconds */ +#define PX4IO_P_SETUP_FORCE_SAFETY_ON 14 /* force safety switch into 'disarmed' (PWM disabled state) */ #define PX4IO_FORCE_SAFETY_MAGIC 22027 /* required argument for force safety (random) */ /* autopilot control values, -10000..10000 */ diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index 7f19e983f4..49c2a9f56b 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -603,6 +603,12 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) dsm_bind(value & 0x0f, (value >> 4) & 0xF); break; + case PX4IO_P_SETUP_FORCE_SAFETY_ON: + if (value == PX4IO_FORCE_SAFETY_MAGIC) { + r_status_flags &= ~PX4IO_P_STATUS_FLAGS_SAFETY_OFF; + } + break; + case PX4IO_P_SETUP_FORCE_SAFETY_OFF: if (value == PX4IO_FORCE_SAFETY_MAGIC) { r_status_flags |= PX4IO_P_STATUS_FLAGS_SAFETY_OFF; From c396a6774626a6a993030c910697873e6b1b1195 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 16 Dec 2013 22:15:37 +1100 Subject: [PATCH 373/411] mpu6000: added logging of good transfers this helps tracking down a startup issue --- src/drivers/mpu6000/mpu6000.cpp | 11 +++++++++++ 1 file changed, 11 insertions(+) diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index b22bb2e070..a95e041a11 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -229,6 +229,7 @@ private: perf_counter_t _gyro_reads; perf_counter_t _sample_perf; perf_counter_t _bad_transfers; + perf_counter_t _good_transfers; math::LowPassFilter2p _accel_filter_x; math::LowPassFilter2p _accel_filter_y; @@ -404,6 +405,7 @@ MPU6000::MPU6000(int bus, const char *path_accel, const char *path_gyro, spi_dev _gyro_reads(perf_alloc(PC_COUNT, "mpu6000_gyro_read")), _sample_perf(perf_alloc(PC_ELAPSED, "mpu6000_read")), _bad_transfers(perf_alloc(PC_COUNT, "mpu6000_bad_transfers")), + _good_transfers(perf_alloc(PC_COUNT, "mpu6000_good_transfers")), _accel_filter_x(MPU6000_ACCEL_DEFAULT_RATE, MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), _accel_filter_y(MPU6000_ACCEL_DEFAULT_RATE, MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), _accel_filter_z(MPU6000_ACCEL_DEFAULT_RATE, MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), @@ -456,6 +458,7 @@ MPU6000::~MPU6000() perf_free(_accel_reads); perf_free(_gyro_reads); perf_free(_bad_transfers); + perf_free(_good_transfers); } int @@ -1279,8 +1282,14 @@ MPU6000::measure() // all zero data - probably a SPI bus error perf_count(_bad_transfers); perf_end(_sample_perf); + // note that we don't call reset() here as a reset() + // costs 20ms with interrupts disabled. That means if + // the mpu6k does go bad it would cause a FMU failure, + // regardless of whether another sensor is available, return; } + + perf_count(_good_transfers); /* @@ -1399,6 +1408,8 @@ MPU6000::print_info() perf_print_counter(_sample_perf); perf_print_counter(_accel_reads); perf_print_counter(_gyro_reads); + perf_print_counter(_bad_transfers); + perf_print_counter(_good_transfers); _accel_reports->print_info("accel queue"); _gyro_reports->print_info("gyro queue"); } From 47367aeed526e7ca72e9cb7290e745dc0e6e2061 Mon Sep 17 00:00:00 2001 From: philipoe Date: Wed, 29 Oct 2014 15:56:31 +0100 Subject: [PATCH 374/411] TECS: Fix bug (underspeed-condition did not have any effect on throttle) --- src/lib/external_lgpl/tecs/tecs.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/lib/external_lgpl/tecs/tecs.cpp b/src/lib/external_lgpl/tecs/tecs.cpp index 6a2a61b04c..0ed210cf4c 100644 --- a/src/lib/external_lgpl/tecs/tecs.cpp +++ b/src/lib/external_lgpl/tecs/tecs.cpp @@ -299,7 +299,7 @@ void TECS::_update_throttle(float throttle_cruise, const math::Matrix<3,3> &rotM // Calculate throttle demand // If underspeed condition is set, then demand full throttle if (_underspeed) { - _throttle_dem_unc = 1.0f; + _throttle_dem = 1.0f; } else { // Calculate gain scaler from specific energy error to throttle @@ -363,10 +363,10 @@ void TECS::_update_throttle(float throttle_cruise, const math::Matrix<3,3> &rotM } else { _throttle_dem = ff_throttle; } - } - // Constrain throttle demand - _throttle_dem = constrain(_throttle_dem, _THRminf, _THRmaxf); + // Constrain throttle demand + _throttle_dem = constrain(_throttle_dem, _THRminf, _THRmaxf); + } } void TECS::_detect_bad_descent(void) From a82f4881c7f0dee36860ca69fef1e63b0d863b8c Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 11 Oct 2014 06:14:16 +1100 Subject: [PATCH 375/411] sdlog2_dump: Fixing incorrect tabbing to allow for CSV generation --- Tools/sdlog2/sdlog2_dump.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) mode change 100644 => 100755 Tools/sdlog2/sdlog2_dump.py diff --git a/Tools/sdlog2/sdlog2_dump.py b/Tools/sdlog2/sdlog2_dump.py old mode 100644 new mode 100755 index ca2efb021c..8b0ccb2d7d --- a/Tools/sdlog2/sdlog2_dump.py +++ b/Tools/sdlog2/sdlog2_dump.py @@ -154,8 +154,8 @@ class SDLog2Parser: first_data_msg = False self.__parseMsg(msg_descr) bytes_read += self.__ptr - if not self.__debug_out and self.__time_msg != None and self.__csv_updated: - self.__printCSVRow() + if not self.__debug_out and self.__time_msg != None and self.__csv_updated: + self.__printCSVRow() f.close() def __bytesLeft(self): From 02e35991d52f280d978c3ddd527269b1f9fd176d Mon Sep 17 00:00:00 2001 From: philipoe Date: Thu, 30 Oct 2014 09:54:27 +0100 Subject: [PATCH 376/411] TECS: Also deleted the _throttle_dem_unc variable from TECS.h --- src/lib/external_lgpl/tecs/tecs.h | 3 --- 1 file changed, 3 deletions(-) diff --git a/src/lib/external_lgpl/tecs/tecs.h b/src/lib/external_lgpl/tecs/tecs.h index 8ac31de6f5..eb45237b7d 100644 --- a/src/lib/external_lgpl/tecs/tecs.h +++ b/src/lib/external_lgpl/tecs/tecs.h @@ -345,9 +345,6 @@ private: // climbout mode bool _climbOutDem; - // throttle demand before limiting - float _throttle_dem_unc; - // pitch demand before limiting float _pitch_dem_unc; From d0d2c7f9c69c5cfdd3aabf757796d2d6ada32875 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 30 Oct 2014 15:10:44 +0100 Subject: [PATCH 377/411] Updated MAVLink revision --- mavlink/include/mavlink/v1.0 | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mavlink/include/mavlink/v1.0 b/mavlink/include/mavlink/v1.0 index 76a9dd7563..90383fac84 160000 --- a/mavlink/include/mavlink/v1.0 +++ b/mavlink/include/mavlink/v1.0 @@ -1 +1 @@ -Subproject commit 76a9dd75632ecf0f622b8b0af098fa51033796a7 +Subproject commit 90383fac84d031aef17989a1497c2473dfa64340 From 2f715b74dd4866945e7ebe6aab735cd1e58fe01d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 30 Oct 2014 15:36:16 +0100 Subject: [PATCH 378/411] Fix build breakage on mavlink update --- src/modules/mavlink/mavlink_receiver.cpp | 10 ++++------ 1 file changed, 4 insertions(+), 6 deletions(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index e8d783847c..9247f999af 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -391,11 +391,9 @@ MavlinkReceiver::handle_message_hil_optical_flow(mavlink_message_t *msg) f.timestamp = hrt_absolute_time(); f.flow_timestamp = flow.time_usec; - f.flow_raw_x = flow.flow_x; - f.flow_raw_y = flow.flow_y; - f.flow_comp_x_m = flow.flow_comp_m_x; - f.flow_comp_y_m = flow.flow_comp_m_y; - f.ground_distance_m = flow.ground_distance; + f.flow_raw_x = flow.integrated_x; + f.flow_raw_y = flow.integrated_y; + f.ground_distance_m = flow.distance; f.quality = flow.quality; f.sensor_id = flow.sensor_id; @@ -413,7 +411,7 @@ MavlinkReceiver::handle_message_hil_optical_flow(mavlink_message_t *msg) r.timestamp = hrt_absolute_time(); r.error_count = 0; r.type = RANGE_FINDER_TYPE_LASER; - r.distance = flow.ground_distance; + r.distance = flow.distance; r.minimum_distance = 0.0f; r.maximum_distance = 40.0f; // this is set to match the typical range of real sensors, could be made configurable r.valid = (r.distance > r.minimum_distance) && (r.distance < r.maximum_distance); From 9f3c3529593033d9acc94fb978a2aa6e34a3d1d9 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 30 Oct 2014 16:38:27 +0100 Subject: [PATCH 379/411] Leave NSH terminal enough stack space --- src/systemcmds/nshterm/module.mk | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/systemcmds/nshterm/module.mk b/src/systemcmds/nshterm/module.mk index 7d2c59f913..41706e137b 100644 --- a/src/systemcmds/nshterm/module.mk +++ b/src/systemcmds/nshterm/module.mk @@ -38,6 +38,6 @@ MODULE_COMMAND = nshterm SRCS = nshterm.c -MODULE_STACKSIZE = 1400 +MODULE_STACKSIZE = 1600 MAXOPTIMIZATION = -Os From c7a3a0db5230d1506c81c5f15254002b64cddf32 Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Fri, 31 Oct 2014 08:58:58 +0100 Subject: [PATCH 380/411] Don't go into an error state if we are temporarily powering via USB on the bench --- 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 b72ebcc50e..46caddd469 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1407,8 +1407,8 @@ int commander_thread_main(int argc, char *argv[]) last_idle_time = system_load.tasks[0].total_runtime; /* check if board is connected via USB */ - //struct stat statbuf; - //on_usb_power = (stat("/dev/ttyACM0", &statbuf) == 0); + struct stat statbuf; + on_usb_power = (stat("/dev/ttyACM0", &statbuf) == 0); } /* if battery voltage is getting lower, warn using buzzer, etc. */ @@ -1418,7 +1418,7 @@ int commander_thread_main(int argc, char *argv[]) status.battery_warning = VEHICLE_BATTERY_WARNING_LOW; status_changed = true; - } else if (status.condition_battery_voltage_valid && status.battery_remaining < 0.09f + } else if (!on_usb_power && status.condition_battery_voltage_valid && status.battery_remaining < 0.09f && !critical_battery_voltage_actions_done && low_battery_voltage_actions_done) { /* critical battery voltage, this is rather an emergency, change state machine */ critical_battery_voltage_actions_done = true; From eeda1886f0e7f15b2e51d863020f794b1a84efc1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 1 Nov 2014 11:07:36 +0100 Subject: [PATCH 381/411] Simplify error messages for NSH term --- src/systemcmds/nshterm/nshterm.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/systemcmds/nshterm/nshterm.c b/src/systemcmds/nshterm/nshterm.c index fca1798e68..f06c49552e 100644 --- a/src/systemcmds/nshterm/nshterm.c +++ b/src/systemcmds/nshterm/nshterm.c @@ -87,7 +87,7 @@ nshterm_main(int argc, char *argv[]) /* Back up the original uart configuration to restore it after exit */ if ((termios_state = tcgetattr(fd, &uart_config)) < 0) { - warnx("ERROR get termios config %s: %d\n", argv[1], termios_state); + warnx("ERR get config %s: %d\n", argv[1], termios_state); close(fd); return -1; } @@ -96,7 +96,7 @@ nshterm_main(int argc, char *argv[]) uart_config.c_oflag |= (ONLCR | OPOST/* | OCRNL*/); if ((termios_state = tcsetattr(fd, TCSANOW, &uart_config)) < 0) { - warnx("ERROR setting baudrate / termios config for %s (tcsetattr)\n", argv[1]); + warnx("ERR set config %s\n", argv[1]); close(fd); return -1; } From 2cadd45f307750bb68c3f926d7d9830f680bd94b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 2 Nov 2014 10:13:11 +0100 Subject: [PATCH 382/411] Configure led ring and enable heartbeat on it --- src/drivers/boards/px4io-v2/board_config.h | 1 + src/drivers/boards/px4io-v2/px4iov2_init.c | 1 + src/modules/px4iofirmware/px4io.c | 2 ++ src/modules/px4iofirmware/px4io.h | 1 + 4 files changed, 5 insertions(+) diff --git a/src/drivers/boards/px4io-v2/board_config.h b/src/drivers/boards/px4io-v2/board_config.h index ef9bb5cadd..10a93be0bf 100644 --- a/src/drivers/boards/px4io-v2/board_config.h +++ b/src/drivers/boards/px4io-v2/board_config.h @@ -77,6 +77,7 @@ #define GPIO_LED1 (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN14) #define GPIO_LED2 (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN15) #define GPIO_LED3 (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN13) +#define GPIO_LED4 (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN11) /* Safety switch button *******************************************************/ diff --git a/src/drivers/boards/px4io-v2/px4iov2_init.c b/src/drivers/boards/px4io-v2/px4iov2_init.c index 9f8c0eeb27..5c3343ccca 100644 --- a/src/drivers/boards/px4io-v2/px4iov2_init.c +++ b/src/drivers/boards/px4io-v2/px4iov2_init.c @@ -108,6 +108,7 @@ __EXPORT void stm32_boardinitialize(void) stm32_configgpio(GPIO_LED1); stm32_configgpio(GPIO_LED2); stm32_configgpio(GPIO_LED3); + stm32_configgpio(GPIO_LED4); stm32_configgpio(GPIO_BTN_SAFETY); diff --git a/src/modules/px4iofirmware/px4io.c b/src/modules/px4iofirmware/px4io.c index cd134ccb40..f978ff4065 100644 --- a/src/modules/px4iofirmware/px4io.c +++ b/src/modules/px4iofirmware/px4io.c @@ -124,6 +124,7 @@ heartbeat_blink(void) { static bool heartbeat = false; LED_BLUE(heartbeat = !heartbeat); + LED_RING(heartbeat); } static uint64_t reboot_time; @@ -191,6 +192,7 @@ user_start(int argc, char *argv[]) LED_AMBER(false); LED_BLUE(false); LED_SAFETY(false); + LED_RING(false); /* turn on servo power (if supported) */ #ifdef POWER_SERVO diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h index e32fcc72b6..8186e4c78b 100644 --- a/src/modules/px4iofirmware/px4io.h +++ b/src/modules/px4iofirmware/px4io.h @@ -140,6 +140,7 @@ extern pwm_limit_t pwm_limit; #define LED_BLUE(_s) stm32_gpiowrite(GPIO_LED1, !(_s)) #define LED_AMBER(_s) stm32_gpiowrite(GPIO_LED2, !(_s)) #define LED_SAFETY(_s) stm32_gpiowrite(GPIO_LED3, !(_s)) +#define LED_RING(_s) stm32_gpiowrite(GPIO_LED4, !(_s)) #ifdef CONFIG_ARCH_BOARD_PX4IO_V1 From 75d0ffe4e5b7659e20bbbdcc1e840e06dfe7a138 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 2 Nov 2014 12:34:33 +0100 Subject: [PATCH 383/411] Build fix --- src/modules/px4iofirmware/px4io.c | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/modules/px4iofirmware/px4io.c b/src/modules/px4iofirmware/px4io.c index f978ff4065..30f32b38e7 100644 --- a/src/modules/px4iofirmware/px4io.c +++ b/src/modules/px4iofirmware/px4io.c @@ -124,7 +124,9 @@ heartbeat_blink(void) { static bool heartbeat = false; LED_BLUE(heartbeat = !heartbeat); +#ifdef GPIO_LED4 LED_RING(heartbeat); +#endif } static uint64_t reboot_time; @@ -192,7 +194,9 @@ user_start(int argc, char *argv[]) LED_AMBER(false); LED_BLUE(false); LED_SAFETY(false); +#ifdef GPIO_LED4 LED_RING(false); +#endif /* turn on servo power (if supported) */ #ifdef POWER_SERVO From ce1ec430f89e2080ad053115459bf91dd6585d3a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 2 Nov 2014 12:58:32 +0100 Subject: [PATCH 384/411] Ensure MAVLink app has enough stack space --- src/modules/mavlink/mavlink_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 4448f8d6f3..6b4edff78a 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1634,7 +1634,7 @@ Mavlink::start(int argc, char *argv[]) task_spawn_cmd(buf, SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, - 2700, + 2900, (main_t)&Mavlink::start_helper, (const char **)argv); From acb739655d5c2ebf50449842ae2b7b9b7c76dbd1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 2 Nov 2014 17:06:35 +0100 Subject: [PATCH 385/411] Remove huge memory overhead in RC channels topic, was completely unnecessary --- src/modules/uORB/topics/rc_channels.h | 1 - 1 file changed, 1 deletion(-) diff --git a/src/modules/uORB/topics/rc_channels.h b/src/modules/uORB/topics/rc_channels.h index 8978de4711..b14ae1edd4 100644 --- a/src/modules/uORB/topics/rc_channels.h +++ b/src/modules/uORB/topics/rc_channels.h @@ -76,7 +76,6 @@ struct rc_channels_s { uint64_t timestamp_last_valid; /**< Timestamp of last valid RC signal */ float channels[RC_CHANNELS_FUNCTION_MAX]; /**< Scaled to -1..1 (throttle: 0..1) */ uint8_t channel_count; /**< Number of valid channels */ - char function_name[RC_CHANNELS_FUNCTION_MAX][20]; /**< String array to store the names of the functions */ int8_t function[RC_CHANNELS_FUNCTION_MAX]; /**< Functions mapping */ uint8_t rssi; /**< Receive signal strength index */ bool signal_lost; /**< Control signal lost, should be checked together with topic timeout */ From baba157785ee1298e70fb358a0ffe76f18fc3b54 Mon Sep 17 00:00:00 2001 From: James Goppert Date: Sat, 1 Nov 2014 18:45:40 -0400 Subject: [PATCH 386/411] Encoder logging support. --- src/modules/sdlog2/sdlog2.c | 16 +++++++++++++++- src/modules/sdlog2/sdlog2_messages.h | 11 +++++++++++ 2 files changed, 26 insertions(+), 1 deletion(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 9bde374324..1820fc104c 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -90,6 +90,7 @@ #include #include #include +#include #include #include @@ -954,6 +955,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct servorail_status_s servorail_status; struct satellite_info_s sat_info; struct wind_estimate_s wind_estimate; + struct encoders_s encoders; } buf; memset(&buf, 0, sizeof(buf)); @@ -996,6 +998,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct log_GS1B_s log_GS1B; struct log_TECS_s log_TECS; struct log_WIND_s log_WIND; + struct log_ENCODERS_s log_ENCODERS; } body; } log_msg = { LOG_PACKET_HEADER_INIT(0) @@ -1033,6 +1036,7 @@ int sdlog2_thread_main(int argc, char *argv[]) int system_power_sub; int servorail_status_sub; int wind_sub; + int encoders_sub; } subs; subs.cmd_sub = orb_subscribe(ORB_ID(vehicle_command)); @@ -1064,7 +1068,7 @@ int sdlog2_thread_main(int argc, char *argv[]) subs.wind_sub = orb_subscribe(ORB_ID(wind_estimate)); /* we need to rate-limit wind, as we do not need the full update rate */ orb_set_interval(subs.wind_sub, 90); - + subs.encoders_sub = orb_subscribe(ORB_ID(encoders)); /* add new topics HERE */ @@ -1667,6 +1671,16 @@ int sdlog2_thread_main(int argc, char *argv[]) LOGBUFFER_WRITE_AND_COUNT(WIND); } + /* --- ENCODERS --- */ + if (copy_if_updated(ORB_ID(encoders), subs.encoders_sub, &buf.encoders)) { + log_msg.msg_type = LOG_ENCODERS_MSG; + log_msg.body.log_ENCODERS.counts_0 = buf.encoders.counts[0]; + log_msg.body.log_ENCODERS.velocity_0 = buf.encoders.velocity[0]; + log_msg.body.log_ENCODERS.counts_1 = buf.encoders.counts[1]; + log_msg.body.log_ENCODERS.velocity_1 = buf.encoders.velocity[1]; + LOGBUFFER_WRITE_AND_COUNT(ENCODERS); + } + /* 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 d49fc0c799..7fd07809d9 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -407,6 +407,16 @@ struct log_VISN_s { float qw; }; +/* --- ENCODERS - ENCODER DATA --- */ +#define LOG_ENCODERS_MSG 39 +struct log_ENCODERS_s { + int64_t counts_0; + float velocity_0; + int64_t counts_1; + float velocity_1; +}; + + /********** SYSTEM MESSAGES, ID > 0x80 **********/ /* --- TIME - TIME STAMP --- */ @@ -471,6 +481,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(GS1B, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"), LOG_FORMAT(TECS, "fffffffffffffB", "ASP,AF,FSP,F,FF,AsSP,AsF,AsDSP,AsD,TERSP,TER,EDRSP,EDR,M"), LOG_FORMAT(WIND, "ffff", "X,Y,CovX,CovY"), + LOG_FORMAT(ENCODERS, "qfqf", "counts_0, velocity_0, counts_1, velocity_1"), /* system-level messages, ID >= 0x80 */ /* FMT: don't write format of format message, it's useless */ From 0e5b91d16fd35b1ad78df1a772ceb23676e5fa7a Mon Sep 17 00:00:00 2001 From: James Goppert Date: Sun, 2 Nov 2014 14:22:23 -0500 Subject: [PATCH 387/411] Shortened encoder logging names. --- src/modules/sdlog2/sdlog2.c | 14 +++++++------- src/modules/sdlog2/sdlog2_messages.h | 14 +++++++------- 2 files changed, 14 insertions(+), 14 deletions(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 1820fc104c..af580f1f79 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -998,7 +998,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct log_GS1B_s log_GS1B; struct log_TECS_s log_TECS; struct log_WIND_s log_WIND; - struct log_ENCODERS_s log_ENCODERS; + struct log_ENCD_s log_ENCD; } body; } log_msg = { LOG_PACKET_HEADER_INIT(0) @@ -1673,12 +1673,12 @@ int sdlog2_thread_main(int argc, char *argv[]) /* --- ENCODERS --- */ if (copy_if_updated(ORB_ID(encoders), subs.encoders_sub, &buf.encoders)) { - log_msg.msg_type = LOG_ENCODERS_MSG; - log_msg.body.log_ENCODERS.counts_0 = buf.encoders.counts[0]; - log_msg.body.log_ENCODERS.velocity_0 = buf.encoders.velocity[0]; - log_msg.body.log_ENCODERS.counts_1 = buf.encoders.counts[1]; - log_msg.body.log_ENCODERS.velocity_1 = buf.encoders.velocity[1]; - LOGBUFFER_WRITE_AND_COUNT(ENCODERS); + log_msg.msg_type = LOG_ENCD_MSG; + log_msg.body.log_ENCD.cnt0 = buf.encoders.counts[0]; + log_msg.body.log_ENCD.vel0 = buf.encoders.velocity[0]; + log_msg.body.log_ENCD.cnt1 = buf.encoders.counts[1]; + log_msg.body.log_ENCD.vel1 = buf.encoders.velocity[1]; + LOGBUFFER_WRITE_AND_COUNT(ENCD); } /* signal the other thread new data, but not yet unlock */ diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 7fd07809d9..fa9bdacb80 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -408,12 +408,12 @@ struct log_VISN_s { }; /* --- ENCODERS - ENCODER DATA --- */ -#define LOG_ENCODERS_MSG 39 -struct log_ENCODERS_s { - int64_t counts_0; - float velocity_0; - int64_t counts_1; - float velocity_1; +#define LOG_ENCD_MSG 39 +struct log_ENCD_s { + int64_t cnt0; + float vel0; + int64_t cnt1; + float vel1; }; @@ -481,7 +481,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(GS1B, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"), LOG_FORMAT(TECS, "fffffffffffffB", "ASP,AF,FSP,F,FF,AsSP,AsF,AsDSP,AsD,TERSP,TER,EDRSP,EDR,M"), LOG_FORMAT(WIND, "ffff", "X,Y,CovX,CovY"), - LOG_FORMAT(ENCODERS, "qfqf", "counts_0, velocity_0, counts_1, velocity_1"), + LOG_FORMAT(ENCD, "qfqf", "cnt0,vel0,cnt1,vel1"), /* system-level messages, ID >= 0x80 */ /* FMT: don't write format of format message, it's useless */ From 82ebf3ca1c46dd228ed1a95fd334cae6af9dd252 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 2 Nov 2014 21:23:02 +0100 Subject: [PATCH 388/411] Update NuttX, print stack trace on stack overflow handler --- NuttX | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/NuttX b/NuttX index 41fffa0df1..5ee4b2b2c2 160000 --- a/NuttX +++ b/NuttX @@ -1 +1 @@ -Subproject commit 41fffa0df192e4a26d2325c7ac5d9b5d7ba0211c +Subproject commit 5ee4b2b2c26bbc35d1669840f0676e8aa383b984 From b51c6693445be6ff230b3d34ad400553aadddd7e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 2 Nov 2014 21:24:50 +0100 Subject: [PATCH 389/411] Commander: Improve calibration routines to produce more conscise and better sequenced instructions --- .../commander/accelerometer_calibration.cpp | 50 ++++++++++++------- .../commander/airspeed_calibration.cpp | 25 +++++++--- src/modules/commander/gyro_calibration.cpp | 5 +- src/modules/commander/mag_calibration.cpp | 2 +- 4 files changed, 54 insertions(+), 28 deletions(-) diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index 7a4e7a766d..0cb41489f8 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -83,7 +83,7 @@ * | accel_T[1][i] | * [ accel_T[2][i] ] * - * b = [ accel_corr_ref[0][i] ] // One measurement per axis is enough + * b = [ accel_corr_ref[0][i] ] // One measurement per side is enough * | accel_corr_ref[2][i] | * [ accel_corr_ref[4][i] ] * @@ -162,6 +162,11 @@ int do_accel_calibration(int mavlink_fd) mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name); + mavlink_log_info(mavlink_fd, "You need to put the system on all six sides"); + sleep(3); + mavlink_log_info(mavlink_fd, "Follow the instructions on the screen"); + sleep(5); + struct accel_scale accel_scale = { 0.0f, 1.0f, @@ -258,7 +263,7 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float const int samples_num = 2500; float accel_ref[6][3]; bool data_collected[6] = { false, false, false, false, false, false }; - const char *orientation_strs[6] = { "x+", "x-", "y+", "y-", "z+", "z-" }; + const char *orientation_strs[6] = { "front", "back", "left", "right", "top", "bottom" }; int sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined)); @@ -287,29 +292,37 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float break; } - mavlink_log_info(mavlink_fd, "directions left: %s%s%s%s%s%s", - (!data_collected[0]) ? "x+ " : "", - (!data_collected[1]) ? "x- " : "", - (!data_collected[2]) ? "y+ " : "", - (!data_collected[3]) ? "y- " : "", - (!data_collected[4]) ? "z+ " : "", - (!data_collected[5]) ? "z- " : ""); + /* inform user which axes are still needed */ + mavlink_log_info(mavlink_fd, "pending: %s%s%s%s%s%s", + (!data_collected[0]) ? "front " : "", + (!data_collected[1]) ? "back " : "", + (!data_collected[2]) ? "left " : "", + (!data_collected[3]) ? "right " : "", + (!data_collected[4]) ? "up " : "", + (!data_collected[5]) ? "down " : ""); + + /* allow user enough time to read the message */ + sleep(3); int orient = detect_orientation(mavlink_fd, sensor_combined_sub); if (orient < 0) { - res = ERROR; - break; - } - - if (data_collected[orient]) { - mavlink_log_info(mavlink_fd, "%s done, rotate to a different axis", orientation_strs[orient]); + mavlink_log_info(mavlink_fd, "invalid motion, hold still..."); + sleep(3); continue; } - mavlink_log_info(mavlink_fd, "accel measurement started: %s axis", orientation_strs[orient]); + /* inform user about already handled side */ + if (data_collected[orient]) { + mavlink_log_info(mavlink_fd, "%s side done, rotate to a different side", orientation_strs[orient]); + sleep(4); + continue; + } + + mavlink_log_info(mavlink_fd, "Hold still, starting to measure %s side", orientation_strs[orient]); + sleep(1); read_accelerometer_avg(sensor_combined_sub, &(accel_ref[orient][0]), samples_num); - mavlink_log_info(mavlink_fd, "result for %s axis: [ %.2f %.2f %.2f ]", orientation_strs[orient], + mavlink_log_info(mavlink_fd, "result for %s side: [ %.2f %.2f %.2f ]", orientation_strs[orient], (double)accel_ref[orient][0], (double)accel_ref[orient][1], (double)accel_ref[orient][2]); @@ -400,7 +413,7 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) /* is still now */ if (t_still == 0) { /* first time */ - mavlink_log_info(mavlink_fd, "detected rest position, waiting..."); + mavlink_log_info(mavlink_fd, "detected rest position, hold still..."); t_still = t; t_timeout = t + timeout; @@ -418,6 +431,7 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) /* not still, reset still start time */ if (t_still != 0) { mavlink_log_info(mavlink_fd, "detected motion, hold still..."); + sleep(3); t_still = 0; } } diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp index 339b11bbe6..cae1d76845 100644 --- a/src/modules/commander/airspeed_calibration.cpp +++ b/src/modules/commander/airspeed_calibration.cpp @@ -61,6 +61,15 @@ static const int ERROR = -1; static const char *sensor_name = "dpress"; +#define HUMAN_ASPD_CAL_FAILED_MSG "Calibration failed, see http://px4.io/help/aspd" + +static void feedback_calibration_failed(int mavlink_fd) +{ + sleep(5); + mavlink_log_critical(mavlink_fd, CAL_FAILED_MSG, sensor_name); + mavlink_log_critical(mavlink_fd, HUMAN_ASPD_CAL_FAILED_MSG); +} + int do_airspeed_calibration(int mavlink_fd) { /* give directions */ @@ -99,7 +108,7 @@ int do_airspeed_calibration(int mavlink_fd) float analog_scaling = 0.0f; param_get(param_find("SENS_DPRES_ANSC"), &(analog_scaling)); if (fabsf(analog_scaling) < 0.1f) { - mavlink_log_critical(mavlink_fd, "If analog sens, retry with [SENS_DPRES_ANSC=1000]"); + mavlink_log_critical(mavlink_fd, "No airspeed sensor, see http://px4.io/help/aspd"); close(diff_pres_sub); return ERROR; } @@ -138,7 +147,7 @@ int do_airspeed_calibration(int mavlink_fd) } else if (poll_ret == 0) { /* any poll failure for 1s is a reason to abort */ - mavlink_log_critical(mavlink_fd, CAL_FAILED_MSG, sensor_name); + feedback_calibration_failed(mavlink_fd); close(diff_pres_sub); return ERROR; } @@ -175,7 +184,7 @@ int do_airspeed_calibration(int mavlink_fd) } } else { - mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name); + feedback_calibration_failed(mavlink_fd); close(diff_pres_sub); return ERROR; } @@ -207,7 +216,7 @@ int do_airspeed_calibration(int mavlink_fd) if (fabsf(diff_pres.differential_pressure_raw_pa) < 50.0f) { if (calibration_counter % 500 == 0) { - mavlink_log_info(mavlink_fd, "Create airflow! (%d, wanted: 50 Pa)", + mavlink_log_info(mavlink_fd, "Create air pressure! (got %d, wanted: 50 Pa)", (int)diff_pres.differential_pressure_raw_pa); } continue; @@ -215,9 +224,9 @@ int do_airspeed_calibration(int mavlink_fd) /* do not allow negative values */ if (diff_pres.differential_pressure_raw_pa < 0.0f) { - mavlink_log_critical(mavlink_fd, "Swap static and dynamic ports!"); mavlink_log_info(mavlink_fd, "ERROR: Negative pressure difference detected! (%d Pa)", (int)diff_pres.differential_pressure_raw_pa); + mavlink_log_critical(mavlink_fd, "Swap static and dynamic ports!"); close(diff_pres_sub); /* the user setup is wrong, wipe the calibration to force a proper re-calibration */ @@ -235,7 +244,7 @@ int do_airspeed_calibration(int mavlink_fd) close(diff_pres_sub); - mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name); + feedback_calibration_failed(mavlink_fd); return ERROR; } else { mavlink_log_info(mavlink_fd, "Positive pressure: OK (%d Pa)", @@ -245,14 +254,14 @@ int do_airspeed_calibration(int mavlink_fd) } else if (poll_ret == 0) { /* any poll failure for 1s is a reason to abort */ - mavlink_log_critical(mavlink_fd, CAL_FAILED_MSG, sensor_name); + feedback_calibration_failed(mavlink_fd); close(diff_pres_sub); return ERROR; } } if (calibration_counter == maxcount) { - mavlink_log_critical(mavlink_fd, CAL_FAILED_MSG, sensor_name); + feedback_calibration_failed(mavlink_fd); close(diff_pres_sub); return ERROR; } diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp index d89c67c2be..8ab14dd52b 100644 --- a/src/modules/commander/gyro_calibration.cpp +++ b/src/modules/commander/gyro_calibration.cpp @@ -63,7 +63,10 @@ static const char *sensor_name = "gyro"; int do_gyro_calibration(int mavlink_fd) { mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name); - mavlink_log_info(mavlink_fd, "don't move system"); + mavlink_log_info(mavlink_fd, "HOLD STILL"); + + /* wait for the user to respond */ + sleep(2); struct gyro_scale gyro_scale = { 0.0f, diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp index 23900f3868..7be8de9c6b 100644 --- a/src/modules/commander/mag_calibration.cpp +++ b/src/modules/commander/mag_calibration.cpp @@ -155,7 +155,7 @@ int do_mag_calibration(int mavlink_fd) uint64_t calibration_deadline = hrt_absolute_time() + calibration_interval; unsigned poll_errcount = 0; - mavlink_log_info(mavlink_fd, "rotate in a figure 8 around all axis"); + mavlink_log_info(mavlink_fd, "Turn on all sides: front/back,left/right,up/down"); calibration_counter = 0; From 166580b8f7415de90f3e5c0171a75b9827158d12 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 2 Nov 2014 21:41:55 +0100 Subject: [PATCH 390/411] Time out on serial instead of just hanging there --- Tools/px_uploader.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/Tools/px_uploader.py b/Tools/px_uploader.py index b46db00b57..0bf2b82dde 100755 --- a/Tools/px_uploader.py +++ b/Tools/px_uploader.py @@ -178,9 +178,9 @@ class uploader(object): MAVLINK_REBOOT_ID1 = bytearray(b'\xfe\x21\x72\xff\x00\x4c\x00\x00\x80\x3f\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\xf6\x00\x01\x00\x00\x48\xf0') MAVLINK_REBOOT_ID0 = bytearray(b'\xfe\x21\x45\xff\x00\x4c\x00\x00\x80\x3f\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\xf6\x00\x00\x00\x00\xd7\xac') - def __init__(self, portname, baudrate, interCharTimeout=0.001, timeout=0.5): + def __init__(self, portname, baudrate): # open the port, keep the default timeout short so we can poll quickly - self.port = serial.Serial(portname, baudrate) + self.port = serial.Serial(portname, baudrate, timeout=0.5) self.otp = b'' self.sn = b'' From 5862f4ffe6b463934cceb7bb034e77f100f97633 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 2 Nov 2014 22:00:42 +0100 Subject: [PATCH 391/411] Fix error handling --- Tools/px_uploader.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Tools/px_uploader.py b/Tools/px_uploader.py index 0bf2b82dde..ea2f646390 100755 --- a/Tools/px_uploader.py +++ b/Tools/px_uploader.py @@ -195,7 +195,7 @@ class uploader(object): def __recv(self, count=1): c = self.port.read(count) if len(c) < 1: - raise RuntimeError("timeout waiting for data (%u bytes)", count) + raise RuntimeError("timeout waiting for data (%u bytes)" % count) # print("recv " + binascii.hexlify(c)) return c From 44a247363248caadecc4518048ab513d2dfd202e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 2 Nov 2014 22:03:49 +0100 Subject: [PATCH 392/411] Fix up reboot logic --- Tools/px_uploader.py | 2 ++ 1 file changed, 2 insertions(+) diff --git a/Tools/px_uploader.py b/Tools/px_uploader.py index ea2f646390..8a89fdf06a 100755 --- a/Tools/px_uploader.py +++ b/Tools/px_uploader.py @@ -511,6 +511,8 @@ while True: up.send_reboot() # wait for the reboot, without we might run into Serial I/O Error 5 time.sleep(0.5) + # always close the port + up.close() continue try: From 72977ee9098758b5e00d9ac8df916bb221b2953f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 2 Nov 2014 22:13:59 +0100 Subject: [PATCH 393/411] Better error handling instructions --- Tools/px_uploader.py | 2 ++ 1 file changed, 2 insertions(+) diff --git a/Tools/px_uploader.py b/Tools/px_uploader.py index 8a89fdf06a..d4e4612262 100755 --- a/Tools/px_uploader.py +++ b/Tools/px_uploader.py @@ -459,6 +459,7 @@ if os.path.exists("/usr/sbin/ModemManager"): # Load the firmware file fw = firmware(args.firmware) print("Loaded firmware for %x,%x, waiting for the bootloader..." % (fw.property('board_id'), fw.property('board_revision'))) +print("If the board does not respond within 1-2 seconds, unplug and re-plug the USB connector.") # Spin waiting for a device to show up while True: @@ -508,6 +509,7 @@ while True: except Exception: # most probably a timeout talking to the port, no bootloader, try to reboot the board print("attempting reboot on %s..." % port) + print("if the board does not respond, unplug and re-plug the USB connector.") up.send_reboot() # wait for the reboot, without we might run into Serial I/O Error 5 time.sleep(0.5) From 5e2330abbae35bfc45e5ac67d3590b53de20f526 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 3 Nov 2014 08:39:16 +0100 Subject: [PATCH 394/411] Add USB load test from Mark Whitehorn --- Tools/usb_serialload.py | 55 +++++++++++++++++++++++++++++++++++++++++ 1 file changed, 55 insertions(+) create mode 100644 Tools/usb_serialload.py diff --git a/Tools/usb_serialload.py b/Tools/usb_serialload.py new file mode 100644 index 0000000000..5c864532f7 --- /dev/null +++ b/Tools/usb_serialload.py @@ -0,0 +1,55 @@ +import serial, time + + +port = serial.Serial('/dev/ttyACM0', baudrate=57600, timeout=2) + +data = '01234567890123456789012345678901234567890123456789' +#data = 'hellohello' +outLine = 'echo %s\n' % data + +port.write('\n\n\n') +port.write('free\n') +line = port.readline(80) +while line != '': + print(line) + line = port.readline(80) + + +i = 0 +bytesOut = 0 +bytesIn = 0 + +startTime = time.time() +lastPrint = startTime +while True: + bytesOut += port.write(outLine) + line = port.readline(80) + bytesIn += len(line) + # check command line echo + if (data not in line): + print('command error %d: %s' % (i,line)) + #break + # read echo output + line = port.readline(80) + if (data not in line): + print('echo output error %d: %s' % (i,line)) + #break + bytesIn += len(line) + #print('%d: %s' % (i,line)) + #print('%d: bytesOut: %d, bytesIn: %d' % (i, bytesOut, bytesIn)) + + elapsedT = time.time() - lastPrint + if (time.time() - lastPrint >= 5): + outRate = bytesOut / elapsedT + inRate = bytesIn / elapsedT + usbRate = (bytesOut + bytesIn) / elapsedT + lastPrint = time.time() + print('elapsed time: %f' % (time.time() - startTime)) + print('data rates (bytes/sec): out: %f, in: %f, total: %f' % (outRate, inRate, usbRate)) + + bytesOut = 0 + bytesIn = 0 + + i += 1 + #if (i > 2): break + From 9ac13745f8356db60322fa92ecdff1125c76f172 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 3 Nov 2014 09:48:06 +0100 Subject: [PATCH 395/411] Adjust MAVLink RX prio to ensure received data can still be processed --- 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 9247f999af..bc092c7e9c 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -1396,7 +1396,7 @@ MavlinkReceiver::receive_start(Mavlink *parent) struct sched_param param; (void)pthread_attr_getschedparam(&receiveloop_attr, ¶m); - param.sched_priority = SCHED_PRIORITY_MAX - 40; + param.sched_priority = SCHED_PRIORITY_MAX - 80; (void)pthread_attr_setschedparam(&receiveloop_attr, ¶m); pthread_attr_setstacksize(&receiveloop_attr, 2900); From 8652ee0b6474fa4fa92b0d6ab5eaec1436a03b8b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 3 Nov 2014 09:48:24 +0100 Subject: [PATCH 396/411] Drop NSH priority to keep system responsive --- src/systemcmds/nshterm/module.mk | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/systemcmds/nshterm/module.mk b/src/systemcmds/nshterm/module.mk index 41706e137b..a12bc369e8 100644 --- a/src/systemcmds/nshterm/module.mk +++ b/src/systemcmds/nshterm/module.mk @@ -41,3 +41,5 @@ SRCS = nshterm.c MODULE_STACKSIZE = 1600 MAXOPTIMIZATION = -Os + +MODULE_PRIORITY = "SCHED_PRIORITY_DEFAULT-30" From 06df0f23a32c87486815a9794c1425fe4534ac13 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 3 Nov 2014 12:56:35 +0100 Subject: [PATCH 397/411] L3GD20: Output gyro temperature in report --- src/drivers/l3gd20/l3gd20.cpp | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index cfae8761c6..ff28713542 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -176,6 +176,7 @@ static const int ERROR = -1; #define L3G4200D_DEFAULT_RATE 800 #define L3GD20_DEFAULT_RANGE_DPS 2000 #define L3GD20_DEFAULT_FILTER_FREQ 30 +#define L3GD20_TEMP_OFFSET_CELCIUS 40 #ifndef SENSOR_BOARD_ROTATION_DEFAULT #define SENSOR_BOARD_ROTATION_DEFAULT SENSOR_BOARD_ROTATION_270_DEG @@ -856,7 +857,7 @@ L3GD20::measure() #pragma pack(push, 1) struct { uint8_t cmd; - uint8_t temp; + int8_t temp; uint8_t status; int16_t x; int16_t y; @@ -930,6 +931,8 @@ L3GD20::measure() report.z_raw = raw_report.z; + report.temperature_raw = raw_report.temp; + report.x = ((report.x_raw * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale; report.y = ((report.y_raw * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale; report.z = ((report.z_raw * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale; @@ -938,6 +941,8 @@ L3GD20::measure() report.y = _gyro_filter_y.apply(report.y); report.z = _gyro_filter_z.apply(report.z); + report.temperature = L3GD20_TEMP_OFFSET_CELCIUS - raw_report.temp; + // apply user specified rotation rotate_3f(_rotation, report.x, report.y, report.z); @@ -1091,9 +1096,11 @@ test() warnx("gyro x: \t% 9.5f\trad/s", (double)g_report.x); warnx("gyro y: \t% 9.5f\trad/s", (double)g_report.y); warnx("gyro z: \t% 9.5f\trad/s", (double)g_report.z); + warnx("temp: \t%d\tC", (int)g_report.temperature); warnx("gyro x: \t%d\traw", (int)g_report.x_raw); warnx("gyro y: \t%d\traw", (int)g_report.y_raw); warnx("gyro z: \t%d\traw", (int)g_report.z_raw); + warnx("temp: \t%d\traw", (int)g_report.temperature_raw); warnx("gyro range: %8.4f rad/s (%d deg/s)", (double)g_report.range_rad_s, (int)((g_report.range_rad_s / M_PI_F) * 180.0f + 0.5f)); From c600a7fbd27c298e855d1f3e6f00f590141f995e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 3 Nov 2014 15:37:24 +0100 Subject: [PATCH 398/411] L3GD20: Fix typo --- src/drivers/l3gd20/l3gd20.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index ff28713542..82fa5cc6e7 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -176,7 +176,7 @@ static const int ERROR = -1; #define L3G4200D_DEFAULT_RATE 800 #define L3GD20_DEFAULT_RANGE_DPS 2000 #define L3GD20_DEFAULT_FILTER_FREQ 30 -#define L3GD20_TEMP_OFFSET_CELCIUS 40 +#define L3GD20_TEMP_OFFSET_CELSIUS 40 #ifndef SENSOR_BOARD_ROTATION_DEFAULT #define SENSOR_BOARD_ROTATION_DEFAULT SENSOR_BOARD_ROTATION_270_DEG @@ -941,7 +941,7 @@ L3GD20::measure() report.y = _gyro_filter_y.apply(report.y); report.z = _gyro_filter_z.apply(report.z); - report.temperature = L3GD20_TEMP_OFFSET_CELCIUS - raw_report.temp; + report.temperature = L3GD20_TEMP_OFFSET_CELSIUS - raw_report.temp; // apply user specified rotation rotate_3f(_rotation, report.x, report.y, report.z); From f5dbfe63b3d7fbfcac524ba03028de962a8716e2 Mon Sep 17 00:00:00 2001 From: tumbili Date: Tue, 4 Nov 2014 20:52:29 +0100 Subject: [PATCH 399/411] added new sensor board rotation option (roll 270, yaw 270) --- src/lib/conversion/rotation.h | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/lib/conversion/rotation.h b/src/lib/conversion/rotation.h index 5187b448f2..739c3a9603 100644 --- a/src/lib/conversion/rotation.h +++ b/src/lib/conversion/rotation.h @@ -74,6 +74,7 @@ enum Rotation { ROTATION_ROLL_270_YAW_135 = 23, ROTATION_PITCH_90 = 24, ROTATION_PITCH_270 = 25, + ROTATION_ROLL_270_YAW_270 = 26, ROTATION_MAX }; @@ -109,7 +110,8 @@ const rot_lookup_t rot_lookup[] = { {270, 0, 90 }, {270, 0, 135 }, { 0, 90, 0 }, - { 0, 270, 0 } + { 0, 270, 0 }, + {270, 0, 270 } }; /** From cbd20f48d670749fcbfe49c0f5135572bdf7ee44 Mon Sep 17 00:00:00 2001 From: tumbili Date: Tue, 4 Nov 2014 20:57:09 +0100 Subject: [PATCH 400/411] fixed style --- src/lib/conversion/rotation.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/conversion/rotation.h b/src/lib/conversion/rotation.h index 739c3a9603..917c7f830e 100644 --- a/src/lib/conversion/rotation.h +++ b/src/lib/conversion/rotation.h @@ -74,7 +74,7 @@ enum Rotation { ROTATION_ROLL_270_YAW_135 = 23, ROTATION_PITCH_90 = 24, ROTATION_PITCH_270 = 25, - ROTATION_ROLL_270_YAW_270 = 26, + ROTATION_ROLL_270_YAW_270 = 26, ROTATION_MAX }; From 182c1c1d52ef370a42b99c825e3f45a03ecbcdc9 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 5 Nov 2014 17:17:34 +0100 Subject: [PATCH 401/411] Fix RC mapping indices - 0 index induces issues right now --- src/drivers/px4io/px4io.cpp | 15 ++++++++++----- 1 file changed, 10 insertions(+), 5 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index fd9eb41709..c6543982ee 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1247,28 +1247,33 @@ PX4IO::io_set_rc_config() */ param_get(param_find("RC_MAP_ROLL"), &ichan); + /* subtract one from 1-based index - this might be + * a negative number now + */ + ichan -= 1; + if ((ichan >= 0) && (ichan < (int)_max_rc_input)) - input_map[ichan - 1] = 0; + input_map[ichan] = 0; param_get(param_find("RC_MAP_PITCH"), &ichan); if ((ichan >= 0) && (ichan < (int)_max_rc_input)) - input_map[ichan - 1] = 1; + input_map[ichan] = 1; param_get(param_find("RC_MAP_YAW"), &ichan); if ((ichan >= 0) && (ichan < (int)_max_rc_input)) - input_map[ichan - 1] = 2; + input_map[ichan] = 2; param_get(param_find("RC_MAP_THROTTLE"), &ichan); if ((ichan >= 0) && (ichan < (int)_max_rc_input)) - input_map[ichan - 1] = 3; + input_map[ichan] = 3; param_get(param_find("RC_MAP_FLAPS"), &ichan); if ((ichan >= 0) && (ichan < (int)_max_rc_input)) - input_map[ichan - 1] = 4; + input_map[ichan] = 4; param_get(param_find("RC_MAP_MODE_SW"), &ichan); From 23a33e31cd91d074f2d16d04b0cd7f2c867d9d4b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 5 Nov 2014 19:26:10 +0100 Subject: [PATCH 402/411] Add missing mode switch channel --- 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 c6543982ee..6d68d9f604 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1279,7 +1279,7 @@ PX4IO::io_set_rc_config() if ((ichan >= 0) && (ichan < (int)_max_rc_input)) { /* use out of normal bounds index to indicate special channel */ - input_map[ichan - 1] = PX4IO_P_RC_CONFIG_ASSIGNMENT_MODESWITCH; + input_map[ichan] = PX4IO_P_RC_CONFIG_ASSIGNMENT_MODESWITCH; } /* From 43418a674903d242271028c4d4eb473f95a24be6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 7 Nov 2014 13:31:33 +0100 Subject: [PATCH 403/411] application layer only, no drivers affected: Fix overflow in RC input topic - this topic is deprecated and will be removed, has been superseded by input_rc and manual_control --- src/modules/uORB/topics/rc_channels.h | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/src/modules/uORB/topics/rc_channels.h b/src/modules/uORB/topics/rc_channels.h index b14ae1edd4..16916cc4df 100644 --- a/src/modules/uORB/topics/rc_channels.h +++ b/src/modules/uORB/topics/rc_channels.h @@ -34,6 +34,8 @@ /** * @file rc_channels.h * Definition of the rc_channels uORB topic. + * + * @deprecated DO NOT USE FOR NEW CODE */ #ifndef RC_CHANNELS_H_ @@ -63,10 +65,13 @@ enum RC_CHANNELS_FUNCTION { 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. */ + AUX_5 }; +// MAXIMUM FUNCTIONS IS != MAXIMUM RC INPUT CHANNELS + +#define RC_CHANNELS_FUNCTION_MAX 18 + /** * @addtogroup topics * @{ From 02d31522cde1de13ab397f4fbd77c0cf9b7f712d Mon Sep 17 00:00:00 2001 From: Daniel Shiels Date: Wed, 29 Oct 2014 22:00:30 +1100 Subject: [PATCH 404/411] First attempt at sbus1 output. --- src/modules/px4iofirmware/sbus.c | 37 +++++++++++++++++++++++++++++--- 1 file changed, 34 insertions(+), 3 deletions(-) diff --git a/src/modules/px4iofirmware/sbus.c b/src/modules/px4iofirmware/sbus.c index 6ead38d617..23dae78985 100644 --- a/src/modules/px4iofirmware/sbus.c +++ b/src/modules/px4iofirmware/sbus.c @@ -80,8 +80,10 @@ static int sbus_fd = -1; static hrt_abstime last_rx_time; static hrt_abstime last_frame_time; +static hrt_abstime last_txframe_time=0; static uint8_t frame[SBUS_FRAME_SIZE]; +static uint8_t oframe[SBUS_FRAME_SIZE]; static unsigned partial_frame_count; @@ -122,10 +124,39 @@ sbus_init(const char *device) void sbus1_output(uint16_t *values, uint16_t num_values) { - char a = 'A'; - write(sbus_fd, &a, 1); -} + //int. first byte of data is offset 1 in sbus + uint8_t byteindex=1; + uint8_t offset=0; + uint16_t value; + hrt_abstime now; + //oframe[0]=0xf0; + oframe[0]=0x0f; + now = hrt_absolute_time(); + if ((now - last_txframe_time) > 14000) { + last_txframe_time = now; + + for (uint16_t i=1;i=8) { + ++byteindex; + offset-=8; + } + oframe[byteindex] |= (value<<(offset))&0xff; + oframe[byteindex+1]|=(value>>(8-offset))&0xff; + oframe[byteindex+2]|=(value>>(16-offset))&0xff; + offset+=11; + } + write(sbus_fd, oframe, SBUS_FRAME_SIZE); + } +} void sbus2_output(uint16_t *values, uint16_t num_values) { From 60ecd8868db3539e552bad5f51473d1baf5d6ecd Mon Sep 17 00:00:00 2001 From: Daniel Shiels Date: Sat, 8 Nov 2014 22:36:45 +1100 Subject: [PATCH 405/411] sbus1 output. Cleaned up. Safer bounds checking. --- src/modules/px4iofirmware/sbus.c | 47 ++++++++++++++++++-------------- 1 file changed, 27 insertions(+), 20 deletions(-) diff --git a/src/modules/px4iofirmware/sbus.c b/src/modules/px4iofirmware/sbus.c index 23dae78985..2f234234ea 100644 --- a/src/modules/px4iofirmware/sbus.c +++ b/src/modules/px4iofirmware/sbus.c @@ -57,6 +57,7 @@ #define SBUS_FLAGS_BYTE 23 #define SBUS_FAILSAFE_BIT 3 #define SBUS_FRAMELOST_BIT 2 +#define SBUS1_FRAME_DELAY 14000 /* Measured values with Futaba FX-30/R6108SB: @@ -80,7 +81,7 @@ static int sbus_fd = -1; static hrt_abstime last_rx_time; static hrt_abstime last_frame_time; -static hrt_abstime last_txframe_time=0; +static hrt_abstime last_txframe_time = 0; static uint8_t frame[SBUS_FRAME_SIZE]; static uint8_t oframe[SBUS_FRAME_SIZE]; @@ -125,35 +126,41 @@ void sbus1_output(uint16_t *values, uint16_t num_values) { //int. first byte of data is offset 1 in sbus - uint8_t byteindex=1; - uint8_t offset=0; + uint8_t byteindex = 1; + uint8_t offset = 0; uint16_t value; hrt_abstime now; - //oframe[0]=0xf0; - oframe[0]=0x0f; + oframe[0] = 0x0f; - now = hrt_absolute_time(); - if ((now - last_txframe_time) > 14000) { + now = hrt_absolute_time(); + + if ((now - last_txframe_time) > SBUS1_FRAME_DELAY) { last_txframe_time = now; - for (uint16_t i=1;i=8) { - ++byteindex; - offset-=8; + if (value > 0x07ff ) { + value = 0x07ff; } - oframe[byteindex] |= (value<<(offset))&0xff; - oframe[byteindex+1]|=(value>>(8-offset))&0xff; - oframe[byteindex+2]|=(value>>(16-offset))&0xff; - offset+=11; - } + + while (offset >= 8) { + ++byteindex; + offset -= 8; + } + + oframe[byteindex] |= (value << (offset)) & 0xff; + oframe[byteindex + 1] |= (value >> (8 - offset)) & 0xff; + oframe[byteindex + 2] |= (value >> (16 - offset)) & 0xff; + offset += 11; + } + write(sbus_fd, oframe, SBUS_FRAME_SIZE); } } From 8599994ebb3e4ce63b5fe77cb1f2935d27604745 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 8 Nov 2014 20:38:57 +0100 Subject: [PATCH 406/411] Add generic uploader tool --- Tools/upload.sh | 28 ++++++++++++++++++++++++++++ 1 file changed, 28 insertions(+) create mode 100755 Tools/upload.sh diff --git a/Tools/upload.sh b/Tools/upload.sh new file mode 100755 index 0000000000..17d87fe610 --- /dev/null +++ b/Tools/upload.sh @@ -0,0 +1,28 @@ +#!/bin/bash + +EXEDIR=`pwd` +BASEDIR=$(dirname $0) + +SYSTYPE=`uname -s` + +# +# Serial port defaults. +# +# XXX The uploader should be smarter than this. +# +if [ $SYSTYPE=Darwin ]; +then +SERIAL_PORTS="/dev/tty.usbmodemPX*,/dev/tty.usbmodem*" +fi + +if [ $SYSTYPE=Linux ]; +then +SERIAL_PORTS="/dev/serial/by-id/usb-3D_Robotics*" +fi + +if [ $SYSTYPE="" ]; +then +SERIAL_PORTS="COM32,COM31,COM30,COM29,COM28,COM27,COM26,COM25,COM24,COM23,COM22,COM21,COM20,COM19,COM18,COM17,COM16,COM15,COM14,COM13,COM12,COM11,COM10,COM9,COM8,COM7,COM6,COM5,COM4,COM3,COM2,COM1,COM0" +fi + +python $BASEDIR/px_uploader.py --port $SERIAL_PORTS $1 \ No newline at end of file From e87a179fc1b9b10bfd22e741229e9cbd75b6ef73 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 9 Nov 2014 15:16:05 +0100 Subject: [PATCH 407/411] MAVLink update --- mavlink/include/mavlink/v1.0 | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mavlink/include/mavlink/v1.0 b/mavlink/include/mavlink/v1.0 index 90383fac84..ad5e5a645d 160000 --- a/mavlink/include/mavlink/v1.0 +++ b/mavlink/include/mavlink/v1.0 @@ -1 +1 @@ -Subproject commit 90383fac84d031aef17989a1497c2473dfa64340 +Subproject commit ad5e5a645dec152419264ad32221f7c113ea5c30 From 62db84fa758ecc30081e30a18464fe3060d2c2d8 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 9 Nov 2014 18:20:53 +0100 Subject: [PATCH 408/411] MAVLink update compile fixes --- src/modules/mavlink/mavlink.c | 6 +----- src/modules/mavlink/mavlink_main.cpp | 12 +++++++----- src/modules/mavlink/mavlink_main.h | 4 ++++ src/modules/mavlink/mavlink_messages.cpp | 12 +++++++----- 4 files changed, 19 insertions(+), 15 deletions(-) diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c index bec706badd..9afe74af1c 100644 --- a/src/modules/mavlink/mavlink.c +++ b/src/modules/mavlink/mavlink.c @@ -78,11 +78,7 @@ PARAM_DEFINE_INT32(MAV_FWDEXTSP, 1); mavlink_system_t mavlink_system = { 100, - 50, - MAV_TYPE_FIXED_WING, - 0, - 0, - 0 + 50 }; // System ID, 1-255, Component/Subsystem ID, 1-255 /* diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 6b4edff78a..c251dd3b2c 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -167,8 +167,10 @@ Mavlink::Mavlink() : _param_initialized(false), _param_system_id(0), _param_component_id(0), - _param_system_type(0), + _param_system_type(MAV_TYPE_FIXED_WING), _param_use_hil_gps(0), + _param_forward_externalsp(0), + _system_type(0), /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "mavlink_el")), @@ -524,7 +526,7 @@ void Mavlink::mavlink_update_system(void) param_get(_param_system_type, &system_type); if (system_type >= 0 && system_type < MAV_TYPE_ENUM_END) { - mavlink_system.type = system_type; + _system_type = system_type; } int32_t use_hil_gps; @@ -755,7 +757,7 @@ Mavlink::send_message(const uint8_t msgid, const void *msg) pthread_mutex_lock(&_send_mutex); - int buf_free = get_free_tx_buf(); + unsigned buf_free = get_free_tx_buf(); uint8_t payload_len = mavlink_message_lengths[msgid]; unsigned packet_len = payload_len + MAVLINK_NUM_NON_PAYLOAD_BYTES; @@ -820,14 +822,14 @@ Mavlink::resend_message(mavlink_message_t *msg) pthread_mutex_lock(&_send_mutex); - int buf_free = get_free_tx_buf(); + unsigned buf_free = get_free_tx_buf(); _last_write_try_time = hrt_absolute_time(); unsigned packet_len = msg->len + MAVLINK_NUM_NON_PAYLOAD_BYTES; /* check if there is space in the buffer, let it overflow else */ - if (buf_free < TX_BUFFER_GAP) { + if ((buf_free < TX_BUFFER_GAP) || (buf_free < packet_len)) { /* no enough space in buffer to send */ count_txerr(); count_txerrbytes(packet_len); diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 1f96e586bb..ad5e5001b9 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -265,6 +265,8 @@ public: struct mavlink_logbuffer *get_logbuffer() { return &_logbuffer; } + unsigned get_system_type() { return _system_type; } + protected: Mavlink *next; @@ -354,6 +356,8 @@ private: param_t _param_use_hil_gps; param_t _param_forward_externalsp; + unsigned _system_type; + perf_counter_t _loop_perf; /**< loop performance counter */ perf_counter_t _txerr_perf; /**< TX error counter */ diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index cccb698bf9..b4911427f6 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -302,7 +302,7 @@ protected: msg.base_mode = 0; msg.custom_mode = 0; get_mavlink_mode_state(&status, &pos_sp_triplet, &msg.system_status, &msg.base_mode, &msg.custom_mode); - msg.type = mavlink_system.type; + msg.type = _mavlink->get_system_type(); msg.autopilot = MAV_AUTOPILOT_PX4; msg.mavlink_version = 3; @@ -1353,15 +1353,17 @@ protected: const float pwm_center = (PWM_HIGHEST_MAX + PWM_LOWEST_MIN) / 2; + unsigned system_type = _mavlink->get_system_type(); + /* scale outputs depending on system type */ - if (mavlink_system.type == MAV_TYPE_QUADROTOR || - mavlink_system.type == MAV_TYPE_HEXAROTOR || - mavlink_system.type == MAV_TYPE_OCTOROTOR) { + if (system_type == MAV_TYPE_QUADROTOR || + system_type == MAV_TYPE_HEXAROTOR || + system_type == MAV_TYPE_OCTOROTOR) { /* multirotors: set number of rotor outputs depending on type */ unsigned n; - switch (mavlink_system.type) { + switch (system_type) { case MAV_TYPE_QUADROTOR: n = 4; break; From 34e9d9efce10ce487b4c5af0cf93a442ab3b8594 Mon Sep 17 00:00:00 2001 From: Daniel Shiels Date: Mon, 10 Nov 2014 15:54:28 +1100 Subject: [PATCH 409/411] Code style cleanup. --- src/modules/px4iofirmware/sbus.c | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) diff --git a/src/modules/px4iofirmware/sbus.c b/src/modules/px4iofirmware/sbus.c index 2f234234ea..90c0e05038 100644 --- a/src/modules/px4iofirmware/sbus.c +++ b/src/modules/px4iofirmware/sbus.c @@ -125,8 +125,7 @@ sbus_init(const char *device) void sbus1_output(uint16_t *values, uint16_t num_values) { - //int. first byte of data is offset 1 in sbus - uint8_t byteindex = 1; + uint8_t byteindex = 1; /*Data starts one byte into the sbus frame. */ uint8_t offset = 0; uint16_t value; hrt_abstime now; @@ -141,11 +140,13 @@ sbus1_output(uint16_t *values, uint16_t num_values) oframe[i] = 0; } - // 16 is sbus number of servos/channels minus 2 single bit channels. - // currently ignoring single bit channels. - for (uint16_t i = 0; (i < num_values) && (i < 16); ++i) { + /* 16 is sbus number of servos/channels minus 2 single bit channels. + * currently ignoring single bit channels. */ + + for (unsigned i = 0; (i < num_values) && (i < 16); ++i) { value = (uint16_t)(((values[i] - SBUS_SCALE_OFFSET) / SBUS_SCALE_FACTOR) + .5f); - //protect from out of bounds values and limit to 11 bits; + + /*protect from out of bounds values and limit to 11 bits*/ if (value > 0x07ff ) { value = 0x07ff; } From 3eeec2cce05c8f32a5c2d70b956786d41114df3c Mon Sep 17 00:00:00 2001 From: Daniel Shiels Date: Mon, 10 Nov 2014 16:34:28 +1100 Subject: [PATCH 410/411] Cleaned up sbus output frame initialisation. --- src/modules/px4iofirmware/sbus.c | 9 ++------- 1 file changed, 2 insertions(+), 7 deletions(-) diff --git a/src/modules/px4iofirmware/sbus.c b/src/modules/px4iofirmware/sbus.c index 90c0e05038..d76ec55f05 100644 --- a/src/modules/px4iofirmware/sbus.c +++ b/src/modules/px4iofirmware/sbus.c @@ -84,7 +84,6 @@ static hrt_abstime last_frame_time; static hrt_abstime last_txframe_time = 0; static uint8_t frame[SBUS_FRAME_SIZE]; -static uint8_t oframe[SBUS_FRAME_SIZE]; static unsigned partial_frame_count; @@ -128,17 +127,13 @@ sbus1_output(uint16_t *values, uint16_t num_values) uint8_t byteindex = 1; /*Data starts one byte into the sbus frame. */ uint8_t offset = 0; uint16_t value; - hrt_abstime now; - oframe[0] = 0x0f; + hrt_abstime now; now = hrt_absolute_time(); if ((now - last_txframe_time) > SBUS1_FRAME_DELAY) { last_txframe_time = now; - - for (uint16_t i = 1; i < SBUS_FRAME_SIZE; ++i) { - oframe[i] = 0; - } + uint8_t oframe[SBUS_FRAME_SIZE] = { 0x0f }; /* 16 is sbus number of servos/channels minus 2 single bit channels. * currently ignoring single bit channels. */ From ea0a59f80650c7bd7c72889f1c7bb72bfd3fdbd5 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 10 Nov 2014 09:26:11 +0100 Subject: [PATCH 411/411] HMC5883: Reduce output, add indices to make back-tracking lines easier, remove output completely for an OK-operation (the driver is NOT the right place to talk to the user!) --- src/drivers/hmc5883/hmc5883.cpp | 49 ++++++++++----------------------- 1 file changed, 15 insertions(+), 34 deletions(-) diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index e4ecfa6b5d..81f7679650 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -1049,11 +1049,9 @@ int HMC5883::calibrate(struct file *filp, unsigned enable) * LSM/Ga, giving 1.16 and 1.08 */ float expected_cal[3] = { 1.16f, 1.08f, 1.08f }; - warnx("starting mag scale calibration"); - /* start the sensor polling at 50 Hz */ if (OK != ioctl(filp, SENSORIOCSPOLLRATE, 50)) { - warn("failed to set 2Hz poll rate"); + warn("FAILED: SENSORIOCSPOLLRATE 2Hz"); ret = 1; goto out; } @@ -1061,25 +1059,25 @@ int HMC5883::calibrate(struct file *filp, unsigned enable) /* Set to 2.5 Gauss. We ask for 3 to get the right part of * the chained if statement above. */ if (OK != ioctl(filp, MAGIOCSRANGE, 3)) { - warnx("failed to set 2.5 Ga range"); + warnx("FAILED: MAGIOCSRANGE 3.3 Ga"); ret = 1; goto out; } if (OK != ioctl(filp, MAGIOCEXSTRAP, 1)) { - warnx("failed to enable sensor calibration mode"); + warnx("FAILED: MAGIOCEXSTRAP 1"); ret = 1; goto out; } if (OK != ioctl(filp, MAGIOCGSCALE, (long unsigned int)&mscale_previous)) { - warn("WARNING: failed to get scale / offsets for mag"); + warn("FAILED: MAGIOCGSCALE 1"); ret = 1; goto out; } if (OK != ioctl(filp, MAGIOCSSCALE, (long unsigned int)&mscale_null)) { - warn("WARNING: failed to set null scale / offsets for mag"); + warn("FAILED: MAGIOCSSCALE 1"); ret = 1; goto out; } @@ -1094,7 +1092,7 @@ int HMC5883::calibrate(struct file *filp, unsigned enable) ret = ::poll(&fds, 1, 2000); if (ret != 1) { - warn("timed out waiting for sensor data"); + warn("ERROR: TIMEOUT 1"); goto out; } @@ -1102,7 +1100,7 @@ int HMC5883::calibrate(struct file *filp, unsigned enable) sz = ::read(fd, &report, sizeof(report)); if (sz != sizeof(report)) { - warn("periodic read failed"); + warn("ERROR: READ 1"); ret = -EIO; goto out; } @@ -1118,7 +1116,7 @@ int HMC5883::calibrate(struct file *filp, unsigned enable) ret = ::poll(&fds, 1, 2000); if (ret != 1) { - warn("timed out waiting for sensor data"); + warn("ERROR: TIMEOUT 2"); goto out; } @@ -1126,7 +1124,7 @@ int HMC5883::calibrate(struct file *filp, unsigned enable) sz = ::read(fd, &report, sizeof(report)); if (sz != sizeof(report)) { - warn("periodic read failed"); + warn("ERROR: READ 2"); ret = -EIO; goto out; } @@ -1142,33 +1140,19 @@ int HMC5883::calibrate(struct file *filp, unsigned enable) sum_excited[1] += cal[1]; sum_excited[2] += cal[2]; } - - //warnx("periodic read %u", i); - //warnx("measurement: %.6f %.6f %.6f", (double)report.x, (double)report.y, (double)report.z); - //warnx("cal: %.6f %.6f %.6f", (double)cal[0], (double)cal[1], (double)cal[2]); } if (good_count < 5) { - warn("failed calibration"); ret = -EIO; goto out; } -#if 0 - warnx("measurement avg: %.6f %.6f %.6f", - (double)sum_excited[0]/good_count, - (double)sum_excited[1]/good_count, - (double)sum_excited[2]/good_count); -#endif - float scaling[3]; scaling[0] = sum_excited[0] / good_count; scaling[1] = sum_excited[1] / good_count; scaling[2] = sum_excited[2] / good_count; - warnx("axes scaling: %.6f %.6f %.6f", (double)scaling[0], (double)scaling[1], (double)scaling[2]); - /* set scaling in device */ mscale_previous.x_scale = scaling[0]; mscale_previous.y_scale = scaling[1]; @@ -1179,29 +1163,26 @@ int HMC5883::calibrate(struct file *filp, unsigned enable) out: if (OK != ioctl(filp, MAGIOCSSCALE, (long unsigned int)&mscale_previous)) { - warn("failed to set new scale / offsets for mag"); + warn("FAILED: MAGIOCSSCALE 2"); } /* set back to normal mode */ /* Set to 1.1 Gauss */ if (OK != ::ioctl(fd, MAGIOCSRANGE, 1)) { - warnx("failed to set 1.1 Ga range"); + warnx("FAILED: MAGIOCSRANGE 1.1 Ga"); } if (OK != ::ioctl(fd, MAGIOCEXSTRAP, 0)) { - warnx("failed to disable sensor calibration mode"); + warnx("FAILED: MAGIOCEXSTRAP 0"); } if (ret == OK) { - if (!check_scale()) { - warnx("mag scale calibration successfully finished."); - } else { - warnx("mag scale calibration finished with invalid results."); + if (check_scale()) { + /* failed */ + warnx("FAILED: SCALE"); ret = ERROR; } - } else { - warnx("mag scale calibration failed."); } return ret;