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(-) (limited to 'src') 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 (limited to 'src') 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 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(+) (limited to 'src') 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(-) (limited to 'src') 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(-) (limited to 'src') 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(-) (limited to 'src') 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 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(-) (limited to 'src') 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(-) (limited to 'src') 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(-) (limited to 'src') 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(-) (limited to 'src') 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(-) (limited to 'src') 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(-) (limited to 'src') 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(-) (limited to 'src') 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 (limited to 'src') 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 (limited to 'src') 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 (limited to 'src') 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(-) (limited to 'src') 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(-) (limited to 'src') 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(-) (limited to 'src') 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(+) (limited to 'src') 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 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(-) (limited to 'src') 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 839710daf841a9528a58e915f8b04484bf54e7dc Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 3 Jul 2014 14:42:59 +0200 Subject: Update offboard control uorb topic Work towards supporting the new external setpoint mavlink messages --- src/modules/commander/commander.cpp | 16 ++---- src/modules/mavlink/mavlink_receiver.cpp | 28 +++++++++ src/modules/mavlink/mavlink_receiver.h | 1 + src/modules/navigator/offboard.cpp | 67 +++++++++++++--------- .../uORB/topics/offboard_control_setpoint.h | 36 +++++++++--- 5 files changed, 102 insertions(+), 46 deletions(-) (limited to 'src') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 38297745d..7185eebc4 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -946,7 +946,7 @@ int commander_thread_main(int argc, char *argv[]) } /* Perform system checks (again) once params are loaded and MAVLink is up. */ - if (!system_checked && mavlink_fd && + if (!system_checked && mavlink_fd && (telemetry.heartbeat_time > 0) && (hrt_elapsed_time(&telemetry.heartbeat_time) < 1 * 1000 * 1000)) { @@ -1847,21 +1847,17 @@ set_control_mode() control_mode.flag_control_position_enabled = false; control_mode.flag_control_velocity_enabled = false; break; - case OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY: - control_mode.flag_control_rates_enabled = true; - control_mode.flag_control_attitude_enabled = true; - control_mode.flag_control_altitude_enabled = true; /* XXX: hack for now */ - control_mode.flag_control_climb_rate_enabled = true; - control_mode.flag_control_position_enabled = true; /* XXX: hack for now */ - control_mode.flag_control_velocity_enabled = true; - break; - case OFFBOARD_CONTROL_MODE_DIRECT_POSITION: + case OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED: + case OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_OFFSET_NED: + case OFFBOARD_CONTROL_MODE_DIRECT_BODY_NED: + case OFFBOARD_CONTROL_MODE_DIRECT_BODY_OFFSET_NED: control_mode.flag_control_rates_enabled = true; control_mode.flag_control_attitude_enabled = true; control_mode.flag_control_altitude_enabled = true; control_mode.flag_control_climb_rate_enabled = true; control_mode.flag_control_position_enabled = true; control_mode.flag_control_velocity_enabled = true; + //XXX: the flags could depend on sp_offboard.ignore break; default: control_mode.flag_control_rates_enabled = false; diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 3747ad3ba..73222637f 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -154,6 +154,10 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) handle_message_quad_swarm_roll_pitch_yaw_thrust(msg); break; + case MAVLINK_MSG_ID_LOCAL_NED_POSITION_SETPOINT_EXTERNAL: + handle_message_local_ned_position_setpoint_external(msg); + break; + case MAVLINK_MSG_ID_RADIO_STATUS: handle_message_radio_status(msg); break; @@ -393,6 +397,30 @@ MavlinkReceiver::handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message } } +void +MavlinkReceiver::handle_message_local_ned_position_setpoint_external(mavlink_message_t *msg) +{ + mavlink_local_ned_position_setpoint_external_t local_ned_position_setpoint_external; + mavlink_msg_local_ned_position_setpoint_external_decode(msg, &local_ned_position_setpoint_external); + + struct offboard_control_setpoint_s offboard_control_sp; + memset(&offboard_control_sp, 0, sizeof(offboard_control_sp)); + + //XXX check if target system/component is correct + + /* convert mavlink type (local, NED) to uORB offboard control struct */ + //XXX do the conversion + // + offboard_control_sp.timestamp = hrt_absolute_time(); + + if (_offboard_control_sp_pub < 0) { + _offboard_control_sp_pub = orb_advertise(ORB_ID(offboard_control_setpoint), &offboard_control_sp); + + } else { + orb_publish(ORB_ID(offboard_control_setpoint), _offboard_control_sp_pub, &offboard_control_sp); + } +} + void MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg) { diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 65ef884a2..658f51562 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -115,6 +115,7 @@ private: void handle_message_set_mode(mavlink_message_t *msg); void handle_message_vicon_position_estimate(mavlink_message_t *msg); void handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message_t *msg); + void handle_message_local_ned_position_setpoint_external(mavlink_message_t *msg); void handle_message_radio_status(mavlink_message_t *msg); void handle_message_manual_control(mavlink_message_t *msg); void handle_message_heartbeat(mavlink_message_t *msg); diff --git a/src/modules/navigator/offboard.cpp b/src/modules/navigator/offboard.cpp index ef7d11a03..27ce46a1d 100644 --- a/src/modules/navigator/offboard.cpp +++ b/src/modules/navigator/offboard.cpp @@ -79,33 +79,46 @@ Offboard::on_active(struct position_setpoint_triplet_s *pos_sp_triplet) bool changed = false; /* copy offboard setpoints to the corresponding topics */ - if (_navigator->get_control_mode()->flag_control_position_enabled - && _offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_POSITION) { - /* position control */ - pos_sp_triplet->current.x = _offboard_control_sp.p1; - pos_sp_triplet->current.y = _offboard_control_sp.p2; - pos_sp_triplet->current.yaw = _offboard_control_sp.p3; - pos_sp_triplet->current.z = -_offboard_control_sp.p4; - - pos_sp_triplet->current.type = SETPOINT_TYPE_OFFBOARD; - pos_sp_triplet->current.valid = true; - pos_sp_triplet->current.position_valid = true; - - changed = true; - - } else if (_navigator->get_control_mode()->flag_control_velocity_enabled - && _offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY) { - /* velocity control */ - pos_sp_triplet->current.vx = _offboard_control_sp.p2; - pos_sp_triplet->current.vy = _offboard_control_sp.p1; - pos_sp_triplet->current.yawspeed = _offboard_control_sp.p3; - pos_sp_triplet->current.vz = _offboard_control_sp.p4; - - pos_sp_triplet->current.type = SETPOINT_TYPE_OFFBOARD; - pos_sp_triplet->current.valid = true; - pos_sp_triplet->current.velocity_valid = true; - - changed = true; + if (_offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED) { + /* We accept position control only if none of the directions is ignored (as pos_sp_triplet does not + * support deactivation of individual directions) */ + if (_navigator->get_control_mode()->flag_control_position_enabled && + (!_offboard_control_sp.ignore[0] && + !_offboard_control_sp.ignore[1] && + !_offboard_control_sp.ignore[2])) { + /* position control */ + pos_sp_triplet->current.x = _offboard_control_sp.p1; + pos_sp_triplet->current.y = _offboard_control_sp.p2; + pos_sp_triplet->current.yaw = _offboard_control_sp.p3; + pos_sp_triplet->current.z = -_offboard_control_sp.p4; + + pos_sp_triplet->current.type = SETPOINT_TYPE_OFFBOARD; + pos_sp_triplet->current.valid = true; + pos_sp_triplet->current.position_valid = true; + + changed = true; + + } + /* We accept velocity control only if none of the directions is ignored (as pos_sp_triplet does not + * support deactivation of individual directions) */ + if (_navigator->get_control_mode()->flag_control_velocity_enabled && + (!_offboard_control_sp.ignore[3] && + !_offboard_control_sp.ignore[4] && + !_offboard_control_sp.ignore[5])) { + /* velocity control */ + pos_sp_triplet->current.vx = _offboard_control_sp.p2; + pos_sp_triplet->current.vy = _offboard_control_sp.p1; + pos_sp_triplet->current.yawspeed = _offboard_control_sp.p3; + pos_sp_triplet->current.vz = _offboard_control_sp.p4; + + pos_sp_triplet->current.type = SETPOINT_TYPE_OFFBOARD; + pos_sp_triplet->current.valid = true; + pos_sp_triplet->current.velocity_valid = true; + + changed = true; + } + + //XXX: map acceleration setpoint once supported in setpoint triplet } return changed; diff --git a/src/modules/uORB/topics/offboard_control_setpoint.h b/src/modules/uORB/topics/offboard_control_setpoint.h index d7b131e3c..427ccc6c8 100644 --- a/src/modules/uORB/topics/offboard_control_setpoint.h +++ b/src/modules/uORB/topics/offboard_control_setpoint.h @@ -53,11 +53,22 @@ enum OFFBOARD_CONTROL_MODE { OFFBOARD_CONTROL_MODE_DIRECT = 0, OFFBOARD_CONTROL_MODE_DIRECT_RATES = 1, OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE = 2, - OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY = 3, - OFFBOARD_CONTROL_MODE_DIRECT_POSITION = 4, - OFFBOARD_CONTROL_MODE_ATT_YAW_RATE = 5, - OFFBOARD_CONTROL_MODE_ATT_YAW_POS = 6, - OFFBOARD_CONTROL_MODE_MULTIROTOR_SIMPLE = 7, /**< roll / pitch rotated aligned to the takeoff orientation, throttle stabilized, yaw pos */ + OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED = 3, + OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_OFFSET_NED = 4, + OFFBOARD_CONTROL_MODE_DIRECT_BODY_NED = 5, + OFFBOARD_CONTROL_MODE_DIRECT_BODY_OFFSET_NED = 6, + OFFBOARD_CONTROL_MODE_DIRECT_GLOBAL = 7, + OFFBOARD_CONTROL_MODE_ATT_YAW_RATE = 8, + OFFBOARD_CONTROL_MODE_ATT_YAW_POS = 9, + OFFBOARD_CONTROL_MODE_MULTIROTOR_SIMPLE = 10, /**< roll / pitch rotated aligned to the takeoff orientation, throttle stabilized, yaw pos */ +}; + +enum OFFBOARD_CONTROL_FRAME { + OFFBOARD_CONTROL_FRAME_LOCAL_NED = 0, + OFFBOARD_CONTROL_FRAME_LOCAL_OFFSET_NED = 1, + OFFBOARD_CONTROL_FRAME_BODY_NED = 2, + OFFBOARD_CONTROL_FRAME_BODY_OFFSET_NED = 3, + OFFBOARD_CONTROL_FRAME_GLOBAL = 4 }; /** @@ -70,10 +81,17 @@ struct offboard_control_setpoint_s { enum OFFBOARD_CONTROL_MODE mode; /**< The current control inputs mode */ - float p1; /**< ailerons roll / roll rate input */ - float p2; /**< elevator / pitch / pitch rate */ - float p3; /**< rudder / yaw rate / yaw */ - float p4; /**< throttle / collective thrust / altitude */ + double p1; /**< ailerons / roll / x pos / lat */ + double p2; /**< elevator / pitch / y pos / lon */ + float p3; /**< rudder / yaw / z pos / alt */ + float p4; /**< throttle / x vel */ + float p5; /**< roll rate / y vel */ + float p6; /**< pitch rate / z vel */ + float p7; /**< yaw rate / x acc */ + float p8; /**< y acc */ + float p9; /**< z acc */ + + bool ignore[9]; /**< if field i is set to true, pi should be ignored */ float override_mode_switch; -- cgit v1.2.3 From cbd602c27cb081502a2f1f1d910abdd1c2d7be13 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 3 Jul 2014 15:00:32 +0200 Subject: check sysid and compid for external setpoint --- src/modules/mavlink/mavlink_receiver.cpp | 20 +++++++++++--------- 1 file changed, 11 insertions(+), 9 deletions(-) (limited to 'src') diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 73222637f..f2b9a4e1e 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -406,18 +406,20 @@ MavlinkReceiver::handle_message_local_ned_position_setpoint_external(mavlink_mes struct offboard_control_setpoint_s offboard_control_sp; memset(&offboard_control_sp, 0, sizeof(offboard_control_sp)); - //XXX check if target system/component is correct + if (mavlink_system.sysid == local_ned_position_setpoint_external.target_system && + mavlink_system.compid == local_ned_position_setpoint_external.target_component) { - /* convert mavlink type (local, NED) to uORB offboard control struct */ - //XXX do the conversion - // - offboard_control_sp.timestamp = hrt_absolute_time(); + /* convert mavlink type (local, NED) to uORB offboard control struct */ + //XXX do the conversion + // + offboard_control_sp.timestamp = hrt_absolute_time(); - if (_offboard_control_sp_pub < 0) { - _offboard_control_sp_pub = orb_advertise(ORB_ID(offboard_control_setpoint), &offboard_control_sp); + if (_offboard_control_sp_pub < 0) { + _offboard_control_sp_pub = orb_advertise(ORB_ID(offboard_control_setpoint), &offboard_control_sp); - } else { - orb_publish(ORB_ID(offboard_control_setpoint), _offboard_control_sp_pub, &offboard_control_sp); + } else { + orb_publish(ORB_ID(offboard_control_setpoint), _offboard_control_sp_pub, &offboard_control_sp); + } } } -- cgit v1.2.3 From 822403e34b5e1b4adf15783b5bd701e1f52484fe Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 3 Jul 2014 16:20:58 +0200 Subject: uorb offboard control topic: add force sp flag The flag has the same meaning as bit 10 of type_mask in MAVLINK_MSG_ID_LOCAL_NED_POSITION_SETPOINT_EXTERNAL --- src/modules/uORB/topics/offboard_control_setpoint.h | 2 ++ 1 file changed, 2 insertions(+) (limited to 'src') diff --git a/src/modules/uORB/topics/offboard_control_setpoint.h b/src/modules/uORB/topics/offboard_control_setpoint.h index 427ccc6c8..2741f0c07 100644 --- a/src/modules/uORB/topics/offboard_control_setpoint.h +++ b/src/modules/uORB/topics/offboard_control_setpoint.h @@ -91,7 +91,9 @@ struct offboard_control_setpoint_s { float p8; /**< y acc */ float p9; /**< z acc */ + //XXX: use a bitmask with wrapper functions instead bool ignore[9]; /**< if field i is set to true, pi should be ignored */ + bool isForceSetpoint; /**< if set to true: p7 to p9 should be interpreted as force instead of acceleration */ float override_mode_switch; -- cgit v1.2.3 From 31474a75fd1ad6e86909bf4af2b484d1decd932a Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 3 Jul 2014 16:22:58 +0200 Subject: parsing of MAVLINK_MSG_ID_LOCAL_NED_POSITION_SETPOINT_EXTERNAL --- src/modules/mavlink/mavlink_receiver.cpp | 34 ++++++++++++++++++++++++++++++-- 1 file changed, 32 insertions(+), 2 deletions(-) (limited to 'src') diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index f2b9a4e1e..c8e8c6d51 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -410,8 +410,38 @@ MavlinkReceiver::handle_message_local_ned_position_setpoint_external(mavlink_mes mavlink_system.compid == local_ned_position_setpoint_external.target_component) { /* convert mavlink type (local, NED) to uORB offboard control struct */ - //XXX do the conversion - // + switch (local_ned_position_setpoint_external.coordinate_frame) { + case MAV_FRAME_LOCAL_NED: + offboard_control_sp.mode = OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED; + break; + case MAV_FRAME_LOCAL_OFFSET_NED: + offboard_control_sp.mode = OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_OFFSET_NED; + break; + case MAV_FRAME_BODY_NED: + offboard_control_sp.mode = OFFBOARD_CONTROL_MODE_DIRECT_BODY_NED; + break; + case MAV_FRAME_BODY_OFFSET_NED: + offboard_control_sp.mode = OFFBOARD_CONTROL_MODE_DIRECT_BODY_OFFSET_NED; + break; + default: + /* invalid setpoint, avoid publishing */ + return; + } + offboard_control_sp.p1 = local_ned_position_setpoint_external.x; + offboard_control_sp.p2 = local_ned_position_setpoint_external.y; + offboard_control_sp.p3 = local_ned_position_setpoint_external.z; + offboard_control_sp.p4 = local_ned_position_setpoint_external.vx; + offboard_control_sp.p5 = local_ned_position_setpoint_external.vy; + offboard_control_sp.p6 = local_ned_position_setpoint_external.vz; + offboard_control_sp.p7 = local_ned_position_setpoint_external.afx; + offboard_control_sp.p8 = local_ned_position_setpoint_external.afy; + offboard_control_sp.p9 = local_ned_position_setpoint_external.afz; + offboard_control_sp.isForceSetpoint = (bool)(local_ned_position_setpoint_external.type_mask & (1 << 9)); + for (int i = 0; i < 9; i++) { + offboard_control_sp.ignore[i] = (bool)(local_ned_position_setpoint_external.type_mask & (1 << i)); + } + + offboard_control_sp.timestamp = hrt_absolute_time(); if (_offboard_control_sp_pub < 0) { -- cgit v1.2.3 From 2d26e913921ce80e509bd79462319d09d12c2171 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 7 Jul 2014 13:00:45 +0200 Subject: WIP, change uorb offboard control sp topic --- src/modules/mavlink/mavlink_receiver.cpp | 30 ++++++++++--------- src/modules/navigator/offboard.cpp | 30 ++++++++++--------- .../uORB/topics/offboard_control_setpoint.h | 34 +++++++++++++++------- 3 files changed, 55 insertions(+), 39 deletions(-) (limited to 'src') diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index b72bfa858..e1a74f599 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -379,10 +379,11 @@ MavlinkReceiver::handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message memset(&offboard_control_sp, 0, sizeof(offboard_control_sp)); /* Convert values * 1000 back */ - offboard_control_sp.p1 = (float)swarm_offboard_control.roll[mavlink_system.sysid - 1] / 1000.0f; - offboard_control_sp.p2 = (float)swarm_offboard_control.pitch[mavlink_system.sysid - 1] / 1000.0f; - offboard_control_sp.p3 = (float)swarm_offboard_control.yaw[mavlink_system.sysid - 1] / 1000.0f; - offboard_control_sp.p4 = (float)swarm_offboard_control.thrust[mavlink_system.sysid - 1] / 1000.0f; + //XXX: convert to quaternion + //offboard_control_sp.p1 = (float)swarm_offboard_control.roll[mavlink_system.sysid - 1] / 1000.0f; + //offboard_control_sp.p2 = (float)swarm_offboard_control.pitch[mavlink_system.sysid - 1] / 1000.0f; + //offboard_control_sp.p3 = (float)swarm_offboard_control.yaw[mavlink_system.sysid - 1] / 1000.0f; + //offboard_control_sp.p4 = (float)swarm_offboard_control.thrust[mavlink_system.sysid - 1] / 1000.0f; offboard_control_sp.mode = (enum OFFBOARD_CONTROL_MODE)swarm_offboard_control.mode; @@ -427,18 +428,19 @@ MavlinkReceiver::handle_message_local_ned_position_setpoint_external(mavlink_mes /* invalid setpoint, avoid publishing */ return; } - offboard_control_sp.p1 = local_ned_position_setpoint_external.x; - offboard_control_sp.p2 = local_ned_position_setpoint_external.y; - offboard_control_sp.p3 = local_ned_position_setpoint_external.z; - offboard_control_sp.p4 = local_ned_position_setpoint_external.vx; - offboard_control_sp.p5 = local_ned_position_setpoint_external.vy; - offboard_control_sp.p6 = local_ned_position_setpoint_external.vz; - offboard_control_sp.p7 = local_ned_position_setpoint_external.afx; - offboard_control_sp.p8 = local_ned_position_setpoint_external.afy; - offboard_control_sp.p9 = local_ned_position_setpoint_external.afz; + offboard_control_sp.position[0] = local_ned_position_setpoint_external.x; + offboard_control_sp.position[1] = local_ned_position_setpoint_external.y; + offboard_control_sp.position[2] = local_ned_position_setpoint_external.z; + offboard_control_sp.velocity[0] = local_ned_position_setpoint_external.vx; + offboard_control_sp.velocity[1] = local_ned_position_setpoint_external.vy; + offboard_control_sp.velocity[2] = local_ned_position_setpoint_external.vz; + offboard_control_sp.acceleration[0] = local_ned_position_setpoint_external.afx; + offboard_control_sp.acceleration[1] = local_ned_position_setpoint_external.afy; + offboard_control_sp.acceleration[2] = local_ned_position_setpoint_external.afz; offboard_control_sp.isForceSetpoint = (bool)(local_ned_position_setpoint_external.type_mask & (1 << 9)); for (int i = 0; i < 9; i++) { - offboard_control_sp.ignore[i] = (bool)(local_ned_position_setpoint_external.type_mask & (1 << i)); + offboard_control_sp.ignore &= ~(local_ned_position_setpoint_external.type_mask & (1 << i)); + offboard_control_sp.ignore |= (local_ned_position_setpoint_external.type_mask & (1 << i)); } diff --git a/src/modules/navigator/offboard.cpp b/src/modules/navigator/offboard.cpp index 27ce46a1d..71a14c029 100644 --- a/src/modules/navigator/offboard.cpp +++ b/src/modules/navigator/offboard.cpp @@ -83,14 +83,15 @@ Offboard::on_active(struct position_setpoint_triplet_s *pos_sp_triplet) /* We accept position control only if none of the directions is ignored (as pos_sp_triplet does not * support deactivation of individual directions) */ if (_navigator->get_control_mode()->flag_control_position_enabled && - (!_offboard_control_sp.ignore[0] && - !_offboard_control_sp.ignore[1] && - !_offboard_control_sp.ignore[2])) { + (!offboard_control_sp_ignore_position(_offboard_control_sp, 0) && + !offboard_control_sp_ignore_position(_offboard_control_sp, 1) && + !offboard_control_sp_ignore_position(_offboard_control_sp, 2))) { /* position control */ - pos_sp_triplet->current.x = _offboard_control_sp.p1; - pos_sp_triplet->current.y = _offboard_control_sp.p2; - pos_sp_triplet->current.yaw = _offboard_control_sp.p3; - pos_sp_triplet->current.z = -_offboard_control_sp.p4; + pos_sp_triplet->current.x = _offboard_control_sp.position[0]; + pos_sp_triplet->current.y = _offboard_control_sp.position[1]; + //pos_sp_triplet->current.yaw = _offboard_control_sp.position[2]; + //XXX: copy yaw + pos_sp_triplet->current.z = -_offboard_control_sp.position[2]; pos_sp_triplet->current.type = SETPOINT_TYPE_OFFBOARD; pos_sp_triplet->current.valid = true; @@ -102,14 +103,15 @@ Offboard::on_active(struct position_setpoint_triplet_s *pos_sp_triplet) /* We accept velocity control only if none of the directions is ignored (as pos_sp_triplet does not * support deactivation of individual directions) */ if (_navigator->get_control_mode()->flag_control_velocity_enabled && - (!_offboard_control_sp.ignore[3] && - !_offboard_control_sp.ignore[4] && - !_offboard_control_sp.ignore[5])) { + (!offboard_control_sp_ignore_velocity(_offboard_control_sp, 0) && + !offboard_control_sp_ignore_velocity(_offboard_control_sp, 1) && + !offboard_control_sp_ignore_velocity(_offboard_control_sp, 2))) { /* velocity control */ - pos_sp_triplet->current.vx = _offboard_control_sp.p2; - pos_sp_triplet->current.vy = _offboard_control_sp.p1; - pos_sp_triplet->current.yawspeed = _offboard_control_sp.p3; - pos_sp_triplet->current.vz = _offboard_control_sp.p4; + pos_sp_triplet->current.vx = _offboard_control_sp.velocity[0]; + pos_sp_triplet->current.vy = _offboard_control_sp.velocity[1]; +// pos_sp_triplet->current.yawspeed = _offboard_control_sp.velocity[; +// //XXX: copy yaw speed + pos_sp_triplet->current.vz = _offboard_control_sp.velocity[2]; pos_sp_triplet->current.type = SETPOINT_TYPE_OFFBOARD; pos_sp_triplet->current.valid = true; diff --git a/src/modules/uORB/topics/offboard_control_setpoint.h b/src/modules/uORB/topics/offboard_control_setpoint.h index 2741f0c07..688135533 100644 --- a/src/modules/uORB/topics/offboard_control_setpoint.h +++ b/src/modules/uORB/topics/offboard_control_setpoint.h @@ -81,19 +81,15 @@ struct offboard_control_setpoint_s { enum OFFBOARD_CONTROL_MODE mode; /**< The current control inputs mode */ - double p1; /**< ailerons / roll / x pos / lat */ - double p2; /**< elevator / pitch / y pos / lon */ - float p3; /**< rudder / yaw / z pos / alt */ - float p4; /**< throttle / x vel */ - float p5; /**< roll rate / y vel */ - float p6; /**< pitch rate / z vel */ - float p7; /**< yaw rate / x acc */ - float p8; /**< y acc */ - float p9; /**< z acc */ + double position[3]; /**< lat, lon, alt / x, y, z */ + float velocity[3]; /**< x vel, y vel, z vel */ + float acceleration[3]; /**< x acc, y acc, z acc */ + float attitude[4]; /**< attitude of vehicle (quaternion) */ + float attitude_rate[3]; /**< body angular rates (x, y, z) */ //XXX: use a bitmask with wrapper functions instead - bool ignore[9]; /**< if field i is set to true, pi should be ignored */ - bool isForceSetpoint; /**< if set to true: p7 to p9 should be interpreted as force instead of acceleration */ + uint16_t ignore; /**< if field i is set to true, pi should be ignored */ + bool isForceSetpoint; /**< the acceleration vector should be interpreted as force */ float override_mode_switch; @@ -107,6 +103,22 @@ struct offboard_control_setpoint_s { * @} */ +inline bool offboard_control_sp_ignore_position(const struct offboard_control_setpoint_s &offboard_control_sp, int index) { + return (bool)(offboard_control_sp.ignore & (1 << index)); +} + +inline bool offboard_control_sp_ignore_velocity(const struct offboard_control_setpoint_s &offboard_control_sp, int index) { + return (bool)(offboard_control_sp.ignore & (1 << (3 + index))); +} + +inline bool offboard_control_sp_ignore_acceleration(const struct offboard_control_setpoint_s &offboard_control_sp, int index) { + return (bool)(offboard_control_sp.ignore & (1 << (6 + index))); +} + +inline bool offboard_control_sp_ignore_bodyrates(const struct offboard_control_setpoint_s &offboard_control_sp, int index) { + return (bool)(offboard_control_sp.ignore & (1 << (9 + index))); +} + /* register this as object request broker structure */ ORB_DECLARE(offboard_control_setpoint); -- cgit v1.2.3 From 19017f100101dbf638bb91f7a520296f979ebb32 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 8 Jul 2014 13:29:59 +0200 Subject: remove comment --- src/modules/uORB/topics/offboard_control_setpoint.h | 1 - 1 file changed, 1 deletion(-) (limited to 'src') diff --git a/src/modules/uORB/topics/offboard_control_setpoint.h b/src/modules/uORB/topics/offboard_control_setpoint.h index 688135533..809412fc0 100644 --- a/src/modules/uORB/topics/offboard_control_setpoint.h +++ b/src/modules/uORB/topics/offboard_control_setpoint.h @@ -87,7 +87,6 @@ struct offboard_control_setpoint_s { float attitude[4]; /**< attitude of vehicle (quaternion) */ float attitude_rate[3]; /**< body angular rates (x, y, z) */ - //XXX: use a bitmask with wrapper functions instead uint16_t ignore; /**< if field i is set to true, pi should be ignored */ bool isForceSetpoint; /**< the acceleration vector should be interpreted as force */ -- cgit v1.2.3 From cf4a11c7e7cfa524992a96d41d885da38ab95ebd Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 8 Jul 2014 13:47:53 +0200 Subject: fix merge errors in offboard.cpp --- src/modules/navigator/offboard.cpp | 19 +++++++++++-------- 1 file changed, 11 insertions(+), 8 deletions(-) (limited to 'src') diff --git a/src/modules/navigator/offboard.cpp b/src/modules/navigator/offboard.cpp index 71a14c029..fc4d183cd 100644 --- a/src/modules/navigator/offboard.cpp +++ b/src/modules/navigator/offboard.cpp @@ -67,17 +67,22 @@ Offboard::~Offboard() { } -bool -Offboard::on_active(struct position_setpoint_triplet_s *pos_sp_triplet) +void +Offboard::on_activation() +{ +} + +void +Offboard::on_active() { + struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); + bool updated; orb_check(_navigator->get_offboard_control_sp_sub(), &updated); if (updated) { update_offboard_control_setpoint(); } - bool changed = false; - /* copy offboard setpoints to the corresponding topics */ if (_offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED) { /* We accept position control only if none of the directions is ignored (as pos_sp_triplet does not @@ -97,8 +102,7 @@ Offboard::on_active(struct position_setpoint_triplet_s *pos_sp_triplet) pos_sp_triplet->current.valid = true; pos_sp_triplet->current.position_valid = true; - changed = true; - + _navigator->set_position_setpoint_triplet_updated(); } /* We accept velocity control only if none of the directions is ignored (as pos_sp_triplet does not * support deactivation of individual directions) */ @@ -117,13 +121,12 @@ Offboard::on_active(struct position_setpoint_triplet_s *pos_sp_triplet) pos_sp_triplet->current.valid = true; pos_sp_triplet->current.velocity_valid = true; - changed = true; + _navigator->set_position_setpoint_triplet_updated(); } //XXX: map acceleration setpoint once supported in setpoint triplet } - return changed; } void -- cgit v1.2.3 From 373b1705c19627b97f1c65c1e947802e6b88af83 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 14 Jul 2014 10:57:53 +0200 Subject: vehicle command: add latest guided commands --- src/modules/uORB/topics/vehicle_command.h | 3 +++ 1 file changed, 3 insertions(+) (limited to 'src') diff --git a/src/modules/uORB/topics/vehicle_command.h b/src/modules/uORB/topics/vehicle_command.h index c21a29b13..faf8cc7db 100644 --- a/src/modules/uORB/topics/vehicle_command.h +++ b/src/modules/uORB/topics/vehicle_command.h @@ -61,6 +61,9 @@ enum VEHICLE_CMD { VEHICLE_CMD_NAV_TAKEOFF = 22, /* Takeoff from ground / hand |Minimum pitch (if airspeed sensor present), desired pitch without sensor| Empty| Empty| Yaw angle (if magnetometer present), ignored without magnetometer| Latitude| Longitude| Altitude| */ VEHICLE_CMD_NAV_ROI = 80, /* Sets the region of interest (ROI) for a sensor set or the vehicle itself. This can then be used by the vehicles control system to control the vehicle attitude and the attitude of various sensors such as cameras. |Region of intereset mode. (see MAV_ROI enum)| MISSION index/ target ID. (see MAV_ROI enum)| ROI index (allows a vehicle to manage multiple ROI's)| Empty| x the location of the fixed ROI (see MAV_FRAME)| y| z| */ VEHICLE_CMD_NAV_PATHPLANNING = 81, /* Control autonomous path planning on the MAV. |0: Disable local obstacle avoidance / local path planning (without resetting map), 1: Enable local path planning, 2: Enable and reset local path planning| 0: Disable full path planning (without resetting map), 1: Enable, 2: Enable and reset map/occupancy grid, 3: Enable and reset planned route, but not occupancy grid| Empty| Yaw angle at goal, in compass degrees, [0..360]| Latitude/X of goal| Longitude/Y of goal| Altitude/Z of goal| */ + VEHICLE_CMD_NAV_GUIDED_LIMITS=90, /* set limits for external control |timeout - maximum time (in seconds) that external controller will be allowed to control vehicle. 0 means no timeout| absolute altitude min (in meters, WGS84) - if vehicle moves below this alt, the command will be aborted and the mission will continue. 0 means no lower altitude limit| absolute altitude max (in meters)- if vehicle moves above this alt, the command will be aborted and the mission will continue. 0 means no upper altitude limit| horizontal move limit (in meters, WGS84) - if vehicle moves more than this distance from it's location at the moment the command was executed, the command will be aborted and the mission will continue. 0 means no horizontal altitude limit| Empty| Empty| Empty| */ + VEHICLE_CMD_NAV_GUIDED_MASTER=91, /* set id of master controller |System ID| Component ID| Empty| Empty| Empty| Empty| Empty| */ + VEHICLE_CMD_NAV_GUIDED_ENABLE=92, /* hand control over to an external controller |On / Off (> 0.5f on)| Empty| Empty| Empty| Empty| Empty| Empty| */ VEHICLE_CMD_NAV_LAST = 95, /* NOP - This command is only used to mark the upper limit of the NAV/ACTION commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ VEHICLE_CMD_CONDITION_DELAY = 112, /* Delay mission state machine. |Delay in seconds (decimal)| Empty| Empty| Empty| Empty| Empty| Empty| */ VEHICLE_CMD_CONDITION_CHANGE_ALT = 113, /* Ascend/descend at rate. Delay mission state machine until desired altitude reached. |Descent / Ascend rate (m/s)| Empty| Empty| Empty| Empty| Empty| Finish Altitude| */ -- cgit v1.2.3 From 747eda92b9396e38746d505e5d79a7528e117c89 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 14 Jul 2014 11:19:06 +0200 Subject: commander: handle VEHICLE_CMD_NAV_GUIDED_ENABLE --- src/modules/commander/commander.cpp | 19 ++++++++++++++++++- 1 file changed, 18 insertions(+), 1 deletion(-) (limited to 'src') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index c11286789..e9db923e6 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -611,7 +611,24 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s } } break; - + case VEHICLE_CMD_NAV_GUIDED_ENABLE: { + transition_result_t res = TRANSITION_DENIED; + static main_state_t main_state_pre_offboard = MAIN_STATE_MANUAL; + if (status_local->main_state != MAIN_STATE_OFFBOARD) { + main_state_pre_offboard = status_local->main_state; + } + if (cmd->param1 > 0.5f) { + res = main_state_transition(status_local, MAIN_STATE_OFFBOARD); + if (res == TRANSITION_DENIED) { + print_reject_mode(status_local, "OFFBOARD"); + } + } else { + /* If the mavlink command is used to enable or disable offboard control: + * switch back to previous mode when disabling */ + res = main_state_transition(status_local, main_state_pre_offboard); + } + } + break; case VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN: case VEHICLE_CMD_PREFLIGHT_CALIBRATION: case VEHICLE_CMD_PREFLIGHT_SET_SENSOR_OFFSETS: -- cgit v1.2.3 From 7a064174110d827182820960b245031e0b4d42ab Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 14 Jul 2014 13:34:06 +0200 Subject: mavlink: external setpoint feed trough functionality --- src/modules/mavlink/mavlink.c | 7 +++++++ src/modules/mavlink/mavlink_main.cpp | 16 ++++++++++++---- src/modules/mavlink/mavlink_main.h | 8 ++++++-- src/modules/mavlink/mavlink_receiver.cpp | 15 +++++++++++++++ 4 files changed, 40 insertions(+), 6 deletions(-) (limited to 'src') diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c index e49288a74..bec706bad 100644 --- a/src/modules/mavlink/mavlink.c +++ b/src/modules/mavlink/mavlink.c @@ -68,6 +68,13 @@ PARAM_DEFINE_INT32(MAV_TYPE, MAV_TYPE_FIXED_WING); * @group MAVLink */ PARAM_DEFINE_INT32(MAV_USEHILGPS, 0); +/** + * Forward external setpoint messages + * If set to 1 incomming external setpoint messages will be directly forwarded to the controllers if in offboard + * control mode + * @group MAVLink + */ +PARAM_DEFINE_INT32(MAV_FWDEXTSP, 1); mavlink_system_t mavlink_system = { 100, diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index cd0581af4..7707c0bc8 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -220,6 +220,7 @@ Mavlink::Mavlink() : _task_running(false), _hil_enabled(false), _use_hil_gps(false), + _forward_externalsp(false), _is_usb_uart(false), _wait_to_transmit(false), _received_messages(false), @@ -246,6 +247,7 @@ Mavlink::Mavlink() : _param_component_id(0), _param_system_type(0), _param_use_hil_gps(0), + _param_forward_externalsp(0), /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "mavlink_el")), @@ -517,6 +519,7 @@ void Mavlink::mavlink_update_system(void) _param_component_id = param_find("MAV_COMP_ID"); _param_system_type = param_find("MAV_TYPE"); _param_use_hil_gps = param_find("MAV_USEHILGPS"); + _param_forward_externalsp = param_find("MAV_FWDEXTSP"); _param_initialized = true; } @@ -546,6 +549,11 @@ void Mavlink::mavlink_update_system(void) param_get(_param_use_hil_gps, &use_hil_gps); _use_hil_gps = (bool)use_hil_gps; + + int32_t forward_externalsp; + param_get(_param_forward_externalsp, &forward_externalsp); + + _forward_externalsp = (bool)forward_externalsp; } int Mavlink::get_system_id() @@ -1501,14 +1509,14 @@ Mavlink::task_main(int argc, char *argv[]) if (read_count > sizeof(mavlink_message_t)) { read_count = sizeof(mavlink_message_t); } - + memcpy(write_ptr, read_ptr, read_count); - + // We hold the mutex until after we complete the second part of the buffer. If we don't // we may end up breaking the empty slot overflow detection semantics when we mark the // possibly partial read below. pthread_mutex_lock(&_message_buffer_mutex); - + message_buffer_mark_read(read_count); /* write second part of buffer if there is some */ @@ -1519,7 +1527,7 @@ Mavlink::task_main(int argc, char *argv[]) memcpy(write_ptr, read_ptr, read_count); message_buffer_mark_read(available); } - + pthread_mutex_unlock(&_message_buffer_mutex); _mavlink_resend_uart(_channel, &msg); diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index acfc8b90e..8738a6f18 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -134,6 +134,8 @@ public: bool get_use_hil_gps() { return _use_hil_gps; } + bool get_forward_externalsp() { return _forward_externalsp; } + bool get_flow_control_enabled() { return _flow_control_enabled; } bool get_forwarding_on() { return _forwarding_on; } @@ -205,7 +207,7 @@ public: * Send a status text with loglevel * * @param string the message to send (will be capped by mavlink max string length) - * @param severity the log level, one of + * @param severity the log level, one of */ int send_statustext(unsigned severity, const char *string); MavlinkStream * get_streams() const { return _streams; } @@ -220,7 +222,7 @@ public: bool should_transmit() { return (!_wait_to_transmit || (_wait_to_transmit && _received_messages)); } bool message_buffer_write(const void *ptr, int size); - + void lockMessageBufferMutex(void) { pthread_mutex_lock(&_message_buffer_mutex); } void unlockMessageBufferMutex(void) { pthread_mutex_unlock(&_message_buffer_mutex); } @@ -241,6 +243,7 @@ private: /* states */ bool _hil_enabled; /**< Hardware In the Loop mode */ bool _use_hil_gps; /**< Accept GPS HIL messages (for example from an external motion capturing system to fake indoor gps) */ + bool _forward_externalsp; /**< Forward external setpoint messages to controllers directly if in offboard mode */ bool _is_usb_uart; /**< Port is USB */ bool _wait_to_transmit; /**< Wait to transmit until received messages. */ bool _received_messages; /**< Whether we've received valid mavlink messages. */ @@ -301,6 +304,7 @@ private: param_t _param_component_id; param_t _param_system_type; param_t _param_use_hil_gps; + param_t _param_forward_externalsp; perf_counter_t _loop_perf; /**< loop performance counter */ perf_counter_t _txerr_perf; /**< TX error counter */ diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index bd1296f62..9df843e58 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -451,6 +451,21 @@ MavlinkReceiver::handle_message_local_ned_position_setpoint_external(mavlink_mes } else { orb_publish(ORB_ID(offboard_control_setpoint), _offboard_control_sp_pub, &offboard_control_sp); } + + /* If we are in offboard control mode and offboard control loop through is enabled + * also publish the setpoint topic which is read by the controller */ + if (_mavlink->get_forward_externalsp()) { + bool updated; + orb_check(_control_mode_sub, &updated); + if (updated) { + orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode); + } + + if (_control_mode.flag_control_offboard_enabled) { + //XXX: copy to and publish setpoint triplet here + } + + } } } -- cgit v1.2.3 From d0cca02e97a24cf0b9554cf429bbd5716f947e4e Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 15 Jul 2014 10:34:56 +0200 Subject: add parsing of external attitude message --- src/modules/mavlink/mavlink_receiver.cpp | 88 +++++++++++++++++++++- src/modules/mavlink/mavlink_receiver.h | 1 + .../uORB/topics/offboard_control_setpoint.h | 6 ++ 3 files changed, 93 insertions(+), 2 deletions(-) (limited to 'src') diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 9df843e58..9e3842614 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -157,6 +157,10 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) handle_message_local_ned_position_setpoint_external(msg); break; + case MAVLINK_MSG_ID_ATTITUDE_SETPOINT_EXTERNAL: + handle_message_attitude_setpoint_external(msg); + break; + case MAVLINK_MSG_ID_RADIO_STATUS: handle_message_radio_status(msg); break; @@ -404,7 +408,7 @@ MavlinkReceiver::handle_message_local_ned_position_setpoint_external(mavlink_mes mavlink_msg_local_ned_position_setpoint_external_decode(msg, &local_ned_position_setpoint_external); struct offboard_control_setpoint_s offboard_control_sp; - memset(&offboard_control_sp, 0, sizeof(offboard_control_sp)); + memset(&offboard_control_sp, 0, sizeof(offboard_control_sp));//XXX breaks compatibility with multiple setpoints if (mavlink_system.sysid == local_ned_position_setpoint_external.target_system && mavlink_system.compid == local_ned_position_setpoint_external.target_component) { @@ -438,7 +442,7 @@ MavlinkReceiver::handle_message_local_ned_position_setpoint_external(mavlink_mes offboard_control_sp.acceleration[2] = local_ned_position_setpoint_external.afz; offboard_control_sp.isForceSetpoint = (bool)(local_ned_position_setpoint_external.type_mask & (1 << 9)); for (int i = 0; i < 9; i++) { - offboard_control_sp.ignore &= ~(local_ned_position_setpoint_external.type_mask & (1 << i)); + offboard_control_sp.ignore &= ~(1 << i); offboard_control_sp.ignore |= (local_ned_position_setpoint_external.type_mask & (1 << i)); } @@ -469,6 +473,86 @@ MavlinkReceiver::handle_message_local_ned_position_setpoint_external(mavlink_mes } } +void +MavlinkReceiver::handle_message_attitude_setpoint_external(mavlink_message_t *msg) +{ + mavlink_attitude_setpoint_external_t attitude_setpoint_external; + mavlink_msg_attitude_setpoint_external_decode(msg, &attitude_setpoint_external); + + struct offboard_control_setpoint_s offboard_control_sp; + memset(&offboard_control_sp, 0, sizeof(offboard_control_sp)); //XXX breaks compatibility with multiple setpoints + + if (mavlink_system.sysid == attitude_setpoint_external.target_system && + mavlink_system.compid == attitude_setpoint_external.target_component) { + for (int i = 0; i < 4; i++) { + offboard_control_sp.attitude[i] = attitude_setpoint_external.q[i]; + } + offboard_control_sp.attitude_rate[0] = attitude_setpoint_external.body_roll_rate; + offboard_control_sp.attitude_rate[1] = attitude_setpoint_external.body_pitch_rate; + offboard_control_sp.attitude_rate[2] = attitude_setpoint_external.body_yaw_rate; + + for (int i = 0; i < 3; i++) { + offboard_control_sp.ignore &= ~(1 << (i + 9)); + offboard_control_sp.ignore |= (attitude_setpoint_external.type_mask & (1 << i)) << 9; + } + offboard_control_sp.ignore &= ~(1 << 10); + offboard_control_sp.ignore |= ((attitude_setpoint_external.type_mask & (1 << 7)) << 3); + + + offboard_control_sp.timestamp = hrt_absolute_time(); + + if (_offboard_control_sp_pub < 0) { + _offboard_control_sp_pub = orb_advertise(ORB_ID(offboard_control_setpoint), &offboard_control_sp); + + } else { + orb_publish(ORB_ID(offboard_control_setpoint), _offboard_control_sp_pub, &offboard_control_sp); + } + + /* If we are in offboard control mode and offboard control loop through is enabled + * also publish the setpoint topic which is read by the controller */ + if (_mavlink->get_forward_externalsp()) { + bool updated; + orb_check(_control_mode_sub, &updated); + if (updated) { + orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode); + } + + if (_control_mode.flag_control_offboard_enabled) { + + /* Publish attitude setpoint if ignore bit is not set */ + if (!(attitude_setpoint_external.type_mask & (1 << 7))) { + struct vehicle_attitude_setpoint_s att_sp; + mavlink_quaternion_to_euler(attitude_setpoint_external.q, + &att_sp.roll_body, &att_sp.pitch_body, &att_sp.yaw_body); + att_sp.thrust = attitude_setpoint_external.thrust; + if (_att_sp_pub < 0) { + _att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp); + } else { + orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &att_sp); + } + } + + /* Publish attitude rate setpoint if ignore bit are not set */ + ///XXX add support for ignoring individual axes + if (!(attitude_setpoint_external.type_mask & (0b111))) { + struct vehicle_rates_setpoint_s rates_sp; + rates_sp.roll = attitude_setpoint_external.body_roll_rate; + rates_sp.pitch = attitude_setpoint_external.body_pitch_rate; + rates_sp.yaw = attitude_setpoint_external.body_yaw_rate; + rates_sp.thrust = attitude_setpoint_external.thrust; + + if (_att_sp_pub < 0) { + _rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &rates_sp); + } else { + orb_publish(ORB_ID(vehicle_rates_setpoint), _rates_sp_pub, &rates_sp); + } + } + } + + } + } +} + void MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg) { diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 4fd80f4ac..0f12cf32c 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -114,6 +114,7 @@ private: void handle_message_vicon_position_estimate(mavlink_message_t *msg); void handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message_t *msg); void handle_message_local_ned_position_setpoint_external(mavlink_message_t *msg); + void handle_message_attitude_setpoint_external(mavlink_message_t *msg); void handle_message_radio_status(mavlink_message_t *msg); void handle_message_manual_control(mavlink_message_t *msg); void handle_message_heartbeat(mavlink_message_t *msg); diff --git a/src/modules/uORB/topics/offboard_control_setpoint.h b/src/modules/uORB/topics/offboard_control_setpoint.h index 809412fc0..809a2ace4 100644 --- a/src/modules/uORB/topics/offboard_control_setpoint.h +++ b/src/modules/uORB/topics/offboard_control_setpoint.h @@ -88,6 +88,7 @@ struct offboard_control_setpoint_s { float attitude_rate[3]; /**< body angular rates (x, y, z) */ uint16_t ignore; /**< if field i is set to true, pi should be ignored */ + //XXX define constants for bit offsets bool isForceSetpoint; /**< the acceleration vector should be interpreted as force */ float override_mode_switch; @@ -118,6 +119,11 @@ inline bool offboard_control_sp_ignore_bodyrates(const struct offboard_control_s return (bool)(offboard_control_sp.ignore & (1 << (9 + index))); } +inline bool offboard_control_sp_ignore_attitude(const struct offboard_control_setpoint_s &offboard_control_sp) { + return (bool)(offboard_control_sp.ignore & (1 << 10)); +} + + /* register this as object request broker structure */ ORB_DECLARE(offboard_control_setpoint); -- cgit v1.2.3 From 596850e9f5343193ab4dc75cfa7849e0eb63ceea Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 15 Jul 2014 14:19:13 +0200 Subject: mavlink external sp: accept 0 sysid and compid --- src/modules/mavlink/mavlink_receiver.cpp | 14 ++++++++++---- 1 file changed, 10 insertions(+), 4 deletions(-) (limited to 'src') diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 9e3842614..64ba93f9c 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -410,8 +410,11 @@ MavlinkReceiver::handle_message_local_ned_position_setpoint_external(mavlink_mes struct offboard_control_setpoint_s offboard_control_sp; memset(&offboard_control_sp, 0, sizeof(offboard_control_sp));//XXX breaks compatibility with multiple setpoints - if (mavlink_system.sysid == local_ned_position_setpoint_external.target_system && - mavlink_system.compid == local_ned_position_setpoint_external.target_component) { + /* Only accept messages which are intended for this system */ + if ((mavlink_system.sysid == local_ned_position_setpoint_external.target_system || + local_ned_position_setpoint_external.target_system == 0) && + (mavlink_system.compid == local_ned_position_setpoint_external.target_component || + local_ned_position_setpoint_external.target_component == 0)) { /* convert mavlink type (local, NED) to uORB offboard control struct */ switch (local_ned_position_setpoint_external.coordinate_frame) { @@ -482,8 +485,11 @@ MavlinkReceiver::handle_message_attitude_setpoint_external(mavlink_message_t *ms struct offboard_control_setpoint_s offboard_control_sp; memset(&offboard_control_sp, 0, sizeof(offboard_control_sp)); //XXX breaks compatibility with multiple setpoints - if (mavlink_system.sysid == attitude_setpoint_external.target_system && - mavlink_system.compid == attitude_setpoint_external.target_component) { + /* Only accept messages which are intended for this system */ + if ((mavlink_system.sysid == attitude_setpoint_external.target_system || + local_ned_position_setpoint_external.target_system == 0) && + (mavlink_system.compid == local_ned_position_setpoint_external.target_component || + local_ned_position_setpoint_external.target_component == 0)) { for (int i = 0; i < 4; i++) { offboard_control_sp.attitude[i] = attitude_setpoint_external.q[i]; } -- cgit v1.2.3 From 35a9dad998961c9f8aa5ab5d015cb4b3a642a9ce Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 15 Jul 2014 14:42:53 +0200 Subject: mavlink receiver: fix copy paste error --- src/modules/mavlink/mavlink_receiver.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) (limited to 'src') diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 64ba93f9c..30eb6d0e5 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -487,9 +487,9 @@ MavlinkReceiver::handle_message_attitude_setpoint_external(mavlink_message_t *ms /* Only accept messages which are intended for this system */ if ((mavlink_system.sysid == attitude_setpoint_external.target_system || - local_ned_position_setpoint_external.target_system == 0) && - (mavlink_system.compid == local_ned_position_setpoint_external.target_component || - local_ned_position_setpoint_external.target_component == 0)) { + attitude_setpoint_external.target_system == 0) && + (mavlink_system.compid == attitude_setpoint_external.target_component || + attitude_setpoint_external.target_component == 0)) { for (int i = 0; i < 4; i++) { offboard_control_sp.attitude[i] = attitude_setpoint_external.q[i]; } -- cgit v1.2.3 From 3c8927c42386a6528f120d04ec93f5ab9b453a5b Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 16 Jul 2014 09:38:57 +0200 Subject: once offboard is set by mavlink command ignore RC mode --- src/modules/commander/commander.cpp | 10 ++++++++++ src/modules/uORB/topics/vehicle_status.h | 2 ++ 2 files changed, 12 insertions(+) (limited to 'src') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index e9db923e6..cc68c4e36 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -621,11 +621,16 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s res = main_state_transition(status_local, MAIN_STATE_OFFBOARD); if (res == TRANSITION_DENIED) { print_reject_mode(status_local, "OFFBOARD"); + status_local->offboard_control_set_by_command = false; + } else { + /* Set flag that offboard was set via command, main state is not overridden by rc */ + status_local->offboard_control_set_by_command = true; } } else { /* If the mavlink command is used to enable or disable offboard control: * switch back to previous mode when disabling */ res = main_state_transition(status_local, main_state_pre_offboard); + status_local->offboard_control_set_by_command = false; } } break; @@ -1749,6 +1754,11 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s /* set main state according to RC switches */ transition_result_t res = TRANSITION_DENIED; + /* if offboard is set allready by a mavlink command, abort */ + if (status.offboard_control_set_by_command) { + return main_state_transition(status_local, MAIN_STATE_OFFBOARD); + } + /* offboard switch overrides main switch */ if (sp_man->offboard_switch == SWITCH_POS_ON) { res = main_state_transition(status_local, MAIN_STATE_OFFBOARD); diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index b46c00b75..7a40ac636 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -206,6 +206,8 @@ struct vehicle_status_s { bool offboard_control_signal_lost; bool offboard_control_signal_weak; uint64_t offboard_control_signal_lost_interval; /**< interval in microseconds without an offboard control message */ + bool offboard_control_set_by_command; /**< true if the offboard mode was set by a mavlink command + and should not be overridden by RC */ /* see SYS_STATUS mavlink message for the following */ uint32_t onboard_control_sensors_present; -- cgit v1.2.3 From a63f3a173174e28fb5df63a1646c62dcbdc52bbb Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 16 Jul 2014 10:36:02 +0200 Subject: external attitude sp: set timestamp --- src/modules/mavlink/mavlink_receiver.cpp | 2 ++ 1 file changed, 2 insertions(+) (limited to 'src') diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 30eb6d0e5..952bc8735 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -528,6 +528,7 @@ MavlinkReceiver::handle_message_attitude_setpoint_external(mavlink_message_t *ms /* Publish attitude setpoint if ignore bit is not set */ if (!(attitude_setpoint_external.type_mask & (1 << 7))) { struct vehicle_attitude_setpoint_s att_sp; + att_sp.timestamp = hrt_absolute_time(); mavlink_quaternion_to_euler(attitude_setpoint_external.q, &att_sp.roll_body, &att_sp.pitch_body, &att_sp.yaw_body); att_sp.thrust = attitude_setpoint_external.thrust; @@ -542,6 +543,7 @@ MavlinkReceiver::handle_message_attitude_setpoint_external(mavlink_message_t *ms ///XXX add support for ignoring individual axes if (!(attitude_setpoint_external.type_mask & (0b111))) { struct vehicle_rates_setpoint_s rates_sp; + rates_sp.timestamp = hrt_absolute_time(); rates_sp.roll = attitude_setpoint_external.body_roll_rate; rates_sp.pitch = attitude_setpoint_external.body_pitch_rate; rates_sp.yaw = attitude_setpoint_external.body_yaw_rate; -- cgit v1.2.3 From 9527cc8293749b9ccdec015f587afef9698be1e6 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 16 Jul 2014 14:03:34 +0200 Subject: att external sp: also write quaternion --- src/modules/mavlink/mavlink_receiver.cpp | 3 +++ 1 file changed, 3 insertions(+) (limited to 'src') diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 952bc8735..e5d380cfb 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -506,6 +506,7 @@ MavlinkReceiver::handle_message_attitude_setpoint_external(mavlink_message_t *ms offboard_control_sp.timestamp = hrt_absolute_time(); + offboard_control_sp.mode =OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE; //XXX handle rate control mode if (_offboard_control_sp_pub < 0) { _offboard_control_sp_pub = orb_advertise(ORB_ID(offboard_control_setpoint), &offboard_control_sp); @@ -532,6 +533,8 @@ MavlinkReceiver::handle_message_attitude_setpoint_external(mavlink_message_t *ms mavlink_quaternion_to_euler(attitude_setpoint_external.q, &att_sp.roll_body, &att_sp.pitch_body, &att_sp.yaw_body); att_sp.thrust = attitude_setpoint_external.thrust; + att_sp.q_d_valid = true; + memcpy(att_sp.q_d, attitude_setpoint_external.q, sizeof(att_sp.q_d)); if (_att_sp_pub < 0) { _att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp); } else { -- cgit v1.2.3 From f4608707389dbc30eb25db524d6e008c8033d052 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 17 Jul 2014 09:11:57 +0200 Subject: support force setpoints --- src/modules/commander/commander.cpp | 9 ++++++++ src/modules/mavlink/mavlink_receiver.cpp | 24 +++++++++++++++++++++- src/modules/mavlink/mavlink_receiver.h | 2 ++ .../uORB/topics/offboard_control_setpoint.h | 7 ++++--- src/modules/uORB/topics/vehicle_control_mode.h | 1 + 5 files changed, 39 insertions(+), 4 deletions(-) (limited to 'src') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index cc68c4e36..fc9560e18 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1951,6 +1951,15 @@ set_control_mode() control_mode.flag_control_position_enabled = false; control_mode.flag_control_velocity_enabled = false; break; + case OFFBOARD_CONTROL_MODE_DIRECT_FORCE: + control_mode.flag_control_rates_enabled = true; + control_mode.flag_control_attitude_enabled = false; + control_mode.flag_control_force_enabled = true; + control_mode.flag_control_altitude_enabled = false; + control_mode.flag_control_climb_rate_enabled = false; + control_mode.flag_control_position_enabled = false; + control_mode.flag_control_velocity_enabled = false; + break; case OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED: case OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_OFFSET_NED: case OFFBOARD_CONTROL_MODE_DIRECT_BODY_NED: diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index e5d380cfb..2cc2d6162 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -105,6 +105,7 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) : _global_vel_sp_pub(-1), _att_sp_pub(-1), _rates_sp_pub(-1), + _force_sp_pub(-1), _vicon_position_pub(-1), _telemetry_status_pub(-1), _rc_pub(-1), @@ -444,6 +445,13 @@ MavlinkReceiver::handle_message_local_ned_position_setpoint_external(mavlink_mes offboard_control_sp.acceleration[1] = local_ned_position_setpoint_external.afy; offboard_control_sp.acceleration[2] = local_ned_position_setpoint_external.afz; offboard_control_sp.isForceSetpoint = (bool)(local_ned_position_setpoint_external.type_mask & (1 << 9)); + + /* If we are in force control mode, for now set offboard mode to force control */ + if (offboard_control_sp.isForceSetpoint) { + offboard_control_sp.mode = OFFBOARD_CONTROL_MODE_DIRECT_FORCE; + } + + /* set ignore flags */ for (int i = 0; i < 9; i++) { offboard_control_sp.ignore &= ~(1 << i); offboard_control_sp.ignore |= (local_ned_position_setpoint_external.type_mask & (1 << i)); @@ -467,9 +475,23 @@ MavlinkReceiver::handle_message_local_ned_position_setpoint_external(mavlink_mes if (updated) { orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode); } - if (_control_mode.flag_control_offboard_enabled) { + if (offboard_control_sp.isForceSetpoint) { + struct vehicle_force_setpoint_s force_sp; + force_sp.x = offboard_control_sp.acceleration[0]; + force_sp.y = offboard_control_sp.acceleration[1]; + force_sp.z = offboard_control_sp.acceleration[2]; + //XXX: yaw + if (_force_sp_pub < 0) { + _force_sp_pub = orb_advertise(ORB_ID(vehicle_force_setpoint), &force_sp); + } else { + orb_publish(ORB_ID(vehicle_force_setpoint), _force_sp_pub, &force_sp); + } + } else { + //XXX: copy to and publish setpoint triplet here + } + } } diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 0f12cf32c..0044b42cb 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -70,6 +70,7 @@ #include #include #include +#include #include "mavlink_ftp.h" @@ -146,6 +147,7 @@ private: orb_advert_t _global_vel_sp_pub; orb_advert_t _att_sp_pub; orb_advert_t _rates_sp_pub; + orb_advert_t _force_sp_pub; orb_advert_t _vicon_position_pub; orb_advert_t _telemetry_status_pub; orb_advert_t _rc_pub; diff --git a/src/modules/uORB/topics/offboard_control_setpoint.h b/src/modules/uORB/topics/offboard_control_setpoint.h index 809a2ace4..19f11ba92 100644 --- a/src/modules/uORB/topics/offboard_control_setpoint.h +++ b/src/modules/uORB/topics/offboard_control_setpoint.h @@ -58,9 +58,10 @@ enum OFFBOARD_CONTROL_MODE { OFFBOARD_CONTROL_MODE_DIRECT_BODY_NED = 5, OFFBOARD_CONTROL_MODE_DIRECT_BODY_OFFSET_NED = 6, OFFBOARD_CONTROL_MODE_DIRECT_GLOBAL = 7, - OFFBOARD_CONTROL_MODE_ATT_YAW_RATE = 8, - OFFBOARD_CONTROL_MODE_ATT_YAW_POS = 9, - OFFBOARD_CONTROL_MODE_MULTIROTOR_SIMPLE = 10, /**< roll / pitch rotated aligned to the takeoff orientation, throttle stabilized, yaw pos */ + OFFBOARD_CONTROL_MODE_DIRECT_FORCE = 8, + OFFBOARD_CONTROL_MODE_ATT_YAW_RATE = 9, + OFFBOARD_CONTROL_MODE_ATT_YAW_POS = 10, + OFFBOARD_CONTROL_MODE_MULTIROTOR_SIMPLE = 11, /**< roll / pitch rotated aligned to the takeoff orientation, throttle stabilized, yaw pos */ }; enum OFFBOARD_CONTROL_FRAME { diff --git a/src/modules/uORB/topics/vehicle_control_mode.h b/src/modules/uORB/topics/vehicle_control_mode.h index 49e2ba4b5..ca7705456 100644 --- a/src/modules/uORB/topics/vehicle_control_mode.h +++ b/src/modules/uORB/topics/vehicle_control_mode.h @@ -77,6 +77,7 @@ struct vehicle_control_mode_s { bool flag_control_offboard_enabled; /**< true if offboard control should be used */ bool flag_control_rates_enabled; /**< true if rates are stabilized */ bool flag_control_attitude_enabled; /**< true if attitude stabilization is mixed in */ + bool flag_control_force_enabled; /**< true if force control is mixed in */ bool flag_control_velocity_enabled; /**< true if horizontal velocity (implies direction) is controlled */ bool flag_control_position_enabled; /**< true if position is controlled */ bool flag_control_altitude_enabled; /**< true if altitude is controlled */ -- cgit v1.2.3 From 7bc0e26734a0319295e488e413db8f618b9b621c Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 23 Jul 2014 11:37:27 +0200 Subject: mavlink external sp: support for local pos and vel --- src/modules/mavlink/mavlink_receiver.cpp | 46 ++++++++++++++++++++++++++++++-- src/modules/mavlink/mavlink_receiver.h | 2 +- 2 files changed, 45 insertions(+), 3 deletions(-) (limited to 'src') diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index b39aebf9e..b7cddf523 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -37,6 +37,7 @@ * * @author Lorenz Meier * @author Anton Babushkin + * @author Thomas Gubler */ /* XXX trim includes */ @@ -103,11 +104,11 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) : _cmd_pub(-1), _flow_pub(-1), _offboard_control_sp_pub(-1), - _local_pos_sp_pub(-1), _global_vel_sp_pub(-1), _att_sp_pub(-1), _rates_sp_pub(-1), _force_sp_pub(-1), + _pos_sp_triplet_pub(-1), _vicon_position_pub(-1), _telemetry_status_pub(-1), _rc_pub(-1), @@ -487,8 +488,49 @@ MavlinkReceiver::handle_message_local_ned_position_setpoint_external(mavlink_mes orb_publish(ORB_ID(vehicle_force_setpoint), _force_sp_pub, &force_sp); } } else { + /* It's not a force setpoint: publish to setpoint triplet topic */ + struct position_setpoint_triplet_s pos_sp_triplet; + pos_sp_triplet.previous.valid = false; + pos_sp_triplet.next.valid = false; + pos_sp_triplet.current.valid = true; + + /* set the local pos values if the setpoint type is 'local pos' and none + * of the local pos fields is set to 'ignore' */ + if (offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED && + !offboard_control_sp_ignore_position(offboard_control_sp, 0) && + !offboard_control_sp_ignore_position(offboard_control_sp, 1) && + !offboard_control_sp_ignore_position(offboard_control_sp, 2)) { + pos_sp_triplet.current.type = SETPOINT_TYPE_POSITION; //XXX support others + pos_sp_triplet.current.position_valid = true; + pos_sp_triplet.current.x = offboard_control_sp.position[0]; + pos_sp_triplet.current.y = offboard_control_sp.position[1]; + pos_sp_triplet.current.z = offboard_control_sp.position[2]; + } + + /* set the local vel values if the setpoint type is 'local pos' and none + * of the local vel fields is set to 'ignore' */ + if (offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED && + !offboard_control_sp_ignore_velocity(offboard_control_sp, 0) && + !offboard_control_sp_ignore_velocity(offboard_control_sp, 1) && + !offboard_control_sp_ignore_velocity(offboard_control_sp, 2)) { + pos_sp_triplet.current.type = SETPOINT_TYPE_POSITION; //XXX support others + pos_sp_triplet.current.velocity_valid = true; + pos_sp_triplet.current.vx = offboard_control_sp.velocity[0]; + pos_sp_triplet.current.vy = offboard_control_sp.velocity[1]; + pos_sp_triplet.current.vz = offboard_control_sp.velocity[2]; + } + + //XXX handle global pos setpoints (different MAV frames) + + + if (_pos_sp_triplet_pub < 0) { + _pos_sp_triplet_pub = orb_advertise(ORB_ID(position_setpoint_triplet), + &pos_sp_triplet); + } else { + orb_publish(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_pub, + &pos_sp_triplet); + } - //XXX: copy to and publish setpoint triplet here } } diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index a6553cb0a..a2ed4264f 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -143,11 +143,11 @@ private: orb_advert_t _cmd_pub; orb_advert_t _flow_pub; orb_advert_t _offboard_control_sp_pub; - orb_advert_t _local_pos_sp_pub; orb_advert_t _global_vel_sp_pub; orb_advert_t _att_sp_pub; orb_advert_t _rates_sp_pub; orb_advert_t _force_sp_pub; + orb_advert_t _pos_sp_triplet_pub; orb_advert_t _vicon_position_pub; orb_advert_t _telemetry_status_pub; orb_advert_t _rc_pub; -- 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(-) (limited to 'src') 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 c7819dbb690680e3fb57a31f04d155ac8be7410d Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sat, 26 Jul 2014 22:08:51 +0200 Subject: mavlink: use pixhawk dialect --- src/modules/mavlink/mavlink_bridge_header.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src') diff --git a/src/modules/mavlink/mavlink_bridge_header.h b/src/modules/mavlink/mavlink_bridge_header.h index 0cd08769e..00d427636 100644 --- a/src/modules/mavlink/mavlink_bridge_header.h +++ b/src/modules/mavlink/mavlink_bridge_header.h @@ -84,7 +84,7 @@ void mavlink_send_uart_bytes(mavlink_channel_t chan, const uint8_t *ch, int leng extern mavlink_status_t *mavlink_get_channel_status(uint8_t chan); extern mavlink_message_t *mavlink_get_channel_buffer(uint8_t chan); -#include +#include __END_DECLS -- 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(-) (limited to 'src') 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(-) (limited to 'src') 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 a04d70ed5a800ff6d74afc6800f7bb94ed472ad4 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 12 Aug 2014 14:54:35 +0200 Subject: offboard setpoints: correctly check for force setpoint --- src/modules/mavlink/mavlink_receiver.cpp | 6 ++++- .../uORB/topics/offboard_control_setpoint.h | 30 ++++++++++++++++++++++ 2 files changed, 35 insertions(+), 1 deletion(-) (limited to 'src') diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index dfc5ddc91..418b81ee1 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -441,7 +441,11 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode); } if (_control_mode.flag_control_offboard_enabled) { - if (offboard_control_sp.isForceSetpoint) { + if (offboard_control_sp.isForceSetpoint && + offboard_control_sp_ignore_position_all(offboard_control_sp) && + offboard_control_sp_ignore_velocity_all(offboard_control_sp)) { + /* The offboard setpoint is a force setpoint only, directly writing to the force + * setpoint topic and not publishing the setpoint triplet topic */ struct vehicle_force_setpoint_s force_sp; force_sp.x = offboard_control_sp.acceleration[0]; force_sp.y = offboard_control_sp.acceleration[1]; diff --git a/src/modules/uORB/topics/offboard_control_setpoint.h b/src/modules/uORB/topics/offboard_control_setpoint.h index 19f11ba92..4f45ac14f 100644 --- a/src/modules/uORB/topics/offboard_control_setpoint.h +++ b/src/modules/uORB/topics/offboard_control_setpoint.h @@ -104,14 +104,44 @@ struct offboard_control_setpoint_s { * @} */ +/** + * Returns true if the position setpoint at index should be ignored + */ inline bool offboard_control_sp_ignore_position(const struct offboard_control_setpoint_s &offboard_control_sp, int index) { return (bool)(offboard_control_sp.ignore & (1 << index)); } +/** + * Returns true if all position setpoints should be ignored + */ +inline bool offboard_control_sp_ignore_position_all(const struct offboard_control_setpoint_s &offboard_control_sp) { + for (int i = 0; i < 3; i++) { + if (offboard_control_sp_ignore_position(offboard_control_sp, i)) { + return true; + } + } + return false; +} + +/** + * Returns true if the velocity setpoint at index should be ignored + */ inline bool offboard_control_sp_ignore_velocity(const struct offboard_control_setpoint_s &offboard_control_sp, int index) { return (bool)(offboard_control_sp.ignore & (1 << (3 + index))); } +/** + * Returns true if all velocity setpoints should be ignored + */ +inline bool offboard_control_sp_ignore_velocity_all(const struct offboard_control_setpoint_s &offboard_control_sp) { + for (int i = 0; i < 3; i++) { + if (offboard_control_sp_ignore_velocity(offboard_control_sp, i)) { + return true; + } + } + return false; +} + inline bool offboard_control_sp_ignore_acceleration(const struct offboard_control_setpoint_s &offboard_control_sp, int index) { return (bool)(offboard_control_sp.ignore & (1 << (6 + index))); } -- cgit v1.2.3 From 85eed22150e4a24098554992a6d77bce6f1ddf31 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 12 Aug 2014 16:27:02 +0200 Subject: offboard sp: correctly map acceleration/force in combined setpoint to setpoint triple Conflicts: src/modules/uORB/topics/position_setpoint_triplet.h --- src/modules/mavlink/mavlink_receiver.cpp | 39 +++++++++++++++------- .../uORB/topics/offboard_control_setpoint.h | 21 ++++++++++++ .../uORB/topics/position_setpoint_triplet.h | 5 +++ 3 files changed, 53 insertions(+), 12 deletions(-) (limited to 'src') diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 418b81ee1..da651e4e7 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -457,7 +457,7 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t orb_publish(ORB_ID(vehicle_force_setpoint), _force_sp_pub, &force_sp); } } else { - /* It's not a force setpoint: publish to setpoint triplet topic */ + /* It's not a pure force setpoint: publish to setpoint triplet topic */ struct position_setpoint_triplet_s pos_sp_triplet; pos_sp_triplet.previous.valid = false; pos_sp_triplet.next.valid = false; @@ -466,29 +466,44 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t /* set the local pos values if the setpoint type is 'local pos' and none * of the local pos fields is set to 'ignore' */ if (offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED && - !offboard_control_sp_ignore_position(offboard_control_sp, 0) && - !offboard_control_sp_ignore_position(offboard_control_sp, 1) && - !offboard_control_sp_ignore_position(offboard_control_sp, 2)) { + !offboard_control_sp_ignore_position_all(offboard_control_sp)) { pos_sp_triplet.current.type = SETPOINT_TYPE_POSITION; //XXX support others pos_sp_triplet.current.position_valid = true; pos_sp_triplet.current.x = offboard_control_sp.position[0]; pos_sp_triplet.current.y = offboard_control_sp.position[1]; pos_sp_triplet.current.z = offboard_control_sp.position[2]; + } else { + pos_sp_triplet.current.position_valid = false; } /* set the local vel values if the setpoint type is 'local pos' and none * of the local vel fields is set to 'ignore' */ if (offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED && - !offboard_control_sp_ignore_velocity(offboard_control_sp, 0) && - !offboard_control_sp_ignore_velocity(offboard_control_sp, 1) && - !offboard_control_sp_ignore_velocity(offboard_control_sp, 2)) { - pos_sp_triplet.current.type = SETPOINT_TYPE_POSITION; //XXX support others - pos_sp_triplet.current.velocity_valid = true; - pos_sp_triplet.current.vx = offboard_control_sp.velocity[0]; - pos_sp_triplet.current.vy = offboard_control_sp.velocity[1]; - pos_sp_triplet.current.vz = offboard_control_sp.velocity[2]; + !offboard_control_sp_ignore_velocity_all(offboard_control_sp)) { + pos_sp_triplet.current.type = SETPOINT_TYPE_POSITION; //XXX support others + pos_sp_triplet.current.velocity_valid = true; + pos_sp_triplet.current.vx = offboard_control_sp.velocity[0]; + pos_sp_triplet.current.vy = offboard_control_sp.velocity[1]; + pos_sp_triplet.current.vz = offboard_control_sp.velocity[2]; + } else { + pos_sp_triplet.current.velocity_valid = false; } + /* set the local acceleration values if the setpoint type is 'local pos' and none + * of the accelerations fields is set to 'ignore' */ + if (offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED && + !offboard_control_sp_ignore_acceleration_all(offboard_control_sp)) { + pos_sp_triplet.current.type = SETPOINT_TYPE_POSITION; //XXX support others + pos_sp_triplet.current.acceleration_valid = true; + pos_sp_triplet.current.a_x = offboard_control_sp.acceleration[0]; + pos_sp_triplet.current.a_y = offboard_control_sp.acceleration[1]; + pos_sp_triplet.current.a_z = offboard_control_sp.acceleration[2]; + pos_sp_triplet.current.acceleration_is_force = + offboard_control_sp.isForceSetpoint; + + } else { + pos_sp_triplet.current.acceleration_valid = false; + } //XXX handle global pos setpoints (different MAV frames) diff --git a/src/modules/uORB/topics/offboard_control_setpoint.h b/src/modules/uORB/topics/offboard_control_setpoint.h index 4f45ac14f..47c89f708 100644 --- a/src/modules/uORB/topics/offboard_control_setpoint.h +++ b/src/modules/uORB/topics/offboard_control_setpoint.h @@ -142,14 +142,35 @@ inline bool offboard_control_sp_ignore_velocity_all(const struct offboard_contro return false; } +/** + * Returns true if the acceleration setpoint at index should be ignored + */ inline bool offboard_control_sp_ignore_acceleration(const struct offboard_control_setpoint_s &offboard_control_sp, int index) { return (bool)(offboard_control_sp.ignore & (1 << (6 + index))); } +/** + * Returns true if all acceleration setpoints should be ignored + */ +inline bool offboard_control_sp_ignore_acceleration_all(const struct offboard_control_setpoint_s &offboard_control_sp) { + for (int i = 0; i < 3; i++) { + if (offboard_control_sp_ignore_acceleration(offboard_control_sp, i)) { + return true; + } + } + return false; +} + +/** + * Returns true if the bodyrate setpoint at index should be ignored + */ inline bool offboard_control_sp_ignore_bodyrates(const struct offboard_control_setpoint_s &offboard_control_sp, int index) { return (bool)(offboard_control_sp.ignore & (1 << (9 + index))); } +/** + * Returns true if the attitude setpoint should be ignored + */ inline bool offboard_control_sp_ignore_attitude(const struct offboard_control_setpoint_s &offboard_control_sp) { return (bool)(offboard_control_sp.ignore & (1 << 10)); } diff --git a/src/modules/uORB/topics/position_setpoint_triplet.h b/src/modules/uORB/topics/position_setpoint_triplet.h index ec2131abd..3ea3f671e 100644 --- a/src/modules/uORB/topics/position_setpoint_triplet.h +++ b/src/modules/uORB/topics/position_setpoint_triplet.h @@ -83,6 +83,11 @@ struct position_setpoint_s float loiter_radius; /**< loiter radius (only for fixed wing), in m */ int8_t loiter_direction; /**< loiter direction: 1 = CW, -1 = CCW */ float pitch_min; /**< minimal pitch angle for fixed wing takeoff waypoints */ + float a_x; //**< acceleration x setpoint */ + float a_y; //**< acceleration y setpoint */ + float a_z; //**< acceleration z setpoint */ + bool acceleration_valid; //*< true if acceleration setpoint is valid/should be used */ + bool acceleration_is_force; //*< interprete acceleration as force */ }; /** -- 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 (limited to 'src') 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(-) (limited to 'src') 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 1e9d77f1d2dfd3f500c8486edf8940bc4a15162e Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 15 Aug 2014 12:58:06 +0200 Subject: offboard: ignore mask cleanup add handling of thrust ignore bit, add defines for the bitnumbers, improve and fix interface to ignore bitmask --- src/modules/mavlink/mavlink_receiver.cpp | 29 ++++--- .../uORB/topics/offboard_control_setpoint.h | 90 ++++++++++++++++++++-- 2 files changed, 100 insertions(+), 19 deletions(-) (limited to 'src') diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 7dd043bbe..6b3c3accb 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -472,7 +472,7 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t /* set the local pos values if the setpoint type is 'local pos' and none * of the local pos fields is set to 'ignore' */ if (offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED && - !offboard_control_sp_ignore_position_all(offboard_control_sp)) { + !offboard_control_sp_ignore_position_some(offboard_control_sp)) { pos_sp_triplet.current.type = SETPOINT_TYPE_POSITION; //XXX support others pos_sp_triplet.current.position_valid = true; pos_sp_triplet.current.x = offboard_control_sp.position[0]; @@ -485,7 +485,7 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t /* set the local vel values if the setpoint type is 'local pos' and none * of the local vel fields is set to 'ignore' */ if (offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED && - !offboard_control_sp_ignore_velocity_all(offboard_control_sp)) { + !offboard_control_sp_ignore_velocity_some(offboard_control_sp)) { pos_sp_triplet.current.type = SETPOINT_TYPE_POSITION; //XXX support others pos_sp_triplet.current.velocity_valid = true; pos_sp_triplet.current.vx = offboard_control_sp.velocity[0]; @@ -498,7 +498,7 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t /* set the local acceleration values if the setpoint type is 'local pos' and none * of the accelerations fields is set to 'ignore' */ if (offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED && - !offboard_control_sp_ignore_acceleration_all(offboard_control_sp)) { + !offboard_control_sp_ignore_acceleration_some(offboard_control_sp)) { pos_sp_triplet.current.type = SETPOINT_TYPE_POSITION; //XXX support others pos_sp_triplet.current.acceleration_valid = true; pos_sp_triplet.current.a_x = offboard_control_sp.acceleration[0]; @@ -589,12 +589,17 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg) offboard_control_sp.attitude_rate[1] = set_attitude_target.body_pitch_rate; offboard_control_sp.attitude_rate[2] = set_attitude_target.body_yaw_rate; + /* set correct ignore flags for body rate fields: copy from mavlink message */ for (int i = 0; i < 3; i++) { - offboard_control_sp.ignore &= ~(1 << (i + 9)); - offboard_control_sp.ignore |= (set_attitude_target.type_mask & (1 << i)) << 9; + offboard_control_sp.ignore &= ~(1 << (i + OFB_IGN_BIT_BODYRATE_X)); + offboard_control_sp.ignore |= (set_attitude_target.type_mask & (1 << i)) << OFB_IGN_BIT_BODYRATE_X; } - offboard_control_sp.ignore &= ~(1 << 10); - offboard_control_sp.ignore |= ((set_attitude_target.type_mask & (1 << 7)) << 3); + /* set correct ignore flags for thrust field: copy from mavlink message */ + offboard_control_sp.ignore &= ~(1 << OFB_IGN_BIT_THRUST); + offboard_control_sp.ignore |= ((set_attitude_target.type_mask & (1 << 6)) << OFB_IGN_BIT_THRUST); + /* set correct ignore flags for attitude field: copy from mavlink message */ + offboard_control_sp.ignore &= ~(1 << OFB_IGN_BIT_ATT); + offboard_control_sp.ignore |= ((set_attitude_target.type_mask & (1 << 7)) << OFB_IGN_BIT_ATT); offboard_control_sp.timestamp = hrt_absolute_time(); @@ -618,8 +623,9 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg) if (_control_mode.flag_control_offboard_enabled) { - /* Publish attitude setpoint if ignore bit is not set */ - if (!(set_attitude_target.type_mask & (1 << 7))) { + /* Publish attitude setpoint if attitude and thrust ignore bits are not set */ + if (!(offboard_control_sp_ignore_attitude(offboard_control_sp) || + offboard_control_sp_ignore_thrust(offboard_control_sp))) { struct vehicle_attitude_setpoint_s att_sp; att_sp.timestamp = hrt_absolute_time(); mavlink_quaternion_to_euler(set_attitude_target.q, @@ -634,9 +640,10 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg) } } - /* Publish attitude rate setpoint if ignore bit are not set */ + /* Publish attitude rate setpoint if bodyrate and thrust ignore bits are not set */ ///XXX add support for ignoring individual axes - if (!(set_attitude_target.type_mask & (0b111))) { + if (!(offboard_control_sp_ignore_bodyrates_some(offboard_control_sp) || + offboard_control_sp_ignore_thrust(offboard_control_sp))) { struct vehicle_rates_setpoint_s rates_sp; rates_sp.timestamp = hrt_absolute_time(); rates_sp.roll = set_attitude_target.body_roll_rate; diff --git a/src/modules/uORB/topics/offboard_control_setpoint.h b/src/modules/uORB/topics/offboard_control_setpoint.h index 47c89f708..6e37896af 100644 --- a/src/modules/uORB/topics/offboard_control_setpoint.h +++ b/src/modules/uORB/topics/offboard_control_setpoint.h @@ -61,7 +61,7 @@ enum OFFBOARD_CONTROL_MODE { OFFBOARD_CONTROL_MODE_DIRECT_FORCE = 8, OFFBOARD_CONTROL_MODE_ATT_YAW_RATE = 9, OFFBOARD_CONTROL_MODE_ATT_YAW_POS = 10, - OFFBOARD_CONTROL_MODE_MULTIROTOR_SIMPLE = 11, /**< roll / pitch rotated aligned to the takeoff orientation, throttle stabilized, yaw pos */ + OFFBOARD_CONTROL_MODE_MULTIROTOR_SIMPLE = 11 /**< roll / pitch rotated aligned to the takeoff orientation, throttle stabilized, yaw pos */ }; enum OFFBOARD_CONTROL_FRAME { @@ -72,6 +72,23 @@ enum OFFBOARD_CONTROL_FRAME { OFFBOARD_CONTROL_FRAME_GLOBAL = 4 }; +/* mappings for the ignore bitmask */ +enum {OFB_IGN_BIT_POS_X, + OFB_IGN_BIT_POS_Y, + OFB_IGN_BIT_POS_Z, + OFB_IGN_BIT_VEL_X, + OFB_IGN_BIT_VEL_Y, + OFB_IGN_BIT_VEL_Z, + OFB_IGN_BIT_ACC_X, + OFB_IGN_BIT_ACC_Y, + OFB_IGN_BIT_ACC_Z, + OFB_IGN_BIT_BODYRATE_X, + OFB_IGN_BIT_BODYRATE_Y, + OFB_IGN_BIT_BODYRATE_Z, + OFB_IGN_BIT_ATT, + OFB_IGN_BIT_THRUST +}; + /** * @addtogroup topics * @{ @@ -87,9 +104,11 @@ struct offboard_control_setpoint_s { float acceleration[3]; /**< x acc, y acc, z acc */ float attitude[4]; /**< attitude of vehicle (quaternion) */ float attitude_rate[3]; /**< body angular rates (x, y, z) */ + float thrust; /**< thrust */ + + uint16_t ignore; /**< if field i is set to true, the value should be ignored, see definition at top of file + for mapping */ - uint16_t ignore; /**< if field i is set to true, pi should be ignored */ - //XXX define constants for bit offsets bool isForceSetpoint; /**< the acceleration vector should be interpreted as force */ float override_mode_switch; @@ -108,13 +127,25 @@ struct offboard_control_setpoint_s { * Returns true if the position setpoint at index should be ignored */ inline bool offboard_control_sp_ignore_position(const struct offboard_control_setpoint_s &offboard_control_sp, int index) { - return (bool)(offboard_control_sp.ignore & (1 << index)); + return (bool)(offboard_control_sp.ignore & (1 << (OFB_IGN_BIT_POS_X + index))); } /** * Returns true if all position setpoints should be ignored */ inline bool offboard_control_sp_ignore_position_all(const struct offboard_control_setpoint_s &offboard_control_sp) { + for (int i = 0; i < 3; i++) { + if (!offboard_control_sp_ignore_position(offboard_control_sp, i)) { + return false; + } + } + return true; +} + +/** + * Returns true if some position setpoints should be ignored + */ +inline bool offboard_control_sp_ignore_position_some(const struct offboard_control_setpoint_s &offboard_control_sp) { for (int i = 0; i < 3; i++) { if (offboard_control_sp_ignore_position(offboard_control_sp, i)) { return true; @@ -127,13 +158,25 @@ inline bool offboard_control_sp_ignore_position_all(const struct offboard_contro * Returns true if the velocity setpoint at index should be ignored */ inline bool offboard_control_sp_ignore_velocity(const struct offboard_control_setpoint_s &offboard_control_sp, int index) { - return (bool)(offboard_control_sp.ignore & (1 << (3 + index))); + return (bool)(offboard_control_sp.ignore & (1 << (OFB_IGN_BIT_VEL_X + index))); } /** * Returns true if all velocity setpoints should be ignored */ inline bool offboard_control_sp_ignore_velocity_all(const struct offboard_control_setpoint_s &offboard_control_sp) { + for (int i = 0; i < 3; i++) { + if (!offboard_control_sp_ignore_velocity(offboard_control_sp, i)) { + return false; + } + } + return true; +} + +/** + * Returns true if some velocity setpoints should be ignored + */ +inline bool offboard_control_sp_ignore_velocity_some(const struct offboard_control_setpoint_s &offboard_control_sp) { for (int i = 0; i < 3; i++) { if (offboard_control_sp_ignore_velocity(offboard_control_sp, i)) { return true; @@ -146,13 +189,25 @@ inline bool offboard_control_sp_ignore_velocity_all(const struct offboard_contro * Returns true if the acceleration setpoint at index should be ignored */ inline bool offboard_control_sp_ignore_acceleration(const struct offboard_control_setpoint_s &offboard_control_sp, int index) { - return (bool)(offboard_control_sp.ignore & (1 << (6 + index))); + return (bool)(offboard_control_sp.ignore & (1 << (OFB_IGN_BIT_ACC_X + index))); } /** * Returns true if all acceleration setpoints should be ignored */ inline bool offboard_control_sp_ignore_acceleration_all(const struct offboard_control_setpoint_s &offboard_control_sp) { + for (int i = 0; i < 3; i++) { + if (!offboard_control_sp_ignore_acceleration(offboard_control_sp, i)) { + return false; + } + } + return true; +} + +/** + * Returns true if some acceleration setpoints should be ignored + */ +inline bool offboard_control_sp_ignore_acceleration_some(const struct offboard_control_setpoint_s &offboard_control_sp) { for (int i = 0; i < 3; i++) { if (offboard_control_sp_ignore_acceleration(offboard_control_sp, i)) { return true; @@ -165,14 +220,33 @@ inline bool offboard_control_sp_ignore_acceleration_all(const struct offboard_co * Returns true if the bodyrate setpoint at index should be ignored */ inline bool offboard_control_sp_ignore_bodyrates(const struct offboard_control_setpoint_s &offboard_control_sp, int index) { - return (bool)(offboard_control_sp.ignore & (1 << (9 + index))); + return (bool)(offboard_control_sp.ignore & (1 << (OFB_IGN_BIT_BODYRATE_X + index))); +} + +/** + * Returns true if some of the bodyrate setpoints should be ignored + */ +inline bool offboard_control_sp_ignore_bodyrates_some(const struct offboard_control_setpoint_s &offboard_control_sp) { + for (int i = 0; i < 3; i++) { + if (offboard_control_sp_ignore_bodyrates(offboard_control_sp, i)) { + return true; + } + } + return false; } /** * Returns true if the attitude setpoint should be ignored */ inline bool offboard_control_sp_ignore_attitude(const struct offboard_control_setpoint_s &offboard_control_sp) { - return (bool)(offboard_control_sp.ignore & (1 << 10)); + return (bool)(offboard_control_sp.ignore & (1 << OFB_IGN_BIT_ATT)); +} + +/** + * Returns true if the thrust setpoint should be ignored + */ +inline bool offboard_control_sp_ignore_thrust(const struct offboard_control_setpoint_s &offboard_control_sp) { + return (bool)(offboard_control_sp.ignore & (1 << OFB_IGN_BIT_THRUST)); } -- 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(-) (limited to 'src') 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(-) (limited to 'src') 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(-) (limited to 'src') 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(+) (limited to 'src') 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(-) (limited to 'src') 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(-) (limited to 'src') 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(-) (limited to 'src') 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 57a250f9461f0ab8fe10cf6988f9e2dfd269bdf2 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 19 Aug 2014 09:23:41 +0200 Subject: navigator: remove offboard mode offboard setpoints are now forwarded directly from mavlink --- src/modules/navigator/module.mk | 1 - src/modules/navigator/navigator.h | 6 +- src/modules/navigator/navigator_main.cpp | 13 +-- src/modules/navigator/offboard.cpp | 142 ------------------------------- src/modules/navigator/offboard.h | 72 ---------------- 5 files changed, 4 insertions(+), 230 deletions(-) delete mode 100644 src/modules/navigator/offboard.cpp delete mode 100644 src/modules/navigator/offboard.h (limited to 'src') diff --git a/src/modules/navigator/module.mk b/src/modules/navigator/module.mk index b50198996..5d680948d 100644 --- a/src/modules/navigator/module.mk +++ b/src/modules/navigator/module.mk @@ -46,7 +46,6 @@ SRCS = navigator_main.cpp \ loiter.cpp \ rtl.cpp \ rtl_params.c \ - offboard.cpp \ mission_feasibility_checker.cpp \ geofence.cpp \ geofence_params.c diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index 8edbb63b3..742cd59ca 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -56,14 +56,13 @@ #include "mission.h" #include "loiter.h" #include "rtl.h" -#include "offboard.h" #include "geofence.h" /** * Number of navigation modes that need on_active/on_inactive calls * Currently: mission, loiter, and rtl */ -#define NAVIGATOR_MODE_ARRAY_SIZE 4 +#define NAVIGATOR_MODE_ARRAY_SIZE 3 class Navigator : public control::SuperBlock { @@ -116,7 +115,6 @@ public: struct position_setpoint_triplet_s* get_position_setpoint_triplet() { return &_pos_sp_triplet; } int get_onboard_mission_sub() { return _onboard_mission_sub; } int get_offboard_mission_sub() { return _offboard_mission_sub; } - int get_offboard_control_sp_sub() { return _offboard_control_sp_sub; } Geofence& get_geofence() { return _geofence; } bool get_can_loiter_at_sp() { return _can_loiter_at_sp; } float get_loiter_radius() { return _param_loiter_radius.get(); } @@ -134,7 +132,6 @@ private: int _home_pos_sub; /**< home position subscription */ int _vstatus_sub; /**< vehicle status subscription */ int _capabilities_sub; /**< notification of vehicle capabilities updates */ - int _offboard_control_sp_sub; /*** offboard control subscription */ int _control_mode_sub; /**< vehicle control mode subscription */ int _onboard_mission_sub; /**< onboard mission subscription */ int _offboard_mission_sub; /**< offboard mission subscription */ @@ -164,7 +161,6 @@ private: Mission _mission; /**< class that handles the missions */ Loiter _loiter; /**< class that handles loiter */ RTL _rtl; /**< class that handles RTL */ - Offboard _offboard; /**< class that handles offboard */ NavigatorMode *_navigation_mode_array[NAVIGATOR_MODE_ARRAY_SIZE]; /**< array of navigation modes */ diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 331a9a728..f11ae72c5 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -67,7 +67,6 @@ #include #include #include -#include #include #include @@ -101,7 +100,6 @@ Navigator::Navigator() : _home_pos_sub(-1), _vstatus_sub(-1), _capabilities_sub(-1), - _offboard_control_sp_sub(-1), _control_mode_sub(-1), _onboard_mission_sub(-1), _offboard_mission_sub(-1), @@ -124,7 +122,6 @@ Navigator::Navigator() : _mission(this, "MIS"), _loiter(this, "LOI"), _rtl(this, "RTL"), - _offboard(this, "OFF"), _can_loiter_at_sp(false), _pos_sp_triplet_updated(false), _param_loiter_radius(this, "LOITER_RAD"), @@ -134,7 +131,6 @@ Navigator::Navigator() : _navigation_mode_array[0] = &_mission; _navigation_mode_array[1] = &_loiter; _navigation_mode_array[2] = &_rtl; - _navigation_mode_array[3] = &_offboard; updateParams(); } @@ -247,7 +243,6 @@ Navigator::task_main() _onboard_mission_sub = orb_subscribe(ORB_ID(onboard_mission)); _offboard_mission_sub = orb_subscribe(ORB_ID(offboard_mission)); _param_update_sub = orb_subscribe(ORB_ID(parameter_update)); - _offboard_control_sp_sub = orb_subscribe(ORB_ID(offboard_control_setpoint)); /* copy all topics first time */ vehicle_status_update(); @@ -353,6 +348,9 @@ Navigator::task_main() case NAVIGATION_STATE_ACRO: case NAVIGATION_STATE_ALTCTL: case NAVIGATION_STATE_POSCTL: + case NAVIGATION_STATE_LAND: + case NAVIGATION_STATE_TERMINATION: + case NAVIGATION_STATE_OFFBOARD: _navigation_mode = nullptr; _can_loiter_at_sp = false; break; @@ -368,11 +366,6 @@ Navigator::task_main() case NAVIGATION_STATE_AUTO_RTGS: _navigation_mode = &_rtl; /* TODO: change this to something else */ break; - case NAVIGATION_STATE_LAND: - case NAVIGATION_STATE_TERMINATION: - case NAVIGATION_STATE_OFFBOARD: - _navigation_mode = &_offboard; - break; default: _navigation_mode = nullptr; _can_loiter_at_sp = false; diff --git a/src/modules/navigator/offboard.cpp b/src/modules/navigator/offboard.cpp deleted file mode 100644 index fc4d183cd..000000000 --- a/src/modules/navigator/offboard.cpp +++ /dev/null @@ -1,142 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2014 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ -/** - * @file offboard.cpp - * - * Helper class for offboard commands - * - * @author Julian Oes - */ - -#include -#include -#include -#include -#include - -#include -#include - -#include -#include - -#include "navigator.h" -#include "offboard.h" - -Offboard::Offboard(Navigator *navigator, const char *name) : - NavigatorMode(navigator, name), - _offboard_control_sp({0}) -{ - /* load initial params */ - updateParams(); - /* initial reset */ - on_inactive(); -} - -Offboard::~Offboard() -{ -} - -void -Offboard::on_activation() -{ -} - -void -Offboard::on_active() -{ - struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); - - bool updated; - orb_check(_navigator->get_offboard_control_sp_sub(), &updated); - if (updated) { - update_offboard_control_setpoint(); - } - - /* copy offboard setpoints to the corresponding topics */ - if (_offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED) { - /* We accept position control only if none of the directions is ignored (as pos_sp_triplet does not - * support deactivation of individual directions) */ - if (_navigator->get_control_mode()->flag_control_position_enabled && - (!offboard_control_sp_ignore_position(_offboard_control_sp, 0) && - !offboard_control_sp_ignore_position(_offboard_control_sp, 1) && - !offboard_control_sp_ignore_position(_offboard_control_sp, 2))) { - /* position control */ - pos_sp_triplet->current.x = _offboard_control_sp.position[0]; - pos_sp_triplet->current.y = _offboard_control_sp.position[1]; - //pos_sp_triplet->current.yaw = _offboard_control_sp.position[2]; - //XXX: copy yaw - pos_sp_triplet->current.z = -_offboard_control_sp.position[2]; - - pos_sp_triplet->current.type = SETPOINT_TYPE_OFFBOARD; - pos_sp_triplet->current.valid = true; - pos_sp_triplet->current.position_valid = true; - - _navigator->set_position_setpoint_triplet_updated(); - } - /* We accept velocity control only if none of the directions is ignored (as pos_sp_triplet does not - * support deactivation of individual directions) */ - if (_navigator->get_control_mode()->flag_control_velocity_enabled && - (!offboard_control_sp_ignore_velocity(_offboard_control_sp, 0) && - !offboard_control_sp_ignore_velocity(_offboard_control_sp, 1) && - !offboard_control_sp_ignore_velocity(_offboard_control_sp, 2))) { - /* velocity control */ - pos_sp_triplet->current.vx = _offboard_control_sp.velocity[0]; - pos_sp_triplet->current.vy = _offboard_control_sp.velocity[1]; -// pos_sp_triplet->current.yawspeed = _offboard_control_sp.velocity[; -// //XXX: copy yaw speed - pos_sp_triplet->current.vz = _offboard_control_sp.velocity[2]; - - pos_sp_triplet->current.type = SETPOINT_TYPE_OFFBOARD; - pos_sp_triplet->current.valid = true; - pos_sp_triplet->current.velocity_valid = true; - - _navigator->set_position_setpoint_triplet_updated(); - } - - //XXX: map acceleration setpoint once supported in setpoint triplet - } - -} - -void -Offboard::on_inactive() -{ -} - -void -Offboard::update_offboard_control_setpoint() -{ - orb_copy(ORB_ID(offboard_control_setpoint), _navigator->get_offboard_control_sp_sub(), &_offboard_control_sp); - -} diff --git a/src/modules/navigator/offboard.h b/src/modules/navigator/offboard.h deleted file mode 100644 index 66b923bdb..000000000 --- a/src/modules/navigator/offboard.h +++ /dev/null @@ -1,72 +0,0 @@ -/*************************************************************************** - * - * Copyright (c) 2014 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ -/** - * @file offboard.h - * - * Helper class for offboard commands - * - * @author Julian Oes - */ - -#ifndef NAVIGATOR_OFFBOARD_H -#define NAVIGATOR_OFFBOARD_H - -#include -#include - -#include -#include - -#include "navigator_mode.h" - -class Navigator; - -class Offboard : public NavigatorMode -{ -public: - Offboard(Navigator *navigator, const char *name); - - ~Offboard(); - - virtual void on_inactive(); - - virtual void on_activation(); - - virtual void on_active(); -private: - void update_offboard_control_setpoint(); - - struct offboard_control_setpoint_s _offboard_control_sp; -}; - -#endif -- 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(-) (limited to 'src') 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(+) (limited to 'src') 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(-) (limited to 'src') 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(-) (limited to 'src') 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(-) (limited to 'src') 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(-) (limited to 'src') 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 1e7dee439111f58fa9c0f737b93132a57dbf644d Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 21 Aug 2014 16:24:00 +0200 Subject: set_attitude_target msg: convert quaternion to R --- src/modules/mavlink/mavlink_receiver.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) (limited to 'src') diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index da594412e..60fc2937a 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -689,9 +689,11 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg) att_sp.timestamp = hrt_absolute_time(); mavlink_quaternion_to_euler(set_attitude_target.q, &att_sp.roll_body, &att_sp.pitch_body, &att_sp.yaw_body); + mavlink_quaternion_to_dcm(set_attitude_target.q, att_sp.R_body); + att_sp.R_valid = true; att_sp.thrust = set_attitude_target.thrust; - att_sp.q_d_valid = true; memcpy(att_sp.q_d, set_attitude_target.q, sizeof(att_sp.q_d)); + att_sp.q_d_valid = true; if (_att_sp_pub < 0) { _att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp); } else { -- 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(-) (limited to 'src') 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(-) (limited to 'src') 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(-) (limited to 'src') 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(-) (limited to 'src') 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(-) (limited to 'src') 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(-) (limited to 'src') 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(-) (limited to 'src') 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(-) (limited to 'src') 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 From 8f2fa6da25ffcb83b27e732e4d22b845a652500f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 24 Aug 2014 14:40:39 +0200 Subject: Adjust rates for attitude and attitude SP --- src/modules/mavlink/mavlink_main.cpp | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) (limited to 'src') diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index ed7e879d3..d5c68c011 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1387,23 +1387,26 @@ Mavlink::task_main(int argc, char *argv[]) configure_stream("SYS_STATUS", 1.0f); configure_stream("GPS_GLOBAL_ORIGIN", 0.5f); configure_stream("HIGHRES_IMU", 1.0f); - configure_stream("ATTITUDE", 10.0f); + configure_stream("ATTITUDE", 15.0f); configure_stream("VFR_HUD", 8.0f); configure_stream("GPS_RAW_INT", 1.0f); configure_stream("GLOBAL_POSITION_INT", 3.0f); configure_stream("LOCAL_POSITION_NED", 3.0f); configure_stream("RC_CHANNELS_RAW", 1.0f); configure_stream("POSITION_TARGET_GLOBAL_INT", 3.0f); - configure_stream("ATTITUDE_TARGET", 3.0f); + configure_stream("ATTITUDE_TARGET", 15.0f); configure_stream("DISTANCE_SENSOR", 0.5f); configure_stream("OPTICAL_FLOW", 0.5f); break; case MAVLINK_MODE_ONBOARD: configure_stream("SYS_STATUS", 1.0f); - configure_stream("ATTITUDE", 15.0f); + // XXX OBC change back + configure_stream("ATTITUDE", 50.0f); configure_stream("GLOBAL_POSITION_INT", 15.0f); configure_stream("CAMERA_CAPTURE", 1.0f); + configure_stream("ATTITUDE_TARGET", 50.0f); + configure_stream("POSITION_TARGET_GLOBAL_INT", 20.0f); break; default: -- cgit v1.2.3 From 55fde23233de3d311d6c8d0b2cd368e45af3caac Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 25 Aug 2014 09:19:36 +0200 Subject: support new yaw and yawrate fields in mavlnk position_target message --- src/modules/mavlink/mavlink_receiver.cpp | 35 +++++++++++++++++++--- .../uORB/topics/offboard_control_setpoint.h | 22 +++++++++++++- .../uORB/topics/position_setpoint_triplet.h | 2 ++ 3 files changed, 54 insertions(+), 5 deletions(-) (limited to 'src') diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 60fc2937a..3ac4d3b03 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -474,6 +474,8 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t offboard_control_sp.acceleration[0] = set_position_target_local_ned.afx; offboard_control_sp.acceleration[1] = set_position_target_local_ned.afy; offboard_control_sp.acceleration[2] = set_position_target_local_ned.afz; + offboard_control_sp.yaw = set_position_target_local_ned.yaw; + offboard_control_sp.yaw_rate = set_position_target_local_ned.yaw_rate; offboard_control_sp.isForceSetpoint = (bool)(set_position_target_local_ned.type_mask & (1 << 9)); /* If we are in force control mode, for now set offboard mode to force control */ @@ -486,7 +488,12 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t offboard_control_sp.ignore &= ~(1 << i); offboard_control_sp.ignore |= (set_position_target_local_ned.type_mask & (1 << i)); } - + offboard_control_sp.ignore &= ~(1 << OFB_IGN_BIT_YAW); + offboard_control_sp.ignore |= (set_position_target_local_ned.type_mask & (1 << 10)) << + OFB_IGN_BIT_YAW; + offboard_control_sp.ignore &= ~(1 << OFB_IGN_BIT_YAWRATE); + offboard_control_sp.ignore |= (set_position_target_local_ned.type_mask & (1 << 11)) << + OFB_IGN_BIT_YAWRATE; offboard_control_sp.timestamp = hrt_absolute_time(); @@ -527,12 +534,12 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t pos_sp_triplet.previous.valid = false; pos_sp_triplet.next.valid = false; pos_sp_triplet.current.valid = true; + pos_sp_triplet.current.type = SETPOINT_TYPE_POSITION; //XXX support others /* set the local pos values if the setpoint type is 'local pos' and none * of the local pos fields is set to 'ignore' */ if (offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED && !offboard_control_sp_ignore_position_some(offboard_control_sp)) { - pos_sp_triplet.current.type = SETPOINT_TYPE_POSITION; //XXX support others pos_sp_triplet.current.position_valid = true; pos_sp_triplet.current.x = offboard_control_sp.position[0]; pos_sp_triplet.current.y = offboard_control_sp.position[1]; @@ -545,7 +552,6 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t * of the local vel fields is set to 'ignore' */ if (offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED && !offboard_control_sp_ignore_velocity_some(offboard_control_sp)) { - pos_sp_triplet.current.type = SETPOINT_TYPE_POSITION; //XXX support others pos_sp_triplet.current.velocity_valid = true; pos_sp_triplet.current.vx = offboard_control_sp.velocity[0]; pos_sp_triplet.current.vy = offboard_control_sp.velocity[1]; @@ -558,7 +564,6 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t * of the accelerations fields is set to 'ignore' */ if (offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED && !offboard_control_sp_ignore_acceleration_some(offboard_control_sp)) { - pos_sp_triplet.current.type = SETPOINT_TYPE_POSITION; //XXX support others pos_sp_triplet.current.acceleration_valid = true; pos_sp_triplet.current.a_x = offboard_control_sp.acceleration[0]; pos_sp_triplet.current.a_y = offboard_control_sp.acceleration[1]; @@ -569,6 +574,28 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t } else { pos_sp_triplet.current.acceleration_valid = false; } + + /* set the yaw sp value if the setpoint type is 'local pos' and the yaw + * field is not set to 'ignore' */ + if (offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED && + !offboard_control_sp_ignore_yaw(offboard_control_sp)) { + pos_sp_triplet.current.yaw_valid = true; + pos_sp_triplet.current.yaw = offboard_control_sp.yaw; + + } else { + pos_sp_triplet.current.yaw_valid = false; + } + + /* set the yawrate sp value if the setpoint type is 'local pos' and the yawrate + * field is not set to 'ignore' */ + if (offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED && + !offboard_control_sp_ignore_yawrate(offboard_control_sp)) { + pos_sp_triplet.current.yawspeed_valid = true; + pos_sp_triplet.current.yawspeed = offboard_control_sp.yaw_rate; + + } else { + pos_sp_triplet.current.yawspeed_valid = false; + } //XXX handle global pos setpoints (different MAV frames) diff --git a/src/modules/uORB/topics/offboard_control_setpoint.h b/src/modules/uORB/topics/offboard_control_setpoint.h index 6e37896af..72a28e501 100644 --- a/src/modules/uORB/topics/offboard_control_setpoint.h +++ b/src/modules/uORB/topics/offboard_control_setpoint.h @@ -86,7 +86,9 @@ enum {OFB_IGN_BIT_POS_X, OFB_IGN_BIT_BODYRATE_Y, OFB_IGN_BIT_BODYRATE_Z, OFB_IGN_BIT_ATT, - OFB_IGN_BIT_THRUST + OFB_IGN_BIT_THRUST, + OFB_IGN_BIT_YAW, + OFB_IGN_BIT_YAWRATE, }; /** @@ -105,6 +107,10 @@ struct offboard_control_setpoint_s { float attitude[4]; /**< attitude of vehicle (quaternion) */ float attitude_rate[3]; /**< body angular rates (x, y, z) */ float thrust; /**< thrust */ + float yaw; /**< yaw: this is the yaw from the position_target message + (not from the full attitude_target message) */ + float yaw_rate; /**< yaw rate: this is the yaw from the position_target message + (not from the full attitude_target message) */ uint16_t ignore; /**< if field i is set to true, the value should be ignored, see definition at top of file for mapping */ @@ -249,6 +255,20 @@ inline bool offboard_control_sp_ignore_thrust(const struct offboard_control_setp return (bool)(offboard_control_sp.ignore & (1 << OFB_IGN_BIT_THRUST)); } +/** + * Returns true if the yaw setpoint should be ignored + */ +inline bool offboard_control_sp_ignore_yaw(const struct offboard_control_setpoint_s &offboard_control_sp) { + return (bool)(offboard_control_sp.ignore & (1 << OFB_IGN_BIT_YAW)); +} + +/** + * Returns true if the yaw rate setpoint should be ignored + */ +inline bool offboard_control_sp_ignore_yawrate(const struct offboard_control_setpoint_s &offboard_control_sp) { + return (bool)(offboard_control_sp.ignore & (1 << OFB_IGN_BIT_YAWRATE)); +} + /* register this as object request broker structure */ ORB_DECLARE(offboard_control_setpoint); diff --git a/src/modules/uORB/topics/position_setpoint_triplet.h b/src/modules/uORB/topics/position_setpoint_triplet.h index 3ea3f671e..cb2262534 100644 --- a/src/modules/uORB/topics/position_setpoint_triplet.h +++ b/src/modules/uORB/topics/position_setpoint_triplet.h @@ -79,7 +79,9 @@ struct position_setpoint_s double lon; /**< longitude, in deg */ float alt; /**< altitude AMSL, in m */ float yaw; /**< yaw (only for multirotors), in rad [-PI..PI), NaN = hold current yaw */ + bool yaw_valid; /**< true if yaw setpoint valid */ float yawspeed; /**< yawspeed (only for multirotors, in rad/s) */ + bool yawspeed_valid; /**< true if yawspeed setpoint valid */ float loiter_radius; /**< loiter radius (only for fixed wing), in m */ int8_t loiter_direction; /**< loiter direction: 1 = CW, -1 = CCW */ float pitch_min; /**< minimal pitch angle for fixed wing takeoff waypoints */ -- cgit v1.2.3 From 9bda573151ae1b5fa87686ee58596ea14e052941 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 25 Aug 2014 13:15:30 +0200 Subject: mc pos control: offboard: set yaw and yawspeed depending on valid flags --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) (limited to 'src') diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index ecc40428d..4f5c62a59 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -614,7 +614,6 @@ MulticopterPositionControl::control_offboard(float dt) _pos_sp(0) = _pos_sp_triplet.current.x; _pos_sp(1) = _pos_sp_triplet.current.y; _pos_sp(2) = _pos_sp_triplet.current.z; - _att_sp.yaw_body = _pos_sp_triplet.current.yaw; } else if (_control_mode.flag_control_velocity_enabled && _pos_sp_triplet.current.velocity_valid) { /* control velocity */ @@ -624,6 +623,11 @@ MulticopterPositionControl::control_offboard(float dt) /* set position setpoint move rate */ _sp_move_rate(0) = _pos_sp_triplet.current.vx; _sp_move_rate(1) = _pos_sp_triplet.current.vy; + } + + if (_pos_sp_triplet.current.yaw_valid) { + _att_sp.yaw_body = _pos_sp_triplet.current.yaw; + } else if (_pos_sp_triplet.current.yawspeed_valid) { _att_sp.yaw_body = _att_sp.yaw_body + _pos_sp_triplet.current.yawspeed * dt; } -- cgit v1.2.3 From 592f6f2bcb632d6bb86c63b1055560d0c8b526b2 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 30 Aug 2014 16:44:30 +0200 Subject: Revert "Fix drop offset: We want to drop so that the wind carries the bottle into the drop zone" This reverts commit ef0a0a1a6e86aff79a0fd8829ca5c86244fa39bc. --- src/modules/bottle_drop/bottle_drop.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) (limited to 'src') diff --git a/src/modules/bottle_drop/bottle_drop.cpp b/src/modules/bottle_drop/bottle_drop.cpp index 65eca1a62..3eec5a879 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 From 056693df44561a8d1e19501585aa5f680b7aa086 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 31 Aug 2014 15:55:03 +0200 Subject: Add drag coefficients to adjust bottle drop to other objects to ease testing --- src/modules/bottle_drop/bottle_drop.cpp | 10 +++- src/modules/bottle_drop/bottle_drop_params.c | 89 +++++++++++++++++++++++++++- 2 files changed, 95 insertions(+), 4 deletions(-) (limited to 'src') diff --git a/src/modules/bottle_drop/bottle_drop.cpp b/src/modules/bottle_drop/bottle_drop.cpp index 3eec5a879..30ee782cc 100644 --- a/src/modules/bottle_drop/bottle_drop.cpp +++ b/src/modules/bottle_drop/bottle_drop.cpp @@ -392,11 +392,17 @@ BottleDrop::task_main() param_t param_gproperties = param_find("BD_GPROPERTIES"); param_t param_turn_radius = param_find("BD_TURNRADIUS"); param_t param_precision = param_find("BD_PRECISION"); + param_t param_cd = param_find("BD_OBJ_CD"); + param_t param_mass = param_find("BD_OBJ_MASS"); + param_t param_surface = param_find("BD_OBJ_SURFACE"); param_get(param_precision, &precision); param_get(param_turn_radius, &turn_radius); param_get(param_gproperties, &z_0); + param_get(param_cd, &cd); + param_get(param_mass, &M); + param_get(param_surface, &A); int vehicle_global_position_sub = orb_subscribe(ORB_ID(vehicle_global_position)); @@ -419,10 +425,10 @@ BottleDrop::task_main() struct wind_estimate_s wind; - /* wakeup source(s) */ + // wakeup source(s) struct pollfd fds[1]; - /* Setup of loop */ + // Setup of loop fds[0].fd = _command_sub; fds[0].events = POLLIN; diff --git a/src/modules/bottle_drop/bottle_drop_params.c b/src/modules/bottle_drop/bottle_drop_params.c index 22e9baf8a..e5d35bf0a 100644 --- a/src/modules/bottle_drop/bottle_drop_params.c +++ b/src/modules/bottle_drop/bottle_drop_params.c @@ -41,6 +41,91 @@ #include #include +/** + * Ground drag property + * + * This parameter encodes the ground drag coefficient and the corresponding + * decrease in wind speed from the plane altitude to ground altitude. + * + * @unit unknown + * @min 0.001 + * @max 0.1 + * @group Payload drop + */ PARAM_DEFINE_FLOAT(BD_GPROPERTIES, 0.03f); -PARAM_DEFINE_FLOAT(BD_TURNRADIUS, 90.0f); -PARAM_DEFINE_FLOAT(BD_PRECISION, 10.0f); + +/** + * Plane turn radius + * + * The planes known minimal turn radius - use a higher value + * to make the plane maneuver more distant from the actual drop + * position. This is to ensure the wings are level during the drop. + * + * @unit meter + * @min 30.0 + * @max 500.0 + * @group Payload drop + */ +PARAM_DEFINE_FLOAT(BD_TURNRADIUS, 120.0f); + +/** + * Drop precision + * + * If the system is closer than this distance on passing over the + * drop position, it will release the payload. This is a safeguard + * to prevent a drop out of the required accuracy. + * + * @unit meter + * @min 1.0 + * @max 80.0 + * @group Payload drop + */ +PARAM_DEFINE_FLOAT(BD_PRECISION, 30.0f); + +/** + * Payload drag coefficient of the dropped object + * + * The drag coefficient (cd) is the typical drag + * constant for air. It is in general object specific, + * but the closest primitive shape to the actual object + * should give good results: + * http://en.wikipedia.org/wiki/Drag_coefficient + * + * @unit meter + * @min 1.0 + * @max 80.0 + * @group Payload drop + */ +PARAM_DEFINE_FLOAT(BD_OBJ_CD, 0.86f); + +/** + * Payload mass + * + * A typical small toy ball: + * 0.025 kg + * + * OBC water bottle: + * 0.6 kg + * + * @unit kilogram + * @min 0.001 + * @max 5.0 + * @group Payload drop + */ +PARAM_DEFINE_FLOAT(BD_OBJ_MASS, 0.6f); + +/** + * Payload front surface area + * + * A typical small toy ball: + * (0.045 * 0.045) / 4.0 * pi = 0.001590 m^2 + * + * OBC water bottle: + * (0.063 * 0.063) / 4.0 * pi = 0.003117 m^2 + * + * @unit m^2 + * @min 0.001 + * @max 0.5 + * @group Payload drop + */ +PARAM_DEFINE_FLOAT(BD_OBJ_SURFACE, 0.00311724531f); -- cgit v1.2.3 From 706e5809a14ed57f75724a2f5aabdfda157fac5d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 31 Aug 2014 17:02:33 +0200 Subject: Fix variable name in param --- src/modules/bottle_drop/bottle_drop.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src') diff --git a/src/modules/bottle_drop/bottle_drop.cpp b/src/modules/bottle_drop/bottle_drop.cpp index 30ee782cc..de3f20a95 100644 --- a/src/modules/bottle_drop/bottle_drop.cpp +++ b/src/modules/bottle_drop/bottle_drop.cpp @@ -401,7 +401,7 @@ BottleDrop::task_main() param_get(param_turn_radius, &turn_radius); param_get(param_gproperties, &z_0); param_get(param_cd, &cd); - param_get(param_mass, &M); + param_get(param_mass, &m); param_get(param_surface, &A); int vehicle_global_position_sub = orb_subscribe(ORB_ID(vehicle_global_position)); -- cgit v1.2.3 From 63e741824cfde42ac563c457e615cf58f10a2529 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 1 Sep 2014 11:27:28 +0200 Subject: fw landing: use terrain estimate from global pos --- .../fw_pos_control_l1/fw_pos_control_l1_main.cpp | 82 +++++++--------------- 1 file changed, 24 insertions(+), 58 deletions(-) (limited to 'src') diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index deccab482..064b8a6fa 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -88,7 +88,6 @@ #include #include #include -#include #include "landingslope.h" #include "mtecs/mTecs.h" @@ -143,7 +142,6 @@ private: int _params_sub; /**< notification of parameter updates */ int _manual_control_sub; /**< notification of manual control updates */ int _sensor_combined_sub; /**< for body frame accelerations */ - int _range_finder_sub; /**< range finder subscription */ orb_advert_t _attitude_sp_pub; /**< attitude setpoint */ orb_advert_t _tecs_status_pub; /**< TECS status publication */ @@ -158,7 +156,6 @@ private: struct vehicle_global_position_s _global_pos; /**< global vehicle position */ struct position_setpoint_triplet_s _pos_sp_triplet; /**< triplet of mission items */ struct sensor_combined_s _sensor_combined; /**< for body frame accelerations */ - struct range_finder_report _range_finder; /**< range finder report */ perf_counter_t _loop_perf; /**< loop performance counter */ @@ -239,7 +236,6 @@ private: float land_flare_alt_relative; float land_thrust_lim_alt_relative; float land_heading_hold_horizontal_distance; - float range_finder_rel_alt; } _parameters; /**< local copies of interesting parameters */ @@ -284,7 +280,6 @@ private: param_t land_flare_alt_relative; param_t land_thrust_lim_alt_relative; param_t land_heading_hold_horizontal_distance; - param_t range_finder_rel_alt; } _parameter_handles; /**< handles for interesting parameters */ @@ -310,12 +305,6 @@ private: */ bool vehicle_airspeed_poll(); - /** - * Check for range finder updates. - */ - bool range_finder_poll(); - - /** * Check for position updates. */ @@ -337,9 +326,9 @@ private: void navigation_capabilities_publish(); /** - * Get the relative alt either from the difference between estimate and waypoint or from the laser range finder + * Return the terrain estimate during landing: uses the wp altitude value or the terrain estimate if available */ - float get_relative_landingalt(float land_setpoint_alt, float current_alt, const struct range_finder_report &range_finder, float range_finder_use_relative_alt); + float get_terrain_altitude_landing(float land_setpoint_alt, const struct vehicle_global_position_s &global_pos); /** * Control position. @@ -411,7 +400,6 @@ FixedwingPositionControl::FixedwingPositionControl() : _params_sub(-1), _manual_control_sub(-1), _sensor_combined_sub(-1), - _range_finder_sub(-1), /* publications */ _attitude_sp_pub(-1), @@ -428,7 +416,6 @@ FixedwingPositionControl::FixedwingPositionControl() : _global_pos(), _pos_sp_triplet(), _sensor_combined(), - _range_finder(), /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "fw l1 control")), @@ -477,7 +464,6 @@ FixedwingPositionControl::FixedwingPositionControl() : _parameter_handles.land_flare_alt_relative = param_find("FW_LND_FLALT"); _parameter_handles.land_thrust_lim_alt_relative = param_find("FW_LND_TLALT"); _parameter_handles.land_heading_hold_horizontal_distance = param_find("FW_LND_HHDIST"); - _parameter_handles.range_finder_rel_alt = param_find("FW_LND_RFRALT"); _parameter_handles.time_const = param_find("FW_T_TIME_CONST"); _parameter_handles.time_const_throt = param_find("FW_T_THRO_CONST"); @@ -577,8 +563,6 @@ FixedwingPositionControl::parameters_update() param_get(_parameter_handles.land_heading_hold_horizontal_distance, &(_parameters.land_heading_hold_horizontal_distance)); - param_get(_parameter_handles.range_finder_rel_alt, &(_parameters.range_finder_rel_alt)); - _l1_control.set_l1_damping(_parameters.l1_damping); _l1_control.set_l1_period(_parameters.l1_period); _l1_control.set_l1_roll_limit(math::radians(_parameters.roll_limit)); @@ -669,20 +653,6 @@ FixedwingPositionControl::vehicle_airspeed_poll() return airspeed_updated; } -bool -FixedwingPositionControl::range_finder_poll() -{ - /* check if there is a range finder measurement */ - bool range_finder_updated; - orb_check(_range_finder_sub, &range_finder_updated); - - if (range_finder_updated) { - orb_copy(ORB_ID(sensor_range_finder), _range_finder_sub, &_range_finder); - } - - return range_finder_updated; -} - void FixedwingPositionControl::vehicle_attitude_poll() { @@ -820,21 +790,14 @@ void FixedwingPositionControl::navigation_capabilities_publish() } } -float FixedwingPositionControl::get_relative_landingalt(float land_setpoint_alt, float current_alt, const struct range_finder_report &range_finder, float range_finder_use_relative_alt) +float FixedwingPositionControl::get_terrain_altitude_landing(float land_setpoint_alt, const struct vehicle_global_position_s &global_pos) { - float rel_alt_estimated = current_alt - land_setpoint_alt; - - /* only use range finder if: - * parameter (range_finder_use_relative_alt) > 0 - * the measurement is valid - * the estimated relative altitude (from global altitude estimate and landing waypoint) <= range_finder_use_relative_alt - */ - if (range_finder_use_relative_alt < 0 || !range_finder.valid || range_finder.distance > range_finder_use_relative_alt ) { - return rel_alt_estimated; + if (global_pos.terrain_alt_valid && + isfinite(global_pos.terrain_alt)) { + return global_pos.terrain_alt; + } else { + return land_setpoint_alt; } - - return range_finder.distance; - } bool @@ -991,15 +954,19 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi float airspeed_land = 1.3f * _parameters.airspeed_min; float airspeed_approach = 1.3f * _parameters.airspeed_min; + /* Get an estimate of the terrain altitude if available, otherwise terrain_alt will be + * equal to _pos_sp_triplet.current.alt */ + float terrain_alt = get_terrain_altitude_landing(_pos_sp_triplet.current.alt, _global_pos); + /* Calculate distance (to landing waypoint) and altitude of last ordinary waypoint L */ - float L_altitude_rel = _pos_sp_triplet.previous.valid ? _pos_sp_triplet.previous.alt - _pos_sp_triplet.current.alt : 0.0f; + float L_altitude_rel = _pos_sp_triplet.previous.valid ? + _pos_sp_triplet.previous.alt - terrain_alt : 0.0f; float bearing_airplane_currwp = get_bearing_to_next_waypoint(current_position(0), current_position(1), curr_wp(0), curr_wp(1)); float landing_slope_alt_rel_desired = landingslope.getLandingSlopeRelativeAltitudeSave(wp_distance, bearing_lastwp_currwp, bearing_airplane_currwp); - float relative_alt = get_relative_landingalt(_pos_sp_triplet.current.alt, _global_pos.alt, _range_finder, _parameters.range_finder_rel_alt); - if ( (relative_alt < landingslope.flare_relative_alt()) || land_noreturn_vertical) { //checking for land_noreturn to avoid unwanted climb out + if ( (_global_pos.alt < terrain_alt + landingslope.flare_relative_alt()) || land_noreturn_vertical) { //checking for land_noreturn to avoid unwanted climb out /* land with minimal speed */ @@ -1009,7 +976,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* kill the throttle if param requests it */ throttle_max = _parameters.throttle_max; - if (relative_alt < landingslope.motor_lim_relative_alt() || land_motor_lim) { + if (_global_pos.alt < terrain_alt + landingslope.motor_lim_relative_alt() || land_motor_lim) { throttle_max = math::min(throttle_max, _parameters.throttle_land_max); if (!land_motor_lim) { land_motor_lim = true; @@ -1027,12 +994,12 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi land_stayonground = true; } - tecs_update_pitch_throttle(_pos_sp_triplet.current.alt + flare_curve_alt_rel, + tecs_update_pitch_throttle(terrain_alt + flare_curve_alt_rel, calculate_target_airspeed(airspeed_land), eas2tas, flare_pitch_angle_rad, math::radians(15.0f), 0.0f, throttle_max, throttle_land, false, flare_pitch_angle_rad, - _pos_sp_triplet.current.alt + relative_alt, ground_speed, + _global_pos.alt, ground_speed, land_motor_lim ? TECS_MODE_LAND_THROTTLELIM : TECS_MODE_LAND); if (!land_noreturn_vertical) { @@ -1053,8 +1020,8 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi * if current position is below the slope continue at previous wp altitude * until the intersection with slope * */ - float altitude_desired_rel = relative_alt; - if (relative_alt > landing_slope_alt_rel_desired || land_onslope) { + float altitude_desired_rel; + if (_global_pos.alt > terrain_alt + landing_slope_alt_rel_desired || land_onslope) { /* stay on slope */ altitude_desired_rel = landing_slope_alt_rel_desired; if (!land_onslope) { @@ -1063,10 +1030,11 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } } else { /* continue horizontally */ - altitude_desired_rel = _pos_sp_triplet.previous.valid ? L_altitude_rel : relative_alt; + altitude_desired_rel = _pos_sp_triplet.previous.valid ? L_altitude_rel : + _global_pos.alt - terrain_alt; } - tecs_update_pitch_throttle(_pos_sp_triplet.current.alt + altitude_desired_rel, + tecs_update_pitch_throttle(terrain_alt + altitude_desired_rel, calculate_target_airspeed(airspeed_approach), eas2tas, math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max), @@ -1075,7 +1043,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi _parameters.throttle_cruise, false, math::radians(_parameters.pitch_limit_min), - _pos_sp_triplet.current.alt + relative_alt, + _global_pos.alt, ground_speed); } @@ -1221,7 +1189,6 @@ FixedwingPositionControl::task_main() _airspeed_sub = orb_subscribe(ORB_ID(airspeed)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); _manual_control_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); - _range_finder_sub = orb_subscribe(ORB_ID(sensor_range_finder)); /* rate limit vehicle status updates to 5Hz */ orb_set_interval(_control_mode_sub, 200); @@ -1295,7 +1262,6 @@ FixedwingPositionControl::task_main() vehicle_setpoint_poll(); vehicle_sensor_combined_poll(); vehicle_airspeed_poll(); - range_finder_poll(); // vehicle_baro_poll(); math::Vector<3> ground_speed(_global_pos.vel_n, _global_pos.vel_e, _global_pos.vel_d); -- cgit v1.2.3 From 7f0ba70b1e8e86334d1acb827d8d2d4bf7305870 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 3 Sep 2014 08:39:26 +0200 Subject: fw landing: if using terrain estimate, don't switch back during landing --- .../fw_pos_control_l1/fw_pos_control_l1_main.cpp | 23 +++++++++++++--------- 1 file changed, 14 insertions(+), 9 deletions(-) (limited to 'src') diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 064b8a6fa..0db857727 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -160,12 +160,12 @@ private: perf_counter_t _loop_perf; /**< loop performance counter */ /* land states */ - /* not in non-abort mode for landing yet */ bool land_noreturn_horizontal; bool land_noreturn_vertical; bool land_stayonground; bool land_motor_lim; bool land_onslope; + bool land_useterrain; /* takeoff/launch states */ bool launch_detected; @@ -425,6 +425,7 @@ FixedwingPositionControl::FixedwingPositionControl() : land_stayonground(false), land_motor_lim(false), land_onslope(false), + land_useterrain(false), launch_detected(false), usePreTakeoffThrust(false), last_manual(false), @@ -792,8 +793,14 @@ void FixedwingPositionControl::navigation_capabilities_publish() float FixedwingPositionControl::get_terrain_altitude_landing(float land_setpoint_alt, const struct vehicle_global_position_s &global_pos) { - if (global_pos.terrain_alt_valid && - isfinite(global_pos.terrain_alt)) { + if (!isfinite(global_pos.terrain_alt)) { + return land_setpoint_alt; + } + + /* Decide if the terrain estimation can be used, once we switched to using the terrain we stick with it + * for the whole landing */ + if (global_pos.terrain_alt_valid || land_useterrain) { + land_useterrain = true; return global_pos.terrain_alt; } else { return land_setpoint_alt; @@ -941,11 +948,6 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* Vertical landing control */ //xxx: using the tecs altitude controller for slope control for now - -// /* do not go down too early */ -// if (wp_distance > 50.0f) { -// altitude_error = (_global_triplet.current.alt + 25.0f) - _global_pos.alt; -// } /* apply minimum pitch (flare) and limit roll if close to touch down, altitude error is negative (going down) */ // XXX this could make a great param @@ -967,7 +969,6 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi if ( (_global_pos.alt < terrain_alt + landingslope.flare_relative_alt()) || land_noreturn_vertical) { //checking for land_noreturn to avoid unwanted climb out - /* land with minimal speed */ // /* force TECS to only control speed with pitch, altitude is only implicitely controlled now */ @@ -993,6 +994,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi flare_curve_alt_rel = 0.0f; // stay on ground land_stayonground = true; } + warnx("flare: alt %4.3f, sp %4.3f", (double)_global_pos.alt, (double)(terrain_alt + flare_curve_alt_rel)); tecs_update_pitch_throttle(terrain_alt + flare_curve_alt_rel, calculate_target_airspeed(airspeed_land), eas2tas, @@ -1034,6 +1036,8 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi _global_pos.alt - terrain_alt; } + warnx("approach: alt %4.3f, sp %4.3f", (double)_global_pos.alt, (double)(terrain_alt + altitude_desired_rel)); + tecs_update_pitch_throttle(terrain_alt + altitude_desired_rel, calculate_target_airspeed(airspeed_approach), eas2tas, math::radians(_parameters.pitch_limit_min), @@ -1326,6 +1330,7 @@ void FixedwingPositionControl::reset_landing_state() land_stayonground = false; land_motor_lim = false; land_onslope = false; + land_useterrain = false; } void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_sp, float eas2tas, -- cgit v1.2.3 From 484f2864d188fa04cbbbd767278b547b7f9969af Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 3 Sep 2014 12:36:44 +0200 Subject: remove warnx --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 3 --- 1 file changed, 3 deletions(-) (limited to 'src') diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 0db857727..6e06b2b78 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -994,7 +994,6 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi flare_curve_alt_rel = 0.0f; // stay on ground land_stayonground = true; } - warnx("flare: alt %4.3f, sp %4.3f", (double)_global_pos.alt, (double)(terrain_alt + flare_curve_alt_rel)); tecs_update_pitch_throttle(terrain_alt + flare_curve_alt_rel, calculate_target_airspeed(airspeed_land), eas2tas, @@ -1036,8 +1035,6 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi _global_pos.alt - terrain_alt; } - warnx("approach: alt %4.3f, sp %4.3f", (double)_global_pos.alt, (double)(terrain_alt + altitude_desired_rel)); - tecs_update_pitch_throttle(terrain_alt + altitude_desired_rel, calculate_target_airspeed(airspeed_approach), eas2tas, math::radians(_parameters.pitch_limit_min), -- cgit v1.2.3 From 5134015800f014e1eeb4895718823d8b0ac508c1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 3 Sep 2014 18:15:12 +0200 Subject: always obey commands in bottle drop --- src/modules/bottle_drop/bottle_drop.cpp | 6 ++++++ 1 file changed, 6 insertions(+) (limited to 'src') diff --git a/src/modules/bottle_drop/bottle_drop.cpp b/src/modules/bottle_drop/bottle_drop.cpp index de3f20a95..0bb65252f 100644 --- a/src/modules/bottle_drop/bottle_drop.cpp +++ b/src/modules/bottle_drop/bottle_drop.cpp @@ -496,6 +496,12 @@ BottleDrop::task_main() param_get(param_precision, &precision); } + orb_check(_command_sub, &updated); + if (updated) { + orb_copy(ORB_ID(vehicle_command), _command_sub, &_command); + handle_command(&_command); + } + float windspeed_norm = sqrtf(wind.windspeed_north * wind.windspeed_north + wind.windspeed_east * wind.windspeed_east); float groundspeed_body = sqrtf(_global_pos.vel_n * _global_pos.vel_n + _global_pos.vel_e * _global_pos.vel_e); -- cgit v1.2.3 From 139ca8c327a80b61213e2d75dd8819cf119d22d7 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 3 Sep 2014 18:53:05 +0200 Subject: fw landing: impose horizontal limit for start of flare --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) (limited to 'src') diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 84c14be01..e71e7a2ac 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -999,7 +999,11 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi float relative_alt = get_relative_landingalt(_pos_sp_triplet.current.alt, _global_pos.alt, _range_finder, _parameters.range_finder_rel_alt); - if ( (relative_alt < landingslope.flare_relative_alt()) || land_noreturn_vertical) { //checking for land_noreturn to avoid unwanted climb out + /* Check if we should start flaring with a vertical and a + * horizontal limit (with some tolerance) */ + if ( ((relative_alt < landingslope.flare_relative_alt()) && + (wp_distance < landingslope.flare_length() + 5.0f)) || + land_noreturn_vertical) { //checking for land_noreturn to avoid unwanted climb out /* land with minimal speed */ -- cgit v1.2.3 From dd1945bb7644b41e765866aefd3c6cc14e433b37 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 4 Sep 2014 10:59:35 +0200 Subject: add heightrate ff for tecs --- src/lib/external_lgpl/tecs/tecs.cpp | 2 +- src/lib/external_lgpl/tecs/tecs.h | 6 + .../fw_pos_control_l1/fw_pos_control_l1_main.cpp | 5 + .../fw_pos_control_l1/fw_pos_control_l1_params.c | 125 +++++++++++---------- 4 files changed, 78 insertions(+), 60 deletions(-) (limited to 'src') diff --git a/src/lib/external_lgpl/tecs/tecs.cpp b/src/lib/external_lgpl/tecs/tecs.cpp index d27bf776f..023bd71bf 100644 --- a/src/lib/external_lgpl/tecs/tecs.cpp +++ b/src/lib/external_lgpl/tecs/tecs.cpp @@ -238,7 +238,7 @@ void TECS::_update_height_demand(float demand, float state) _hgt_dem_adj = demand;//0.025f * demand + 0.975f * _hgt_dem_adj_last; _hgt_dem_adj_last = _hgt_dem_adj; - _hgt_rate_dem = (_hgt_dem_adj-state)*_heightrate_p; + _hgt_rate_dem = (_hgt_dem_adj-state)*_heightrate_p + _heightrate_ff * (_hgt_dem_adj - _hgt_dem_adj_last)/_DT; // Limit height rate of change if (_hgt_rate_dem > _maxClimbRate) { _hgt_rate_dem = _maxClimbRate; diff --git a/src/lib/external_lgpl/tecs/tecs.h b/src/lib/external_lgpl/tecs/tecs.h index 36ae4ecaf..8ac31de6f 100644 --- a/src/lib/external_lgpl/tecs/tecs.h +++ b/src/lib/external_lgpl/tecs/tecs.h @@ -47,6 +47,7 @@ public: _rollComp(0.0f), _spdWeight(0.5f), _heightrate_p(0.0f), + _heightrate_ff(0.0f), _speedrate_p(0.0f), _throttle_dem(0.0f), _pitch_dem(0.0f), @@ -220,6 +221,10 @@ public: _heightrate_p = heightrate_p; } + void set_heightrate_ff(float heightrate_ff) { + _heightrate_ff = heightrate_ff; + } + void set_speedrate_p(float speedrate_p) { _speedrate_p = speedrate_p; } @@ -256,6 +261,7 @@ private: float _rollComp; float _spdWeight; float _heightrate_p; + float _heightrate_ff; float _speedrate_p; // throttle demand in the range from 0.0 to 1.0 diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 84c14be01..d339b1c4d 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -211,6 +211,7 @@ private: float max_climb_rate; float climbout_diff; float heightrate_p; + float heightrate_ff; float speedrate_p; float throttle_damp; float integrator_gain; @@ -256,6 +257,7 @@ private: param_t max_climb_rate; param_t climbout_diff; param_t heightrate_p; + param_t heightrate_ff; param_t speedrate_p; param_t throttle_damp; param_t integrator_gain; @@ -494,6 +496,7 @@ FixedwingPositionControl::FixedwingPositionControl() : _parameter_handles.speed_weight = param_find("FW_T_SPDWEIGHT"); _parameter_handles.pitch_damping = param_find("FW_T_PTCH_DAMP"); _parameter_handles.heightrate_p = param_find("FW_T_HRATE_P"); + _parameter_handles.heightrate_ff = param_find("FW_T_HRATE_FF"); _parameter_handles.speedrate_p = param_find("FW_T_SRATE_P"); /* fetch initial parameter values */ @@ -563,6 +566,7 @@ FixedwingPositionControl::parameters_update() param_get(_parameter_handles.climbout_diff, &(_parameters.climbout_diff)); param_get(_parameter_handles.heightrate_p, &(_parameters.heightrate_p)); + param_get(_parameter_handles.heightrate_ff, &(_parameters.heightrate_ff)); param_get(_parameter_handles.speedrate_p, &(_parameters.speedrate_p)); param_get(_parameter_handles.land_slope_angle, &(_parameters.land_slope_angle)); @@ -600,6 +604,7 @@ FixedwingPositionControl::parameters_update() _tecs.set_indicated_airspeed_max(_parameters.airspeed_max); _tecs.set_max_climb_rate(_parameters.max_climb_rate); _tecs.set_heightrate_p(_parameters.heightrate_p); + _tecs.set_heightrate_ff(_parameters.heightrate_ff); _tecs.set_speedrate_p(_parameters.speedrate_p); /* sanity check parameters */ diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c index 890ab3bad..847ecbb5c 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c @@ -131,8 +131,8 @@ PARAM_DEFINE_FLOAT(FW_R_LIM, 45.0f); /** * Throttle limit max * - * This is the maximum throttle % that can be used by the controller. - * For overpowered aircraft, this should be reduced to a value that + * This is the maximum throttle % that can be used by the controller. + * For overpowered aircraft, this should be reduced to a value that * provides sufficient thrust to climb at the maximum pitch angle PTCH_MAX. * * @group L1 Control @@ -142,10 +142,10 @@ PARAM_DEFINE_FLOAT(FW_THR_MAX, 1.0f); /** * Throttle limit min * - * This is the minimum throttle % that can be used by the controller. - * For electric aircraft this will normally be set to zero, but can be set - * to a small non-zero value if a folding prop is fitted to prevent the - * prop from folding and unfolding repeatedly in-flight or to provide + * This is the minimum throttle % that can be used by the controller. + * For electric aircraft this will normally be set to zero, but can be set + * to a small non-zero value if a folding prop is fitted to prevent the + * prop from folding and unfolding repeatedly in-flight or to provide * some aerodynamic drag from a turning prop to improve the descent rate. * * For aircraft with internal combustion engine this parameter should be set @@ -158,7 +158,7 @@ PARAM_DEFINE_FLOAT(FW_THR_MIN, 0.0f); /** * Throttle limit value before flare * - * This throttle value will be set as throttle limit at FW_LND_TLALT, + * This throttle value will be set as throttle limit at FW_LND_TLALT, * before arcraft will flare. * * @group L1 Control @@ -180,17 +180,17 @@ PARAM_DEFINE_FLOAT(FW_CLMBOUT_DIFF, 25.0f); /** * Maximum climb rate * - * This is the best climb rate that the aircraft can achieve with - * the throttle set to THR_MAX and the airspeed set to the - * default value. For electric aircraft make sure this number can be - * achieved towards the end of flight when the battery voltage has reduced. - * The setting of this parameter can be checked by commanding a positive - * altitude change of 100m in loiter, RTL or guided mode. If the throttle - * required to climb is close to THR_MAX and the aircraft is maintaining - * airspeed, then this parameter is set correctly. If the airspeed starts - * to reduce, then the parameter is set to high, and if the throttle - * demand required to climb and maintain speed is noticeably less than - * FW_THR_MAX, then either FW_T_CLMB_MAX should be increased or + * This is the best climb rate that the aircraft can achieve with + * the throttle set to THR_MAX and the airspeed set to the + * default value. For electric aircraft make sure this number can be + * achieved towards the end of flight when the battery voltage has reduced. + * The setting of this parameter can be checked by commanding a positive + * altitude change of 100m in loiter, RTL or guided mode. If the throttle + * required to climb is close to THR_MAX and the aircraft is maintaining + * airspeed, then this parameter is set correctly. If the airspeed starts + * to reduce, then the parameter is set to high, and if the throttle + * demand required to climb and maintain speed is noticeably less than + * FW_THR_MAX, then either FW_T_CLMB_MAX should be increased or * FW_THR_MAX reduced. * * @group L1 Control @@ -200,8 +200,8 @@ PARAM_DEFINE_FLOAT(FW_T_CLMB_MAX, 5.0f); /** * Minimum descent rate * - * This is the sink rate of the aircraft with the throttle - * set to THR_MIN and flown at the same airspeed as used + * This is the sink rate of the aircraft with the throttle + * set to THR_MIN and flown at the same airspeed as used * to measure FW_T_CLMB_MAX. * * @group Fixed Wing TECS @@ -211,10 +211,10 @@ PARAM_DEFINE_FLOAT(FW_T_SINK_MIN, 2.0f); /** * Maximum descent rate * - * This sets the maximum descent rate that the controller will use. - * If this value is too large, the aircraft can over-speed on descent. - * This should be set to a value that can be achieved without - * exceeding the lower pitch angle limit and without over-speeding + * This sets the maximum descent rate that the controller will use. + * If this value is too large, the aircraft can over-speed on descent. + * This should be set to a value that can be achieved without + * exceeding the lower pitch angle limit and without over-speeding * the aircraft. * * @group Fixed Wing TECS @@ -224,7 +224,7 @@ PARAM_DEFINE_FLOAT(FW_T_SINK_MAX, 5.0f); /** * TECS time constant * - * This is the time constant of the TECS control algorithm (in seconds). + * This is the time constant of the TECS control algorithm (in seconds). * Smaller values make it faster to respond, larger values make it slower * to respond. * @@ -235,7 +235,7 @@ PARAM_DEFINE_FLOAT(FW_T_TIME_CONST, 5.0f); /** * TECS Throttle time constant * - * This is the time constant of the TECS throttle control algorithm (in seconds). + * This is the time constant of the TECS throttle control algorithm (in seconds). * Smaller values make it faster to respond, larger values make it slower * to respond. * @@ -246,7 +246,7 @@ PARAM_DEFINE_FLOAT(FW_T_THRO_CONST, 8.0f); /** * Throttle damping factor * - * This is the damping gain for the throttle demand loop. + * This is the damping gain for the throttle demand loop. * Increase to add damping to correct for oscillations in speed and height. * * @group Fixed Wing TECS @@ -256,9 +256,9 @@ PARAM_DEFINE_FLOAT(FW_T_THR_DAMP, 0.5f); /** * Integrator gain * - * This is the integrator gain on the control loop. - * Increasing this gain increases the speed at which speed - * and height offsets are trimmed out, but reduces damping and + * This is the integrator gain on the control loop. + * Increasing this gain increases the speed at which speed + * and height offsets are trimmed out, but reduces damping and * increases overshoot. * * @group Fixed Wing TECS @@ -269,9 +269,9 @@ PARAM_DEFINE_FLOAT(FW_T_INTEG_GAIN, 0.1f); * Maximum vertical acceleration * * This is the maximum vertical acceleration (in metres/second square) - * either up or down that the controller will use to correct speed - * or height errors. The default value of 7 m/s/s (equivalent to +- 0.7 g) - * allows for reasonably aggressive pitch changes if required to recover + * either up or down that the controller will use to correct speed + * or height errors. The default value of 7 m/s/s (equivalent to +- 0.7 g) + * allows for reasonably aggressive pitch changes if required to recover * from under-speed conditions. * * @group Fixed Wing TECS @@ -281,10 +281,10 @@ PARAM_DEFINE_FLOAT(FW_T_VERT_ACC, 7.0f); /** * Complementary filter "omega" parameter for height * - * This is the cross-over frequency (in radians/second) of the complementary - * filter used to fuse vertical acceleration and barometric height to obtain - * an estimate of height rate and height. Increasing this frequency weights - * the solution more towards use of the barometer, whilst reducing it weights + * This is the cross-over frequency (in radians/second) of the complementary + * filter used to fuse vertical acceleration and barometric height to obtain + * an estimate of height rate and height. Increasing this frequency weights + * the solution more towards use of the barometer, whilst reducing it weights * the solution more towards use of the accelerometer data. * * @group Fixed Wing TECS @@ -294,10 +294,10 @@ PARAM_DEFINE_FLOAT(FW_T_HGT_OMEGA, 3.0f); /** * Complementary filter "omega" parameter for speed * - * This is the cross-over frequency (in radians/second) of the complementary - * filter used to fuse longitudinal acceleration and airspeed to obtain an + * This is the cross-over frequency (in radians/second) of the complementary + * filter used to fuse longitudinal acceleration and airspeed to obtain an * improved airspeed estimate. Increasing this frequency weights the solution - * more towards use of the arispeed sensor, whilst reducing it weights the + * more towards use of the arispeed sensor, whilst reducing it weights the * solution more towards use of the accelerometer data. * * @group Fixed Wing TECS @@ -307,13 +307,13 @@ PARAM_DEFINE_FLOAT(FW_T_SPD_OMEGA, 2.0f); /** * Roll -> Throttle feedforward * - * Increasing this gain turn increases the amount of throttle that will - * be used to compensate for the additional drag created by turning. - * Ideally this should be set to approximately 10 x the extra sink rate - * in m/s created by a 45 degree bank turn. Increase this gain if - * the aircraft initially loses energy in turns and reduce if the - * aircraft initially gains energy in turns. Efficient high aspect-ratio - * aircraft (eg powered sailplanes) can use a lower value, whereas + * Increasing this gain turn increases the amount of throttle that will + * be used to compensate for the additional drag created by turning. + * Ideally this should be set to approximately 10 x the extra sink rate + * in m/s created by a 45 degree bank turn. Increase this gain if + * the aircraft initially loses energy in turns and reduce if the + * aircraft initially gains energy in turns. Efficient high aspect-ratio + * aircraft (eg powered sailplanes) can use a lower value, whereas * inefficient low aspect-ratio models (eg delta wings) can use a higher value. * * @group Fixed Wing TECS @@ -323,15 +323,15 @@ PARAM_DEFINE_FLOAT(FW_T_RLL2THR, 10.0f); /** * Speed <--> Altitude priority * - * This parameter adjusts the amount of weighting that the pitch control - * applies to speed vs height errors. Setting it to 0.0 will cause the - * pitch control to control height and ignore speed errors. This will - * normally improve height accuracy but give larger airspeed errors. - * Setting it to 2.0 will cause the pitch control loop to control speed - * and ignore height errors. This will normally reduce airspeed errors, - * but give larger height errors. The default value of 1.0 allows the pitch - * control to simultaneously control height and speed. - * Note to Glider Pilots - set this parameter to 2.0 (The glider will + * This parameter adjusts the amount of weighting that the pitch control + * applies to speed vs height errors. Setting it to 0.0 will cause the + * pitch control to control height and ignore speed errors. This will + * normally improve height accuracy but give larger airspeed errors. + * Setting it to 2.0 will cause the pitch control loop to control speed + * and ignore height errors. This will normally reduce airspeed errors, + * but give larger height errors. The default value of 1.0 allows the pitch + * control to simultaneously control height and speed. + * Note to Glider Pilots - set this parameter to 2.0 (The glider will * adjust its pitch angle to maintain airspeed, ignoring changes in height). * * @group Fixed Wing TECS @@ -341,9 +341,9 @@ PARAM_DEFINE_FLOAT(FW_T_SPDWEIGHT, 1.0f); /** * Pitch damping factor * - * This is the damping gain for the pitch demand loop. Increase to add - * damping to correct for oscillations in height. The default value of 0.0 - * will work well provided the pitch to servo controller has been tuned + * This is the damping gain for the pitch demand loop. Increase to add + * damping to correct for oscillations in height. The default value of 0.0 + * will work well provided the pitch to servo controller has been tuned * properly. * * @group Fixed Wing TECS @@ -357,6 +357,13 @@ PARAM_DEFINE_FLOAT(FW_T_PTCH_DAMP, 0.0f); */ PARAM_DEFINE_FLOAT(FW_T_HRATE_P, 0.05f); +/** + * Height rate FF factor + * + * @group Fixed Wing TECS + */ +PARAM_DEFINE_FLOAT(FW_T_HRATE_FF, 0.0f); + /** * Speed rate P factor * -- cgit v1.2.3 From bf3f8861ddbf2af80fc121589ab6189538c1cadb Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 4 Sep 2014 11:11:03 +0200 Subject: fw landing: horiz flare check: fix wp distance better calculation of wp distance when behind wp --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 14 +++++++++++--- 1 file changed, 11 insertions(+), 3 deletions(-) (limited to 'src') diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 409b57d63..e8e316064 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -909,10 +909,17 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } else if (pos_sp_triplet.current.type == SETPOINT_TYPE_LAND) { float bearing_lastwp_currwp = get_bearing_to_next_waypoint(prev_wp(0), prev_wp(1), curr_wp(0), curr_wp(1)); + float bearing_airplane_currwp = get_bearing_to_next_waypoint(current_position(0), current_position(1), curr_wp(0), curr_wp(1)); /* Horizontal landing control */ /* switch to heading hold for the last meters, continue heading hold after */ float wp_distance = get_distance_to_next_waypoint(current_position(0), current_position(1), curr_wp(0), curr_wp(1)); + /* calculate a waypoint distance value which is 0 when the aircraft is behind the waypoint */ + float wp_distance_save = wp_distance; + if (fabsf(bearing_airplane_currwp - bearing_lastwp_currwp) >= math::radians(90.0f)) { + wp_distance_save = 0.0f; + } + //warnx("wp dist: %d, alt err: %d, noret: %s", (int)wp_distance, (int)altitude_error, (land_noreturn) ? "YES" : "NO"); if (wp_distance < _parameters.land_heading_hold_horizontal_distance || land_noreturn_horizontal) { @@ -964,13 +971,14 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi float L_altitude_rel = _pos_sp_triplet.previous.valid ? _pos_sp_triplet.previous.alt - terrain_alt : 0.0f; - float bearing_airplane_currwp = get_bearing_to_next_waypoint(current_position(0), current_position(1), curr_wp(0), curr_wp(1)); float landing_slope_alt_rel_desired = landingslope.getLandingSlopeRelativeAltitudeSave(wp_distance, bearing_lastwp_currwp, bearing_airplane_currwp); /* Check if we should start flaring with a vertical and a - * horizontal limit (with some tolerance) */ + * horizontal limit (with some tolerance) + * The horizontal limit is only applied when we are in front of the wp + */ if (((_global_pos.alt < terrain_alt + landingslope.flare_relative_alt()) && - (wp_distance < landingslope.flare_length() + 5.0f)) || + (wp_distance_save < landingslope.flare_length() + 5.0f)) || land_noreturn_vertical) { //checking for land_noreturn to avoid unwanted climb out /* land with minimal speed */ -- cgit v1.2.3 From 4e9a52fe45655aa853bf9af10223d32767bb60c4 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 4 Sep 2014 12:32:10 +0200 Subject: heightrate ff: fix order of calculations --- src/lib/external_lgpl/tecs/tecs.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) (limited to 'src') diff --git a/src/lib/external_lgpl/tecs/tecs.cpp b/src/lib/external_lgpl/tecs/tecs.cpp index 023bd71bf..da99aa5b1 100644 --- a/src/lib/external_lgpl/tecs/tecs.cpp +++ b/src/lib/external_lgpl/tecs/tecs.cpp @@ -236,9 +236,8 @@ void TECS::_update_height_demand(float demand, float state) // // _hgt_rate_dem); _hgt_dem_adj = demand;//0.025f * demand + 0.975f * _hgt_dem_adj_last; - _hgt_dem_adj_last = _hgt_dem_adj; - _hgt_rate_dem = (_hgt_dem_adj-state)*_heightrate_p + _heightrate_ff * (_hgt_dem_adj - _hgt_dem_adj_last)/_DT; + _hgt_dem_adj_last = _hgt_dem_adj; // Limit height rate of change if (_hgt_rate_dem > _maxClimbRate) { _hgt_rate_dem = _maxClimbRate; -- cgit v1.2.3 From 3684bf71d5068a03587005ba365c8705e327e936 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 4 Sep 2014 18:12:20 +0200 Subject: make flare pitch angle a param --- .../fw_pos_control_l1/fw_pos_control_l1_main.cpp | 14 ++- .../fw_pos_control_l1/fw_pos_control_l1_params.c | 134 ++++++++++++--------- 2 files changed, 86 insertions(+), 62 deletions(-) (limited to 'src') diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index e8e316064..c0c238ba9 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -237,6 +237,8 @@ private: float land_flare_alt_relative; float land_thrust_lim_alt_relative; float land_heading_hold_horizontal_distance; + float land_flare_pitch_min_deg; + float land_flare_pitch_max_deg; } _parameters; /**< local copies of interesting parameters */ @@ -281,6 +283,8 @@ private: param_t land_flare_alt_relative; param_t land_thrust_lim_alt_relative; param_t land_heading_hold_horizontal_distance; + param_t land_flare_pitch_min_deg; + param_t land_flare_pitch_max_deg; } _parameter_handles; /**< handles for interesting parameters */ @@ -465,6 +469,8 @@ FixedwingPositionControl::FixedwingPositionControl() : _parameter_handles.land_flare_alt_relative = param_find("FW_LND_FLALT"); _parameter_handles.land_thrust_lim_alt_relative = param_find("FW_LND_TLALT"); _parameter_handles.land_heading_hold_horizontal_distance = param_find("FW_LND_HHDIST"); + _parameter_handles.land_flare_pitch_min_deg = param_find("FW_FLARE_PMIN"); + _parameter_handles.land_flare_pitch_max_deg = param_find("FW_FLARE_PMAX"); _parameter_handles.time_const = param_find("FW_T_TIME_CONST"); _parameter_handles.time_const_throt = param_find("FW_T_THRO_CONST"); @@ -563,6 +569,8 @@ FixedwingPositionControl::parameters_update() } param_get(_parameter_handles.land_heading_hold_horizontal_distance, &(_parameters.land_heading_hold_horizontal_distance)); + param_get(_parameter_handles.land_flare_pitch_min_deg, &(_parameters.land_flare_pitch_min_deg)); + param_get(_parameter_handles.land_flare_pitch_max_deg, &(_parameters.land_flare_pitch_max_deg)); _l1_control.set_l1_damping(_parameters.l1_damping); _l1_control.set_l1_period(_parameters.l1_period); @@ -958,7 +966,6 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* apply minimum pitch (flare) and limit roll if close to touch down, altitude error is negative (going down) */ // XXX this could make a great param - float flare_pitch_angle_rad = -math::radians(5.0f);//math::radians(pos_sp_triplet.current.param1) float throttle_land = _parameters.throttle_min + (_parameters.throttle_max - _parameters.throttle_min) * 0.1f; float airspeed_land = 1.3f * _parameters.airspeed_min; float airspeed_approach = 1.3f * _parameters.airspeed_min; @@ -1008,9 +1015,10 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi tecs_update_pitch_throttle(terrain_alt + flare_curve_alt_rel, calculate_target_airspeed(airspeed_land), eas2tas, - flare_pitch_angle_rad, math::radians(15.0f), + math::radians(_parameters.land_flare_pitch_min_deg), + math::radians(_parameters.land_flare_pitch_max_deg), 0.0f, throttle_max, throttle_land, - false, flare_pitch_angle_rad, + false, math::radians(_parameters.land_flare_pitch_min_deg), _global_pos.alt, ground_speed, land_motor_lim ? TECS_MODE_LAND_THROTTLELIM : TECS_MODE_LAND); diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c index 890ab3bad..f2e8c422f 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c @@ -131,8 +131,8 @@ PARAM_DEFINE_FLOAT(FW_R_LIM, 45.0f); /** * Throttle limit max * - * This is the maximum throttle % that can be used by the controller. - * For overpowered aircraft, this should be reduced to a value that + * This is the maximum throttle % that can be used by the controller. + * For overpowered aircraft, this should be reduced to a value that * provides sufficient thrust to climb at the maximum pitch angle PTCH_MAX. * * @group L1 Control @@ -142,10 +142,10 @@ PARAM_DEFINE_FLOAT(FW_THR_MAX, 1.0f); /** * Throttle limit min * - * This is the minimum throttle % that can be used by the controller. - * For electric aircraft this will normally be set to zero, but can be set - * to a small non-zero value if a folding prop is fitted to prevent the - * prop from folding and unfolding repeatedly in-flight or to provide + * This is the minimum throttle % that can be used by the controller. + * For electric aircraft this will normally be set to zero, but can be set + * to a small non-zero value if a folding prop is fitted to prevent the + * prop from folding and unfolding repeatedly in-flight or to provide * some aerodynamic drag from a turning prop to improve the descent rate. * * For aircraft with internal combustion engine this parameter should be set @@ -158,7 +158,7 @@ PARAM_DEFINE_FLOAT(FW_THR_MIN, 0.0f); /** * Throttle limit value before flare * - * This throttle value will be set as throttle limit at FW_LND_TLALT, + * This throttle value will be set as throttle limit at FW_LND_TLALT, * before arcraft will flare. * * @group L1 Control @@ -180,17 +180,17 @@ PARAM_DEFINE_FLOAT(FW_CLMBOUT_DIFF, 25.0f); /** * Maximum climb rate * - * This is the best climb rate that the aircraft can achieve with - * the throttle set to THR_MAX and the airspeed set to the - * default value. For electric aircraft make sure this number can be - * achieved towards the end of flight when the battery voltage has reduced. - * The setting of this parameter can be checked by commanding a positive - * altitude change of 100m in loiter, RTL or guided mode. If the throttle - * required to climb is close to THR_MAX and the aircraft is maintaining - * airspeed, then this parameter is set correctly. If the airspeed starts - * to reduce, then the parameter is set to high, and if the throttle - * demand required to climb and maintain speed is noticeably less than - * FW_THR_MAX, then either FW_T_CLMB_MAX should be increased or + * This is the best climb rate that the aircraft can achieve with + * the throttle set to THR_MAX and the airspeed set to the + * default value. For electric aircraft make sure this number can be + * achieved towards the end of flight when the battery voltage has reduced. + * The setting of this parameter can be checked by commanding a positive + * altitude change of 100m in loiter, RTL or guided mode. If the throttle + * required to climb is close to THR_MAX and the aircraft is maintaining + * airspeed, then this parameter is set correctly. If the airspeed starts + * to reduce, then the parameter is set to high, and if the throttle + * demand required to climb and maintain speed is noticeably less than + * FW_THR_MAX, then either FW_T_CLMB_MAX should be increased or * FW_THR_MAX reduced. * * @group L1 Control @@ -200,8 +200,8 @@ PARAM_DEFINE_FLOAT(FW_T_CLMB_MAX, 5.0f); /** * Minimum descent rate * - * This is the sink rate of the aircraft with the throttle - * set to THR_MIN and flown at the same airspeed as used + * This is the sink rate of the aircraft with the throttle + * set to THR_MIN and flown at the same airspeed as used * to measure FW_T_CLMB_MAX. * * @group Fixed Wing TECS @@ -211,10 +211,10 @@ PARAM_DEFINE_FLOAT(FW_T_SINK_MIN, 2.0f); /** * Maximum descent rate * - * This sets the maximum descent rate that the controller will use. - * If this value is too large, the aircraft can over-speed on descent. - * This should be set to a value that can be achieved without - * exceeding the lower pitch angle limit and without over-speeding + * This sets the maximum descent rate that the controller will use. + * If this value is too large, the aircraft can over-speed on descent. + * This should be set to a value that can be achieved without + * exceeding the lower pitch angle limit and without over-speeding * the aircraft. * * @group Fixed Wing TECS @@ -224,7 +224,7 @@ PARAM_DEFINE_FLOAT(FW_T_SINK_MAX, 5.0f); /** * TECS time constant * - * This is the time constant of the TECS control algorithm (in seconds). + * This is the time constant of the TECS control algorithm (in seconds). * Smaller values make it faster to respond, larger values make it slower * to respond. * @@ -235,7 +235,7 @@ PARAM_DEFINE_FLOAT(FW_T_TIME_CONST, 5.0f); /** * TECS Throttle time constant * - * This is the time constant of the TECS throttle control algorithm (in seconds). + * This is the time constant of the TECS throttle control algorithm (in seconds). * Smaller values make it faster to respond, larger values make it slower * to respond. * @@ -246,7 +246,7 @@ PARAM_DEFINE_FLOAT(FW_T_THRO_CONST, 8.0f); /** * Throttle damping factor * - * This is the damping gain for the throttle demand loop. + * This is the damping gain for the throttle demand loop. * Increase to add damping to correct for oscillations in speed and height. * * @group Fixed Wing TECS @@ -256,9 +256,9 @@ PARAM_DEFINE_FLOAT(FW_T_THR_DAMP, 0.5f); /** * Integrator gain * - * This is the integrator gain on the control loop. - * Increasing this gain increases the speed at which speed - * and height offsets are trimmed out, but reduces damping and + * This is the integrator gain on the control loop. + * Increasing this gain increases the speed at which speed + * and height offsets are trimmed out, but reduces damping and * increases overshoot. * * @group Fixed Wing TECS @@ -269,9 +269,9 @@ PARAM_DEFINE_FLOAT(FW_T_INTEG_GAIN, 0.1f); * Maximum vertical acceleration * * This is the maximum vertical acceleration (in metres/second square) - * either up or down that the controller will use to correct speed - * or height errors. The default value of 7 m/s/s (equivalent to +- 0.7 g) - * allows for reasonably aggressive pitch changes if required to recover + * either up or down that the controller will use to correct speed + * or height errors. The default value of 7 m/s/s (equivalent to +- 0.7 g) + * allows for reasonably aggressive pitch changes if required to recover * from under-speed conditions. * * @group Fixed Wing TECS @@ -281,10 +281,10 @@ PARAM_DEFINE_FLOAT(FW_T_VERT_ACC, 7.0f); /** * Complementary filter "omega" parameter for height * - * This is the cross-over frequency (in radians/second) of the complementary - * filter used to fuse vertical acceleration and barometric height to obtain - * an estimate of height rate and height. Increasing this frequency weights - * the solution more towards use of the barometer, whilst reducing it weights + * This is the cross-over frequency (in radians/second) of the complementary + * filter used to fuse vertical acceleration and barometric height to obtain + * an estimate of height rate and height. Increasing this frequency weights + * the solution more towards use of the barometer, whilst reducing it weights * the solution more towards use of the accelerometer data. * * @group Fixed Wing TECS @@ -294,10 +294,10 @@ PARAM_DEFINE_FLOAT(FW_T_HGT_OMEGA, 3.0f); /** * Complementary filter "omega" parameter for speed * - * This is the cross-over frequency (in radians/second) of the complementary - * filter used to fuse longitudinal acceleration and airspeed to obtain an + * This is the cross-over frequency (in radians/second) of the complementary + * filter used to fuse longitudinal acceleration and airspeed to obtain an * improved airspeed estimate. Increasing this frequency weights the solution - * more towards use of the arispeed sensor, whilst reducing it weights the + * more towards use of the arispeed sensor, whilst reducing it weights the * solution more towards use of the accelerometer data. * * @group Fixed Wing TECS @@ -307,13 +307,13 @@ PARAM_DEFINE_FLOAT(FW_T_SPD_OMEGA, 2.0f); /** * Roll -> Throttle feedforward * - * Increasing this gain turn increases the amount of throttle that will - * be used to compensate for the additional drag created by turning. - * Ideally this should be set to approximately 10 x the extra sink rate - * in m/s created by a 45 degree bank turn. Increase this gain if - * the aircraft initially loses energy in turns and reduce if the - * aircraft initially gains energy in turns. Efficient high aspect-ratio - * aircraft (eg powered sailplanes) can use a lower value, whereas + * Increasing this gain turn increases the amount of throttle that will + * be used to compensate for the additional drag created by turning. + * Ideally this should be set to approximately 10 x the extra sink rate + * in m/s created by a 45 degree bank turn. Increase this gain if + * the aircraft initially loses energy in turns and reduce if the + * aircraft initially gains energy in turns. Efficient high aspect-ratio + * aircraft (eg powered sailplanes) can use a lower value, whereas * inefficient low aspect-ratio models (eg delta wings) can use a higher value. * * @group Fixed Wing TECS @@ -323,15 +323,15 @@ PARAM_DEFINE_FLOAT(FW_T_RLL2THR, 10.0f); /** * Speed <--> Altitude priority * - * This parameter adjusts the amount of weighting that the pitch control - * applies to speed vs height errors. Setting it to 0.0 will cause the - * pitch control to control height and ignore speed errors. This will - * normally improve height accuracy but give larger airspeed errors. - * Setting it to 2.0 will cause the pitch control loop to control speed - * and ignore height errors. This will normally reduce airspeed errors, - * but give larger height errors. The default value of 1.0 allows the pitch - * control to simultaneously control height and speed. - * Note to Glider Pilots - set this parameter to 2.0 (The glider will + * This parameter adjusts the amount of weighting that the pitch control + * applies to speed vs height errors. Setting it to 0.0 will cause the + * pitch control to control height and ignore speed errors. This will + * normally improve height accuracy but give larger airspeed errors. + * Setting it to 2.0 will cause the pitch control loop to control speed + * and ignore height errors. This will normally reduce airspeed errors, + * but give larger height errors. The default value of 1.0 allows the pitch + * control to simultaneously control height and speed. + * Note to Glider Pilots - set this parameter to 2.0 (The glider will * adjust its pitch angle to maintain airspeed, ignoring changes in height). * * @group Fixed Wing TECS @@ -341,9 +341,9 @@ PARAM_DEFINE_FLOAT(FW_T_SPDWEIGHT, 1.0f); /** * Pitch damping factor * - * This is the damping gain for the pitch demand loop. Increase to add - * damping to correct for oscillations in height. The default value of 0.0 - * will work well provided the pitch to servo controller has been tuned + * This is the damping gain for the pitch demand loop. Increase to add + * damping to correct for oscillations in height. The default value of 0.0 + * will work well provided the pitch to servo controller has been tuned * properly. * * @group Fixed Wing TECS @@ -414,3 +414,19 @@ PARAM_DEFINE_FLOAT(FW_LND_HHDIST, 15.0f); * @group L1 Control */ PARAM_DEFINE_FLOAT(FW_LND_RFRALT, -1.0f); + +/** + * Flare, minimum pitch + * + * Minimum pitch during flare, a positive sign means nose up + * + */ +PARAM_DEFINE_FLOAT(FW_FLARE_PMIN, 2.5f); + +/** + * Flare, maximum pitch + * + * Maximum pitch during flare, a positive sign means nose up + * + */ +PARAM_DEFINE_FLOAT(FW_FLARE_PMAX, 15.0f); -- cgit v1.2.3 From 033e4892ca2864c61e68c11c952d867ca6dc129e Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 5 Sep 2014 08:59:00 +0200 Subject: engine failure detection --- src/modules/commander/commander.cpp | 52 +++++++++++++++++++++++++++++--- src/modules/commander/commander_params.c | 32 ++++++++++++++++++++ src/modules/systemlib/circuit_breaker.c | 14 +++++++++ src/modules/systemlib/circuit_breaker.h | 1 + 4 files changed, 95 insertions(+), 4 deletions(-) (limited to 'src') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 07fcb5d40..447e2189d 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -686,6 +686,9 @@ int commander_thread_main(int argc, char *argv[]) param_t _param_datalink_loss_timeout = param_find("COM_DL_LOSS_T"); param_t _param_datalink_regain_timeout = param_find("COM_DL_REG_T"); param_t _param_onboard_sysid = param_find("COM_ONBSYSID"); + param_t _param_ef_throttle_thres = param_find("COM_EF_THROT"); + param_t _param_ef_current2throttle_thres = param_find("COM_EF_C2T"); + param_t _param_ef_time_thres = param_find("COM_EF_TIME"); /* welcome user */ warnx("starting"); @@ -974,8 +977,16 @@ int commander_thread_main(int argc, char *argv[]) int32_t datalink_loss_enabled = false; int32_t datalink_loss_timeout = 10; int32_t datalink_regain_timeout = 0; - uint8_t onboard_sysid = 42; /**< systemid of the onboard computer, telemetry from this sysid - is not validated for the datalink loss check */ + int32_t onboard_sysid = 42; /**< systemid of the onboard computer, + telemetry from this sysid is not + validated for the datalink loss check */ + + /* Thresholds for engine failure detection */ + int32_t ef_throttle_thres = 1.0f; + int32_t ef_current2throttle_thres = 0.0f; + int32_t ef_time_thres = 1000.0f; + uint64_t timestamp_engine_healthy = 0; /**< absolute time when engine + was healty*/ /* check which state machines for changes, clear "changed" flag */ bool arming_state_changed = false; @@ -1039,6 +1050,9 @@ int commander_thread_main(int argc, char *argv[]) param_get(_param_datalink_loss_timeout, &datalink_loss_timeout); param_get(_param_datalink_regain_timeout, &datalink_regain_timeout); param_get(_param_onboard_sysid, &onboard_sysid); + param_get(_param_ef_throttle_thres, &ef_throttle_thres); + param_get(_param_ef_current2throttle_thres, &ef_current2throttle_thres); + param_get(_param_ef_time_thres, &ef_time_thres); } orb_check(sp_man_sub, &updated); @@ -1572,10 +1586,11 @@ int commander_thread_main(int argc, char *argv[]) /* If this is not an onboard link/onboard computer: * set flag that we have a valid link */ - if (telemetry_sysid[i] != onboard_sysid) { + if (telemetry_sysid[i] != (uint8_t)onboard_sysid) { have_link = true; } - } else if (!telemetry_lost[i] && telemetry_sysid[i] != onboard_sysid) { + } else if (!telemetry_lost[i] && telemetry_sysid[i] != + (uint8_t)onboard_sysid) { /* telemetry was healthy also in last iteration * we don't have to check a timeout, * telemetry from onboard computers is not accepted as a valid datalink @@ -1608,6 +1623,35 @@ int commander_thread_main(int argc, char *argv[]) } } + /* Check engine failure + * only for fixed wing for now + */ + if (!circuit_breaker_enabled("CBRK_ENGINEFAIL", CBRK_ENGINEFAIL_KEY) && + status.is_rotary_wing == false && + armed.armed && + ((actuator_controls.control[3] > ef_throttle_thres && + battery.current_a/actuator_controls.control[3] < + ef_current2throttle_thres) || + (status.engine_failure))) { + /* potential failure, measure time */ + if (timestamp_engine_healthy > 0 && + hrt_elapsed_time(×tamp_engine_healthy) > + ef_time_thres * 1e6 && + !status.engine_failure) { + status.engine_failure = true; + status_changed = true; + mavlink_log_critical(mavlink_fd, "Engine Failure"); + } + } else { + /* no failure reset flag */ + timestamp_engine_healthy = hrt_absolute_time(); + if (status.engine_failure) { + status.engine_failure = false; + status_changed = true; + } + } + + /* handle commands last, as the system needs to be updated to handle them */ orb_check(cmd_sub, &updated); diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index 98c0982b2..15c299a8b 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -138,3 +138,35 @@ PARAM_DEFINE_INT32(COM_DL_REG_T, 0); * @max 255 */ PARAM_DEFINE_INT32(COM_ONBSYSID, 42); + +/** Engine Failure Throttle Threshold + * + * Engine failure triggers only above this throttle value + * + * @group commander + * @min 0.0f + * @max 1.0f + */ +PARAM_DEFINE_FLOAT(COM_EF_THROT, 0.5f); + +/** Engine Failure Current/Throttle Threshold + * + * Engine failure triggers only below this current/throttle value + * + * @group commander + * @min 0.0f + * @max 7.0f + */ +PARAM_DEFINE_FLOAT(COM_EF_C2T, 5.0f); + +/** Engine Failure Time Threshold + * + * Engine failure triggers only if the throttle threshold and the + * current to throttle threshold are violated for this time + * + * @group commander + * @unit second + * @min 0.0f + * @max 7.0f + */ +PARAM_DEFINE_FLOAT(COM_EF_TIME, 5.0f); diff --git a/src/modules/systemlib/circuit_breaker.c b/src/modules/systemlib/circuit_breaker.c index b0f95aedf..9e5429988 100644 --- a/src/modules/systemlib/circuit_breaker.c +++ b/src/modules/systemlib/circuit_breaker.c @@ -108,6 +108,20 @@ PARAM_DEFINE_INT32(CBRK_AIRSPD_CHK, 0); */ PARAM_DEFINE_INT32(CBRK_FLIGHTTERM, 0); +/** + * Circuit breaker for engine failure detection + * + * Setting this parameter to 284953 will disable the engine failure detection. + * If the aircraft is in engine failure mode the enine failure flag will be + * set to healthy + * WARNING: ENABLING THIS CIRCUIT BREAKER IS AT OWN RISK + * + * @min 0 + * @max 284953 + * @group Circuit Breaker + */ +PARAM_DEFINE_INT32(CBRK_ENGINEFAIL, 0); + bool circuit_breaker_enabled(const char* breaker, int32_t magic) { int32_t val; diff --git a/src/modules/systemlib/circuit_breaker.h b/src/modules/systemlib/circuit_breaker.h index 445a89d3a..6a55e4948 100644 --- a/src/modules/systemlib/circuit_breaker.h +++ b/src/modules/systemlib/circuit_breaker.h @@ -54,6 +54,7 @@ #define CBRK_IO_SAFETY_KEY 22027 #define CBRK_AIRSPD_CHK_KEY 162128 #define CBRK_FLIGHTTERM_KEY 121212 +#define CBRK_ENGINEFAIL_KEY 284953 #include -- cgit v1.2.3 From d7f93f4ece19a4244cacf0227d0818f418a0f214 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 5 Sep 2014 10:04:42 +0200 Subject: loiter mission items: better reached check for FW --- src/modules/navigator/mission_block.cpp | 13 +++++++++++++ 1 file changed, 13 insertions(+) (limited to 'src') diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index 4adf77dce..157a1cf71 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -113,6 +113,19 @@ MissionBlock::is_mission_item_reached() if (dist >= 0.0f && dist <= _navigator->get_acceptance_radius()) { _waypoint_position_reached = true; } + } else if (!_navigator->get_vstatus()->is_rotary_wing && + (_mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED || + _mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || + _mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT)) { + /* Loiter mission item on a non rotary wing: the aircraft is going to circle the + * coordinates with a radius equal to the loiter_radius field + * Therefore the item is marked as reached once the system reaches the loiter + * radius (+ some margin). Time inside and turn count is handled elsewhere. + */ + if (dist >= 0.0f && dist <= _mission_item.loiter_radius * 1.2f) { + _waypoint_position_reached = true; + } + } else { /* for normal mission items used their acceptance radius */ if (dist >= 0.0f && dist <= _mission_item.acceptance_radius) { -- cgit v1.2.3 From 0a6267e010a1401ba111d5477c26a0b8ff17ffea Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 5 Sep 2014 10:18:25 +0200 Subject: update comment --- src/modules/navigator/mission_block.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) (limited to 'src') diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index 157a1cf71..723caec7c 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -118,14 +118,14 @@ MissionBlock::is_mission_item_reached() _mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || _mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT)) { /* Loiter mission item on a non rotary wing: the aircraft is going to circle the - * coordinates with a radius equal to the loiter_radius field + * coordinates with a radius equal to the loiter_radius field. It is not flying + * through the waypoint center. * Therefore the item is marked as reached once the system reaches the loiter * radius (+ some margin). Time inside and turn count is handled elsewhere. */ if (dist >= 0.0f && dist <= _mission_item.loiter_radius * 1.2f) { _waypoint_position_reached = true; } - } else { /* for normal mission items used their acceptance radius */ if (dist >= 0.0f && dist <= _mission_item.acceptance_radius) { -- cgit v1.2.3 From 1d9c99956f3f84e4350734f1eef41df4c03411f8 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 5 Sep 2014 12:06:05 +0200 Subject: make rc loss timeout a param --- src/modules/commander/commander.cpp | 7 +++++-- src/modules/commander/commander_params.c | 11 +++++++++++ 2 files changed, 16 insertions(+), 2 deletions(-) (limited to 'src') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 447e2189d..8dd6fb086 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -128,7 +128,6 @@ extern struct system_load_s system_load; #define POSITION_TIMEOUT (2 * 1000 * 1000) /**< consider the local or global position estimate invalid after 600ms */ #define FAILSAFE_DEFAULT_TIMEOUT (3 * 1000 * 1000) /**< hysteresis time - the failsafe will trigger after 3 seconds in this state */ -#define RC_TIMEOUT 500000 #define OFFBOARD_TIMEOUT 500000 #define DIFFPRESS_TIMEOUT 2000000 @@ -684,6 +683,7 @@ int commander_thread_main(int argc, char *argv[]) param_t _param_enable_parachute = param_find("NAV_PARACHUTE_EN"); param_t _param_enable_datalink_loss = param_find("COM_DL_LOSS_EN"); param_t _param_datalink_loss_timeout = param_find("COM_DL_LOSS_T"); + param_t _param_rc_loss_timeout = param_find("COM_RC_LOSS_T"); param_t _param_datalink_regain_timeout = param_find("COM_DL_REG_T"); param_t _param_onboard_sysid = param_find("COM_ONBSYSID"); param_t _param_ef_throttle_thres = param_find("COM_EF_THROT"); @@ -976,6 +976,7 @@ int commander_thread_main(int argc, char *argv[]) int32_t datalink_loss_enabled = false; int32_t datalink_loss_timeout = 10; + float rc_loss_timeout = 0.5; int32_t datalink_regain_timeout = 0; int32_t onboard_sysid = 42; /**< systemid of the onboard computer, telemetry from this sysid is not @@ -1048,6 +1049,7 @@ int commander_thread_main(int argc, char *argv[]) param_get(_param_enable_parachute, ¶chute_enabled); param_get(_param_enable_datalink_loss, &datalink_loss_enabled); param_get(_param_datalink_loss_timeout, &datalink_loss_timeout); + param_get(_param_rc_loss_timeout, &rc_loss_timeout); param_get(_param_datalink_regain_timeout, &datalink_regain_timeout); param_get(_param_onboard_sysid, &onboard_sysid); param_get(_param_ef_throttle_thres, &ef_throttle_thres); @@ -1462,7 +1464,8 @@ int commander_thread_main(int argc, char *argv[]) } /* RC input check */ - if (!status.rc_input_blocked && sp_man.timestamp != 0 && hrt_absolute_time() < sp_man.timestamp + RC_TIMEOUT) { + if (!status.rc_input_blocked && sp_man.timestamp != 0 && + hrt_absolute_time() < sp_man.timestamp + (uint64_t)(rc_loss_timeout * 1e6f)) { /* handle the case where RC signal was regained */ if (!status.rc_signal_found_once) { status.rc_signal_found_once = true; diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index 15c299a8b..7d06003c9 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -170,3 +170,14 @@ PARAM_DEFINE_FLOAT(COM_EF_C2T, 5.0f); * @max 7.0f */ PARAM_DEFINE_FLOAT(COM_EF_TIME, 5.0f); + +/** RC loss time threshold + * + * After this amount of seconds without RC connection the rc lost flag is set to true + * + * @group commander + * @unit second + * @min 0 + * @max 35 + */ +PARAM_DEFINE_FLOAT(COM_RC_LOSS_T, 0.5); -- cgit v1.2.3 From 960587b10ff351c2ba9dc0ef4e9b5d5b2fabfbe7 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 6 Sep 2014 11:03:32 +0200 Subject: Drop at the exact timing, drop only if facing into the right direction --- src/modules/bottle_drop/bottle_drop.cpp | 15 +++++++++------ 1 file changed, 9 insertions(+), 6 deletions(-) (limited to 'src') diff --git a/src/modules/bottle_drop/bottle_drop.cpp b/src/modules/bottle_drop/bottle_drop.cpp index 0bb65252f..56c09970d 100644 --- a/src/modules/bottle_drop/bottle_drop.cpp +++ b/src/modules/bottle_drop/bottle_drop.cpp @@ -626,9 +626,15 @@ BottleDrop::task_main() } else { // We're close enough - open the bay - distance_open_door = math::max(5.0f, 3.0f * fabsf(t_door * groundspeed_body)); + distance_open_door = math::max(10.0f, 3.0f * fabsf(t_door * groundspeed_body)); - if (isfinite(distance_real) && distance_real < distance_open_door) { + float ground_direction = atan2f(_global_pos.vel_e, _global_pos.vel_n); + float approach_direction = get_bearing_to_next_waypoint(flight_vector_s.lat, flight_vector_s.lon, flight_vector_e.lat, flight_vector_e.lon); + + float approach_error = math::wrap_pi(ground_direction - approach_direction); + + if (isfinite(distance_real) && distance_real < distance_open_door && + approach_error < math::radians(20.0f)) { open_bay(); _drop_state = DROP_STATE_BAY_OPEN; mavlink_log_info(_mavlink_fd, "#audio: opening bay"); @@ -646,11 +652,8 @@ BottleDrop::task_main() map_projection_reproject(&ref, x_f, y_f, &x_f_NED, &y_f_NED); future_distance = get_distance_to_next_waypoint(x_f_NED, y_f_NED, _drop_position.lat, _drop_position.lon); - warnx("Distance real: %.2f", (double)distance_real); - if (isfinite(distance_real) && - (distance_real < precision) && ((distance_real < future_distance) || - (distance_real < precision / 10.0f))) { + (distance_real < precision) && ((distance_real < future_distance))) { drop(); _drop_state = DROP_STATE_DROPPED; mavlink_log_info(_mavlink_fd, "#audio: payload dropped"); -- cgit v1.2.3 From 3834796ce94d424701e20c047b1bf29fcc4ffd61 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 6 Sep 2014 11:39:56 +0200 Subject: More user feedback on approach angle and approach error. Fix check for approach error --- src/modules/bottle_drop/bottle_drop.cpp | 27 ++++++++++++++++++--------- 1 file changed, 18 insertions(+), 9 deletions(-) (limited to 'src') diff --git a/src/modules/bottle_drop/bottle_drop.cpp b/src/modules/bottle_drop/bottle_drop.cpp index 56c09970d..a3eb721e3 100644 --- a/src/modules/bottle_drop/bottle_drop.cpp +++ b/src/modules/bottle_drop/bottle_drop.cpp @@ -384,6 +384,7 @@ BottleDrop::task_main() float x_f, y_f; // to-be position of the UAV after dt_runs seconds in projected coordinates double x_f_NED, y_f_NED; // to-be position of the UAV after dt_runs seconds in NED float distance_open_door; // The distance the UAV travels during its doors open [m] + float approach_error = 0.0f; // The error in radians between current ground vector and desired ground vector float distance_real = 0; // The distance between the UAVs position and the drop point [m] float future_distance = 0; // The distance between the UAVs to-be position and the drop point [m] @@ -505,11 +506,21 @@ BottleDrop::task_main() float windspeed_norm = sqrtf(wind.windspeed_north * wind.windspeed_north + wind.windspeed_east * wind.windspeed_east); float groundspeed_body = sqrtf(_global_pos.vel_n * _global_pos.vel_n + _global_pos.vel_e * _global_pos.vel_e); - distance_real = fabsf(get_distance_to_next_waypoint(_global_pos.lat, _global_pos.lon, _drop_position.lat, _drop_position.lon)); ground_distance = _global_pos.alt - _target_position.alt; - if (counter % 90 == 0) { - mavlink_log_info(_mavlink_fd, "#audio: drop distance %.2f", (double)distance_real); + // Distance to drop position and angle error to approach vector + // are relevant in all states greater than target valid (which calculates these positions) + if (_drop_state > DROP_STATE_TARGET_VALID) { + distance_real = fabsf(get_distance_to_next_waypoint(_global_pos.lat, _global_pos.lon, _drop_position.lat, _drop_position.lon)); + + float ground_direction = atan2f(_global_pos.vel_e, _global_pos.vel_n); + float approach_direction = get_bearing_to_next_waypoint(flight_vector_s.lat, flight_vector_s.lon, flight_vector_e.lat, flight_vector_e.lon); + + approach_error = _wrap_pi(ground_direction - approach_direction); + + if (counter % 90 == 0) { + mavlink_log_critical(_mavlink_fd, "drop distance %u, heading: %u", (unsigned)distance_real, (unsigned)math::degrees(approach_error)); + } } switch (_drop_state) { @@ -612,6 +623,9 @@ BottleDrop::task_main() _onboard_mission_pub = orb_advertise(ORB_ID(onboard_mission), &_onboard_mission); } + float approach_direction = get_bearing_to_next_waypoint(flight_vector_s.lat, flight_vector_s.lon, flight_vector_e.lat, flight_vector_e.lon); + mavlink_log_critical(_mavlink_fd, "position set, approach heading: %u", (unsigned)distance_real, (unsigned)math::degrees(approach_direction + M_PI_F)); + _drop_state = DROP_STATE_TARGET_SET; } break; @@ -628,13 +642,8 @@ BottleDrop::task_main() // We're close enough - open the bay distance_open_door = math::max(10.0f, 3.0f * fabsf(t_door * groundspeed_body)); - float ground_direction = atan2f(_global_pos.vel_e, _global_pos.vel_n); - float approach_direction = get_bearing_to_next_waypoint(flight_vector_s.lat, flight_vector_s.lon, flight_vector_e.lat, flight_vector_e.lon); - - float approach_error = math::wrap_pi(ground_direction - approach_direction); - if (isfinite(distance_real) && distance_real < distance_open_door && - approach_error < math::radians(20.0f)) { + fabsf(approach_error) < math::radians(20.0f)) { open_bay(); _drop_state = DROP_STATE_BAY_OPEN; mavlink_log_info(_mavlink_fd, "#audio: opening bay"); -- cgit v1.2.3 From cb11d1f99e6c8dd37a844eabdbd77d3fde2f2398 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 6 Sep 2014 12:00:37 +0200 Subject: Better message formatting --- src/modules/bottle_drop/bottle_drop.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src') diff --git a/src/modules/bottle_drop/bottle_drop.cpp b/src/modules/bottle_drop/bottle_drop.cpp index a3eb721e3..91f1b1c95 100644 --- a/src/modules/bottle_drop/bottle_drop.cpp +++ b/src/modules/bottle_drop/bottle_drop.cpp @@ -519,7 +519,7 @@ BottleDrop::task_main() approach_error = _wrap_pi(ground_direction - approach_direction); if (counter % 90 == 0) { - mavlink_log_critical(_mavlink_fd, "drop distance %u, heading: %u", (unsigned)distance_real, (unsigned)math::degrees(approach_error)); + mavlink_log_critical(_mavlink_fd, "drop distance %u, heading error %u", (unsigned)distance_real, (unsigned)math::degrees(approach_error)); } } -- cgit v1.2.3 From 8296a14e9befae6b36ef04aff78c502f3c7b08a7 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 6 Sep 2014 14:36:25 +0200 Subject: Bottle drop: Better cd default --- src/modules/bottle_drop/bottle_drop_params.c | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) (limited to 'src') diff --git a/src/modules/bottle_drop/bottle_drop_params.c b/src/modules/bottle_drop/bottle_drop_params.c index e5d35bf0a..51ebfb9a1 100644 --- a/src/modules/bottle_drop/bottle_drop_params.c +++ b/src/modules/bottle_drop/bottle_drop_params.c @@ -92,11 +92,11 @@ PARAM_DEFINE_FLOAT(BD_PRECISION, 30.0f); * http://en.wikipedia.org/wiki/Drag_coefficient * * @unit meter - * @min 1.0 - * @max 80.0 + * @min 0.08 + * @max 1.5 * @group Payload drop */ -PARAM_DEFINE_FLOAT(BD_OBJ_CD, 0.86f); +PARAM_DEFINE_FLOAT(BD_OBJ_CD, 0.1f); /** * Payload mass -- cgit v1.2.3 From dbe35d64ca4288de551ec11b4d5bff43650e8012 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 6 Sep 2014 15:32:12 +0200 Subject: Fixed delay calculation of laser --- src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src') diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index 97abb76a9..2c50e5c75 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -1369,7 +1369,7 @@ FixedwingEstimator::task_main() if (newRangeData) { _ekf->fuseRngData = true; _ekf->useRangeFinder = true; - _ekf->RecallStates(_ekf->statesAtRngTime, (IMUmsec - 500.0f)); + _ekf->RecallStates(_ekf->statesAtRngTime, (IMUmsec - 100.0f)); _ekf->GroundEKF(); } -- cgit v1.2.3 From 5418412777ca96362cb5d866b2d0f5bb7efc3131 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 7 Sep 2014 13:05:03 +0200 Subject: do send termination info only once --- src/modules/commander/commander.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) (limited to 'src') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 8dd6fb086..bd5b464aa 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1684,9 +1684,9 @@ int commander_thread_main(int argc, char *argv[]) static bool flight_termination_printed = false; if (!flight_termination_printed) { warnx("Flight termination because of data link loss && gps failure"); + mavlink_log_critical(mavlink_fd, "DL and GPS lost: flight termination"); flight_termination_printed = true; } - mavlink_log_critical(mavlink_fd, "DL and GPS lost: flight termination"); } /* At this point the rc signal and the gps system have been checked @@ -1703,9 +1703,9 @@ int commander_thread_main(int argc, char *argv[]) static bool flight_termination_printed = false; if (!flight_termination_printed) { warnx("Flight termination because of RC signal loss && gps failure"); + mavlink_log_critical(mavlink_fd, "RC and GPS lost: flight termination"); flight_termination_printed = true; } - mavlink_log_critical(mavlink_fd, "RC and GPS lost: flight termination"); } } -- cgit v1.2.3 From 74601939a82174fcb6ef2d681e1f32b14a0a4ba0 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 7 Sep 2014 13:45:21 +0200 Subject: Revert "do send termination info only once" This reverts commit 5418412777ca96362cb5d866b2d0f5bb7efc3131. --- src/modules/commander/commander.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) (limited to 'src') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index bd5b464aa..8dd6fb086 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1684,9 +1684,9 @@ int commander_thread_main(int argc, char *argv[]) static bool flight_termination_printed = false; if (!flight_termination_printed) { warnx("Flight termination because of data link loss && gps failure"); - mavlink_log_critical(mavlink_fd, "DL and GPS lost: flight termination"); flight_termination_printed = true; } + mavlink_log_critical(mavlink_fd, "DL and GPS lost: flight termination"); } /* At this point the rc signal and the gps system have been checked @@ -1703,9 +1703,9 @@ int commander_thread_main(int argc, char *argv[]) static bool flight_termination_printed = false; if (!flight_termination_printed) { warnx("Flight termination because of RC signal loss && gps failure"); - mavlink_log_critical(mavlink_fd, "RC and GPS lost: flight termination"); flight_termination_printed = true; } + mavlink_log_critical(mavlink_fd, "RC and GPS lost: flight termination"); } } -- cgit v1.2.3 From 9608e7adeb069878d5cedd3ece9edc7f4ac40ce4 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 7 Sep 2014 15:29:53 +0200 Subject: flight termination mavlink outtput: limit rate --- src/modules/commander/commander.cpp | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) (limited to 'src') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 8dd6fb086..3a8272091 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1684,9 +1684,12 @@ int commander_thread_main(int argc, char *argv[]) static bool flight_termination_printed = false; if (!flight_termination_printed) { warnx("Flight termination because of data link loss && gps failure"); + mavlink_log_critical(mavlink_fd, "DL and GPS lost: flight termination"); flight_termination_printed = true; } - mavlink_log_critical(mavlink_fd, "DL and GPS lost: flight termination"); + if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0 ) { + mavlink_log_critical(mavlink_fd, "DL and GPS lost: flight termination"); + } } /* At this point the rc signal and the gps system have been checked @@ -1705,7 +1708,9 @@ int commander_thread_main(int argc, char *argv[]) warnx("Flight termination because of RC signal loss && gps failure"); flight_termination_printed = true; } - mavlink_log_critical(mavlink_fd, "RC and GPS lost: flight termination"); + if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0 ) { + mavlink_log_critical(mavlink_fd, "RC and GPS lost: flight termination"); + } } } -- cgit v1.2.3 From eb90979ba8254d7ade8ce530c7e4643d48c4196b Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 7 Sep 2014 15:31:58 +0200 Subject: if V_RCL_LT < 0 go directly to termination --- src/modules/navigator/rcloss.cpp | 15 +++++++++++---- src/modules/navigator/rcloss_params.c | 3 ++- 2 files changed, 13 insertions(+), 5 deletions(-) (limited to 'src') diff --git a/src/modules/navigator/rcloss.cpp b/src/modules/navigator/rcloss.cpp index 651e31184..5564a1c42 100644 --- a/src/modules/navigator/rcloss.cpp +++ b/src/modules/navigator/rcloss.cpp @@ -153,10 +153,17 @@ RCLoss::advance_rcl() { switch (_rcl_state) { case RCL_STATE_NONE: - /* Check the number of data link losses. If above home fly home directly */ - warnx("RC loss, OBC mode, loiter"); - mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: rc loss, loitering"); - _rcl_state = RCL_STATE_LOITER; + if (_param_loitertime.get() > 0.0f) { + warnx("RC loss, OBC mode, loiter"); + mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: rc loss, loitering"); + _rcl_state = RCL_STATE_LOITER; + } else { + warnx("RC loss, OBC mode, slip loiter, terminate"); + mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: rc loss, terminating"); + _rcl_state = RCL_STATE_TERMINATE; + _navigator->get_mission_result()->stay_in_failsafe = true; + _navigator->publish_mission_result(); + } break; case RCL_STATE_LOITER: _rcl_state = RCL_STATE_TERMINATE; diff --git a/src/modules/navigator/rcloss_params.c b/src/modules/navigator/rcloss_params.c index 83d23cf49..f1a01c73b 100644 --- a/src/modules/navigator/rcloss_params.c +++ b/src/modules/navigator/rcloss_params.c @@ -51,9 +51,10 @@ * Loiter Time * * The amount of time in seconds the system should loiter at current position before termination + * Set to -1 to make the system skip loitering * * @unit seconds - * @min 0.0 + * @min -1.0 * @group RCL */ PARAM_DEFINE_FLOAT(NAV_RCL_LT, 120.0f); -- cgit v1.2.3 From 97f7c0088f07cb86463501e81b58c5dba971a50c Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 7 Sep 2014 15:32:28 +0200 Subject: fix typo in comment --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src') diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 79992bf45..d177f7552 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -1223,7 +1223,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } if (_vehicle_status.engine_failure || _vehicle_status.engine_failure_cmd) { - /* Set thrrust to 0 to minimize damage */ + /* Set thrust to 0 to minimize damage */ _att_sp.thrust = 0.0f; } else if (pos_sp_triplet.current.type == SETPOINT_TYPE_TAKEOFF && launch_detection_state != LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS) { -- cgit v1.2.3 From e8503ab3f90353957136d60dbc3fa7668249119c Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 7 Sep 2014 15:33:11 +0200 Subject: gps failure has priority over engine falure, in case both fail make sure that the gps failure mode does not turn on the engine --- src/modules/commander/state_machine_helper.cpp | 4 ++-- src/modules/fw_att_control/fw_att_control_main.cpp | 8 ++++++-- 2 files changed, 8 insertions(+), 4 deletions(-) (limited to 'src') diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index e3b5d30e4..9568752ae 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -511,10 +511,10 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en } else if (status->rc_signal_lost_cmd) { status->nav_state = NAVIGATION_STATE_AUTO_RTGS; //XXX /* Finished handling commands which have priority , now handle failures */ - } else if (status->engine_failure) { - status->nav_state = NAVIGATION_STATE_AUTO_LANDENGFAIL; } else if (status->gps_failure) { status->nav_state = NAVIGATION_STATE_AUTO_LANDGPSFAIL; + } else if (status->engine_failure) { + status->nav_state = NAVIGATION_STATE_AUTO_LANDENGFAIL; } else if (((status->data_link_lost && data_link_loss_enabled) && status->rc_signal_lost) || (!data_link_loss_enabled && status->rc_signal_lost && mission_finished)) { status->failsafe = true; diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index bef2f7ca5..e770c11a2 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -893,8 +893,12 @@ FixedwingAttitudeControl::task_main() } } - /* throttle passed through */ - _actuators.control[3] = (isfinite(throttle_sp)) ? throttle_sp : 0.0f; + /* throttle passed through if it is finite and if no engine failure was + * detected */ + _actuators.control[3] = (isfinite(throttle_sp) && + !(_vehicle_status.engine_failure || + _vehicle_status.engine_failure_cmd)) ? + throttle_sp : 0.0f; if (!isfinite(throttle_sp)) { if (_debug && loop_counter % 10 == 0) { warnx("throttle_sp %.4f", (double)throttle_sp); -- cgit v1.2.3 From bc880a3ff9a297d39c7f53068bbe3ad1be572d44 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 11 Sep 2014 01:02:35 +0200 Subject: Fix dt calculation which is used to calculate correct drop time. --- src/modules/bottle_drop/bottle_drop.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) (limited to 'src') diff --git a/src/modules/bottle_drop/bottle_drop.cpp b/src/modules/bottle_drop/bottle_drop.cpp index 0bb65252f..811470308 100644 --- a/src/modules/bottle_drop/bottle_drop.cpp +++ b/src/modules/bottle_drop/bottle_drop.cpp @@ -466,7 +466,7 @@ BottleDrop::task_main() const unsigned sleeptime_us = 50000; hrt_abstime last_run = hrt_absolute_time(); - float dt_runs = 1e6f / sleeptime_us; + float dt_runs = sleeptime_us / 1e6f; // switch to faster updates during the drop while (_drop_state > DROP_STATE_INIT) { @@ -691,7 +691,7 @@ BottleDrop::task_main() // run at roughly 20 Hz usleep(sleeptime_us); - dt_runs = 1e6f / hrt_elapsed_time(&last_run); + dt_runs = hrt_elapsed_time(&last_run) / 1e6f; last_run = hrt_absolute_time(); } } -- cgit v1.2.3 From 1512bf727c7ad19400fc4777fbccce7b6e951437 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 11 Sep 2014 01:04:02 +0200 Subject: Remove useless modulo throttling, which is a nice source of potential non-trivial non-determinism / timing issues. --- src/modules/bottle_drop/bottle_drop.cpp | 86 ++++++++++++++++----------------- 1 file changed, 41 insertions(+), 45 deletions(-) (limited to 'src') diff --git a/src/modules/bottle_drop/bottle_drop.cpp b/src/modules/bottle_drop/bottle_drop.cpp index 811470308..128e40619 100644 --- a/src/modules/bottle_drop/bottle_drop.cpp +++ b/src/modules/bottle_drop/bottle_drop.cpp @@ -517,53 +517,49 @@ BottleDrop::task_main() case DROP_STATE_TARGET_VALID: { - // Update drop point at 10 Hz - if (counter % 10 == 0) { - - az = g; // acceleration in z direction[m/s^2] - vz = 0; // velocity in z direction [m/s] - z = 0; // fallen distance [m] - h_0 = _global_pos.alt - _target_position.alt; // height over target at start[m] - h = h_0; // height over target [m] - ax = 0; // acceleration in x direction [m/s^2] - vx = groundspeed_body;// XXX project // ground speed in x direction [m/s] - x = 0; // traveled distance in x direction [m] - vw = 0; // wind speed [m/s] - vrx = 0; // relative velocity in x direction [m/s] - v = groundspeed_body; // relative speed vector [m/s] - Fd = 0; // Drag force [N] - Fdx = 0; // Drag force in x direction [N] - Fdz = 0; // Drag force in z direction [N] - - - // 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 * (v * v); - Fdx = Fd * vrx / v; - Fdz = Fd * vz / v; - - // acceleration - az = g - Fdz / m; - ax = -Fdx / m; - } - - // compute drop vector - x = groundspeed_body * t_signal + x; + 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 * (v * v); + Fdx = Fd * vrx / v; + Fdz = Fd * vz / v; + + // acceleration + az = g - Fdz / m; + ax = -Fdx / m; } + // compute drop vector + x = groundspeed_body * t_signal + x; + x_t = 0.0f; y_t = 0.0f; -- cgit v1.2.3 From 6791ab72a910b00818026ac60d95d8df20bfa0d3 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 11 Sep 2014 01:06:30 +0200 Subject: Run faster for better accuracy. --- src/modules/bottle_drop/bottle_drop.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) (limited to 'src') diff --git a/src/modules/bottle_drop/bottle_drop.cpp b/src/modules/bottle_drop/bottle_drop.cpp index 128e40619..53071630d 100644 --- a/src/modules/bottle_drop/bottle_drop.cpp +++ b/src/modules/bottle_drop/bottle_drop.cpp @@ -463,7 +463,7 @@ BottleDrop::task_main() continue; } - const unsigned sleeptime_us = 50000; + const unsigned sleeptime_us = 9500; hrt_abstime last_run = hrt_absolute_time(); float dt_runs = sleeptime_us / 1e6f; @@ -684,7 +684,7 @@ BottleDrop::task_main() // update_actuators(); - // run at roughly 20 Hz + // run at roughly 100 Hz usleep(sleeptime_us); dt_runs = hrt_elapsed_time(&last_run) / 1e6f; -- cgit v1.2.3 From 940264f6ab565caee7a5426cfa1023b306c97e74 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 13 Sep 2014 16:23:28 +0200 Subject: Revert "datalink check: ignore onboard computer" This reverts commit 3f8793210b47bd8e09ed2adaabc2fab966db5df6. Conflicts: src/modules/commander/commander.cpp src/modules/commander/commander_params.c --- src/modules/commander/commander.cpp | 23 +++-------------------- src/modules/commander/commander_params.c | 10 ---------- 2 files changed, 3 insertions(+), 30 deletions(-) (limited to 'src') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 3a8272091..c53b14349 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -685,7 +685,6 @@ int commander_thread_main(int argc, char *argv[]) param_t _param_datalink_loss_timeout = param_find("COM_DL_LOSS_T"); param_t _param_rc_loss_timeout = param_find("COM_RC_LOSS_T"); param_t _param_datalink_regain_timeout = param_find("COM_DL_REG_T"); - param_t _param_onboard_sysid = param_find("COM_ONBSYSID"); param_t _param_ef_throttle_thres = param_find("COM_EF_THROT"); param_t _param_ef_current2throttle_thres = param_find("COM_EF_C2T"); param_t _param_ef_time_thres = param_find("COM_EF_TIME"); @@ -883,14 +882,12 @@ int commander_thread_main(int argc, char *argv[]) uint64_t telemetry_last_heartbeat[TELEMETRY_STATUS_ORB_ID_NUM]; uint64_t telemetry_last_dl_loss[TELEMETRY_STATUS_ORB_ID_NUM]; bool telemetry_lost[TELEMETRY_STATUS_ORB_ID_NUM]; - uint8_t telemetry_sysid[TELEMETRY_STATUS_ORB_ID_NUM]; for (int i = 0; i < TELEMETRY_STATUS_ORB_ID_NUM; i++) { telemetry_subs[i] = orb_subscribe(telemetry_status_orb_id[i]); telemetry_last_heartbeat[i] = 0; telemetry_last_dl_loss[i] = 0; telemetry_lost[i] = true; - telemetry_sysid[i] = 0; } /* Subscribe to global position */ @@ -978,9 +975,6 @@ int commander_thread_main(int argc, char *argv[]) int32_t datalink_loss_timeout = 10; float rc_loss_timeout = 0.5; int32_t datalink_regain_timeout = 0; - int32_t onboard_sysid = 42; /**< systemid of the onboard computer, - telemetry from this sysid is not - validated for the datalink loss check */ /* Thresholds for engine failure detection */ int32_t ef_throttle_thres = 1.0f; @@ -988,7 +982,6 @@ int commander_thread_main(int argc, char *argv[]) int32_t ef_time_thres = 1000.0f; uint64_t timestamp_engine_healthy = 0; /**< absolute time when engine was healty*/ - /* check which state machines for changes, clear "changed" flag */ bool arming_state_changed = false; bool main_state_changed = false; @@ -1051,7 +1044,6 @@ int commander_thread_main(int argc, char *argv[]) param_get(_param_datalink_loss_timeout, &datalink_loss_timeout); param_get(_param_rc_loss_timeout, &rc_loss_timeout); param_get(_param_datalink_regain_timeout, &datalink_regain_timeout); - param_get(_param_onboard_sysid, &onboard_sysid); param_get(_param_ef_throttle_thres, &ef_throttle_thres); param_get(_param_ef_current2throttle_thres, &ef_current2throttle_thres); param_get(_param_ef_time_thres, &ef_time_thres); @@ -1101,7 +1093,6 @@ int commander_thread_main(int argc, char *argv[]) } telemetry_last_heartbeat[i] = telemetry.heartbeat_time; - telemetry_sysid[i] = telemetry.system_id; } } @@ -1586,18 +1577,10 @@ int commander_thread_main(int argc, char *argv[]) mavlink_log_critical(mavlink_fd, "data link %i regained", i); telemetry_lost[i] = false; - - /* If this is not an onboard link/onboard computer: - * set flag that we have a valid link */ - if (telemetry_sysid[i] != (uint8_t)onboard_sysid) { - have_link = true; - } - } else if (!telemetry_lost[i] && telemetry_sysid[i] != - (uint8_t)onboard_sysid) { + have_link = true; + } else if (!telemetry_lost[i]) { /* telemetry was healthy also in last iteration - * we don't have to check a timeout, - * telemetry from onboard computers is not accepted as a valid datalink - */ + * we don't have to check a timeout */ have_link = true; } diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index 7d06003c9..f4bc7fd7f 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -129,16 +129,6 @@ PARAM_DEFINE_INT32(COM_DL_LOSS_T, 10); */ PARAM_DEFINE_INT32(COM_DL_REG_T, 0); -/** Onboard computer system id - * - * The system id of the onboard computer. Heartbeats from this system are ignored during the datalink check - * - * @group commander - * @min 0 - * @max 255 - */ -PARAM_DEFINE_INT32(COM_ONBSYSID, 42); - /** Engine Failure Throttle Threshold * * Engine failure triggers only above this throttle value -- cgit v1.2.3 From 2148464a705edea20a0f0311b1904079f598eab8 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 20 Sep 2014 08:44:30 +0200 Subject: geofence: better usefeedback if loaded --- src/modules/navigator/geofence.cpp | 6 +++++- src/modules/navigator/geofence.h | 4 ++++ src/modules/navigator/navigator_main.cpp | 2 ++ 3 files changed, 11 insertions(+), 1 deletion(-) (limited to 'src') diff --git a/src/modules/navigator/geofence.cpp b/src/modules/navigator/geofence.cpp index 5504239c5..0f431ded2 100644 --- a/src/modules/navigator/geofence.cpp +++ b/src/modules/navigator/geofence.cpp @@ -48,6 +48,7 @@ #include #include #include +#include /* Oddly, ERROR is not defined for C++ */ @@ -66,7 +67,8 @@ Geofence::Geofence() : _param_altitude_mode(this, "ALTMODE"), _param_source(this, "SOURCE"), _param_counter_threshold(this, "COUNT"), - _outside_counter(0) + _outside_counter(0), + _mavlinkFd(-1) { /* Load initial params */ updateParams(); @@ -330,8 +332,10 @@ Geofence::loadFromFile(const char *filename) { _verticesCount = pointCounter; warnx("Geofence: imported successfully"); + mavlink_log_info(_mavlinkFd, "Geofence imported"); } else { warnx("Geofence: import error"); + mavlink_log_critical(_mavlinkFd, "#audio: Geofence import error"); } return ERROR; diff --git a/src/modules/navigator/geofence.h b/src/modules/navigator/geofence.h index 65e5b4042..9d647cb68 100644 --- a/src/modules/navigator/geofence.h +++ b/src/modules/navigator/geofence.h @@ -98,6 +98,8 @@ public: int getSource() { return _param_source.get(); } + void setMavlinkFd(int value) { _mavlinkFd = value; } + private: orb_advert_t _fence_pub; /**< publish fence topic */ @@ -114,6 +116,8 @@ private: uint8_t _outside_counter; + int _mavlinkFd; + bool inside(double lat, double lon, float altitude); bool inside(const struct vehicle_global_position_s &global_position); bool inside(const struct vehicle_global_position_s &global_position, float baro_altitude_amsl); diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 9a8c54e7e..6710247a7 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -252,6 +252,7 @@ Navigator::task_main() warnx("Initializing.."); _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); + _geofence.setMavlinkFd(_mavlink_fd); /* Try to load the geofence: * if /fs/microsd/etc/geofence.txt load from this file @@ -263,6 +264,7 @@ Navigator::task_main() _geofence.loadFromFile(GEOFENCE_FILENAME); } else { + mavlink_log_critical(_mavlink_fd, "#audio: No geofence file"); if (_geofence.clearDm() > 0) warnx("Geofence cleared"); else -- cgit v1.2.3 From 54380e34b344108c72d2c46e9ca5e6b0b22390c8 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 20 Sep 2014 09:45:22 +0200 Subject: navigator: fix status information, remove fence_valid flag (this is handled by the geofence class) --- src/modules/navigator/navigator.h | 1 - src/modules/navigator/navigator_main.cpp | 5 ++--- 2 files changed, 2 insertions(+), 4 deletions(-) (limited to 'src') diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index 840b43f1b..bf42acff9 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -191,7 +191,6 @@ private: Geofence _geofence; /**< class that handles the geofence */ bool _geofence_violation_warning_sent; /**< prevents spaming to mavlink */ - bool _fence_valid; /**< flag if fence is valid */ bool _inside_fence; /**< vehicle is inside fence */ NavigatorMode *_navigation_mode; /**< abstract pointer to current navigation mode class */ diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 6710247a7..0e7f116ea 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -127,7 +127,6 @@ Navigator::Navigator() : _loop_perf(perf_alloc(PC_ELAPSED, "navigator")), _geofence{}, _geofence_violation_warning_sent(false), - _fence_valid(false), _inside_fence(true), _navigation_mode(nullptr), _mission(this, "MIS"), @@ -532,7 +531,7 @@ Navigator::status() // warnx("Compass heading in degrees %5.5f", (double)(_global_pos.yaw * M_RAD_TO_DEG_F)); // } - if (_fence_valid) { + if (_geofence.valid()) { warnx("Geofence is valid"); /* TODO: needed? */ // warnx("Vertex longitude latitude"); @@ -540,7 +539,7 @@ Navigator::status() // warnx("%6u %9.5f %8.5f", i, (double)_fence.vertices[i].lon, (double)_fence.vertices[i].lat); } else { - warnx("Geofence not set"); + warnx("Geofence not set (no /etc/geofence.txt on microsd) or not valid"); } } -- cgit v1.2.3 From f681c6b5a3d1057434c1743496b3704d8244da2f Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sat, 20 Sep 2014 22:37:10 +1000 Subject: bottle_drop: don't talk about distance and error --- src/modules/bottle_drop/bottle_drop.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) (limited to 'src') diff --git a/src/modules/bottle_drop/bottle_drop.cpp b/src/modules/bottle_drop/bottle_drop.cpp index 49919fd4a..b16d9adda 100644 --- a/src/modules/bottle_drop/bottle_drop.cpp +++ b/src/modules/bottle_drop/bottle_drop.cpp @@ -519,7 +519,7 @@ BottleDrop::task_main() approach_error = _wrap_pi(ground_direction - approach_direction); if (counter % 90 == 0) { - mavlink_log_critical(_mavlink_fd, "drop distance %u, heading error %u", (unsigned)distance_real, (unsigned)math::degrees(approach_error)); + mavlink_log_info(_mavlink_fd, "drop distance %u, heading error %u", (unsigned)distance_real, (unsigned)math::degrees(approach_error)); } } @@ -663,7 +663,7 @@ BottleDrop::task_main() _drop_state = DROP_STATE_DROPPED; mavlink_log_info(_mavlink_fd, "#audio: payload dropped"); } else { - + float distance_wp2 = get_distance_to_next_waypoint(_global_pos.lat, _global_pos.lon, flight_vector_e.lat, flight_vector_e.lon); if (distance_wp2 < distance_real) { -- cgit v1.2.3 From 3e41945cc6b43e91d08082b1ce976d5382e0405a Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 21 Sep 2014 10:46:46 +0200 Subject: flare pitch limit: apply at throttle lim altitude --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 2 +- src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c | 2 ++ 2 files changed, 3 insertions(+), 1 deletion(-) (limited to 'src') diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index c0c238ba9..77eb78097 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -1018,7 +1018,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi math::radians(_parameters.land_flare_pitch_min_deg), math::radians(_parameters.land_flare_pitch_max_deg), 0.0f, throttle_max, throttle_land, - false, math::radians(_parameters.land_flare_pitch_min_deg), + false, land_motor_lim ? math::radians(_parameters.land_flare_pitch_min_deg) : math::radians(_parameters.pitch_limit_min), _global_pos.alt, ground_speed, land_motor_lim ? TECS_MODE_LAND_THROTTLELIM : TECS_MODE_LAND); diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c index f2e8c422f..859cef8bd 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c @@ -419,6 +419,7 @@ PARAM_DEFINE_FLOAT(FW_LND_RFRALT, -1.0f); * Flare, minimum pitch * * Minimum pitch during flare, a positive sign means nose up + * Applied once FW_LND_TLALT is reached * */ PARAM_DEFINE_FLOAT(FW_FLARE_PMIN, 2.5f); @@ -427,6 +428,7 @@ PARAM_DEFINE_FLOAT(FW_FLARE_PMIN, 2.5f); * Flare, maximum pitch * * Maximum pitch during flare, a positive sign means nose up + * Applied once FW_LND_TLALT is reached * */ PARAM_DEFINE_FLOAT(FW_FLARE_PMAX, 15.0f); -- cgit v1.2.3 From 980175bb60ba40d29091f92fa8d494aed79b9ef6 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 21 Sep 2014 13:32:47 +0200 Subject: commander: move position of gps failure check --- src/modules/commander/commander.cpp | 35 +++++++++++++++++------------------ 1 file changed, 17 insertions(+), 18 deletions(-) (limited to 'src') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index c53b14349..03b73447f 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1205,24 +1205,6 @@ int commander_thread_main(int argc, char *argv[]) check_valid(global_position.timestamp, POSITION_TIMEOUT, eph_good, &(status.condition_global_position_valid), &status_changed); - /* check if GPS fix is ok */ - if (gps_position.fix_type >= 3 && //XXX check eph and epv ? - hrt_elapsed_time(&gps_position.timestamp_position) < FAILSAFE_DEFAULT_TIMEOUT) { - /* handle the case where gps was regained */ - if (status.gps_failure) { - status.gps_failure = false; - status_changed = true; - mavlink_log_critical(mavlink_fd, "gps regained"); - } - } else { - if (!status.gps_failure) { - status.gps_failure = true; - status_changed = true; - mavlink_log_critical(mavlink_fd, "gps fix lost"); - } - } - - /* update home position */ if (!status.condition_home_position_valid && status.condition_global_position_valid && !armed.armed && (global_position.eph < eph_threshold) && (global_position.epv < epv_threshold)) { @@ -1434,6 +1416,23 @@ int commander_thread_main(int argc, char *argv[]) globallocalconverter_init((double)gps_position.lat * 1.0e-7, (double)gps_position.lon * 1.0e-7, (float)gps_position.alt * 1.0e-3f, hrt_absolute_time()); } + /* check if GPS fix is ok */ + if (gps_position.fix_type >= 3 && //XXX check eph and epv ? + hrt_elapsed_time(&gps_position.timestamp_position) < FAILSAFE_DEFAULT_TIMEOUT) { + /* handle the case where gps was regained */ + if (status.gps_failure) { + status.gps_failure = false; + status_changed = true; + mavlink_log_critical(mavlink_fd, "gps regained"); + } + } else { + if (!status.gps_failure) { + status.gps_failure = true; + status_changed = true; + mavlink_log_critical(mavlink_fd, "gps fix lost"); + } + } + /* start mission result check */ orb_check(mission_result_sub, &updated); -- cgit v1.2.3 From e65ec1b98b6f0ed4d526b2d9d1929d64fd7eceda Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 21 Sep 2014 13:50:43 +0200 Subject: bottle drop: Be less aggressive about scheduling --- src/modules/bottle_drop/bottle_drop.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src') diff --git a/src/modules/bottle_drop/bottle_drop.cpp b/src/modules/bottle_drop/bottle_drop.cpp index b16d9adda..31c9157e1 100644 --- a/src/modules/bottle_drop/bottle_drop.cpp +++ b/src/modules/bottle_drop/bottle_drop.cpp @@ -222,7 +222,7 @@ BottleDrop::start() /* start the task */ _main_task = task_spawn_cmd("bottle_drop", SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 5, + SCHED_PRIORITY_DEFAULT + 15, 2048, (main_t)&BottleDrop::task_main_trampoline, nullptr); -- cgit v1.2.3 From 4b8a3856583d77e3f4c22d0f2999ccd3275e2520 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 21 Sep 2014 18:03:31 +0200 Subject: commander: improve output on gf violation --- src/modules/commander/commander.cpp | 4 ++++ 1 file changed, 4 insertions(+) (limited to 'src') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index c53b14349..b43f08f67 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1449,8 +1449,12 @@ int commander_thread_main(int argc, char *argv[]) static bool flight_termination_printed = false; if (!flight_termination_printed) { warnx("Flight termination because of navigator request or geofence"); + mavlink_log_critical(mavlink_fd, "GF violation: flight termination"); flight_termination_printed = true; } + if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0 ) { + mavlink_log_critical(mavlink_fd, "GF violation: flight termination"); + } } // no reset is done here on purpose, on geofence violation we want to stay in flighttermination } -- cgit v1.2.3 From 885d3e8ca4a0f3c5bfad9936860d549b5b2f0674 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 21 Sep 2014 18:03:52 +0200 Subject: datman: reduce task priority --- src/modules/dataman/dataman.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src') diff --git a/src/modules/dataman/dataman.c b/src/modules/dataman/dataman.c index ca1fe9bbb..b2355d4d8 100644 --- a/src/modules/dataman/dataman.c +++ b/src/modules/dataman/dataman.c @@ -797,7 +797,7 @@ start(void) sem_init(&g_init_sema, 1, 0); /* start the worker thread */ - if ((task = task_spawn_cmd("dataman", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 2000, task_main, NULL)) <= 0) { + if ((task = task_spawn_cmd("dataman", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, 2000, task_main, NULL)) <= 0) { warn("task start failed"); return -1; } -- cgit v1.2.3 From 808cc60cdad5c085d74e583d967cd86126ba1fca Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 21 Sep 2014 18:04:11 +0200 Subject: navigator: geofence with global pos: reduce update rate --- src/modules/navigator/navigator_main.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) (limited to 'src') diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 0e7f116ea..b63394544 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -387,9 +387,13 @@ Navigator::task_main() /* global position updated */ if (fds[0].revents & POLLIN) { global_position_update(); - if (_geofence.getSource() == Geofence::GF_SOURCE_GLOBALPOS) { + static int gposcounter = 0; + if (_geofence.getSource() == Geofence::GF_SOURCE_GLOBALPOS && + gposcounter % 10 == 0) { + /* Geofence is checked only every 10th gpos update */ have_geofence_position_data = true; } + gposcounter++; } /* Check geofence violation */ -- cgit v1.2.3 From 25b2d4b823d339c69365baecc7b9eada37e0a529 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Mon, 22 Sep 2014 10:57:17 +1000 Subject: change rates for mavlink streams --- src/modules/mavlink/mavlink_main.cpp | 45 ++++++++++++++++++++++-------------- 1 file changed, 28 insertions(+), 17 deletions(-) (limited to 'src') diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index dd830071d..69565e3ab 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1328,7 +1328,7 @@ Mavlink::task_main(int argc, char *argv[]) * make space for two messages plus off-by-one space as we use the empty element * marker ring buffer approach. */ - if (OK != message_buffer_init(2 * sizeof(mavlink_message_t) + 1)) { + if (OK != message_buffer_init(10 * sizeof(mavlink_message_t) + 1)) { errx(1, "can't allocate message buffer, exiting"); } @@ -1374,39 +1374,50 @@ Mavlink::task_main(int argc, char *argv[]) _parameters_manager->set_interval(interval_from_rate(30.0f)); LL_APPEND(_streams, _parameters_manager); - /* MISSION_STREAM stream, actually sends all MISSION_XXX messages at some rate depending on - * remote requests rate. Rate specified here controls how much bandwidth we will reserve for - * mission messages. */ - _mission_manager = (MavlinkMissionManager *) MavlinkMissionManager::new_instance(this); - _mission_manager->set_interval(interval_from_rate(10.0f)); - _mission_manager->set_verbose(_verbose); - LL_APPEND(_streams, _mission_manager); + switch (_mode) { case MAVLINK_MODE_NORMAL: + /* MISSION_STREAM stream, actually sends all MISSION_XXX messages at some rate depending on + * remote requests rate. Rate specified here controls how much bandwidth we will reserve for + * mission messages. */ + _mission_manager = (MavlinkMissionManager *) MavlinkMissionManager::new_instance(this); + _mission_manager->set_interval(interval_from_rate(2.0f)); + _mission_manager->set_verbose(_verbose); + LL_APPEND(_streams, _mission_manager); + configure_stream("SYS_STATUS", 1.0f); configure_stream("GPS_GLOBAL_ORIGIN", 0.5f); configure_stream("HIGHRES_IMU", 1.0f); - configure_stream("ATTITUDE", 15.0f); - configure_stream("VFR_HUD", 8.0f); + configure_stream("ATTITUDE", 4.0f); + configure_stream("VFR_HUD", 4.0f); configure_stream("GPS_RAW_INT", 1.0f); - configure_stream("GLOBAL_POSITION_INT", 3.0f); - configure_stream("LOCAL_POSITION_NED", 3.0f); + configure_stream("GLOBAL_POSITION_INT", 1.0f); + configure_stream("LOCAL_POSITION_NED", 1.0f); configure_stream("RC_CHANNELS_RAW", 1.0f); - configure_stream("POSITION_TARGET_GLOBAL_INT", 3.0f); - configure_stream("ATTITUDE_TARGET", 15.0f); + configure_stream("POSITION_TARGET_GLOBAL_INT", 1.0f); + configure_stream("ATTITUDE_TARGET", 1.0f); configure_stream("DISTANCE_SENSOR", 0.5f); - configure_stream("OPTICAL_FLOW", 20.0f); + //configure_stream("OPTICAL_FLOW", 20.0f); break; case MAVLINK_MODE_ONBOARD: + /* MISSION_STREAM stream, actually sends all MISSION_XXX messages at some rate depending on + * remote requests rate. Rate specified here controls how much bandwidth we will reserve for + * mission messages. */ + _mission_manager = (MavlinkMissionManager *) MavlinkMissionManager::new_instance(this); + _mission_manager->set_interval(interval_from_rate(10.0f)); + _mission_manager->set_verbose(_verbose); + LL_APPEND(_streams, _mission_manager); + configure_stream("SYS_STATUS", 1.0f); // XXX OBC change back: We need to be bandwidth-efficient here too configure_stream("ATTITUDE", 50.0f); configure_stream("GLOBAL_POSITION_INT", 50.0f); configure_stream("CAMERA_CAPTURE", 2.0f); - configure_stream("ATTITUDE_TARGET", 50.0f); - configure_stream("POSITION_TARGET_GLOBAL_INT", 20.0f); + configure_stream("ATTITUDE_TARGET", 10.0f); + configure_stream("POSITION_TARGET_GLOBAL_INT", 10.0f); + configure_stream("VFR_HUD", 10.0f); break; default: -- cgit v1.2.3 From 98bc5ece10987c177be5035cfca6ecea841b8843 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Mon, 22 Sep 2014 11:03:36 +1000 Subject: switch mission manager back to what is was before --- src/modules/mavlink/mavlink_main.cpp | 24 ++++++++---------------- 1 file changed, 8 insertions(+), 16 deletions(-) (limited to 'src') diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 69565e3ab..114b4678b 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1374,18 +1374,18 @@ Mavlink::task_main(int argc, char *argv[]) _parameters_manager->set_interval(interval_from_rate(30.0f)); LL_APPEND(_streams, _parameters_manager); + /* MISSION_STREAM stream, actually sends all MISSION_XXX messages at some rate depending on + * remote requests rate. Rate specified here controls how much bandwidth we will reserve for + * mission messages. */ + _mission_manager = (MavlinkMissionManager *) MavlinkMissionManager::new_instance(this); + _mission_manager->set_interval(interval_from_rate(10.0f)); + _mission_manager->set_verbose(_verbose); + LL_APPEND(_streams, _mission_manager); + switch (_mode) { case MAVLINK_MODE_NORMAL: - /* MISSION_STREAM stream, actually sends all MISSION_XXX messages at some rate depending on - * remote requests rate. Rate specified here controls how much bandwidth we will reserve for - * mission messages. */ - _mission_manager = (MavlinkMissionManager *) MavlinkMissionManager::new_instance(this); - _mission_manager->set_interval(interval_from_rate(2.0f)); - _mission_manager->set_verbose(_verbose); - LL_APPEND(_streams, _mission_manager); - configure_stream("SYS_STATUS", 1.0f); configure_stream("GPS_GLOBAL_ORIGIN", 0.5f); configure_stream("HIGHRES_IMU", 1.0f); @@ -1402,14 +1402,6 @@ Mavlink::task_main(int argc, char *argv[]) break; case MAVLINK_MODE_ONBOARD: - /* MISSION_STREAM stream, actually sends all MISSION_XXX messages at some rate depending on - * remote requests rate. Rate specified here controls how much bandwidth we will reserve for - * mission messages. */ - _mission_manager = (MavlinkMissionManager *) MavlinkMissionManager::new_instance(this); - _mission_manager->set_interval(interval_from_rate(10.0f)); - _mission_manager->set_verbose(_verbose); - LL_APPEND(_streams, _mission_manager); - configure_stream("SYS_STATUS", 1.0f); // XXX OBC change back: We need to be bandwidth-efficient here too configure_stream("ATTITUDE", 50.0f); -- cgit v1.2.3 From bc23b6239c50527aa550eed3bc6f17dec15c5c97 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Mon, 22 Sep 2014 11:03:49 +1000 Subject: increase ram --- src/modules/mavlink/mavlink_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src') diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 114b4678b..b9f486c95 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1630,7 +1630,7 @@ Mavlink::start(int argc, char *argv[]) task_spawn_cmd(buf, SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, - 2700, + 5000, (main_t)&Mavlink::start_helper, (const char **)argv); -- cgit v1.2.3 From 73aa7d81e31e18a6d1405697856e389a7a7b89af Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 22 Sep 2014 17:20:50 +0200 Subject: mf checker: fix landing check, ensure feedback from all checks is sent --- .../navigator/mission_feasibility_checker.cpp | 20 +++++++++++++++----- 1 file changed, 15 insertions(+), 5 deletions(-) (limited to 'src') diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp index 937e4fa5a..389cdd0d2 100644 --- a/src/modules/navigator/mission_feasibility_checker.cpp +++ b/src/modules/navigator/mission_feasibility_checker.cpp @@ -84,7 +84,12 @@ bool MissionFeasibilityChecker::checkMissionFeasible(bool isRotarywing, dm_item_ bool MissionFeasibilityChecker::checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt) { - return (checkGeofence(dm_current, nMissionItems, geofence) && checkHomePositionAltitude(dm_current, nMissionItems, home_alt)); + /* Perform checks and issue feedback to the user for all checks */ + bool resGeofence = checkGeofence(dm_current, nMissionItems, geofence); + bool resHomeAltitude = checkHomePositionAltitude(dm_current, nMissionItems, home_alt); + + /* Mission is only marked as feasible if all checks return true */ + return (resGeofence && resHomeAltitude); } bool MissionFeasibilityChecker::checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt) @@ -93,7 +98,13 @@ bool MissionFeasibilityChecker::checkMissionFeasibleFixedwing(dm_item_t dm_curre updateNavigationCapabilities(); // warnx("_nav_caps.landing_slope_angle_rad %.4f, _nav_caps.landing_horizontal_slope_displacement %.4f", _nav_caps.landing_slope_angle_rad, _nav_caps.landing_horizontal_slope_displacement); - return (checkFixedWingLanding(dm_current, nMissionItems) && checkGeofence(dm_current, nMissionItems, geofence) && checkHomePositionAltitude(dm_current, nMissionItems, home_alt)); + /* Perform checks and issue feedback to the user for all checks */ + bool resLanding = checkFixedWingLanding(dm_current, nMissionItems); + bool resGeofence = checkGeofence(dm_current, nMissionItems, geofence); + bool resHomeAltitude = checkHomePositionAltitude(dm_current, nMissionItems, home_alt); + + /* Mission is only marked as feasible if all checks return true */ + return (resLanding && resGeofence && resHomeAltitude); } bool MissionFeasibilityChecker::checkGeofence(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence) @@ -216,9 +227,8 @@ bool MissionFeasibilityChecker::checkFixedWingLanding(dm_item_t dm_current, size } } - -// float slope_alt = wp_altitude + _H0 * expf(-math::max(0.0f, _flare_length - wp_distance)/_flare_constant) - _H1_virt; - return false; + /* No landing waypoints or no waypoints */ + return true; } void MissionFeasibilityChecker::updateNavigationCapabilities() -- cgit v1.2.3 From 69eb222d4e2cf3d0894992a50a7a5822489f757f Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 22 Sep 2014 19:04:24 +0200 Subject: ubx: add sbas config, default to disable --- src/drivers/gps/ubx.cpp | 12 ++++++++++++ src/drivers/gps/ubx.h | 17 +++++++++++++++++ 2 files changed, 29 insertions(+) (limited to 'src') diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp index d0854f5e9..c54c474b3 100644 --- a/src/drivers/gps/ubx.cpp +++ b/src/drivers/gps/ubx.cpp @@ -189,6 +189,16 @@ UBX::configure(unsigned &baudrate) return 1; } + /* send a SBAS message to set the SBAS options */ + memset(&_buf.payload_tx_cfg_sbas, 0, sizeof(_buf.payload_tx_cfg_sbas)); + _buf.payload_tx_cfg_sbas.mode = UBX_TX_CFG_SBAS_MODE; + + send_message(UBX_MSG_CFG_SBAS, _buf.raw, sizeof(_buf.payload_tx_cfg_sbas)); + + if (wait_for_ack(UBX_MSG_CFG_SBAS, UBX_CONFIG_TIMEOUT, true) < 0) { + return 1; + } + /* configure message rates */ /* the last argument is divisor for measurement rate (set by CFG RATE), i.e. 1 means 5Hz */ @@ -265,6 +275,8 @@ UBX::wait_for_ack(const uint16_t msg, const unsigned timeout, const bool report) } } + warnx("msg %u ret %d", msg, ret); + _ack_state = UBX_ACK_IDLE; return ret; } diff --git a/src/drivers/gps/ubx.h b/src/drivers/gps/ubx.h index 219a5762a..c74eb9168 100644 --- a/src/drivers/gps/ubx.h +++ b/src/drivers/gps/ubx.h @@ -73,6 +73,7 @@ #define UBX_ID_CFG_MSG 0x01 #define UBX_ID_CFG_RATE 0x08 #define UBX_ID_CFG_NAV5 0x24 +#define UBX_ID_CFG_SBAS 0x16 #define UBX_ID_MON_VER 0x04 #define UBX_ID_MON_HW 0x09 @@ -89,6 +90,7 @@ #define UBX_MSG_CFG_MSG ((UBX_CLASS_CFG) | UBX_ID_CFG_MSG << 8) #define UBX_MSG_CFG_RATE ((UBX_CLASS_CFG) | UBX_ID_CFG_RATE << 8) #define UBX_MSG_CFG_NAV5 ((UBX_CLASS_CFG) | UBX_ID_CFG_NAV5 << 8) +#define UBX_MSG_CFG_SBAS ((UBX_CLASS_CFG) | UBX_ID_CFG_SBAS << 8) #define UBX_MSG_MON_HW ((UBX_CLASS_MON) | UBX_ID_MON_HW << 8) #define UBX_MSG_MON_VER ((UBX_CLASS_MON) | UBX_ID_MON_VER << 8) @@ -128,6 +130,11 @@ #define UBX_TX_CFG_NAV5_DYNMODEL 7 /**< 0 Portable, 2 Stationary, 3 Pedestrian, 4 Automotive, 5 Sea, 6 Airborne <1g, 7 Airborne <2g, 8 Airborne <4g */ #define UBX_TX_CFG_NAV5_FIXMODE 2 /**< 1 2D only, 2 3D only, 3 Auto 2D/3D */ +/* TX CFG-SBAS message contents */ +#define UBX_TX_CFG_SBAS_MODE_ENABLED 1 /**< SBAS enabled */ +#define UBX_TX_CFG_SBAS_MODE_DISABLED 0 /**< SBAS disabled */ +#define UBX_TX_CFG_SBAS_MODE UBX_TX_CFG_SBAS_MODE_DISABLED /**< SBAS enabled or disabled */ + /* TX CFG-MSG message contents */ #define UBX_TX_CFG_MSG_RATE1_5HZ 0x01 /**< {0x00, 0x01, 0x00, 0x00, 0x00, 0x00} the second entry is for UART1 */ #define UBX_TX_CFG_MSG_RATE1_1HZ 0x05 /**< {0x00, 0x05, 0x00, 0x00, 0x00, 0x00} the second entry is for UART1 */ @@ -383,6 +390,15 @@ typedef struct { uint32_t reserved4; } ubx_payload_tx_cfg_nav5_t; +/* tx cfg-sbas */ +typedef struct { + uint8_t mode; + uint8_t usage; + uint8_t maxSBAS; + uint8_t scanmode2; + uint32_t scanmode1; +} ubx_payload_tx_cfg_sbas_t; + /* Tx CFG-MSG */ typedef struct { union { @@ -413,6 +429,7 @@ typedef union { ubx_payload_tx_cfg_prt_t payload_tx_cfg_prt; ubx_payload_tx_cfg_rate_t payload_tx_cfg_rate; ubx_payload_tx_cfg_nav5_t payload_tx_cfg_nav5; + ubx_payload_tx_cfg_sbas_t payload_tx_cfg_sbas; ubx_payload_tx_cfg_msg_t payload_tx_cfg_msg; uint8_t raw[]; } ubx_buf_t; -- cgit v1.2.3 From 32131a069eeb77637a67e10011b4b756bf4d718a Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 22 Sep 2014 09:44:38 +0200 Subject: inform in GCS when switching to laser --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) (limited to 'src') diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index eb4ffdabb..ddfbc233b 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -814,7 +814,10 @@ float FixedwingPositionControl::get_terrain_altitude_landing(float land_setpoint /* Decide if the terrain estimation can be used, once we switched to using the terrain we stick with it * for the whole landing */ if (global_pos.terrain_alt_valid || land_useterrain) { - land_useterrain = true; + if(!land_useterrain) { + mavlink_log_info(_mavlink_fd, "#audio: Landing, using terrain estimate"); + land_useterrain = true; + } return global_pos.terrain_alt; } else { return land_setpoint_alt; -- cgit v1.2.3 From 852159d9edaa85c3abee3059359264f3329841e8 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 22 Sep 2014 10:03:23 +0200 Subject: fw pos control: add param to enable/disable usage of terrain estimate during landing --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 6 +++++- src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c | 8 +++----- 2 files changed, 8 insertions(+), 6 deletions(-) (limited to 'src') diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index ddfbc233b..a94311ccb 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -240,6 +240,7 @@ private: float land_heading_hold_horizontal_distance; float land_flare_pitch_min_deg; float land_flare_pitch_max_deg; + int land_use_terrain_estimate; } _parameters; /**< local copies of interesting parameters */ @@ -287,6 +288,7 @@ private: param_t land_heading_hold_horizontal_distance; param_t land_flare_pitch_min_deg; param_t land_flare_pitch_max_deg; + param_t land_use_terrain_estimate; } _parameter_handles; /**< handles for interesting parameters */ @@ -474,6 +476,7 @@ FixedwingPositionControl::FixedwingPositionControl() : _parameter_handles.land_heading_hold_horizontal_distance = param_find("FW_LND_HHDIST"); _parameter_handles.land_flare_pitch_min_deg = param_find("FW_FLARE_PMIN"); _parameter_handles.land_flare_pitch_max_deg = param_find("FW_FLARE_PMAX"); + _parameter_handles.land_use_terrain_estimate= param_find("FW_LND_USETER"); _parameter_handles.time_const = param_find("FW_T_TIME_CONST"); _parameter_handles.time_const_throt = param_find("FW_T_THRO_CONST"); @@ -576,6 +579,7 @@ FixedwingPositionControl::parameters_update() param_get(_parameter_handles.land_heading_hold_horizontal_distance, &(_parameters.land_heading_hold_horizontal_distance)); param_get(_parameter_handles.land_flare_pitch_min_deg, &(_parameters.land_flare_pitch_min_deg)); param_get(_parameter_handles.land_flare_pitch_max_deg, &(_parameters.land_flare_pitch_max_deg)); + param_get(_parameter_handles.land_use_terrain_estimate, &(_parameters.land_use_terrain_estimate)); _l1_control.set_l1_damping(_parameters.l1_damping); _l1_control.set_l1_period(_parameters.l1_period); @@ -813,7 +817,7 @@ float FixedwingPositionControl::get_terrain_altitude_landing(float land_setpoint /* Decide if the terrain estimation can be used, once we switched to using the terrain we stick with it * for the whole landing */ - if (global_pos.terrain_alt_valid || land_useterrain) { + if (_parameters.land_use_terrain_estimate && (global_pos.terrain_alt_valid || land_useterrain)) { if(!land_useterrain) { mavlink_log_info(_mavlink_fd, "#audio: Landing, using terrain estimate"); land_useterrain = true; diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c index 41a366538..c00d82232 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c @@ -412,15 +412,13 @@ PARAM_DEFINE_FLOAT(FW_LND_TLALT, -1.0f); PARAM_DEFINE_FLOAT(FW_LND_HHDIST, 15.0f); /** - * Relative altitude threshold for range finder measurements for use during landing + * Enable or disable usage of terrain estimate during landing * - * range finder measurements will only be used if the estimated relative altitude (gobal_pos.alt - landing_waypoint.alt) is < FW_LND_RFRALT - * set to < 0 to disable - * the correct value of this parameter depends on your range measuring device as well as on the terrain at the landing location + * 0: disabled, 1: enabled * * @group L1 Control */ -PARAM_DEFINE_FLOAT(FW_LND_RFRALT, -1.0f); +PARAM_DEFINE_INT32(FW_LND_USETER, 0); /** * Flare, minimum pitch -- cgit v1.2.3 From 6da52c5543012a1391c5e5c4384434c1292b919b Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 30 Sep 2014 15:39:01 +0200 Subject: remove unnecessary variable --- src/modules/navigator/navigator_main.cpp | 2 -- 1 file changed, 2 deletions(-) (limited to 'src') diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index fad46998e..b81155efe 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -388,11 +388,9 @@ Navigator::task_main() /* global position updated */ if (fds[0].revents & POLLIN) { global_position_update(); - static int gposcounter = 0; if (_geofence.getSource() == Geofence::GF_SOURCE_GLOBALPOS) { have_geofence_position_data = true; } - gposcounter++; } /* Check geofence violation */ -- cgit v1.2.3 From 4fbeb73bda55f14d1ffd259d064e116047b20301 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 30 Sep 2014 15:48:48 +0200 Subject: fix merge fail, remove double declaration of circuit breaker --- src/modules/systemlib/circuit_breaker.c | 14 -------------- 1 file changed, 14 deletions(-) (limited to 'src') diff --git a/src/modules/systemlib/circuit_breaker.c b/src/modules/systemlib/circuit_breaker.c index 8ad79f94d..12187d70e 100644 --- a/src/modules/systemlib/circuit_breaker.c +++ b/src/modules/systemlib/circuit_breaker.c @@ -136,20 +136,6 @@ PARAM_DEFINE_INT32(CBRK_ENGINEFAIL, 284953); */ PARAM_DEFINE_INT32(CBRK_GPSFAIL, 240024); -/** - * Circuit breaker for gps failure detection - * - * Setting this parameter to 240024 will disable the gps failure detection. - * If the aircraft is in gps failure mode the gps failure flag will be - * set to healthy - * WARNING: ENABLING THIS CIRCUIT BREAKER IS AT OWN RISK - * - * @min 0 - * @max 240024 - * @group Circuit Breaker - */ -PARAM_DEFINE_INT32(CBRK_GPSFAIL, 240024); - bool circuit_breaker_enabled(const char* breaker, int32_t magic) { int32_t val; -- cgit v1.2.3 From f4851c1b148f21143c4a0446a843948af6793ed3 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 30 Sep 2014 15:50:58 +0200 Subject: ubx: disable sbas configuration per default --- src/drivers/gps/ubx.cpp | 2 ++ 1 file changed, 2 insertions(+) (limited to 'src') diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp index c54c474b3..45bd445c4 100644 --- a/src/drivers/gps/ubx.cpp +++ b/src/drivers/gps/ubx.cpp @@ -189,6 +189,7 @@ UBX::configure(unsigned &baudrate) return 1; } +#ifdef UBX_CONFIGURE_SBAS /* send a SBAS message to set the SBAS options */ memset(&_buf.payload_tx_cfg_sbas, 0, sizeof(_buf.payload_tx_cfg_sbas)); _buf.payload_tx_cfg_sbas.mode = UBX_TX_CFG_SBAS_MODE; @@ -198,6 +199,7 @@ UBX::configure(unsigned &baudrate) if (wait_for_ack(UBX_MSG_CFG_SBAS, UBX_CONFIG_TIMEOUT, true) < 0) { return 1; } +#endif /* configure message rates */ /* the last argument is divisor for measurement rate (set by CFG RATE), i.e. 1 means 5Hz */ -- cgit v1.2.3 From 5452732610bd1d5b795558fa8f9d9ffb2639dc0b Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 30 Sep 2014 15:54:27 +0200 Subject: switch back to common mavlink dialect --- src/modules/mavlink/mavlink_bridge_header.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src') diff --git a/src/modules/mavlink/mavlink_bridge_header.h b/src/modules/mavlink/mavlink_bridge_header.h index 00d427636..0cd08769e 100644 --- a/src/modules/mavlink/mavlink_bridge_header.h +++ b/src/modules/mavlink/mavlink_bridge_header.h @@ -84,7 +84,7 @@ void mavlink_send_uart_bytes(mavlink_channel_t chan, const uint8_t *ch, int leng extern mavlink_status_t *mavlink_get_channel_status(uint8_t chan); extern mavlink_message_t *mavlink_get_channel_buffer(uint8_t chan); -#include +#include __END_DECLS -- cgit v1.2.3 From 2766285d56cab6f9916efde535b979bc5475f6a5 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 30 Sep 2014 16:00:14 +0200 Subject: mavlink: change message buffer size to 4 --- src/modules/mavlink/mavlink_main.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) (limited to 'src') diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index b9f486c95..980362ce5 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1328,7 +1328,7 @@ Mavlink::task_main(int argc, char *argv[]) * make space for two messages plus off-by-one space as we use the empty element * marker ring buffer approach. */ - if (OK != message_buffer_init(10 * sizeof(mavlink_message_t) + 1)) { + if (OK != message_buffer_init(4 * sizeof(mavlink_message_t) + 1)) { errx(1, "can't allocate message buffer, exiting"); } @@ -1382,7 +1382,7 @@ Mavlink::task_main(int argc, char *argv[]) _mission_manager->set_verbose(_verbose); LL_APPEND(_streams, _mission_manager); - + switch (_mode) { case MAVLINK_MODE_NORMAL: -- cgit v1.2.3 From 61352b8e1af8d076ea9f6edb9932f3b498a72e7a Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 5 Oct 2014 12:52:04 +0200 Subject: remove warnx --- src/drivers/gps/ubx.cpp | 2 -- 1 file changed, 2 deletions(-) (limited to 'src') diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp index 45bd445c4..b0eb4ab66 100644 --- a/src/drivers/gps/ubx.cpp +++ b/src/drivers/gps/ubx.cpp @@ -277,8 +277,6 @@ UBX::wait_for_ack(const uint16_t msg, const unsigned timeout, const bool report) } } - warnx("msg %u ret %d", msg, ret); - _ack_state = UBX_ACK_IDLE; return ret; } -- cgit v1.2.3 From ebc84b9f44dd3d26bc8d26b18f8c059f900e5e1a Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 5 Oct 2014 13:02:13 +0200 Subject: reduce mavlink message buffer size --- src/modules/mavlink/mavlink_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src') diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 980362ce5..25dc01844 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1328,7 +1328,7 @@ Mavlink::task_main(int argc, char *argv[]) * make space for two messages plus off-by-one space as we use the empty element * marker ring buffer approach. */ - if (OK != message_buffer_init(4 * sizeof(mavlink_message_t) + 1)) { + if (OK != message_buffer_init(2 * sizeof(mavlink_message_t) + 1)) { errx(1, "can't allocate message buffer, exiting"); } -- cgit v1.2.3 From ba2f55c3d7f5872aaf07e20b58b15df85417d43a Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 7 Oct 2014 10:09:03 +0200 Subject: Revert "increase ram" This reverts commit bc23b6239c50527aa550eed3bc6f17dec15c5c97. --- src/modules/mavlink/mavlink_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src') diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 25dc01844..34596320f 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1630,7 +1630,7 @@ Mavlink::start(int argc, char *argv[]) task_spawn_cmd(buf, SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, - 5000, + 2700, (main_t)&Mavlink::start_helper, (const char **)argv); -- cgit v1.2.3 From c7f7de352d7e0f2921526e33077c6da6a46b404a Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 7 Oct 2014 10:12:56 +0200 Subject: revert some of the OBC rate changes --- src/modules/mavlink/mavlink_main.cpp | 17 +++++++---------- 1 file changed, 7 insertions(+), 10 deletions(-) (limited to 'src') diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 34596320f..4738d1ea7 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1382,28 +1382,25 @@ Mavlink::task_main(int argc, char *argv[]) _mission_manager->set_verbose(_verbose); LL_APPEND(_streams, _mission_manager); - - switch (_mode) { case MAVLINK_MODE_NORMAL: configure_stream("SYS_STATUS", 1.0f); configure_stream("GPS_GLOBAL_ORIGIN", 0.5f); configure_stream("HIGHRES_IMU", 1.0f); - configure_stream("ATTITUDE", 4.0f); - configure_stream("VFR_HUD", 4.0f); + configure_stream("ATTITUDE", 10.0f); + configure_stream("VFR_HUD", 8.0f); configure_stream("GPS_RAW_INT", 1.0f); - configure_stream("GLOBAL_POSITION_INT", 1.0f); - configure_stream("LOCAL_POSITION_NED", 1.0f); + configure_stream("GLOBAL_POSITION_INT", 3.0f); + configure_stream("LOCAL_POSITION_NED", 3.0f); configure_stream("RC_CHANNELS_RAW", 1.0f); - configure_stream("POSITION_TARGET_GLOBAL_INT", 1.0f); - configure_stream("ATTITUDE_TARGET", 1.0f); + configure_stream("POSITION_TARGET_GLOBAL_INT", 3.0f); + configure_stream("ATTITUDE_TARGET", 3.0f); configure_stream("DISTANCE_SENSOR", 0.5f); - //configure_stream("OPTICAL_FLOW", 20.0f); + configure_stream("OPTICAL_FLOW", 5.0f); break; case MAVLINK_MODE_ONBOARD: configure_stream("SYS_STATUS", 1.0f); - // XXX OBC change back: We need to be bandwidth-efficient here too configure_stream("ATTITUDE", 50.0f); configure_stream("GLOBAL_POSITION_INT", 50.0f); configure_stream("CAMERA_CAPTURE", 2.0f); -- cgit v1.2.3 From 72fbd76c84f8ecfc6844b3d23fa90ce0c3bc1227 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 7 Oct 2014 12:46:09 +0200 Subject: Updated and fixed parser for SF02/F laser sensor, test harness runs clean --- Tools/tests-host/.gitignore | 1 + Tools/tests-host/Makefile | 5 +- Tools/tests-host/sf0x_test | Bin 9228 -> 0 bytes Tools/tests-host/sf0x_test.cpp | 137 +++++++------------------------------ src/drivers/sf0x/sf0x_parser.cpp | 143 +++++++++++++++++++++++++++++++++++++++ src/drivers/sf0x/sf0x_parser.h | 51 ++++++++++++++ 6 files changed, 222 insertions(+), 115 deletions(-) delete mode 100755 Tools/tests-host/sf0x_test create mode 100644 src/drivers/sf0x/sf0x_parser.cpp create mode 100644 src/drivers/sf0x/sf0x_parser.h (limited to 'src') diff --git a/Tools/tests-host/.gitignore b/Tools/tests-host/.gitignore index 87b314c61..ec11e0fec 100644 --- a/Tools/tests-host/.gitignore +++ b/Tools/tests-host/.gitignore @@ -1,4 +1,5 @@ ./obj/* mixer_test +sf0x_test sbus2_test autodeclination_test diff --git a/Tools/tests-host/Makefile b/Tools/tests-host/Makefile index b1547c23f..903c3a4ca 100644 --- a/Tools/tests-host/Makefile +++ b/Tools/tests-host/Makefile @@ -22,9 +22,10 @@ SBUS2_FILES=../../src/modules/px4iofirmware/sbus.c \ SF0X_FILES= \ hrt.cpp \ - sf0x_test.cpp + sf0x_test.cpp \ + ../../src/drivers/sf0x/sf0x_parser.cpp -AUTODECLINATION_FILES= ../../src/lib/geo/geo_mag_declination.c \ +AUTODECLINATION_FILES= ../../src/lib/geo_lookup/geo_mag_declination.c \ hrt.cpp \ autodeclination_test.cpp diff --git a/Tools/tests-host/sf0x_test b/Tools/tests-host/sf0x_test deleted file mode 100755 index e56c35465..000000000 Binary files a/Tools/tests-host/sf0x_test and /dev/null differ diff --git a/Tools/tests-host/sf0x_test.cpp b/Tools/tests-host/sf0x_test.cpp index d41f7e54a..ebc893b60 100644 --- a/Tools/tests-host/sf0x_test.cpp +++ b/Tools/tests-host/sf0x_test.cpp @@ -1,152 +1,63 @@ -#include #include #include #include +#include #include #include -enum SF0X_PARSE_STATE { - SF0X_PARSE_STATE0_UNSYNC = 0, - SF0X_PARSE_STATE1_SYNC, - SF0X_PARSE_STATE2_GOT_DIGIT0, - SF0X_PARSE_STATE3_GOT_DOT, - SF0X_PARSE_STATE4_GOT_DIGIT1, - SF0X_PARSE_STATE5_GOT_DIGIT2, - SF0X_PARSE_STATE6_GOT_CARRIAGE_RETURN -}; - -int sf0x_parser(char c, char *parserbuf, unsigned *parserbuf_index, enum *SF0X_PARSE_STATE state, float *dist) -{ - int ret = -1; - - unsigned len = strlen(parserbuf); - switch (state) { - case SF0X_PARSE_STATE0_UNSYNC: - if (c == '\n') { - *state = SF0X_PARSE_STATE1_SYNC; - *parserbuf_index = 0; - } - break; - - case SF0X_PARSE_STATE1_SYNC: - if (c >= '0' && c <= '9') { - *state = SF0X_PARSE_STATE2_GOT_DIGIT0; - parserbuf[parserbuf_index] = c; - parserbuf_index++; - } - break; - - case SF0X_PARSE_STATE2_GOT_DIGIT0: - if (c >= '0' && c <= '9') { - *state = SF0X_PARSE_STATE2_GOT_DIGIT0; - parserbuf[parserbuf_index] = c; - parserbuf_index++; - } else if (c == '.') { - *state = SF0X_PARSE_STATE3_GOT_DOT; - parserbuf[parserbuf_index] = c; - parserbuf_index++; - } else { - *state = SF0X_PARSE_STATE0_UNSYNC; - } - break; - - case SF0X_PARSE_STATE3_GOT_DOT: - if (c >= '0' && c <= '9') { - *state = SF0X_PARSE_STATE4_GOT_DIGIT1; - parserbuf[parserbuf_index] = c; - parserbuf_index++; - } else { - *state = SF0X_PARSE_STATE0_UNSYNC; - } - break; - - case SF0X_PARSE_STATE4_GOT_DIGIT1: - if (c >= '0' && c <= '9') { - *state = SF0X_PARSE_STATE4_GOT_DIGIT2; - parserbuf[parserbuf_index] = c; - parserbuf_index++; - } else { - *state = SF0X_PARSE_STATE0_UNSYNC; - } - break; - - case SF0X_PARSE_STATE5_GOT_DIGIT2: - if (c == '\r') { - *state = SF0X_PARSE_STATE6_GOT_CARRIAGE_RETURN; - } else { - *state = SF0X_PARSE_STATE0_UNSYNC; - } - break; - - case SF0X_PARSE_STATE6_GOT_CARRIAGE_RETURN: - if (c == '\n') { - parserbuf[parserbuf_index] = '\0'; - *dist = strtod(linebuf, &end); - *state = SF0X_PARSE_STATE0_SYNC; - *parserbuf_index = 0; - ret = 0; - } else { - *state = SF0X_PARSE_STATE0_UNSYNC; - } - break; - } - - - - return ret; -} +#include int main(int argc, char *argv[]) { warnx("SF0X test started"); - if (argc < 2) - errx(1, "Need a filename for the input file"); - - warnx("loading data from: %s", argv[1]); - - FILE *fp; - - fp = fopen(argv[1],"rt"); - - if (!fp) - errx(1, "failed opening file"); - int ret = 0; const char LINE_MAX = 20; char _linebuf[LINE_MAX]; _linebuf[0] = '\0'; - char *end; + const char *lines[] = {"0.01\r\n", + "0.02\r\n", + "0.03\r\n", + "0.04\r\n", + "0", + ".", + "0", + "5", + "\r", + "\n", + "0", + "3\r", + "\n" + "\r\n", + "0.06", + "\r\n" + }; enum SF0X_PARSE_STATE state = SF0X_PARSE_STATE0_UNSYNC; float dist_m; char _parserbuf[LINE_MAX]; unsigned _parsebuf_index = 0; - while (fgets(_linebuf, LINE_MAX, fp) != NULL) { + for (unsigned l = 0; l < sizeof(lines) / sizeof(lines[0]); l++) { printf("\n%s", _linebuf); int parse_ret; - for (int i = 0; i < strlen(_linebuf); i++) + for (int i = 0; i < strlen(lines[l]); i++) { - printf("%0x ", _linebuf[i]); - parse_ret = sf0x_parser(_linebuf[i], _parserbuf, &_parsebuf_index, &state, &dist_m); + parse_ret = sf0x_parser(lines[l][i], _parserbuf, &_parsebuf_index, &state, &dist_m); if (parse_ret == 0) { - printf("PARSED!"); + printf("\nparsed: %f %s\n", dist_m, (parse_ret == 0) ? "OK" : ""); } } + printf("%s", lines[l]); - printf("\nparsed: %f %s\n", dist_m, (parse_ret == 0) ? "OK" : ""); } - // Init the parser - - if (ret == EOF) { warnx("Test finished, reached end of file"); } else { diff --git a/src/drivers/sf0x/sf0x_parser.cpp b/src/drivers/sf0x/sf0x_parser.cpp new file mode 100644 index 000000000..b9ceb52f5 --- /dev/null +++ b/src/drivers/sf0x/sf0x_parser.cpp @@ -0,0 +1,143 @@ +/**************************************************************************** + * + * Copyright (c) 2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file sf0x_parser.cpp + * @author Lorenz Meier + * + * Driver for the Lightware SF0x laser rangefinder series + */ + +#include "sf0x_parser.h" +#include +#include + +//#define SF0X_DEBUG + +#ifdef SF0X_DEBUG +#include + +const char* parser_state[] = { + "0_UNSYNC", + "1_SYNC", + "2_GOT_DIGIT0", + "3_GOT_DOT", + "4_GOT_DIGIT1", + "5_GOT_DIGIT2", + "6_GOT_CARRIAGE_RETURN" +}; +#endif + +int sf0x_parser(char c, char *parserbuf, unsigned *parserbuf_index, enum SF0X_PARSE_STATE *state, float *dist) +{ + int ret = -1; + char *end; + + unsigned len = strlen(parserbuf); + switch (*state) { + case SF0X_PARSE_STATE0_UNSYNC: + if (c == '\n') { + *state = SF0X_PARSE_STATE1_SYNC; + (*parserbuf_index) = 0; + } + break; + + case SF0X_PARSE_STATE1_SYNC: + if (c >= '0' && c <= '9') { + *state = SF0X_PARSE_STATE2_GOT_DIGIT0; + parserbuf[*parserbuf_index] = c; + (*parserbuf_index)++; + } + break; + + case SF0X_PARSE_STATE2_GOT_DIGIT0: + if (c >= '0' && c <= '9') { + *state = SF0X_PARSE_STATE2_GOT_DIGIT0; + parserbuf[*parserbuf_index] = c; + (*parserbuf_index)++; + } else if (c == '.') { + *state = SF0X_PARSE_STATE3_GOT_DOT; + parserbuf[*parserbuf_index] = c; + (*parserbuf_index)++; + } else { + *state = SF0X_PARSE_STATE0_UNSYNC; + } + break; + + case SF0X_PARSE_STATE3_GOT_DOT: + if (c >= '0' && c <= '9') { + *state = SF0X_PARSE_STATE4_GOT_DIGIT1; + parserbuf[*parserbuf_index] = c; + (*parserbuf_index)++; + } else { + *state = SF0X_PARSE_STATE0_UNSYNC; + } + break; + + case SF0X_PARSE_STATE4_GOT_DIGIT1: + if (c >= '0' && c <= '9') { + *state = SF0X_PARSE_STATE5_GOT_DIGIT2; + parserbuf[*parserbuf_index] = c; + (*parserbuf_index)++; + } else { + *state = SF0X_PARSE_STATE0_UNSYNC; + } + break; + + case SF0X_PARSE_STATE5_GOT_DIGIT2: + if (c == '\r') { + *state = SF0X_PARSE_STATE6_GOT_CARRIAGE_RETURN; + } else { + *state = SF0X_PARSE_STATE0_UNSYNC; + } + break; + + case SF0X_PARSE_STATE6_GOT_CARRIAGE_RETURN: + if (c == '\n') { + parserbuf[*parserbuf_index] = '\0'; + *dist = strtod(parserbuf, &end); + *state = SF0X_PARSE_STATE1_SYNC; + *parserbuf_index = 0; + ret = 0; + } else { + *state = SF0X_PARSE_STATE0_UNSYNC; + } + break; + } + +#ifdef SF0X_DEBUG + printf("state: SF0X_PARSE_STATE%s\n", parser_state[*state]); +#endif + + return ret; +} \ No newline at end of file diff --git a/src/drivers/sf0x/sf0x_parser.h b/src/drivers/sf0x/sf0x_parser.h new file mode 100644 index 000000000..20892d50e --- /dev/null +++ b/src/drivers/sf0x/sf0x_parser.h @@ -0,0 +1,51 @@ +/**************************************************************************** + * + * Copyright (c) 2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file sf0x_parser.cpp + * @author Lorenz Meier + * + * Declarations of parser for the Lightware SF0x laser rangefinder series + */ + +enum SF0X_PARSE_STATE { + SF0X_PARSE_STATE0_UNSYNC = 0, + SF0X_PARSE_STATE1_SYNC, + SF0X_PARSE_STATE2_GOT_DIGIT0, + SF0X_PARSE_STATE3_GOT_DOT, + SF0X_PARSE_STATE4_GOT_DIGIT1, + SF0X_PARSE_STATE5_GOT_DIGIT2, + SF0X_PARSE_STATE6_GOT_CARRIAGE_RETURN +}; + +int sf0x_parser(char c, char *parserbuf, unsigned *parserbuf_index, enum SF0X_PARSE_STATE *state, float *dist); \ No newline at end of file -- cgit v1.2.3 From 4183444de626698137ab3fbb413a96a806da3ade Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 7 Oct 2014 12:50:00 +0200 Subject: Changed to proper parser for SF02/F laser rangefinder --- src/drivers/sf0x/module.mk | 3 +- src/drivers/sf0x/sf0x.cpp | 89 ++++++++-------------------------------- src/drivers/sf0x/sf0x_parser.cpp | 1 - 3 files changed, 18 insertions(+), 75 deletions(-) (limited to 'src') diff --git a/src/drivers/sf0x/module.mk b/src/drivers/sf0x/module.mk index dc2c66d56..26e77d1cc 100644 --- a/src/drivers/sf0x/module.mk +++ b/src/drivers/sf0x/module.mk @@ -37,6 +37,7 @@ MODULE_COMMAND = sf0x -SRCS = sf0x.cpp +SRCS = sf0x.cpp \ + sf0x_parser.cpp MAXOPTIMIZATION = -Os diff --git a/src/drivers/sf0x/sf0x.cpp b/src/drivers/sf0x/sf0x.cpp index d382d08d0..52f7413a9 100644 --- a/src/drivers/sf0x/sf0x.cpp +++ b/src/drivers/sf0x/sf0x.cpp @@ -72,6 +72,8 @@ #include +#include "sf0x_parser.h" + /* Configuration Constants */ /* oddly, ERROR is not defined for c++ */ @@ -120,6 +122,7 @@ private: int _fd; char _linebuf[10]; unsigned _linebuf_index; + enum SF0X_PARSE_STATE _parse_state; hrt_abstime _last_read; orb_advert_t _range_finder_topic; @@ -186,6 +189,7 @@ SF0X::SF0X(const char *port) : _collect_phase(false), _fd(-1), _linebuf_index(0), + _parse_state(SF0X_PARSE_STATE0_UNSYNC), _last_read(0), _range_finder_topic(-1), _consecutive_fail_count(0), @@ -526,14 +530,14 @@ SF0X::collect() } /* the buffer for read chars is buflen minus null termination */ - unsigned readlen = sizeof(_linebuf) - 1; + char readbuf[20]; + unsigned readlen = sizeof(readbuf) - 1; /* read from the sensor (uart buffer) */ - ret = ::read(_fd, &_linebuf[_linebuf_index], readlen - _linebuf_index); + ret = ::read(_fd, &readbuf[0], readlen); if (ret < 0) { - _linebuf[sizeof(_linebuf) - 1] = '\0'; - debug("read err: %d lbi: %d buf: %s", ret, (int)_linebuf_index, _linebuf); + debug("read err: %d", ret); perf_count(_comms_errors); perf_end(_sample_perf); @@ -548,84 +552,23 @@ SF0X::collect() return -EAGAIN; } - /* let the write pointer point to the next free entry */ - _linebuf_index += ret; - _last_read = hrt_absolute_time(); - /* require a reasonable amount of minimum bytes */ - if (_linebuf_index < 6) { - /* we need at this format: x.xx\r\n */ - return -EAGAIN; - - } else if (_linebuf[_linebuf_index - 2] != '\r' || _linebuf[_linebuf_index - 1] != '\n') { - - if (_linebuf_index == readlen) { - /* we have a full buffer, but no line ending - abort */ - _linebuf_index = 0; - perf_count(_comms_errors); - return -ENOMEM; - } else { - /* incomplete read, reschedule ourselves */ - return -EAGAIN; - } - } - - char *end; float si_units; - bool valid; - - /* enforce line ending */ - _linebuf[_linebuf_index] = '\0'; - - if (_linebuf[0] == '-' && _linebuf[1] == '-' && _linebuf[2] == '.') { - si_units = -1.0f; - valid = false; - - } else { - - /* we need to find a dot in the string, as we're missing the meters part else */ - valid = false; - - /* wipe out partially read content from last cycle(s), check for dot */ - for (unsigned i = 0; i < (_linebuf_index - 2); i++) { - if (_linebuf[i] == '\n') { - /* wipe out any partial measurements */ - for (unsigned j = 0; j <= i; j++) { - _linebuf[j] = ' '; - } - } - - /* we need a digit before the dot and a dot for a valid number */ - if (i > 0 && ((_linebuf[i - 1] >= '0') && (_linebuf[i - 1] <= '9')) && (_linebuf[i] == '.')) { - valid = true; - } - } - - if (valid) { - si_units = strtod(_linebuf, &end); - - /* we require at least four characters for a valid number */ - if (end > _linebuf + 3) { - valid = true; - } else { - si_units = -1.0f; - valid = false; - } + bool valid = false; + + for (unsigned i = 0; i < ret; i++) { + if (OK == sf0x_parser(readbuf[i], _linebuf, &_linebuf_index, &_parse_state, &si_units)) { + valid = true; } } - debug("val (float): %8.4f, raw: %s, valid: %s", (double)si_units, _linebuf, ((valid) ? "OK" : "NO")); - - /* done with this chunk, resetting - even if invalid */ - _linebuf_index = 0; - - /* if its invalid, there is no reason to forward the value */ if (!valid) { - perf_count(_comms_errors); - return -EINVAL; + return -EAGAIN; } + debug("val (float): %8.4f, raw: %s, valid: %s", (double)si_units, _linebuf, ((valid) ? "OK" : "NO")); + struct range_finder_report report; /* this should be fairly close to the end of the measurement, so the best approximation of the time */ diff --git a/src/drivers/sf0x/sf0x_parser.cpp b/src/drivers/sf0x/sf0x_parser.cpp index b9ceb52f5..cddc3fe99 100644 --- a/src/drivers/sf0x/sf0x_parser.cpp +++ b/src/drivers/sf0x/sf0x_parser.cpp @@ -63,7 +63,6 @@ int sf0x_parser(char c, char *parserbuf, unsigned *parserbuf_index, enum SF0X_PA int ret = -1; char *end; - unsigned len = strlen(parserbuf); switch (*state) { case SF0X_PARSE_STATE0_UNSYNC: if (c == '\n') { -- cgit v1.2.3 From 4ba4135c3b74f786b6ad795e2e9efd7271409b7f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 7 Oct 2014 12:52:48 +0200 Subject: Code style fixes, no code changes --- Tools/tests-host/sf0x_test.cpp | 39 +++++----- src/drivers/sf0x/sf0x_parser.cpp | 151 +++++++++++++++++++++------------------ 2 files changed, 102 insertions(+), 88 deletions(-) (limited to 'src') diff --git a/Tools/tests-host/sf0x_test.cpp b/Tools/tests-host/sf0x_test.cpp index 6b98c2427..82d19fcbe 100644 --- a/Tools/tests-host/sf0x_test.cpp +++ b/Tools/tests-host/sf0x_test.cpp @@ -8,7 +8,8 @@ #include -int main(int argc, char *argv[]) { +int main(int argc, char *argv[]) +{ warnx("SF0X test started"); int ret = 0; @@ -18,22 +19,22 @@ int main(int argc, char *argv[]) { _linebuf[0] = '\0'; const char *lines[] = {"0.01\r\n", - "0.02\r\n", - "0.03\r\n", - "0.04\r\n", - "0", - ".", - "0", - "5", - "\r", - "\n", - "0", - "3\r", - "\n" - "\r\n", - "0.06", - "\r\n" - }; + "0.02\r\n", + "0.03\r\n", + "0.04\r\n", + "0", + ".", + "0", + "5", + "\r", + "\n", + "0", + "3\r", + "\n" + "\r\n", + "0.06", + "\r\n" + }; enum SF0X_PARSE_STATE state = SF0X_PARSE_STATE0_UNSYNC; float dist_m; @@ -46,14 +47,14 @@ int main(int argc, char *argv[]) { int parse_ret; - for (int i = 0; i < strlen(lines[l]); i++) - { + for (int i = 0; i < strlen(lines[l]); i++) { parse_ret = sf0x_parser(lines[l][i], _parserbuf, &_parsebuf_index, &state, &dist_m); if (parse_ret == 0) { printf("\nparsed: %f %s\n", dist_m, (parse_ret == 0) ? "OK" : ""); } } + printf("%s", lines[l]); } diff --git a/src/drivers/sf0x/sf0x_parser.cpp b/src/drivers/sf0x/sf0x_parser.cpp index cddc3fe99..8e73b0ad3 100644 --- a/src/drivers/sf0x/sf0x_parser.cpp +++ b/src/drivers/sf0x/sf0x_parser.cpp @@ -47,7 +47,7 @@ #ifdef SF0X_DEBUG #include -const char* parser_state[] = { +const char *parser_state[] = { "0_UNSYNC", "1_SYNC", "2_GOT_DIGIT0", @@ -64,74 +64,87 @@ int sf0x_parser(char c, char *parserbuf, unsigned *parserbuf_index, enum SF0X_PA char *end; switch (*state) { - case SF0X_PARSE_STATE0_UNSYNC: - if (c == '\n') { - *state = SF0X_PARSE_STATE1_SYNC; - (*parserbuf_index) = 0; - } - break; - - case SF0X_PARSE_STATE1_SYNC: - if (c >= '0' && c <= '9') { - *state = SF0X_PARSE_STATE2_GOT_DIGIT0; - parserbuf[*parserbuf_index] = c; - (*parserbuf_index)++; - } - break; - - case SF0X_PARSE_STATE2_GOT_DIGIT0: - if (c >= '0' && c <= '9') { - *state = SF0X_PARSE_STATE2_GOT_DIGIT0; - parserbuf[*parserbuf_index] = c; - (*parserbuf_index)++; - } else if (c == '.') { - *state = SF0X_PARSE_STATE3_GOT_DOT; - parserbuf[*parserbuf_index] = c; - (*parserbuf_index)++; - } else { - *state = SF0X_PARSE_STATE0_UNSYNC; - } - break; - - case SF0X_PARSE_STATE3_GOT_DOT: - if (c >= '0' && c <= '9') { - *state = SF0X_PARSE_STATE4_GOT_DIGIT1; - parserbuf[*parserbuf_index] = c; - (*parserbuf_index)++; - } else { - *state = SF0X_PARSE_STATE0_UNSYNC; - } - break; - - case SF0X_PARSE_STATE4_GOT_DIGIT1: - if (c >= '0' && c <= '9') { - *state = SF0X_PARSE_STATE5_GOT_DIGIT2; - parserbuf[*parserbuf_index] = c; - (*parserbuf_index)++; - } else { - *state = SF0X_PARSE_STATE0_UNSYNC; - } - break; - - case SF0X_PARSE_STATE5_GOT_DIGIT2: - if (c == '\r') { - *state = SF0X_PARSE_STATE6_GOT_CARRIAGE_RETURN; - } else { - *state = SF0X_PARSE_STATE0_UNSYNC; - } - break; - - case SF0X_PARSE_STATE6_GOT_CARRIAGE_RETURN: - if (c == '\n') { - parserbuf[*parserbuf_index] = '\0'; - *dist = strtod(parserbuf, &end); - *state = SF0X_PARSE_STATE1_SYNC; - *parserbuf_index = 0; - ret = 0; - } else { - *state = SF0X_PARSE_STATE0_UNSYNC; - } - break; + case SF0X_PARSE_STATE0_UNSYNC: + if (c == '\n') { + *state = SF0X_PARSE_STATE1_SYNC; + (*parserbuf_index) = 0; + } + + break; + + case SF0X_PARSE_STATE1_SYNC: + if (c >= '0' && c <= '9') { + *state = SF0X_PARSE_STATE2_GOT_DIGIT0; + parserbuf[*parserbuf_index] = c; + (*parserbuf_index)++; + } + + break; + + case SF0X_PARSE_STATE2_GOT_DIGIT0: + if (c >= '0' && c <= '9') { + *state = SF0X_PARSE_STATE2_GOT_DIGIT0; + parserbuf[*parserbuf_index] = c; + (*parserbuf_index)++; + + } else if (c == '.') { + *state = SF0X_PARSE_STATE3_GOT_DOT; + parserbuf[*parserbuf_index] = c; + (*parserbuf_index)++; + + } else { + *state = SF0X_PARSE_STATE0_UNSYNC; + } + + break; + + case SF0X_PARSE_STATE3_GOT_DOT: + if (c >= '0' && c <= '9') { + *state = SF0X_PARSE_STATE4_GOT_DIGIT1; + parserbuf[*parserbuf_index] = c; + (*parserbuf_index)++; + + } else { + *state = SF0X_PARSE_STATE0_UNSYNC; + } + + break; + + case SF0X_PARSE_STATE4_GOT_DIGIT1: + if (c >= '0' && c <= '9') { + *state = SF0X_PARSE_STATE5_GOT_DIGIT2; + parserbuf[*parserbuf_index] = c; + (*parserbuf_index)++; + + } else { + *state = SF0X_PARSE_STATE0_UNSYNC; + } + + break; + + case SF0X_PARSE_STATE5_GOT_DIGIT2: + if (c == '\r') { + *state = SF0X_PARSE_STATE6_GOT_CARRIAGE_RETURN; + + } else { + *state = SF0X_PARSE_STATE0_UNSYNC; + } + + break; + + case SF0X_PARSE_STATE6_GOT_CARRIAGE_RETURN: + if (c == '\n') { + parserbuf[*parserbuf_index] = '\0'; + *dist = strtod(parserbuf, &end); + *state = SF0X_PARSE_STATE1_SYNC; + *parserbuf_index = 0; + ret = 0; + + } else { + *state = SF0X_PARSE_STATE0_UNSYNC; + } + + break; } #ifdef SF0X_DEBUG -- cgit v1.2.3 From 760e72cc721c6a2b90515d37eba705c30be4928e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 7 Oct 2014 14:25:07 +0200 Subject: Remove unnecessary commands, unneeded output / resets --- src/drivers/sf0x/sf0x.cpp | 13 +------------ 1 file changed, 1 insertion(+), 12 deletions(-) (limited to 'src') diff --git a/src/drivers/sf0x/sf0x.cpp b/src/drivers/sf0x/sf0x.cpp index 52f7413a9..801bcf40a 100644 --- a/src/drivers/sf0x/sf0x.cpp +++ b/src/drivers/sf0x/sf0x.cpp @@ -204,12 +204,6 @@ SF0X::SF0X(const char *port) : warnx("FAIL: laser fd"); } - /* tell it to stop auto-triggering */ - char stop_auto = ' '; - (void)::write(_fd, &stop_auto, 1); - usleep(100); - (void)::write(_fd, &stop_auto, 1); - struct termios uart_config; int termios_state; @@ -524,13 +518,8 @@ SF0X::collect() /* clear buffer if last read was too long ago */ uint64_t read_elapsed = hrt_elapsed_time(&_last_read); - /* timed out - retry */ - if (read_elapsed > (SF0X_CONVERSION_INTERVAL * 2)) { - _linebuf_index = 0; - } - /* the buffer for read chars is buflen minus null termination */ - char readbuf[20]; + char readbuf[sizeof(_linebuf)]; unsigned readlen = sizeof(readbuf) - 1; /* read from the sensor (uart buffer) */ -- cgit v1.2.3 From c58d845339c0d09fc703a1c730d89a7a00990906 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 7 Oct 2014 14:27:09 +0200 Subject: Remove range command --- src/drivers/sf0x/sf0x.cpp | 5 +++++ 1 file changed, 5 insertions(+) (limited to 'src') diff --git a/src/drivers/sf0x/sf0x.cpp b/src/drivers/sf0x/sf0x.cpp index 801bcf40a..11913b6cd 100644 --- a/src/drivers/sf0x/sf0x.cpp +++ b/src/drivers/sf0x/sf0x.cpp @@ -494,6 +494,10 @@ SF0X::measure() /* * Send the command to begin a measurement. */ + + // XXX we probably should not try to talk to the sensor + // it seems to mind UART traffic. + #if 0 char cmd = SF0X_TAKE_RANGE_REG; ret = ::write(_fd, &cmd, 1); @@ -502,6 +506,7 @@ SF0X::measure() log("write fail %d", ret); return ret; } + #endif ret = OK; -- cgit v1.2.3 From 537991b83ca979c4a41465896cda789d9ebb6370 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 7 Oct 2014 14:48:41 +0200 Subject: Revert "Remove range command" This reverts commit c58d845339c0d09fc703a1c730d89a7a00990906. --- src/drivers/sf0x/sf0x.cpp | 5 ----- 1 file changed, 5 deletions(-) (limited to 'src') diff --git a/src/drivers/sf0x/sf0x.cpp b/src/drivers/sf0x/sf0x.cpp index 11913b6cd..801bcf40a 100644 --- a/src/drivers/sf0x/sf0x.cpp +++ b/src/drivers/sf0x/sf0x.cpp @@ -494,10 +494,6 @@ SF0X::measure() /* * Send the command to begin a measurement. */ - - // XXX we probably should not try to talk to the sensor - // it seems to mind UART traffic. - #if 0 char cmd = SF0X_TAKE_RANGE_REG; ret = ::write(_fd, &cmd, 1); @@ -506,7 +502,6 @@ SF0X::measure() log("write fail %d", ret); return ret; } - #endif ret = OK; -- cgit v1.2.3