From 23237df84ef1fcf955c739c721a15ca356fddf96 Mon Sep 17 00:00:00 2001 From: Juchli D Date: Tue, 5 Nov 2013 13:24:41 +0100 Subject: 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 0441566e9..a2c62cf06 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) { -- cgit v1.2.3 From 56f1ea9266eacd689a7b4065231d94a78ea96de6 Mon Sep 17 00:00:00 2001 From: Juchli D Date: Tue, 5 Nov 2013 13:25:49 +0100 Subject: 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 761fb8d9d..a2fb63422 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 000000000..69720b439 --- /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 000000000..222858b27 --- /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 -- cgit v1.2.3 From 4c9a4bc9a4686a2806a0119c262a744ca5517c02 Mon Sep 17 00:00:00 2001 From: Juchli D Date: Tue, 5 Nov 2013 13:26:34 +0100 Subject: Added custom start up of bottle drop --- ROMFS/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 000000000..522f78210 --- /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 cff8446a6..6e954f58a 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 -- cgit v1.2.3 From af3a56f17f6863210b0bcbf9569754a8c4ba3e53 Mon Sep 17 00:00:00 2001 From: Juchli D Date: Tue, 5 Nov 2013 13:27:05 +0100 Subject: 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 a2c62cf06..38cfce2e9 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 */ -- cgit v1.2.3 From adee47978236aa113ee29f071f3498a60a802477 Mon Sep 17 00:00:00 2001 From: Juchli D Date: Fri, 15 Nov 2013 10:30:48 +0100 Subject: Working bottle Drop test --- ROMFS/px4fmu_common/init.d/900_bottle_drop_test | 29 +++++++++++ src/modules/bottle_drop/bottle_drop.c | 68 +++++++++++++++++++------ 2 files changed, 81 insertions(+), 16 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/900_bottle_drop_test b/ROMFS/px4fmu_common/init.d/900_bottle_drop_test index 522f78210..c0f50fec0 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 69720b439..09bbf4687 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] = 0.5f; - actuators.control[1] = 0.5f; - actuators.control[2] = 0.5f; - actuators.control[3] = 0.5f; + 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; - actuators.timestamp = hrt_absolute_time(); - orb_publish(ORB_ID(actuator_controls_1), actuator_pub, &actuators); + } else { - usleep(50); + // 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++; + } - i++; } warnx("exiting.\n"); -- cgit v1.2.3 From 3c2133b9f1af370638cee82d6cda0b31df15dffe Mon Sep 17 00:00:00 2001 From: Juchli D Date: Fri, 29 Nov 2013 12:36:42 +0100 Subject: Working with faked imputs --- src/modules/bottle_drop/bottle_drop.c | 303 ++++++++++++++++++++++++++++++---- 1 file changed, 271 insertions(+), 32 deletions(-) diff --git a/src/modules/bottle_drop/bottle_drop.c b/src/modules/bottle_drop/bottle_drop.c index 09bbf4687..bb12674af 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; - if (open_now && (drop_start + 2e6 > hrt_absolute_time())) { // open door - actuators.control[0] = -1.0f; - actuators.control[1] = 1.0f; + orb_check(parameter_update_sub, &updated); + if (updated){ + /* copy global position */ + orb_copy(ORB_ID(parameter_update), parameter_update_sub, &update); - } else if (open_now) { // unlock bottle + /* 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); - 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); + // 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; + + + //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; + 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; } -- cgit v1.2.3 From 76e5a755dfdaec4bfc00493e8f16a5e40ffa5093 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 29 Nov 2013 17:03:18 +0100 Subject: 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 bb12674af..8d3d6d599 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++; } } -- cgit v1.2.3 From a839a09834a5ab3217e794a0a98af2eeffaf571b Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 29 Nov 2013 17:03:48 +0100 Subject: Bottle drop: Added HIL startup --- .../px4fmu_common/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 000000000..060c7d0b5 --- /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 46bb6d866..5327ee7ca 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 ] -- cgit v1.2.3 From 0ec609f49167e1e6857e970088b4e8de93bba032 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 2 Dec 2013 23:01:02 +0100 Subject: Bottle drop: added custom HIL startup script --- ROMFS/px4fmu_common/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 060c7d0b5..472351e9e 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 5327ee7ca..6c76f5fe9 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 ] -- cgit v1.2.3 From 6048f9beda68507d74e321ad4816aa137e20fe3a Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 2 Dec 2013 23:03:29 +0100 Subject: 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 c1d73dd87..b848f6c37 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) { -- cgit v1.2.3 From 808294b01a0df96817701526b24c6bb80e97775c Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 2 Dec 2013 23:03:52 +0100 Subject: 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 abc91d34f..634586fdb 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; -- cgit v1.2.3 From f2c303679ea859bf722eaa7289fd10f8b806264a Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 2 Dec 2013 23:05:28 +0100 Subject: Bottle drop: lots of changes, working in HIL --- src/modules/bottle_drop/bottle_drop.c | 136 ++++++++++++++++++++-------------- 1 file changed, 81 insertions(+), 55 deletions(-) diff --git a/src/modules/bottle_drop/bottle_drop.c b/src/modules/bottle_drop/bottle_drop.c index 8d3d6d599..1c2e008ba 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; - - //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; + 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); } - // 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 { -- cgit v1.2.3 From 56834414f1e37dd5f091685d3acd643d767dc0fa Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 4 Dec 2013 10:37:51 +0100 Subject: 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 dd3573d9a..acd612d9e 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 9e1f789ad..dab96eb9b 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 */ -- cgit v1.2.3 From 0aeada16b9601783da4fa1a7d628b34e1dce1bf4 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 4 Dec 2013 10:38:56 +0100 Subject: Navigator: handle onboard and mavlink missions --- src/modules/navigator/navigator_main.cpp | 172 ++++++++++++++++++++----------- src/modules/uORB/topics/mission.h | 3 +- 2 files changed, 113 insertions(+), 62 deletions(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index c6aac6af1..e2e2949e2 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) } } +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(unsigned mission_item_index, struct mission_item_s *new_mission_item) +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; + + 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); + } - mavlink_log_info(_mavlink_fd, "[navigator] heading to WP %d", _current_mission_index); + _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 30f06c359..370242007 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 -- cgit v1.2.3 From d0444497eddb75beae4bc0001e6aacbc137e0d66 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sat, 14 Dec 2013 20:52:25 +0100 Subject: 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 e2194bae2..1911329d0 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; + 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); + 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); } -- cgit v1.2.3 From 73907d1ac00b59759137d71f18693228cc3e75c9 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sat, 14 Dec 2013 20:54:25 +0100 Subject: 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 acd612d9e..874a47be7 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 dab96eb9b..2a781405a 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 7aad5038d..52a580d5b 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 7de87b476..5b121e34e 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; } -- cgit v1.2.3 From e685c65365511341edb7cb4eded8d645643114a6 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sat, 14 Dec 2013 20:55:03 +0100 Subject: Navigator: Use state table for main FSM --- src/modules/navigator/navigator_main.cpp | 1049 +++++++++++++++++------------- src/modules/systemlib/state_table.h | 75 +++ 2 files changed, 660 insertions(+), 464 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 e2e2949e2..d258aa8a6 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 global position + */ + void global_position_update(); /** - * Retrieve mission. - */ - void mission_update(); + * Retrieve home position + */ + void home_position_update(); /** - * Retrieve onboard mission. - */ + * 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); - - 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); + /** + * 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(); - void publish_mission_item_triplet(); + /** + * Guards offboard mission + */ + bool offboard_mission_available(unsigned relative_index); - void publish_mission_result(); + /** + * Guards onboard mission + */ + bool onboard_mission_available(unsigned relative_index); - int advance_current_mission_item(); + /** + * Check if current mission item has been reached. + */ + bool mission_item_reached(); - void reset_mission_item_reached(); + /** + * Move to next waypoint + */ + int advance_mission(); - void check_mission_item_reached(); + /** + * Helper function to set a mission item + */ + int set_mission_item(unsigned relative_index, struct mission_item_s *mission_item) ; - void report_mission_reached(); + /** + * Helper function to set a loiter item + */ + void set_loiter_item(mission_item_s *new_loiter_position); - void start_waypoint(); + /** + * Publish a new mission item triplet for position controller + */ + void publish_mission_item_triplet(); - 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::global_position_update() +{ + /* load local copies */ + orb_copy(ORB_ID(vehicle_global_position), _global_pos_sub, &_global_pos); } void -Navigator::mission_update() +Navigator::home_position_update() { - struct mission_s mission; - if (orb_copy(ORB_ID(mission), _mission_sub, &mission) == OK) { + orb_copy(ORB_ID(home_position), _home_pos_sub, &_home_pos); +} - _mission_item_count = mission.count; - _current_mission_index = mission.current_index; +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,262 +858,360 @@ Navigator::fence_point(int argc, char *argv[]) errx(1, "can't store fence point"); } -void -Navigator::set_mode(navigation_mode_t new_nav_mode) -{ - if (new_nav_mode == _mode) { - /* no change, return */ - return; - } - switch (new_nav_mode) { - case NAVIGATION_MODE_NONE: - // warnx("Set mode NONE"); - _mode = new_nav_mode; - break; +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}, + }, +}; - 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; +void +Navigator::start_none() +{ + _mission_item_triplet.previous_valid = false; + _mission_item_triplet.current_valid = false; + _mission_item_triplet.next_valid = false; - case NAVIGATION_MODE_LOITER: - default: - // warnx("previous navigation mode not supported"); - break; - } - break; + publish_mission_item_triplet(); +} - case NAVIGATION_MODE_WAYPOINT: +void +Navigator::start_loiter() +{ + struct mission_item_s loiter_item; - /* Start mission if there is one available */ - start_waypoint(); - _mode = new_nav_mode; - mavlink_log_info(_mavlink_fd, "[navigator] start waypoint mission"); - break; + loiter_item.lat = (double)_global_pos.lat / 1e7; + loiter_item.lon = (double)_global_pos.lon / 1e7; + loiter_item.yaw = 0.0f; - case NAVIGATION_MODE_LOITER_WAYPOINT: + /* XXX get rid of ugly conversion for home position altitude */ + float global_min_alt = _parameters.min_altitude + (float)_home_pos.alt/1e3f; - 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; + /* 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"); + } - case NAVIGATION_MODE_RTL: + set_loiter_item(&loiter_item); - /* 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: + publish_mission_item_triplet(); +} - /* 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; +void +Navigator::start_mission() +{ + /* leave previous mission item as isas is */ - case NAVIGATION_MODE_RTL: - default: - warnx("previous navigation mode not supported"); - break; - } - break; + 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"); + } - case NAVIGATION_MODE_LOITER_RTL: + if(set_mission_item(1, &_mission_item_triplet.next) == OK) { + _mission_item_triplet.next_valid = true; + } else { + _mission_item_triplet.next_valid = false; + } - /* 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; + 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; } -} -bool -Navigator::mission_possible() -{ - return _mission_item_count > 0 && - !(_current_mission_index >= _mission_item_count); + publish_mission_item_triplet(); } -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; + 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 (dm_read(dm_item, mission_item_index, &mission_item, sizeof(struct mission_item_s)) != sizeof(struct mission_item_s)) { + /* 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; } - memcpy(new_mission_item, &mission_item, sizeof(struct mission_item_s)); + /* 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 (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 + if(set_mission_item(0, &_mission_item_triplet.current) == OK) { + _mission_item_triplet.current_valid = true; + + } else { + /* should never ever happen */ + _mission_item_triplet.current_valid = false; + warnx("no current available"); + return ERROR; + } + + if(set_mission_item(1, &_mission_item_triplet.next) == OK) { + _mission_item_triplet.next_valid = true; + + } else { + _mission_item_triplet.next_valid = false; } + publish_mission_item_triplet(); return OK; } void -Navigator::publish_mission_item_triplet() +Navigator::start_mission_loiter() { - /* 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); + /* make sure the current WP is valid */ + if (!_mission_item_triplet.current_valid) { + warnx("ERROR: cannot switch to offboard mission loiter"); + return; } -} -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); + set_loiter_item(&_mission_item_triplet.current); - } else { - /* advertise and publish */ - _mission_result_pub = orb_advertise(ORB_ID(mission_result), &_mission_result); + 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; } } -int -Navigator::advance_current_mission_item() +void +Navigator::start_rtl() { - reset_mission_item_reached(); - // warnx("advancing from %d to %d", _current_mission_index, _current_mission_index+1); + /* 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; - /* 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 (!_mission_item_triplet.next_valid) { - // warnx("no next available"); - return ERROR; - } + _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; - /* 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; + publish_mission_item_triplet(); - /* 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; + mavlink_log_info(_mavlink_fd, "[navigator] return to launch"); +} - 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"); - } +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; - if(ret == OK) { - _mission_item_triplet.next_valid = true; - } - else { - _mission_item_triplet.next_valid = false; - } + set_loiter_item(&home_position_mission_item); - publish_mission_item_triplet(); + mavlink_log_info(_mavlink_fd, "[navigator] loiter after RTL"); +} - return OK; +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; +} -void -Navigator::reset_mission_item_reached() +int +Navigator::set_mission_item(unsigned relative_index, struct mission_item_s *mission_item) { - /* reset all states */ - _waypoint_position_reached = false; - _waypoint_yaw_reached = false; - _time_first_inside_orbit = 0; + 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)) { - _mission_item_reached = false; + warnx("fallback try offboard: %d / %d", _current_offboard_mission_index + relative_index, _offboard_mission_item_count); - _mission_result.mission_reached = false; - _mission_result.mission_index = 0; + 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; } -void -Navigator::check_mission_item_reached() +bool +Navigator::mission_item_reached() { - /* don't check if mission item is already reached */ - if (_mission_item_reached) { - return; + /* 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 */ + /* 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; + 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() -{ - _mission_result.mission_reached = true; - _mission_result.mission_index = _current_mission_index; - - publish_mission_result(); -} - -void -Navigator::start_waypoint() +Navigator::set_loiter_item(struct mission_item_s *new_loiter_position) { - 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 000000000..f2709d29f --- /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 -- cgit v1.2.3 From 9d4ba6e4f64d631c67a419518a8398debade4641 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 16 Dec 2013 16:59:24 +0100 Subject: 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 7f7443c43..fc59c956a 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 d258aa8a6..d93ecc7cd 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(); - - /** - * Helper function to set a mission item - */ - int set_mission_item(unsigned relative_index, struct mission_item_s *mission_item) ; + void advance_mission(); /** - * Helper function to set a loiter item + * Helper function to get 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,83 +1085,19 @@ 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; - - 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--; - } + _mission_item_triplet.previous_valid = false; + _mission_item_triplet.current_valid = true; + _mission_item_triplet.next_valid = false; - 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; - } + _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; + + get_loiter_item(&_mission_item_triplet.current); - 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 - } + publish_mission_item_triplet(); - memcpy(mission_item, &new_mission_item, sizeof(struct mission_item_s)); - - return OK; + mavlink_log_info(_mavlink_fd, "[navigator] loiter after RTL"); } bool @@ -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 000000000..993f8f133 --- /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 000000000..e8e476382 --- /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 -- cgit v1.2.3 From d422d443eebbba24d785f96de5b74fab143e6652 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sat, 29 Mar 2014 12:14:57 +0100 Subject: 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 000000000..7672ace9e --- /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 222858b27..bb1f6c6b5 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 -- cgit v1.2.3 From 9da8e249fd2d1504b01571965c4dd5acdbf77ff1 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sat, 29 Mar 2014 20:36:39 +0100 Subject: 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 7672ace9e..1dcad2144 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() +{ + 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::status() +void +BottleDrop::task_main_trampoline(int argc, char *argv[]) { + bottle_drop::g_bottle_drop->task_main(); } static void usage() -- cgit v1.2.3 From 947b09a1209e5fd0c1e6e6cbe6a5820d15c83809 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sat, 29 Mar 2014 20:37:16 +0100 Subject: 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 cf7ba757e..46ca04661 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; } -- cgit v1.2.3 From 5e51812c8b1cb9af57145eeb85705e1f914aa77c Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sat, 29 Mar 2014 20:37:58 +0100 Subject: fw_att_control: workaround, don't publish to actuator_1 --- src/modules/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 f139c25f4..111f56a90 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); +// } } -- cgit v1.2.3 From 1d75f3eb8a1dba5b919c417b27a44e16ddcb0d32 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sat, 29 Mar 2014 20:38:27 +0100 Subject: 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 e6e4743c6..9f1bbb317 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| */ -- cgit v1.2.3 From eb4e250da88cf519850a53266fffbd06fd4a0872 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sun, 30 Mar 2014 15:00:37 +0200 Subject: 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 000000000..9c8f8a585 --- /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 5ec735d39..6c1329c10 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 -- cgit v1.2.3 From 64148a9e2af255357df8aeae06a4ad2ef972ffec Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sun, 30 Mar 2014 15:01:07 +0200 Subject: 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 1dcad2144..ce760adc3 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; } -- cgit v1.2.3 From f24c5184e8c1716795f10980a8c3b383c463facd Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sun, 30 Mar 2014 15:01:31 +0200 Subject: 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 429abc5ec..bb17d44eb 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 -- cgit v1.2.3 From ab80459b326aaef5ce9e2d7cae04613af6653c66 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 11 Aug 2014 08:56:06 +0200 Subject: 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 19d42dfa5..24b2a299a 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 # -- cgit v1.2.3 From b0fe5ae1ba1f6bbdb0fff1b450c0bf0a032095ec Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 11 Aug 2014 08:58:39 +0200 Subject: Bring back nominal state of fw attitude controller --- src/modules/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 e9480336a..0cea13cc4 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); + } } -- cgit v1.2.3 From 01a25547717736ef1692ffac4364dac1e75fb848 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 11 Aug 2014 14:51:49 +0200 Subject: 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 a4c1e832d..cb2b26c3c 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 -- cgit v1.2.3 From c81c94d74f049fac409d97e5dcafb3b3afa72ab6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 11 Aug 2014 14:52:07 +0200 Subject: 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 79c4447be..b05a8c8b0 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 -- cgit v1.2.3 From f47cf9a002cc2fbe27e922adfdedf111f4b28093 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 11 Aug 2014 14:52:45 +0200 Subject: 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 1911329d0..b86782075 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 ce760adc3..84c7900ce 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 bb1f6c6b5..61ac89e38 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 -- cgit v1.2.3 From f0458af28b3414cd43276c08c029be7900d0d9cf Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 11 Aug 2014 14:53:04 +0200 Subject: 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 15cdc517f..44aa50572 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 */ -- cgit v1.2.3 From f4cf94b08427bfda10919de1cee6b58171ed5e50 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 12 Aug 2014 08:21:06 +0200 Subject: 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 a4c1e832d..3714b612f 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 1de0abb58..e44cd0953 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 ecb408a54..739df7ac0 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 c9e6a27ca..c1948460d 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) # -- cgit v1.2.3 From 309a718db41ea683502792199d8dc9fa6fbdeabc Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 12 Aug 2014 17:57:14 +0200 Subject: 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 0ec663e35..7fed488af 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 -- cgit v1.2.3 From d6c018d14b283f67cc5d1f96d8bc6de3ccd73326 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 12 Aug 2014 17:57:47 +0200 Subject: Bottle drop iteration - commandline tool for component testing --- src/modules/bottle_drop/bottle_drop.c | 48 ------- src/modules/bottle_drop/bottle_drop.cpp | 188 +++++++++++++++++---------- src/modules/bottle_drop/bottle_drop_params.c | 48 +++++++ src/modules/bottle_drop/module.mk | 2 +- 4 files changed, 168 insertions(+), 118 deletions(-) delete mode 100644 src/modules/bottle_drop/bottle_drop.c create mode 100644 src/modules/bottle_drop/bottle_drop_params.c diff --git a/src/modules/bottle_drop/bottle_drop.c b/src/modules/bottle_drop/bottle_drop.c deleted file mode 100644 index b86782075..000000000 --- a/src/modules/bottle_drop/bottle_drop.c +++ /dev/null @@ -1,48 +0,0 @@ -/**************************************************************************** - * - * 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.c - * Bottle drop parameters - * - * @author Dominik Juchli - */ - -#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); diff --git a/src/modules/bottle_drop/bottle_drop.cpp b/src/modules/bottle_drop/bottle_drop.cpp index 84c7900ce..e73f36163 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_params.c b/src/modules/bottle_drop/bottle_drop_params.c new file mode 100644 index 000000000..0f60b58e4 --- /dev/null +++ b/src/modules/bottle_drop/bottle_drop_params.c @@ -0,0 +1,48 @@ +/**************************************************************************** + * + * 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_params.c + * Bottle drop parameters + * + * @author Dominik Juchli + */ + +#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); diff --git a/src/modules/bottle_drop/module.mk b/src/modules/bottle_drop/module.mk index 61ac89e38..6b18fff55 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 -- cgit v1.2.3 From 0c78794e5729d17a4649b567899cf9492a498c2b Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Thu, 14 Aug 2014 16:43:49 +0200 Subject: 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 b05a8c8b0..5a0381bd8 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 e73f36163..e5acc3c7e 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(); -- cgit v1.2.3 From 20ceba48cfc4e306f2dc4f5f98d0730730cd3233 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 15 Aug 2014 20:34:04 +0200 Subject: 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 767c54629..c97b3477f 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 -- cgit v1.2.3 From f8f72d665d2a0f07d14bb9c27293423e715486c4 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 15 Aug 2014 23:40:53 +0200 Subject: 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 e5acc3c7e..4367d1343 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 0f60b58e4..239f25884 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); -- cgit v1.2.3 From 61d052ede751274969869e53e98b1d8b2004a128 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 15 Aug 2014 23:41:57 +0200 Subject: 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 239f25884..22e9baf8a 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); -- cgit v1.2.3 From 26b2f73c6fce19982136e12a73faf96eee4895c6 Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Sat, 16 Aug 2014 14:42:18 +0200 Subject: 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 4367d1343..1c5c0f57f 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"); } -- cgit v1.2.3 From 584394d2bc1d88d291349cce2beee161c2a8448e Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Sat, 16 Aug 2014 17:53:23 +0200 Subject: 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 6c2c03070..db9ec9005 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; -- cgit v1.2.3 From 8a066a85faa7dd15265f705eba78a9dfe53f9f1d Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Sat, 16 Aug 2014 17:53:51 +0200 Subject: Fixed bottle drop to operate in GPS outdoor test, pending test flight --- src/modules/bottle_drop/bottle_drop.cpp | 396 +++++++++++++++++--------------- 1 file changed, 214 insertions(+), 182 deletions(-) diff --git a/src/modules/bottle_drop/bottle_drop.cpp b/src/modules/bottle_drop/bottle_drop.cpp index 1c5c0f57f..ddc41372e 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) { + + case DROP_STATE_INIT: + // exit inner loop, wait for new drop mission + break; + + 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) { + + 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; + + // 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; + + //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; + + float wind_direction_n, wind_direction_e; - 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 = _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; - - // 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; - - //acceleration - az = g - Fdz / m; - ax = -Fdx / m; + 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; } - // Compute drop vector - x = groundspeed_body * t_signal + x; - } + 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 - 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; + // 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 - } else { - wind_direction_n = wind.windspeed_north / windspeed_norm; - wind_direction_e = wind.windspeed_east / windspeed_norm; - } + // Drop Cancellation if terms are not met - 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"); - } + // 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); - 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"); - } + //warnx("position to-be: = %.7f %.7f" ,x_f_NED, y_f_NED ); //////////////////////////////////////////////////////////////////// DEBUGGING - onboard_mission.count = 2; + // 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); + // } - if (state_run && !state_drop) { - onboard_mission.current_seq = 0; + /* Save WPs in datamanager */ + const ssize_t len = sizeof(struct mission_item_s); - } else { - onboard_mission.current_seq = -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 (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 (dm_write(DM_KEY_WAYPOINTS_ONBOARD, 1, DM_PERSIST_IN_FLIGHT_RESET, &flight_vector_e, len) != len) { + warnx("ERROR: could not save onboard WP"); + } - if (onboard_mission_pub > 0) { - orb_publish(ORB_ID(onboard_mission), onboard_mission_pub, &onboard_mission); + onboard_mission.count = 2; - } else { - onboard_mission_pub = orb_advertise(ORB_ID(onboard_mission), &onboard_mission); - } + if (state_run && !state_drop) { + onboard_mission.current_seq = 0; - } + } else { + onboard_mission.current_seq = -1; + } - if (isfinite(distance_real) && distance_real < distance_open_door && _drop_approval) { - open_bay(); - } else { - close_bay(); - } + if (onboard_mission_pub > 0) { + orb_publish(ORB_ID(onboard_mission), onboard_mission_pub, &onboard_mission); - 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; - // } + } 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; + + 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)); + + 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; + // } + } + } + break; + + 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(); } -- cgit v1.2.3 From 789f949e746cdd79e1214830a4ef822ef5634bbe Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Sun, 17 Aug 2014 10:19:48 +0200 Subject: 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 ddc41372e..14b9a8379 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); -- cgit v1.2.3 From 47b3f1253eb2a46c216aee1ca9f08d348266ea41 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 18 Aug 2014 16:47:30 +0200 Subject: Fix bottle drop logic --- src/modules/bottle_drop/bottle_drop.cpp | 47 ++++++++++++++------------------- 1 file changed, 20 insertions(+), 27 deletions(-) diff --git a/src/modules/bottle_drop/bottle_drop.cpp b/src/modules/bottle_drop/bottle_drop.cpp index 14b9a8379..77f691f6c 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)); - - 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 (_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); + + 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); -- cgit v1.2.3 From 2a99ff39dcdb78d97fd645f2c47bf6d36798a1dc Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 21 Aug 2014 15:14:55 +0200 Subject: 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 77f691f6c..35b2266e4 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: -- cgit v1.2.3 From 0bdf28f9ba6f0875bfb70ff961fc61e73b813edc Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 21 Aug 2014 15:26:04 +0200 Subject: 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 35b2266e4..a899cbe60 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)); -- cgit v1.2.3 From 6051b2112efb584e5f84fb0648d4dddf5ac6c918 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 21 Aug 2014 15:31:06 +0200 Subject: 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 a899cbe60..6a501d97c 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)); -- cgit v1.2.3 From 1026805422495e9b6d5351a38f2a2ca3ce70a352 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 21 Aug 2014 15:41:03 +0200 Subject: 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 6a501d97c..dbf94cb3d 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 -- cgit v1.2.3 From 7c7dce3b440a7b6deb8f0f9340c4ba6b21daa800 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 21 Aug 2014 15:46:58 +0200 Subject: 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 dbf94cb3d..bc182b501 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", -- cgit v1.2.3 From 8e7722f8948d3f8f21a5a661e9cdcc6097472fd9 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 21 Aug 2014 15:47:37 +0200 Subject: 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 bc182b501..5d2343cfb 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", -- cgit v1.2.3 From 7187e29b5205ba6c5592c1b9e92ac5d28bb2539a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 21 Aug 2014 20:58:24 +0200 Subject: 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 5d2343cfb..30c9ce887 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 -- cgit v1.2.3 From e6a8eebd0a86c0dd7b8b23882772ccd3ba962ddc Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 21 Aug 2014 21:43:53 +0200 Subject: 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 30c9ce887..ae97cc570 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; + + case DROP_STATE_TARGET_SET - 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)))); + float distance_wp2 = get_distance_to_next_waypoint(flight_vector_e.lat, flight_vector_e.lon, _drop_position.lat, _drop_position.lon); - 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"); + 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); -- cgit v1.2.3 From 04ad990254e51ceb68c91e7ef601f1e281fd7062 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 22 Aug 2014 07:08:30 +0200 Subject: 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 ae97cc570..6c3bdb002 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 { -- cgit v1.2.3 From 0f7c3926143a917b3e444f0f2c6c2cb673f073e4 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 22 Aug 2014 14:30:34 +0200 Subject: 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 5d2343cfb..8e96c3e09 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)); -- cgit v1.2.3 From bdd4f028eec79f09335f888b4857db44968aa07d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 22 Aug 2014 14:36:31 +0200 Subject: 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 058e90829..5b7f7bd7f 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 -- cgit v1.2.3 From 3f37efc046c9f3d2d6ee4a39f67ed5e20a6866ac Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 22 Aug 2014 14:41:35 +0200 Subject: 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 81d1238cf..2bd1f679b 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; -- cgit v1.2.3 From d604985e080f1c04278f5f28acf0eaaf3b334041 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 22 Aug 2014 17:22:35 +0200 Subject: 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 2bd1f679b..3eec5a879 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); -- cgit v1.2.3 From ef0a0a1a6e86aff79a0fd8829ca5c86244fa39bc Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 24 Aug 2014 12:05:12 +0200 Subject: 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 3eec5a879..65eca1a62 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; -- cgit v1.2.3