From 23237df84ef1fcf955c739c721a15ca356fddf96 Mon Sep 17 00:00:00 2001 From: Juchli D Date: Tue, 5 Nov 2013 13:24:41 +0100 Subject: fmu: Also take into account actuator group 1 --- src/drivers/px4fmu/fmu.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 0441566e9..a2c62cf06 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -542,7 +542,8 @@ PX4FMU::task_main() if (fds[0].revents & POLLIN) { /* get controls - must always do this to avoid spinning */ - orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, _t_actuators, &_controls); + orb_copy(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS : + ORB_ID(actuator_controls_1), _t_actuators, &_controls); /* can we mix? */ if (_mixers != nullptr) { -- cgit v1.2.3 From 56f1ea9266eacd689a7b4065231d94a78ea96de6 Mon Sep 17 00:00:00 2001 From: Juchli D Date: Tue, 5 Nov 2013 13:25:49 +0100 Subject: Added Bottle Drop app --- makefiles/config_px4fmu-v2_default.mk | 5 + src/modules/bottle_drop/bottle_drop.c | 188 ++++++++++++++++++++++++++++++++++ src/modules/bottle_drop/module.mk | 40 ++++++++ 3 files changed, 233 insertions(+) create mode 100644 src/modules/bottle_drop/bottle_drop.c create mode 100644 src/modules/bottle_drop/module.mk diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index 761fb8d9d..a2fb63422 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -114,6 +114,11 @@ MODULES += lib/external_lgpl MODULES += lib/geo MODULES += lib/conversion +# +# OBC challenge +# +MODULES += modules/bottle_drop + # # Demo apps # diff --git a/src/modules/bottle_drop/bottle_drop.c b/src/modules/bottle_drop/bottle_drop.c new file mode 100644 index 000000000..69720b439 --- /dev/null +++ b/src/modules/bottle_drop/bottle_drop.c @@ -0,0 +1,188 @@ +/**************************************************************************** + * + * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file px4_daemon_app.c + * daemon application example for PX4 autopilot + * + * @author Example User + */ + +#include +#include +#include +#include + +#include +#include +#include +#include + +#include + +static bool thread_should_exit = false; /**< daemon exit flag */ +static bool thread_running = false; /**< daemon status flag */ +static int daemon_task; /**< Handle of daemon task / thread */ + +static bool drop = false; + +/** + * daemon management function. + */ +__EXPORT int bottle_drop_main(int argc, char *argv[]); + +/** + * Mainloop of daemon. + */ +int bottle_drop_thread_main(int argc, char *argv[]); + +/** + * Print the correct usage. + */ +static void usage(const char *reason); + +static void +usage(const char *reason) +{ + if (reason) + warnx("%s\n", reason); + errx(1, "usage: daemon {start|stop|status} [-p ]\n\n"); +} + +/** + * The daemon app only briefly exists to start + * the background job. The stack size assigned in the + * Makefile does only apply to this management task. + * + * The actual stack size should be set in the call + * to task_create(). + */ +int bottle_drop_main(int argc, char *argv[]) +{ + if (argc < 1) + usage("missing command"); + + if (!strcmp(argv[1], "start")) { + + if (thread_running) { + warnx("daemon already running\n"); + /* this is not an error */ + exit(0); + } + + thread_should_exit = false; + daemon_task = task_spawn_cmd("daemon", + SCHED_DEFAULT, + SCHED_PRIORITY_DEFAULT, + 4096, + bottle_drop_thread_main, + (argv) ? (const char **)&argv[2] : (const char **)NULL); + exit(0); + } + + if (!strcmp(argv[1], "drop")) { + drop = true; + exit(0); + } + + if (!strcmp(argv[1], "stop")) { + thread_should_exit = true; + exit(0); + } + + if (!strcmp(argv[1], "status")) { + if (thread_running) { + warnx("\trunning\n"); + } else { + warnx("\tnot started\n"); + } + exit(0); + } + + usage("unrecognized command"); + exit(1); +} + +int bottle_drop_thread_main(int argc, char *argv[]) { + + warnx("starting\n"); + + unsigned i = 0; + + thread_running = true; + + int rgbleds = open(RGBLED_DEVICE_PATH, 0); + + struct actuator_controls_s actuators; + memset(&actuators, 0, sizeof(actuators)); + + orb_advert_t actuator_pub = orb_advertise(ORB_ID(actuator_controls_1), &actuators); + + while (!thread_should_exit) { + +// warnx("in while!\n"); + // values from -1 to 1 + + if (drop) { + // drop here + drop = false; + + ioctl(rgbleds, RGBLED_SET_MODE, RGBLED_MODE_ON); + ioctl(rgbleds, RGBLED_SET_COLOR, RGBLED_COLOR_RED); + } + + + actuators.control[0] = 0.5f; + actuators.control[1] = 0.5f; + actuators.control[2] = 0.5f; + actuators.control[3] = 0.5f; + + + + actuators.timestamp = hrt_absolute_time(); + orb_publish(ORB_ID(actuator_controls_1), actuator_pub, &actuators); + + usleep(50); + + i++; + } + + warnx("exiting.\n"); + + thread_running = false; + + ioctl(rgbleds, RGBLED_SET_MODE, RGBLED_MODE_OFF); + close(rgbleds); + + return 0; +} diff --git a/src/modules/bottle_drop/module.mk b/src/modules/bottle_drop/module.mk new file mode 100644 index 000000000..222858b27 --- /dev/null +++ b/src/modules/bottle_drop/module.mk @@ -0,0 +1,40 @@ +############################################################################ +# +# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +# +# Daemon application +# + +MODULE_COMMAND = bottle_drop + +SRCS = bottle_drop.c -- cgit v1.2.3 From 4c9a4bc9a4686a2806a0119c262a744ca5517c02 Mon Sep 17 00:00:00 2001 From: Juchli D Date: Tue, 5 Nov 2013 13:26:34 +0100 Subject: Added custom start up of bottle drop --- ROMFS/px4fmu_common/init.d/900_bottle_drop_test | 46 +++++++++++++++++++++++++ ROMFS/px4fmu_common/init.d/rcS | 6 ++++ 2 files changed, 52 insertions(+) create mode 100644 ROMFS/px4fmu_common/init.d/900_bottle_drop_test diff --git a/ROMFS/px4fmu_common/init.d/900_bottle_drop_test b/ROMFS/px4fmu_common/init.d/900_bottle_drop_test new file mode 100644 index 000000000..522f78210 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/900_bottle_drop_test @@ -0,0 +1,46 @@ +#!nsh + +echo "[init] bottle drop test + +# +# Load default params for this platform +# +if param compare SYS_AUTOCONFIG 1 +then + # Set all params here, then disable autoconfig + param set SYS_AUTOCONFIG 0 + + param save +fi + +# +# Force some key parameters to sane values +# MAV_TYPE 2 = quadrotor +# +param set MAV_TYPE 2 + +set EXIT_ON_END no + +# +# Start and configure PX4IO and FMU interface +# +# Start MAVLink (depends on orb) +mavlink start +usleep 5000 + + +sh /etc/init.d/rc.io + +fmu mode_pwm + +mixer load /dev/px4fmu /etc/mixers/FMU_pass.mix + +pwm arm -d /dev/px4fmu + +bottle_drop start + + +if [ $EXIT_ON_END == yes ] +then + exit +fi diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index cff8446a6..6e954f58a 100755 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -309,6 +309,12 @@ then set MODE custom fi + if param compare SYS_AUTOSTART 900 + then + sh /etc/init.d/900_bottle_drop_test + set MODE custom + fi + # Start any custom extensions that might be missing if [ -f /fs/microsd/etc/rc.local ] then -- cgit v1.2.3 From af3a56f17f6863210b0bcbf9569754a8c4ba3e53 Mon Sep 17 00:00:00 2001 From: Juchli D Date: Tue, 5 Nov 2013 13:27:05 +0100 Subject: Hack to always arm --- src/drivers/px4fmu/fmu.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index a2c62cf06..38cfce2e9 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -587,6 +587,9 @@ PX4FMU::task_main() uint16_t pwm_limited[num_outputs]; + // XXX: hack: always armed + _armed = true; + pwm_limit_calc(_armed, num_outputs, _disarmed_pwm, _min_pwm, _max_pwm, outputs.output, pwm_limited, &_pwm_limit); /* output actual limited values */ -- cgit v1.2.3 From adee47978236aa113ee29f071f3498a60a802477 Mon Sep 17 00:00:00 2001 From: Juchli D Date: Fri, 15 Nov 2013 10:30:48 +0100 Subject: Working bottle Drop test --- ROMFS/px4fmu_common/init.d/900_bottle_drop_test | 29 +++++++++++ src/modules/bottle_drop/bottle_drop.c | 68 +++++++++++++++++++------ 2 files changed, 81 insertions(+), 16 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/900_bottle_drop_test b/ROMFS/px4fmu_common/init.d/900_bottle_drop_test index 522f78210..c0f50fec0 100644 --- a/ROMFS/px4fmu_common/init.d/900_bottle_drop_test +++ b/ROMFS/px4fmu_common/init.d/900_bottle_drop_test @@ -28,6 +28,32 @@ set EXIT_ON_END no mavlink start usleep 5000 +# +# Start the sensors and test them. +# +sh /etc/init.d/rc.sensors + +# +# Start logging (depends on sensors) +# +#sh /etc/init.d/rc.logging +sdlog2 start -r 200 -e -b 16 + + +# +# Start GPS interface (depends on orb) +# +gps start + +# +# Start the attitude estimator +# +attitude_estimator_ekf start + +# +# Start position estimator +# +position_estimator_inav start sh /etc/init.d/rc.io @@ -35,6 +61,9 @@ fmu mode_pwm mixer load /dev/px4fmu /etc/mixers/FMU_pass.mix +pwm min -d /dev/px4fmu -c 123 -p 900 +pwm max -d /dev/px4fmu -c 123 -p 2100 + pwm arm -d /dev/px4fmu bottle_drop start diff --git a/src/modules/bottle_drop/bottle_drop.c b/src/modules/bottle_drop/bottle_drop.c index 69720b439..09bbf4687 100644 --- a/src/modules/bottle_drop/bottle_drop.c +++ b/src/modules/bottle_drop/bottle_drop.c @@ -42,13 +42,16 @@ #include #include #include +#include #include #include #include #include +#include #include +#include static bool thread_should_exit = false; /**< daemon exit flag */ static bool thread_running = false; /**< daemon status flag */ @@ -101,7 +104,7 @@ int bottle_drop_main(int argc, char *argv[]) } thread_should_exit = false; - daemon_task = task_spawn_cmd("daemon", + daemon_task = task_spawn_cmd("bottle_drop", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, 4096, @@ -143,38 +146,71 @@ int bottle_drop_thread_main(int argc, char *argv[]) { int rgbleds = open(RGBLED_DEVICE_PATH, 0); + struct vehicle_attitude_s att; + memset(&att, 0, sizeof(att)); + int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude)); + orb_set_interval(vehicle_attitude_sub, 20); + struct actuator_controls_s actuators; memset(&actuators, 0, sizeof(actuators)); - orb_advert_t actuator_pub = orb_advertise(ORB_ID(actuator_controls_1), &actuators); + struct pollfd fds[1] = { + { .fd = vehicle_attitude_sub, .events = POLLIN }, + }; + + uint64_t drop_start = 0; + bool open_now = false; + while (!thread_should_exit) { // warnx("in while!\n"); // values from -1 to 1 - if (drop) { - // drop here - drop = false; + int ret = poll(fds, 1, 500); - ioctl(rgbleds, RGBLED_SET_MODE, RGBLED_MODE_ON); - ioctl(rgbleds, RGBLED_SET_COLOR, RGBLED_COLOR_RED); - } + if (ret < 0) { + /* poll error, count it in perf */ + warnx("poll error"); + + } else if (ret > 0) { + /* attitude */ + orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &att); + if (drop) { + // drop here + open_now = true; + drop = false; + drop_start = hrt_absolute_time(); + } + if (open_now && (drop_start + 2e6 > hrt_absolute_time())) { // open door - actuators.control[0] = 0.5f; - actuators.control[1] = 0.5f; - actuators.control[2] = 0.5f; - actuators.control[3] = 0.5f; + actuators.control[0] = -1.0f; + actuators.control[1] = 1.0f; + } else if (open_now) { // unlock bottle + ioctl(rgbleds, RGBLED_SET_MODE, RGBLED_MODE_ON); + ioctl(rgbleds, RGBLED_SET_COLOR, RGBLED_COLOR_RED); + actuators.control[2] = 0.5f; - actuators.timestamp = hrt_absolute_time(); - orb_publish(ORB_ID(actuator_controls_1), actuator_pub, &actuators); + } else { - usleep(50); + // leave closed + // warnx( "%4.4f", att.pitch); + actuators.control[0] = 0.5f; + actuators.control[1] = -0.5f; + actuators.control[2] = -0.5f; + } + + + + actuators.timestamp = hrt_absolute_time(); + orb_publish(ORB_ID(actuator_controls_1), actuator_pub, &actuators); + + i++; + } - i++; } warnx("exiting.\n"); -- cgit v1.2.3 From 3c2133b9f1af370638cee82d6cda0b31df15dffe Mon Sep 17 00:00:00 2001 From: Juchli D Date: Fri, 29 Nov 2013 12:36:42 +0100 Subject: Working with faked imputs --- src/modules/bottle_drop/bottle_drop.c | 303 ++++++++++++++++++++++++++++++---- 1 file changed, 271 insertions(+), 32 deletions(-) diff --git a/src/modules/bottle_drop/bottle_drop.c b/src/modules/bottle_drop/bottle_drop.c index 09bbf4687..bb12674af 100644 --- a/src/modules/bottle_drop/bottle_drop.c +++ b/src/modules/bottle_drop/bottle_drop.c @@ -32,27 +32,47 @@ ****************************************************************************/ /** - * @file px4_daemon_app.c - * daemon application example for PX4 autopilot + * @file bottle_drop.c + * bottle_drop application * - * @author Example User + * @author Dominik Juchli */ #include #include #include #include +#include #include +#include +#include +#include +#include + #include #include +#include + +#include + #include #include #include +#include +#include -#include #include +PARAM_DEFINE_FLOAT(BD_HEIGHT, 60.0f); +PARAM_DEFINE_FLOAT(BD_GPROPERTIES, 0.03f); +PARAM_DEFINE_FLOAT(BD_TURNRADIUS, 70.0f); +PARAM_DEFINE_FLOAT(BD_PRECISION, 1.0f); +PARAM_DEFINE_INT32(BD_APPROVAL, 0); + + + + static bool thread_should_exit = false; /**< daemon exit flag */ static bool thread_running = false; /**< daemon status flag */ static int daemon_task; /**< Handle of daemon task / thread */ @@ -140,27 +160,120 @@ int bottle_drop_thread_main(int argc, char *argv[]) { warnx("starting\n"); - unsigned i = 0; + bool updated = false; + + float height; // height at which the normal should be dropped NED + float z_0; // ground properties + float turn_radius; // turn radius of the UAV + float precision; // Expected precision of the UAV + bool drop_approval; // if approval is given = true, otherwise = false thread_running = true; - int rgbleds = open(RGBLED_DEVICE_PATH, 0); + /* XXX TODO: create, publish and read in wind speed topic */ + struct wind_speed_s { + float vx; // m/s + float vy; // m/s + float altitude; // m + } wind_speed; + + wind_speed.vx = 4.2f; + wind_speed.vy = 0.0f; + wind_speed.altitude = 62.0f; + + + /* XXX TODO: create, publish and read in target position in NED*/ + struct position_s { + double lat; //degrees 1E7 + double lon; //degrees 1E7 + float alt; //m + } target_position, drop_position, flight_vector_s, flight_vector_e; + + target_position.lat = 47.385806; + target_position.lon = 8.589093; + target_position.alt = 0.0f; + + + // constant + float g = 9.81f; // constant of gravity [m/s^2] + float m = 0.5f; // mass of bottle [kg] + float rho = 1.2f; // air density [kg/m^3] + float A = (powf(0.063f, 2.0f)/4.0f*M_PI_F); // Bottle cross section [m^2] + float dt = 0.01f; // step size [s] + float dt2 = 0.05f; // step size 2 [s] + + // Has to be estimated by experiment + float cd = 0.86f; // Drag coefficient for a cylinder with a d/l ratio of 1/3 [] + float t_signal = 0.084f; // Time span between sending the signal and the bottle top reaching level height with the bottom of the plane [s] + float t_door = 0.7f; // The time the system needs to open the door + safety, is also the time the palyload needs to safely escape the shaft [s] + + + // Definition + float h_0; // height over target + float az; // acceleration in z direction[m/s^2] + float vz; // velocity in z direction [m/s] + float z; // fallen distance [m] + float h; // height over target [m] + float ax; // acceleration in x direction [m/s^2] + float vx; // ground speed in x direction [m/s] + float x; // traveled distance in x direction [m] + float vw; // wind speed [m/s] + float vrx; // relative velocity in x direction [m/s] + float v; // relative speed vector [m/s] + float Fd; // Drag force [N] + float Fdx; // Drag force in x direction [N] + float Fdz; // Drag force in z direction [N] + float vr; // absolute wind speed [m/s] + float x_drop, y_drop; // coordinates of the drop point in reference to the target (projection of NED) + float x_t,y_t; // coordinates of the target in reference to the target x_t = 0, y_t = 0 (projection of NED) + float x_l,y_l; // local position in projected coordinates + float x_f,y_f; // to-be position of the UAV after dt2 seconds in projected coordinates + double x_f_NED, y_f_NED; // to-be position of the UAV after dt2 seconds in NED + float distance_open_door; // The distance the UAV travels during its doors open [m] + float distance_real = 0; // The distance between the UAVs position and the drop point [m] + float future_distance = 0; // The distance between the UAVs to-be position and the drop point [m] + + // states + bool state_door = false; // Doors are closed = false, open = true + bool state_drop = false; // Drop occurred = true, Drop din't occur = false + bool state_run = false; // A drop was attempted = true, the drop is still in progress = false + + + param_t param_height = param_find("BD_HEIGHT"); + param_t param_gproperties = param_find("BD_GPROPERTIES"); + param_t param_turn_radius = param_find("BD_TURNRADIUS"); + param_t param_precision = param_find("BD_PRECISION"); + param_t param_approval = param_find("BD_APPROVAL"); + + + param_get(param_approval, &drop_approval); + param_get(param_precision, &precision); + param_get(param_turn_radius, &turn_radius); + param_get(param_height, &height); + param_get(param_gproperties, &z_0); + struct vehicle_attitude_s att; memset(&att, 0, sizeof(att)); int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude)); - orb_set_interval(vehicle_attitude_sub, 20); + orb_set_interval(vehicle_attitude_sub, 100); + + struct vehicle_global_position_s globalpos; + memset(&globalpos, 0, sizeof(globalpos)); + int vehicle_global_position_sub = orb_subscribe(ORB_ID(vehicle_global_position)); + + struct parameter_update_s update; + memset(&update, 0, sizeof(update)); + int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update)); struct actuator_controls_s actuators; memset(&actuators, 0, sizeof(actuators)); orb_advert_t actuator_pub = orb_advertise(ORB_ID(actuator_controls_1), &actuators); - struct pollfd fds[1] = { - { .fd = vehicle_attitude_sub, .events = POLLIN }, + struct pollfd fds[] = { + { .fd = vehicle_attitude_sub, .events = POLLIN } }; - uint64_t drop_start = 0; - bool open_now = false; while (!thread_should_exit) { @@ -174,51 +287,177 @@ int bottle_drop_thread_main(int argc, char *argv[]) { warnx("poll error"); } else if (ret > 0) { - /* attitude */ + orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &att); - if (drop) { - // drop here - open_now = true; - drop = false; - drop_start = hrt_absolute_time(); + + orb_check(vehicle_global_position_sub, &updated); + if (updated){ + /* copy global position */ + orb_copy(ORB_ID(vehicle_global_position), vehicle_global_position_sub, &globalpos); } + //////////////////////////////////////////////////////////////////// DEBUGGING + globalpos.lat = 47.384486; + globalpos.lon = 8.588239; + globalpos.vx = 18.0f; + globalpos.vy = 0.0f; + globalpos.alt = 60.0f; + globalpos.yaw = M_PI_F/2.0f; - if (open_now && (drop_start + 2e6 > hrt_absolute_time())) { // open door - actuators.control[0] = -1.0f; - actuators.control[1] = 1.0f; + orb_check(parameter_update_sub, &updated); + if (updated){ + /* copy global position */ + orb_copy(ORB_ID(parameter_update), parameter_update_sub, &update); - } else if (open_now) { // unlock bottle + /* update all parameters */ + param_get(param_height, &height); + param_get(param_gproperties, &z_0); + param_get(param_turn_radius, &turn_radius); + param_get(param_approval, &drop_approval); + param_get(param_precision, &precision); - ioctl(rgbleds, RGBLED_SET_MODE, RGBLED_MODE_ON); - ioctl(rgbleds, RGBLED_SET_COLOR, RGBLED_COLOR_RED); - actuators.control[2] = 0.5f; - } else { + } - // leave closed - // warnx( "%4.4f", att.pitch); + // Initialization + az = g; // acceleration in z direction[m/s^2] + vz = 0; // velocity in z direction [m/s] + z = 0; // fallen distance [m] + h_0 = globalpos.alt - target_position.alt; // height over target at start[m] + h = h_0; // height over target [m] + ax = 0; // acceleration in x direction [m/s^2] + vx = globalpos.vx; // ground speed in x direction [m/s] + x = 0; // traveled distance in x direction [m] + vw = 0; // wind speed [m/s] + vrx = 0; // relative velocity in x direction [m/s] + v = globalpos.vx; // relative speed vector [m/s] + Fd = 0; // Drag force [N] + Fdx = 0; // Drag force in x direction [N] + Fdz = 0; // Drag force in z direction [N] + vr = sqrt(pow(wind_speed.vx,2) + pow(wind_speed.vy,2)); // absolute wind speed [m/s] + distance_open_door = t_door * globalpos.vx; + + + //warnx("absolut wind speed = %.4f", vr); //////////////////////////////////////////////////////////////////// DEBUGGING + + + //warnx("Initialization complete\n"); //////////////////////////////////////////////////////////////////// DEBUGGING + + + if (drop_approval && !state_drop) + { + //warnx("approval given\n"); //////////////////////////////////////////////////////////////////// DEBUGGING + // drop here + //open_now = true; + //drop = false; + //drop_start = hrt_absolute_time(); + + unsigned counter = 0; + + // Compute the distance the bottle will travel after it is dropped in body frame coordinates --> x + while( h > 0.05f) + { + // z-direction + vz = vz + az*dt; + z = z + vz*dt; + h = h_0 - z; + + // x-direction + vw = vr*logf(h/z_0)/logf(wind_speed.altitude/z_0); + vx = vx + ax*dt; + x = x + vx*dt; + vrx = vx + vw; + + //Drag force + v = sqrtf(powf(vz,2.0f) + powf(vrx,2.0f)); + Fd = 0.5f*rho*A*cd*powf(v,2.0f); + Fdx = Fd*vrx/v; + Fdz = Fd*vz/v; + + //acceleration + az = g - Fdz/m; + ax = -Fdx/m; + + } + // Compute Drop point + x = globalpos.vx*t_signal + x; + map_projection_init(target_position.lat, target_position.lon); + //warnx("x = %.4f", x); //////////////////////////////////////////////////////////////////// DEBUGGING + + + + map_projection_project(target_position.lat, target_position.lon, &x_t, &y_t); + if( vr < 0.001f) // if there is no wind, an arbitrarily direction is chosen + { + vr = 1; + wind_speed.vx = 1; + wind_speed.vy = 0; + } + x_drop = x_t + x*wind_speed.vx/vr; + y_drop = y_t + x*wind_speed.vy/vr; + map_projection_reproject(x_drop, y_drop, &drop_position.lat, &drop_position.lon); + drop_position.alt = height; + //warnx("drop point: lat = %.7f , lon = %.7f", drop_position.lat, drop_position.lon); //////////////////////////////////////////////////////////////////// DEBUGGING + + + + // Compute flight vector + map_projection_reproject(x_drop + 2*turn_radius*wind_speed.vx/vr, y_drop + 2*turn_radius*wind_speed.vy/vr, &flight_vector_s.lat, &flight_vector_s.lon); + flight_vector_s.alt = height; + map_projection_reproject(x_drop - turn_radius*wind_speed.vx/vr, y_drop - turn_radius*wind_speed.vy/vr, &flight_vector_e.lat, &flight_vector_e.lon); + flight_vector_e.alt = height; + //warnx("Flight vector: starting point = %.7f %.7f , end point = %.7f %.7f", flight_vector_s.lat,flight_vector_s.lon,flight_vector_e.lat,flight_vector_e.lon); //////////////////////////////////////////////////////////////////// DEBUGGING + + + // Drop Cancellation if terms are not met + distance_real = get_distance_to_next_waypoint(globalpos.lat, globalpos.lon, drop_position.lat, drop_position.lon); + map_projection_project(globalpos.lat, globalpos.lon, &x_l, &y_l); + x_f = x_l + globalpos.vx*cosf(globalpos.yaw)*dt2 - globalpos.vy*sinf(globalpos.yaw)*dt2; // Attention to sign, has to be checked + y_f = y_l + globalpos.vy*cosf(globalpos.yaw)*dt2 - globalpos.vx*sinf(globalpos.yaw)*dt2; + map_projection_reproject(x_f, y_f, &x_f_NED, &y_f_NED); + future_distance = get_distance_to_next_waypoint(x_f_NED, y_f_NED, drop_position.lat, drop_position.lon); + //warnx("position to-be: = %.7f %.7f" ,x_f_NED, y_f_NED ); //////////////////////////////////////////////////////////////////// DEBUGGING + + } + + if(distance_real < distance_open_door && drop_approval) + { + actuators.control[0] = -1.0f; // open door + actuators.control[1] = 1.0f; + state_door = true; + } + else + { // closed door and locked survival kit actuators.control[0] = 0.5f; actuators.control[1] = -0.5f; actuators.control[2] = -0.5f; + state_door = false; + } + if(distance_real < precision && distance_real < future_distance && state_door) // Drop only if the distance between drop point and actual position is getting larger again + { + if(fabsf(acosf(cosf(globalpos.yaw))+acosf(wind_speed.vx)) < 10.0f/180.0f*M_PI_F) // if flight trajectory deviates more than 10 degrees from calculated path, it will no drop + { + actuators.control[2] = 0.5f; + state_drop = true; + state_run = true; + } + else + { + state_run = true; + } } - actuators.timestamp = hrt_absolute_time(); orb_publish(ORB_ID(actuator_controls_1), actuator_pub, &actuators); - i++; } - } warnx("exiting.\n"); thread_running = false; - ioctl(rgbleds, RGBLED_SET_MODE, RGBLED_MODE_OFF); - close(rgbleds); return 0; } -- cgit v1.2.3 From 76e5a755dfdaec4bfc00493e8f16a5e40ffa5093 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 29 Nov 2013 17:03:18 +0100 Subject: Bottle_drop: Publish to onboard mission --- src/modules/bottle_drop/bottle_drop.c | 55 +++++++++++++++++++++++------------ 1 file changed, 37 insertions(+), 18 deletions(-) diff --git a/src/modules/bottle_drop/bottle_drop.c b/src/modules/bottle_drop/bottle_drop.c index bb12674af..8d3d6d599 100644 --- a/src/modules/bottle_drop/bottle_drop.c +++ b/src/modules/bottle_drop/bottle_drop.c @@ -61,6 +61,7 @@ #include #include #include +#include #include @@ -187,7 +188,7 @@ int bottle_drop_thread_main(int argc, char *argv[]) { double lat; //degrees 1E7 double lon; //degrees 1E7 float alt; //m - } target_position, drop_position, flight_vector_s, flight_vector_e; + } target_position, drop_position; target_position.lat = 47.385806; target_position.lon = 8.589093; @@ -238,6 +239,7 @@ int bottle_drop_thread_main(int argc, char *argv[]) { bool state_drop = false; // Drop occurred = true, Drop din't occur = false bool state_run = false; // A drop was attempted = true, the drop is still in progress = false + unsigned counter = 0; param_t param_height = param_find("BD_HEIGHT"); param_t param_gproperties = param_find("BD_GPROPERTIES"); @@ -270,6 +272,22 @@ int bottle_drop_thread_main(int argc, char *argv[]) { memset(&actuators, 0, sizeof(actuators)); orb_advert_t actuator_pub = orb_advertise(ORB_ID(actuator_controls_1), &actuators); + struct mission_s onboard_mission; + memset(&onboard_mission, 0, sizeof(onboard_mission)); + onboard_mission.items = (struct mission_item_s*)malloc(sizeof(struct mission_item_s) * 2); + + struct mission_item_s *flight_vector_s = &onboard_mission.items[0]; + struct mission_item_s *flight_vector_e = &onboard_mission.items[1]; + + flight_vector_s->nav_cmd = NAV_CMD_WAYPOINT; + flight_vector_s->radius = 50; // TODO: make parameter + flight_vector_s->autocontinue = true; + flight_vector_e->nav_cmd = NAV_CMD_WAYPOINT; + flight_vector_e->radius = 50; // TODO: make parameter + flight_vector_e->autocontinue = true; + + orb_advert_t onboard_mission_pub = orb_advertise(ORB_ID(onboard_mission), &onboard_mission); + struct pollfd fds[] = { { .fd = vehicle_attitude_sub, .events = POLLIN } }; @@ -295,14 +313,6 @@ int bottle_drop_thread_main(int argc, char *argv[]) { /* copy global position */ orb_copy(ORB_ID(vehicle_global_position), vehicle_global_position_sub, &globalpos); } - //////////////////////////////////////////////////////////////////// DEBUGGING - globalpos.lat = 47.384486; - globalpos.lon = 8.588239; - globalpos.vx = 18.0f; - globalpos.vy = 0.0f; - globalpos.alt = 60.0f; - globalpos.yaw = M_PI_F/2.0f; - orb_check(parameter_update_sub, &updated); if (updated){ @@ -344,7 +354,7 @@ int bottle_drop_thread_main(int argc, char *argv[]) { //warnx("Initialization complete\n"); //////////////////////////////////////////////////////////////////// DEBUGGING - if (drop_approval && !state_drop) + if (drop_approval && !state_drop && counter % 10 == 0) { //warnx("approval given\n"); //////////////////////////////////////////////////////////////////// DEBUGGING // drop here @@ -352,8 +362,6 @@ int bottle_drop_thread_main(int argc, char *argv[]) { //drop = false; //drop_start = hrt_absolute_time(); - unsigned counter = 0; - // Compute the distance the bottle will travel after it is dropped in body frame coordinates --> x while( h > 0.05f) { @@ -402,13 +410,12 @@ int bottle_drop_thread_main(int argc, char *argv[]) { // Compute flight vector - map_projection_reproject(x_drop + 2*turn_radius*wind_speed.vx/vr, y_drop + 2*turn_radius*wind_speed.vy/vr, &flight_vector_s.lat, &flight_vector_s.lon); - flight_vector_s.alt = height; - map_projection_reproject(x_drop - turn_radius*wind_speed.vx/vr, y_drop - turn_radius*wind_speed.vy/vr, &flight_vector_e.lat, &flight_vector_e.lon); - flight_vector_e.alt = height; + map_projection_reproject(x_drop + 2*turn_radius*wind_speed.vx/vr, y_drop + 2*turn_radius*wind_speed.vy/vr, &(flight_vector_s->lat), &(flight_vector_s->lon)); + flight_vector_s->altitude = height; + map_projection_reproject(x_drop - turn_radius*wind_speed.vx/vr, y_drop - turn_radius*wind_speed.vy/vr, &(flight_vector_e)->lat, &(flight_vector_e)->lon); + flight_vector_e->altitude = height; //warnx("Flight vector: starting point = %.7f %.7f , end point = %.7f %.7f", flight_vector_s.lat,flight_vector_s.lon,flight_vector_e.lat,flight_vector_e.lon); //////////////////////////////////////////////////////////////////// DEBUGGING - // Drop Cancellation if terms are not met distance_real = get_distance_to_next_waypoint(globalpos.lat, globalpos.lon, drop_position.lat, drop_position.lon); map_projection_project(globalpos.lat, globalpos.lon, &x_l, &y_l); @@ -418,6 +425,18 @@ int bottle_drop_thread_main(int argc, char *argv[]) { future_distance = get_distance_to_next_waypoint(x_f_NED, y_f_NED, drop_position.lat, drop_position.lon); //warnx("position to-be: = %.7f %.7f" ,x_f_NED, y_f_NED ); //////////////////////////////////////////////////////////////////// DEBUGGING + + onboard_mission.count = 2; + + if (state_run && !state_drop) { + onboard_mission.current_index = 0; + } else { + onboard_mission.current_index = -1; + } + + + orb_publish(ORB_ID(onboard_mission), onboard_mission_pub, &onboard_mission); + } if(distance_real < distance_open_door && drop_approval) @@ -447,10 +466,10 @@ int bottle_drop_thread_main(int argc, char *argv[]) { } } - actuators.timestamp = hrt_absolute_time(); orb_publish(ORB_ID(actuator_controls_1), actuator_pub, &actuators); + counter++; } } -- cgit v1.2.3 From a839a09834a5ab3217e794a0a98af2eeffaf571b Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 29 Nov 2013 17:03:48 +0100 Subject: Bottle drop: Added HIL startup --- .../px4fmu_common/init.d/901_bottle_drop_test.hil | 98 ++++++++++++++++++++++ ROMFS/px4fmu_common/init.d/rcS | 6 ++ 2 files changed, 104 insertions(+) create mode 100644 ROMFS/px4fmu_common/init.d/901_bottle_drop_test.hil diff --git a/ROMFS/px4fmu_common/init.d/901_bottle_drop_test.hil b/ROMFS/px4fmu_common/init.d/901_bottle_drop_test.hil new file mode 100644 index 000000000..060c7d0b5 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/901_bottle_drop_test.hil @@ -0,0 +1,98 @@ +#!nsh +# +# USB HIL start +# + +echo "[HIL] HILStar starting.." + +# +# Load default params for this platform +# +if param compare SYS_AUTOCONFIG 1 +then + # Set all params here, then disable autoconfig + + param set FW_P_D 0 + param set FW_P_I 0 + param set FW_P_IMAX 15 + param set FW_P_LIM_MAX 50 + param set FW_P_LIM_MIN -50 + param set FW_P_P 60 + param set FW_P_RMAX_NEG 0 + param set FW_P_RMAX_POS 0 + param set FW_P_ROLLFF 1.1 + param set FW_R_D 0 + param set FW_R_I 5 + param set FW_R_IMAX 20 + param set FW_R_P 100 + param set FW_R_RMAX 100 + param set FW_THR_CRUISE 0.65 + param set FW_THR_MAX 1 + param set FW_THR_MIN 0 + param set FW_T_SINK_MAX 5.0 + param set FW_T_SINK_MIN 4.0 + param set FW_Y_ROLLFF 1.1 + param set FW_L1_PERIOD 16 + param set RC_SCALE_ROLL 1.0 + param set RC_SCALE_PITCH 1.0 + + param set SYS_AUTOCONFIG 0 + param save +fi + +# Allow USB some time to come up +sleep 1 +# Tell MAVLink that this link is "fast" +mavlink start -b 230400 -d /dev/ttyACM0 + +# Create a fake HIL /dev/pwm_output interface +hil mode_pwm + +# +# Force some key parameters to sane values +# MAV_TYPE 1 = fixed wing, 2 = quadrotor, 13 = hexarotor +# see https://pixhawk.ethz.ch/mavlink/ +# +param set MAV_TYPE 1 + +# +# Check if we got an IO +# +if px4io start +then + echo "IO started" +else + fmu mode_serial + echo "FMU started" +fi + +# +# Start the sensors (depends on orb, px4io) +# +sh /etc/init.d/rc.sensors + +# +# Start the attitude estimator (depends on orb) +# +att_pos_estimator_ekf start + +# +# Load mixer and start controllers (depends on px4io) +# +mixer load /dev/pwm_output /etc/mixers/FMU_AET.mix +fw_pos_control_l1 start +fw_att_control start + +#fmu mode_pwm + +mixer load /dev/px4io /etc/mixers/FMU_pass.mix + +pwm min -d /dev/px4io -c 123 -p 900 +pwm max -d /dev/px4io -c 123 -p 2100 + +pwm arm -d /dev/px4io + +bottle_drop start + +echo "[HIL] setup done, running" + diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 46bb6d866..5327ee7ca 100755 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -324,6 +324,12 @@ then sh /etc/init.d/900_bottle_drop_test set MODE custom fi + + if param compare SYS_AUTOSTART 901 + then + sh /etc/init.d/901_bottle_drop_test.hil + set MODE custom + fi # Start any custom extensions that might be missing if [ -f /fs/microsd/etc/rc.local ] -- cgit v1.2.3 From 0ec609f49167e1e6857e970088b4e8de93bba032 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 2 Dec 2013 23:01:02 +0100 Subject: Bottle drop: added custom HIL startup script --- ROMFS/px4fmu_common/init.d/901_bottle_drop_test.hil | 17 +++++------------ ROMFS/px4fmu_common/init.d/rcS | 12 ++++++------ 2 files changed, 11 insertions(+), 18 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/901_bottle_drop_test.hil b/ROMFS/px4fmu_common/init.d/901_bottle_drop_test.hil index 060c7d0b5..472351e9e 100644 --- a/ROMFS/px4fmu_common/init.d/901_bottle_drop_test.hil +++ b/ROMFS/px4fmu_common/init.d/901_bottle_drop_test.hil @@ -55,17 +55,6 @@ hil mode_pwm # param set MAV_TYPE 1 -# -# Check if we got an IO -# -if px4io start -then - echo "IO started" -else - fmu mode_serial - echo "FMU started" -fi - # # Start the sensors (depends on orb, px4io) # @@ -83,7 +72,11 @@ mixer load /dev/pwm_output /etc/mixers/FMU_AET.mix fw_pos_control_l1 start fw_att_control start -#fmu mode_pwm +# +# Start IO +# +px4io start + mixer load /dev/px4io /etc/mixers/FMU_pass.mix diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 5327ee7ca..6c76f5fe9 100755 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -145,6 +145,12 @@ then sh /etc/init.d/1004_rc_fw_Rascal110.hil set MODE custom fi + + if param compare SYS_AUTOSTART 901 + then + sh /etc/init.d/901_bottle_drop_test.hil + set MODE custom + fi if [ $MODE != custom ] then @@ -324,12 +330,6 @@ then sh /etc/init.d/900_bottle_drop_test set MODE custom fi - - if param compare SYS_AUTOSTART 901 - then - sh /etc/init.d/901_bottle_drop_test.hil - set MODE custom - fi # Start any custom extensions that might be missing if [ -f /fs/microsd/etc/rc.local ] -- cgit v1.2.3 From 6048f9beda68507d74e321ad4816aa137e20fe3a Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 2 Dec 2013 23:03:29 +0100 Subject: HIL: copy correct actuator group --- src/drivers/hil/hil.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/drivers/hil/hil.cpp b/src/drivers/hil/hil.cpp index c1d73dd87..b848f6c37 100644 --- a/src/drivers/hil/hil.cpp +++ b/src/drivers/hil/hil.cpp @@ -391,7 +391,8 @@ HIL::task_main() if (fds[0].revents & POLLIN) { /* get controls - must always do this to avoid spinning */ - orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, _t_actuators, &_controls); + orb_copy(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS : + ORB_ID(actuator_controls_1), _t_actuators, &_controls); /* can we mix? */ if (_mixers != nullptr) { -- cgit v1.2.3 From 808294b01a0df96817701526b24c6bb80e97775c Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 2 Dec 2013 23:03:52 +0100 Subject: HIL: only send attitude actuator group 0 --- src/modules/mavlink/orb_listener.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mavlink/orb_listener.c b/src/modules/mavlink/orb_listener.c index abc91d34f..634586fdb 100644 --- a/src/modules/mavlink/orb_listener.c +++ b/src/modules/mavlink/orb_listener.c @@ -507,7 +507,7 @@ l_actuator_outputs(const struct listener *l) act_outputs.output[7]); /* only send in HIL mode */ - if (mavlink_hil_enabled && armed.armed) { + if (mavlink_hil_enabled && armed.armed && l->arg == 0) { /* translate the current syste state to mavlink state and mode */ uint8_t mavlink_state = 0; -- cgit v1.2.3 From f2c303679ea859bf722eaa7289fd10f8b806264a Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 2 Dec 2013 23:05:28 +0100 Subject: Bottle drop: lots of changes, working in HIL --- src/modules/bottle_drop/bottle_drop.c | 136 ++++++++++++++++++++-------------- 1 file changed, 81 insertions(+), 55 deletions(-) diff --git a/src/modules/bottle_drop/bottle_drop.c b/src/modules/bottle_drop/bottle_drop.c index 8d3d6d599..1c2e008ba 100644 --- a/src/modules/bottle_drop/bottle_drop.c +++ b/src/modules/bottle_drop/bottle_drop.c @@ -258,7 +258,7 @@ int bottle_drop_thread_main(int argc, char *argv[]) { struct vehicle_attitude_s att; memset(&att, 0, sizeof(att)); int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude)); - orb_set_interval(vehicle_attitude_sub, 100); + orb_set_interval(vehicle_attitude_sub, 20); struct vehicle_global_position_s globalpos; memset(&globalpos, 0, sizeof(globalpos)); @@ -292,6 +292,9 @@ int bottle_drop_thread_main(int argc, char *argv[]) { { .fd = vehicle_attitude_sub, .events = POLLIN } }; + double latitude; + double longitude; + while (!thread_should_exit) { @@ -312,6 +315,9 @@ int bottle_drop_thread_main(int argc, char *argv[]) { if (updated){ /* copy global position */ orb_copy(ORB_ID(vehicle_global_position), vehicle_global_position_sub, &globalpos); + + latitude = (double)globalpos.lat / 1e7; + longitude = (double)globalpos.lon / 1e7; } orb_check(parameter_update_sub, &updated); @@ -330,22 +336,10 @@ int bottle_drop_thread_main(int argc, char *argv[]) { } // Initialization - az = g; // acceleration in z direction[m/s^2] - vz = 0; // velocity in z direction [m/s] - z = 0; // fallen distance [m] - h_0 = globalpos.alt - target_position.alt; // height over target at start[m] - h = h_0; // height over target [m] - ax = 0; // acceleration in x direction [m/s^2] - vx = globalpos.vx; // ground speed in x direction [m/s] - x = 0; // traveled distance in x direction [m] - vw = 0; // wind speed [m/s] - vrx = 0; // relative velocity in x direction [m/s] - v = globalpos.vx; // relative speed vector [m/s] - Fd = 0; // Drag force [N] - Fdx = 0; // Drag force in x direction [N] - Fdz = 0; // Drag force in z direction [N] - vr = sqrt(pow(wind_speed.vx,2) + pow(wind_speed.vy,2)); // absolute wind speed [m/s] - distance_open_door = t_door * globalpos.vx; + + + vr = sqrtf(powf(wind_speed.vx,2) + powf(wind_speed.vy,2)); // absolute wind speed [m/s] + distance_open_door = fabsf(t_door * globalpos.vx); //warnx("absolut wind speed = %.4f", vr); //////////////////////////////////////////////////////////////////// DEBUGGING @@ -354,7 +348,7 @@ int bottle_drop_thread_main(int argc, char *argv[]) { //warnx("Initialization complete\n"); //////////////////////////////////////////////////////////////////// DEBUGGING - if (drop_approval && !state_drop && counter % 10 == 0) + if (drop_approval && !state_drop) { //warnx("approval given\n"); //////////////////////////////////////////////////////////////////// DEBUGGING // drop here @@ -362,37 +356,53 @@ int bottle_drop_thread_main(int argc, char *argv[]) { //drop = false; //drop_start = hrt_absolute_time(); - // Compute the distance the bottle will travel after it is dropped in body frame coordinates --> x - while( h > 0.05f) - { - // z-direction - vz = vz + az*dt; - z = z + vz*dt; - h = h_0 - z; - - // x-direction - vw = vr*logf(h/z_0)/logf(wind_speed.altitude/z_0); - vx = vx + ax*dt; - x = x + vx*dt; - vrx = vx + vw; - - //Drag force - v = sqrtf(powf(vz,2.0f) + powf(vrx,2.0f)); - Fd = 0.5f*rho*A*cd*powf(v,2.0f); - Fdx = Fd*vrx/v; - Fdz = Fd*vz/v; - - //acceleration - az = g - Fdz/m; - ax = -Fdx/m; + if (counter % 50 == 0) { + + az = g; // acceleration in z direction[m/s^2] + vz = 0; // velocity in z direction [m/s] + z = 0; // fallen distance [m] + h_0 = globalpos.alt - target_position.alt; // height over target at start[m] + h = h_0; // height over target [m] + ax = 0; // acceleration in x direction [m/s^2] + vx = sqrtf(powf(globalpos.vx,2.0f) + powf(globalpos.vy,2.0f)); // ground speed in x direction [m/s] + x = 0; // traveled distance in x direction [m] + vw = 0; // wind speed [m/s] + vrx = 0; // relative velocity in x direction [m/s] + v = sqrtf(powf(globalpos.vx,2.0f) + powf(globalpos.vy,2.0f)); // relative speed vector [m/s] + Fd = 0; // Drag force [N] + Fdx = 0; // Drag force in x direction [N] + Fdz = 0; // Drag force in z direction [N] + + + // Compute the distance the bottle will travel after it is dropped in body frame coordinates --> x + while( h > 0.05f) + { + // z-direction + vz = vz + az*dt; + z = z + vz*dt; + h = h_0 - z; + + // x-direction + vw = vr*logf(h/z_0)/logf(wind_speed.altitude/z_0); + vx = vx + ax*dt; + x = x + vx*dt; + vrx = vx + vw; + + //Drag force + v = sqrtf(powf(vz,2.0f) + powf(vrx,2.0f)); + Fd = 0.5f*rho*A*cd*powf(v,2.0f); + Fdx = Fd*vrx/v; + Fdz = Fd*vz/v; + + //acceleration + az = g - Fdz/m; + ax = -Fdx/m; + } + // Compute Drop point + x = sqrtf(powf(globalpos.vx,2.0f) + powf(globalpos.vy,2.0f))*t_signal + x; + map_projection_init(target_position.lat, target_position.lon); } - // Compute Drop point - x = globalpos.vx*t_signal + x; - map_projection_init(target_position.lat, target_position.lon); - //warnx("x = %.4f", x); //////////////////////////////////////////////////////////////////// DEBUGGING - - map_projection_project(target_position.lat, target_position.lon, &x_t, &y_t); if( vr < 0.001f) // if there is no wind, an arbitrarily direction is chosen @@ -417,14 +427,26 @@ int bottle_drop_thread_main(int argc, char *argv[]) { //warnx("Flight vector: starting point = %.7f %.7f , end point = %.7f %.7f", flight_vector_s.lat,flight_vector_s.lon,flight_vector_e.lat,flight_vector_e.lon); //////////////////////////////////////////////////////////////////// DEBUGGING // Drop Cancellation if terms are not met - distance_real = get_distance_to_next_waypoint(globalpos.lat, globalpos.lon, drop_position.lat, drop_position.lon); - map_projection_project(globalpos.lat, globalpos.lon, &x_l, &y_l); - x_f = x_l + globalpos.vx*cosf(globalpos.yaw)*dt2 - globalpos.vy*sinf(globalpos.yaw)*dt2; // Attention to sign, has to be checked - y_f = y_l + globalpos.vy*cosf(globalpos.yaw)*dt2 - globalpos.vx*sinf(globalpos.yaw)*dt2; + + // warnx("latitude:%.2f", latitude); + // warnx("longitude:%.2f", longitude); + // warnx("drop_position.lat:%.2f", drop_position.lat); + // warnx("drop_position.lon:%.2f", drop_position.lon); + + distance_real = fabsf(get_distance_to_next_waypoint(latitude, longitude, drop_position.lat, drop_position.lon)); + map_projection_project(latitude, longitude, &x_l, &y_l); + x_f = x_l + globalpos.vx*dt2; + y_f = y_l + globalpos.vy*dt2; map_projection_reproject(x_f, y_f, &x_f_NED, &y_f_NED); - future_distance = get_distance_to_next_waypoint(x_f_NED, y_f_NED, drop_position.lat, drop_position.lon); + future_distance = fabsf(get_distance_to_next_waypoint(x_f_NED, y_f_NED, drop_position.lat, drop_position.lon)); //warnx("position to-be: = %.7f %.7f" ,x_f_NED, y_f_NED ); //////////////////////////////////////////////////////////////////// DEBUGGING + // if (counter % 10 ==0) { + // warnx("x: %.4f", x); + // warnx("drop_position.lat: %.4f, drop_position.lon: %.4f", drop_position.lat, drop_position.lon); + // warnx("latitude %.4f, longitude: %.4f", latitude, longitude); + // warnx("future_distance: %.2f, precision: %.2f", future_distance, precision); + // } onboard_mission.count = 2; @@ -434,16 +456,19 @@ int bottle_drop_thread_main(int argc, char *argv[]) { onboard_mission.current_index = -1; } + // if (counter % 10 ==0) + // warnx("Distance real: %.2f, distance_open_door: %.2f, angle to wind: %.2f", distance_real, distance_open_door, fabsf(_wrap_pi(globalpos.yaw-atan2f(wind_speed.vy,wind_speed.vx)+M_PI_F))); orb_publish(ORB_ID(onboard_mission), onboard_mission_pub, &onboard_mission); } - if(distance_real < distance_open_door && drop_approval) + if(isfinite(distance_real) && distance_real < distance_open_door && drop_approval) { actuators.control[0] = -1.0f; // open door actuators.control[1] = 1.0f; state_door = true; + warnx("open doors"); } else { // closed door and locked survival kit @@ -452,13 +477,14 @@ int bottle_drop_thread_main(int argc, char *argv[]) { actuators.control[2] = -0.5f; state_door = false; } - if(distance_real < precision && distance_real < future_distance && state_door) // Drop only if the distance between drop point and actual position is getting larger again + if(isfinite(distance_real) && distance_real < precision && distance_real < future_distance && state_door) // Drop only if the distance between drop point and actual position is getting larger again { - if(fabsf(acosf(cosf(globalpos.yaw))+acosf(wind_speed.vx)) < 10.0f/180.0f*M_PI_F) // if flight trajectory deviates more than 10 degrees from calculated path, it will no drop + if(fabsf(_wrap_pi(globalpos.yaw-atan2f(wind_speed.vy,wind_speed.vx)+M_PI_F)) < 10.0f/180.0f*M_PI_F) // if flight trajectory deviates more than 10 degrees from calculated path, it will no drop { actuators.control[2] = 0.5f; state_drop = true; state_run = true; + warnx("dropping now"); } else { -- cgit v1.2.3 From 56834414f1e37dd5f091685d3acd643d767dc0fa Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 4 Dec 2013 10:37:51 +0100 Subject: Dataman: Also reserve space for onboard missions --- src/modules/dataman/dataman.c | 1 + src/modules/dataman/dataman.h | 6 ++++-- 2 files changed, 5 insertions(+), 2 deletions(-) diff --git a/src/modules/dataman/dataman.c b/src/modules/dataman/dataman.c index dd3573d9a..acd612d9e 100644 --- a/src/modules/dataman/dataman.c +++ b/src/modules/dataman/dataman.c @@ -112,6 +112,7 @@ static const unsigned g_per_item_max_index[DM_KEY_NUM_KEYS] = { DM_KEY_SAFE_POINTS_MAX, DM_KEY_FENCE_POINTS_MAX, DM_KEY_WAYPOINTS_MAX, + DM_KEY_WAYPOINTS_ONBOARD_MAX }; /* Table of offset for index 0 of each item type */ diff --git a/src/modules/dataman/dataman.h b/src/modules/dataman/dataman.h index 9e1f789ad..dab96eb9b 100644 --- a/src/modules/dataman/dataman.h +++ b/src/modules/dataman/dataman.h @@ -50,7 +50,8 @@ extern "C" { typedef enum { DM_KEY_SAFE_POINTS = 0, /* Safe points coordinates, safe point 0 is home point */ DM_KEY_FENCE_POINTS, /* Fence vertex coordinates */ - DM_KEY_WAYPOINTS, /* Mission way point coordinates */ + DM_KEY_WAYPOINTS, /* Mission way point coordinates sent over mavlink */ + DM_KEY_WAYPOINTS_ONBOARD, /* Mission way point coordinates generated onboard */ DM_KEY_NUM_KEYS /* Total number of item types defined */ } dm_item_t; @@ -58,7 +59,8 @@ extern "C" { enum { DM_KEY_SAFE_POINTS_MAX = 8, DM_KEY_FENCE_POINTS_MAX = GEOFENCE_MAX_VERTICES, - DM_KEY_WAYPOINTS_MAX = NUM_MISSIONS_SUPPORTED + DM_KEY_WAYPOINTS_MAX = NUM_MISSIONS_SUPPORTED, + DM_KEY_WAYPOINTS_ONBOARD_MAX = NUM_MISSIONS_SUPPORTED }; /* Data persistence levels */ -- cgit v1.2.3 From 0aeada16b9601783da4fa1a7d628b34e1dce1bf4 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 4 Dec 2013 10:38:56 +0100 Subject: Navigator: handle onboard and mavlink missions --- src/modules/navigator/navigator_main.cpp | 172 ++++++++++++++++++++----------- src/modules/uORB/topics/mission.h | 3 +- 2 files changed, 113 insertions(+), 62 deletions(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index c6aac6af1..e2e2949e2 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -142,6 +142,7 @@ private: int _vstatus_sub; /**< vehicle status subscription */ int _params_sub; /**< notification of parameter updates */ int _mission_sub; /**< notification of mission updates */ + int _onboard_mission_sub; /**< notification of onboard mission updates */ int _capabilities_sub; /**< notification of vehicle capabilities updates */ orb_advert_t _triplet_pub; /**< publish position setpoint triplet */ @@ -155,11 +156,9 @@ private: struct mission_result_s _mission_result; /**< mission result for commander/mavlink */ perf_counter_t _loop_perf; /**< loop performance counter */ - - unsigned _max_mission_item_count; /**< maximum number of mission items supported */ unsigned _mission_item_count; /** number of mission items copied */ - + unsigned _onboard_mission_item_count; /** number of onboard mission items copied */ struct fence_s _fence; /**< storage for fence vertices */ bool _fence_valid; /**< flag if fence is valid */ bool _inside_fence; /**< vehicle is inside fence */ @@ -173,6 +172,7 @@ private: navigation_mode_t _mode; unsigned _current_mission_index; + unsigned _current_onboard_mission_index; struct { float min_altitude; @@ -198,6 +198,11 @@ private: */ void mission_update(); + /** + * Retrieve onboard mission. + */ + void onboard_mission_update(); + /** * Shim for calling task_main from task_create. */ @@ -216,7 +221,11 @@ private: void set_mode(navigation_mode_t new_nav_mode); - int set_waypoint_mission_item(unsigned mission_item_index, struct mission_item_s *new_mission_item); + bool mission_possible(); + + bool onboard_mission_possible(); + + int set_waypoint_mission_item(dm_item_t dm_item, unsigned mission_item_index, struct mission_item_s *new_mission_item); void publish_mission_item_triplet(); @@ -270,6 +279,7 @@ Navigator::Navigator() : _vstatus_sub(-1), _params_sub(-1), _mission_sub(-1), + _onboard_mission_sub(-1), _capabilities_sub(-1), /* publications */ @@ -280,8 +290,8 @@ Navigator::Navigator() : /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "navigator")), /* states */ - _max_mission_item_count(10), _mission_item_count(0), + _onboard_mission_item_count(0), _fence_valid(false), _inside_fence(true), _waypoint_position_reached(false), @@ -289,7 +299,8 @@ Navigator::Navigator() : _time_first_inside_orbit(0), _mission_item_reached(false), _mode(NAVIGATION_MODE_NONE), - _current_mission_index(0) + _current_mission_index(0), + _current_onboard_mission_index(0) { _global_pos.valid = false; memset(&_fence, 0, sizeof(_fence)); @@ -352,32 +363,6 @@ Navigator::mission_update() struct mission_s mission; if (orb_copy(ORB_ID(mission), _mission_sub, &mission) == OK) { -// /* Check if first part of mission (up to _current_mission_index - 1) changed: -// * if the first part changed: start again at first waypoint -// * if the first part remained unchanged: continue with the (possibly changed second part) -// */ -// if (mission.current_index == -1 && _current_mission_index < _mission_item_count && _current_mission_index < mission.count) { //check if not finished and if the new mission is not a shorter mission -// for (unsigned i = 0; i < _current_mission_index; i++) { -// if (!cmp_mission_item_equivalent(_mission_item[i], mission.items[i])) { -// /* set flag to restart mission next we're in auto */ -// _current_mission_index = 0; -// mavlink_log_info(_mavlink_fd, "[navigator] Reset to WP %d", _current_mission_index); -// //warnx("First part of mission differs i=%d", i); -// break; -// } -// // else { -// // warnx("Mission item is equivalent i=%d", i); -// // } -// } -// } else if (mission.current_index >= 0 && mission.current_index < mission.count) { -// /* set flag to restart mission next we're in auto */ -// _current_mission_index = mission.current_index; -// mavlink_log_info(_mavlink_fd, "[navigator] Reset to WP %d", _current_mission_index); -// } else { -// _current_mission_index = 0; -// mavlink_log_info(_mavlink_fd, "[navigator] Reset to WP %d", _current_mission_index); -// } - _mission_item_count = mission.count; _current_mission_index = mission.current_index; @@ -385,7 +370,7 @@ Navigator::mission_update() _mission_item_count = 0; _current_mission_index = 0; } - if (_mission_item_count == 0 && _mode == NAVIGATION_MODE_WAYPOINT) { + if ((!mission_possible() || !onboard_mission_possible()) && _mode == NAVIGATION_MODE_WAYPOINT) { set_mode(NAVIGATION_MODE_LOITER); } else if (_mode == NAVIGATION_MODE_WAYPOINT) { @@ -393,7 +378,27 @@ Navigator::mission_update() } } +void +Navigator::onboard_mission_update() +{ + struct mission_s onboard_mission; + if (orb_copy(ORB_ID(onboard_mission), _onboard_mission_sub, &onboard_mission) == OK) { + + _onboard_mission_item_count = onboard_mission.count; + _current_onboard_mission_index = onboard_mission.current_index; + + } else { + _onboard_mission_item_count = 0; + _current_onboard_mission_index = 0; + } + if ((!mission_possible() || !onboard_mission_possible()) && _mode == NAVIGATION_MODE_WAYPOINT) { + set_mode(NAVIGATION_MODE_LOITER); + } + else if (_mode == NAVIGATION_MODE_WAYPOINT) { + start_waypoint(); + } +} void Navigator::task_main_trampoline(int argc, char *argv[]) @@ -414,6 +419,7 @@ Navigator::task_main() */ _global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); _mission_sub = orb_subscribe(ORB_ID(mission)); + _onboard_mission_sub = orb_subscribe(ORB_ID(onboard_mission)); _capabilities_sub = orb_subscribe(ORB_ID(navigation_capabilities)); _vstatus_sub = orb_subscribe(ORB_ID(vehicle_status)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); @@ -425,6 +431,7 @@ Navigator::task_main() } mission_update(); + onboard_mission_update(); /* rate limit vehicle status updates to 5Hz */ orb_set_interval(_vstatus_sub, 200); @@ -439,7 +446,7 @@ Navigator::task_main() orb_copy(ORB_ID(navigation_capabilities), _capabilities_sub, &_nav_caps); /* wakeup source(s) */ - struct pollfd fds[6]; + struct pollfd fds[7]; /* Setup of loop */ fds[0].fd = _params_sub; @@ -452,8 +459,10 @@ Navigator::task_main() fds[3].events = POLLIN; fds[4].fd = _mission_sub; fds[4].events = POLLIN; - fds[5].fd = _vstatus_sub; + fds[5].fd = _onboard_mission_sub; fds[5].events = POLLIN; + fds[6].fd = _vstatus_sub; + fds[6].events = POLLIN; while (!_task_should_exit) { @@ -475,7 +484,7 @@ Navigator::task_main() perf_begin(_loop_perf); /* only update vehicle status if it changed */ - if (fds[5].revents & POLLIN) { + if (fds[6].revents & POLLIN) { /* read from param to clear updated flag */ orb_copy(ORB_ID(vehicle_status), _vstatus_sub, &_vstatus); @@ -505,8 +514,8 @@ Navigator::task_main() case NAVIGATION_STATE_AUTO_MISSION: - if (_mission_item_count > 0 && !(_current_mission_index >= _mission_item_count)) { - /* Start mission if there is a mission available and the last waypoint has not been reached */ + if (mission_possible() || onboard_mission_possible()) { + /* Start mission or onboard mission if available */ set_mode(NAVIGATION_MODE_WAYPOINT); } else { /* else fallback to loiter */ @@ -556,6 +565,10 @@ Navigator::task_main() mission_update(); } + if (fds[5].revents & POLLIN) { + onboard_mission_update(); + } + if (fds[2].revents & POLLIN) { orb_copy(ORB_ID(home_position), _home_pos_sub, &_home_pos); } @@ -586,9 +599,15 @@ Navigator::task_main() if (_mission_item_reached) { - report_mission_reached(); + + + if (onboard_mission_possible()) { + mavlink_log_info(_mavlink_fd, "[navigator] reached onboard WP %d", _current_onboard_mission_index); + } else { + mavlink_log_info(_mavlink_fd, "[navigator] reached WP %d", _current_mission_index); + report_mission_reached(); + } - mavlink_log_info(_mavlink_fd, "[navigator] reached WP %d", _current_mission_index); if (advance_current_mission_item() != OK) { set_mode(NAVIGATION_MODE_LOITER_WAYPOINT); } @@ -863,16 +882,27 @@ Navigator::set_mode(navigation_mode_t new_nav_mode) } } +bool +Navigator::mission_possible() +{ + return _mission_item_count > 0 && + !(_current_mission_index >= _mission_item_count); +} + +bool +Navigator::onboard_mission_possible() +{ + return _onboard_mission_item_count > 0 && + !(_current_onboard_mission_index >= _onboard_mission_item_count) && + _parameters.onboard_mission_enabled; +} + int -Navigator::set_waypoint_mission_item(unsigned mission_item_index, struct mission_item_s *new_mission_item) +Navigator::set_waypoint_mission_item(dm_item_t dm_item, unsigned mission_item_index, struct mission_item_s *new_mission_item) { - if (mission_item_index >= _mission_item_count) { - return ERROR; - } - struct mission_item_s mission_item; - - if (dm_read(DM_KEY_WAYPOINTS, mission_item_index, &mission_item, sizeof(struct mission_item_s)) != sizeof(struct mission_item_s)) { + + if (dm_read(dm_item, mission_item_index, &mission_item, sizeof(struct mission_item_s)) != sizeof(struct mission_item_s)) { return ERROR; } @@ -926,8 +956,7 @@ Navigator::advance_current_mission_item() // warnx("advancing from %d to %d", _current_mission_index, _current_mission_index+1); /* ultimately this index will be == _mission_item_count and this flags the mission as completed */ - _current_mission_index++; - + /* if there is no more mission available, don't advance and return */ if (!_mission_item_triplet.next_valid) { // warnx("no next available"); @@ -941,9 +970,20 @@ Navigator::advance_current_mission_item() /* copy the next to current */ memcpy(&_mission_item_triplet.current, &_mission_item_triplet.next, sizeof(mission_item_s)); _mission_item_triplet.current_valid = _mission_item_triplet.next_valid; + + int ret = ERROR; + + if (onboard_mission_possible()) { + _current_onboard_mission_index++; + ret = set_waypoint_mission_item(DM_KEY_WAYPOINTS_ONBOARD, _current_onboard_mission_index + 1, &_mission_item_triplet.next); + } else if (mission_possible()) { + _current_mission_index++; + ret = set_waypoint_mission_item(DM_KEY_WAYPOINTS, _current_mission_index + 1, &_mission_item_triplet.next); + } else { + warnx("Error: nothing to advance"); + } - - if(set_waypoint_mission_item(_current_mission_index + 1, &_mission_item_triplet.next) == OK) { + if(ret == OK) { _mission_item_triplet.next_valid = true; } else { @@ -1078,17 +1118,27 @@ Navigator::start_waypoint() { reset_mission_item_reached(); - if (_current_mission_index > 0) { - set_waypoint_mission_item(_current_mission_index - 1, &_mission_item_triplet.previous); - _mission_item_triplet.previous_valid = true; - } else { - _mission_item_triplet.previous_valid = false; - } + // if (_current_mission_index > 0) { + // set_waypoint_mission_item(_current_mission_index - 1, &_mission_item_triplet.previous); + // _mission_item_triplet.previous_valid = true; + // } else { + // _mission_item_triplet.previous_valid = false; + // } + _mission_item_triplet.previous_valid = false; - set_waypoint_mission_item(_current_mission_index, &_mission_item_triplet.current); - _mission_item_triplet.current_valid = true; + int ret = ERROR; + + if (onboard_mission_possible()) { + set_waypoint_mission_item(DM_KEY_WAYPOINTS_ONBOARD, _current_onboard_mission_index, &_mission_item_triplet.current); + mavlink_log_info(_mavlink_fd, "[navigator] heading to onboard WP %d", _current_onboard_mission_index); + ret = set_waypoint_mission_item(DM_KEY_WAYPOINTS_ONBOARD, _current_onboard_mission_index + 1, &_mission_item_triplet.next); + } else if (mission_possible()) { + set_waypoint_mission_item(DM_KEY_WAYPOINTS, _current_mission_index, &_mission_item_triplet.current); + mavlink_log_info(_mavlink_fd, "[navigator] heading to WP %d", _current_mission_index); + ret = set_waypoint_mission_item(DM_KEY_WAYPOINTS, _current_mission_index + 1, &_mission_item_triplet.next); + } - mavlink_log_info(_mavlink_fd, "[navigator] heading to WP %d", _current_mission_index); + _mission_item_triplet.current_valid = true; // if (_mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_UNLIMITED) { @@ -1100,7 +1150,7 @@ Navigator::start_waypoint() // _mission_item_triplet.next_valid = true; // } - if(set_waypoint_mission_item(_current_mission_index + 1, &_mission_item_triplet.next) == OK) { + if(ret == OK) { _mission_item_triplet.next_valid = true; } else { diff --git a/src/modules/uORB/topics/mission.h b/src/modules/uORB/topics/mission.h index 30f06c359..370242007 100644 --- a/src/modules/uORB/topics/mission.h +++ b/src/modules/uORB/topics/mission.h @@ -96,7 +96,7 @@ struct mission_item_s struct mission_s { - unsigned count; + unsigned count; /**< count of the missions stored in the datamanager */ int current_index; /**< default -1, start at the one changed latest */ }; @@ -106,5 +106,6 @@ struct mission_s /* register this as object request broker structure */ ORB_DECLARE(mission); +ORB_DECLARE(onboard_mission); #endif -- cgit v1.2.3 From d0444497eddb75beae4bc0001e6aacbc137e0d66 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sat, 14 Dec 2013 20:52:25 +0100 Subject: Bottle_drop: Store WPs in datamanager --- src/modules/bottle_drop/bottle_drop.c | 63 ++++++++++++++++++++--------------- 1 file changed, 37 insertions(+), 26 deletions(-) diff --git a/src/modules/bottle_drop/bottle_drop.c b/src/modules/bottle_drop/bottle_drop.c index e2194bae2..1911329d0 100644 --- a/src/modules/bottle_drop/bottle_drop.c +++ b/src/modules/bottle_drop/bottle_drop.c @@ -55,6 +55,7 @@ #include #include +#include #include #include @@ -271,22 +272,21 @@ int bottle_drop_thread_main(int argc, char *argv[]) { struct actuator_controls_s actuators; memset(&actuators, 0, sizeof(actuators)); orb_advert_t actuator_pub = orb_advertise(ORB_ID(actuator_controls_1), &actuators); - - // struct mission_s onboard_mission; - // memset(&onboard_mission, 0, sizeof(onboard_mission)); - // onboard_mission.items = (struct mission_item_s*)malloc(sizeof(struct mission_item_s) * 2); - struct mission_item_s *flight_vector_s = NULL;//&onboard_mission.items[0]; - struct mission_item_s *flight_vector_e = NULL;//&onboard_mission.items[1]; + struct mission_item_s flight_vector_s; + struct mission_item_s flight_vector_e; + + flight_vector_s.nav_cmd = NAV_CMD_WAYPOINT; + flight_vector_s.radius = 50; // TODO: make parameter + flight_vector_s.autocontinue = true; + flight_vector_e.nav_cmd = NAV_CMD_WAYPOINT; + flight_vector_e.radius = 50; // TODO: make parameter + flight_vector_e.autocontinue = true; - // flight_vector_s->nav_cmd = NAV_CMD_WAYPOINT; - // flight_vector_s->radius = 50; // TODO: make parameter - // flight_vector_s->autocontinue = true; - // flight_vector_e->nav_cmd = NAV_CMD_WAYPOINT; - // flight_vector_e->radius = 50; // TODO: make parameter - // flight_vector_e->autocontinue = true; + struct mission_s onboard_mission; + memset(&onboard_mission, 0, sizeof(onboard_mission)); - // orb_advert_t onboard_mission_pub = orb_advertise(ORB_ID(onboard_mission), &onboard_mission); + orb_advert_t onboard_mission_pub = orb_advertise(ORB_ID(onboard_mission), &onboard_mission); struct pollfd fds[] = { { .fd = vehicle_attitude_sub, .events = POLLIN } @@ -420,10 +420,10 @@ int bottle_drop_thread_main(int argc, char *argv[]) { // Compute flight vector - map_projection_reproject(x_drop + 2*turn_radius*wind_speed.vx/vr, y_drop + 2*turn_radius*wind_speed.vy/vr, &(flight_vector_s->lat), &(flight_vector_s->lon)); - flight_vector_s->altitude = height; - map_projection_reproject(x_drop - turn_radius*wind_speed.vx/vr, y_drop - turn_radius*wind_speed.vy/vr, &(flight_vector_e)->lat, &(flight_vector_e)->lon); - flight_vector_e->altitude = height; + map_projection_reproject(x_drop + 2*turn_radius*wind_speed.vx/vr, y_drop + 2*turn_radius*wind_speed.vy/vr, &(flight_vector_s.lat), &(flight_vector_s.lon)); + flight_vector_s.altitude = height; + map_projection_reproject(x_drop - turn_radius*wind_speed.vx/vr, y_drop - turn_radius*wind_speed.vy/vr, &flight_vector_e.lat, &flight_vector_e.lon); + flight_vector_e.altitude = height; //warnx("Flight vector: starting point = %.7f %.7f , end point = %.7f %.7f", flight_vector_s.lat,flight_vector_s.lon,flight_vector_e.lat,flight_vector_e.lon); //////////////////////////////////////////////////////////////////// DEBUGGING // Drop Cancellation if terms are not met @@ -448,18 +448,29 @@ int bottle_drop_thread_main(int argc, char *argv[]) { // warnx("future_distance: %.2f, precision: %.2f", future_distance, precision); // } - // onboard_mission.count = 2; + /* Save WPs in datamanager */ + const size_t len = sizeof(struct mission_item_s); - // if (state_run && !state_drop) { - // onboard_mission.current_index = 0; - // } else { - // onboard_mission.current_index = -1; - // } + if (dm_write(DM_KEY_WAYPOINTS_ONBOARD, 0, DM_PERSIST_IN_FLIGHT_RESET, &flight_vector_s, len) != len) { + warnx("ERROR: could not save onboard WP"); + } + + if (dm_write(DM_KEY_WAYPOINTS_ONBOARD, 1, DM_PERSIST_IN_FLIGHT_RESET, &flight_vector_e, len) != len) { + warnx("ERROR: could not save onboard WP"); + } + + onboard_mission.count = 2; + + if (state_run && !state_drop) { + onboard_mission.current_index = 0; + } else { + onboard_mission.current_index = -1; + } - // if (counter % 10 ==0) - // warnx("Distance real: %.2f, distance_open_door: %.2f, angle to wind: %.2f", distance_real, distance_open_door, fabsf(_wrap_pi(globalpos.yaw-atan2f(wind_speed.vy,wind_speed.vx)+M_PI_F))); + if (counter % 10 ==0) + warnx("Distance real: %.2f, distance_open_door: %.2f, angle to wind: %.2f", distance_real, distance_open_door, fabsf(_wrap_pi(globalpos.yaw-atan2f(wind_speed.vy,wind_speed.vx)+M_PI_F))); - // orb_publish(ORB_ID(onboard_mission), onboard_mission_pub, &onboard_mission); + orb_publish(ORB_ID(onboard_mission), onboard_mission_pub, &onboard_mission); } -- cgit v1.2.3 From 73907d1ac00b59759137d71f18693228cc3e75c9 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sat, 14 Dec 2013 20:54:25 +0100 Subject: Datamanager: Rename mavlink/offboard key --- src/modules/dataman/dataman.c | 2 +- src/modules/dataman/dataman.h | 4 ++-- src/modules/mavlink/waypoints.c | 6 +++--- src/systemcmds/tests/test_dataman.c | 8 ++++---- 4 files changed, 10 insertions(+), 10 deletions(-) diff --git a/src/modules/dataman/dataman.c b/src/modules/dataman/dataman.c index acd612d9e..874a47be7 100644 --- a/src/modules/dataman/dataman.c +++ b/src/modules/dataman/dataman.c @@ -111,7 +111,7 @@ static unsigned g_func_counts[dm_number_of_funcs]; static const unsigned g_per_item_max_index[DM_KEY_NUM_KEYS] = { DM_KEY_SAFE_POINTS_MAX, DM_KEY_FENCE_POINTS_MAX, - DM_KEY_WAYPOINTS_MAX, + DM_KEY_WAYPOINTS_OFFBOARD_MAX, DM_KEY_WAYPOINTS_ONBOARD_MAX }; diff --git a/src/modules/dataman/dataman.h b/src/modules/dataman/dataman.h index dab96eb9b..2a781405a 100644 --- a/src/modules/dataman/dataman.h +++ b/src/modules/dataman/dataman.h @@ -50,7 +50,7 @@ extern "C" { typedef enum { DM_KEY_SAFE_POINTS = 0, /* Safe points coordinates, safe point 0 is home point */ DM_KEY_FENCE_POINTS, /* Fence vertex coordinates */ - DM_KEY_WAYPOINTS, /* Mission way point coordinates sent over mavlink */ + DM_KEY_WAYPOINTS_OFFBOARD, /* Mission way point coordinates sent over mavlink */ DM_KEY_WAYPOINTS_ONBOARD, /* Mission way point coordinates generated onboard */ DM_KEY_NUM_KEYS /* Total number of item types defined */ } dm_item_t; @@ -59,7 +59,7 @@ extern "C" { enum { DM_KEY_SAFE_POINTS_MAX = 8, DM_KEY_FENCE_POINTS_MAX = GEOFENCE_MAX_VERTICES, - DM_KEY_WAYPOINTS_MAX = NUM_MISSIONS_SUPPORTED, + DM_KEY_WAYPOINTS_OFFBOARD_MAX = NUM_MISSIONS_SUPPORTED, DM_KEY_WAYPOINTS_ONBOARD_MAX = NUM_MISSIONS_SUPPORTED }; diff --git a/src/modules/mavlink/waypoints.c b/src/modules/mavlink/waypoints.c index 7aad5038d..52a580d5b 100644 --- a/src/modules/mavlink/waypoints.c +++ b/src/modules/mavlink/waypoints.c @@ -701,7 +701,7 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi struct mission_item_s mission_item; ssize_t len = sizeof(struct mission_item_s); - if (dm_read(DM_KEY_WAYPOINTS, wpr.seq, &mission_item, len) == len) { + if (dm_read(DM_KEY_WAYPOINTS_OFFBOARD, wpr.seq, &mission_item, len) == len) { if (mission.current_index == wpr.seq) { wp.current = true; @@ -975,7 +975,7 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi size_t len = sizeof(struct mission_item_s); - if (dm_write(DM_KEY_WAYPOINTS, wp.seq, DM_PERSIST_IN_FLIGHT_RESET, &mission_item, len) != len) { + if (dm_write(DM_KEY_WAYPOINTS_OFFBOARD, wp.seq, DM_PERSIST_IN_FLIGHT_RESET, &mission_item, len) != len) { mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_ERROR); wpm->current_state = MAVLINK_WPM_STATE_IDLE; break; @@ -1117,7 +1117,7 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi /* prepare mission topic */ mission.count = 0; - if (dm_clear(DM_KEY_WAYPOINTS) == OK) { + if (dm_clear(DM_KEY_WAYPOINTS_OFFBOARD) == OK) { mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_ACCEPTED); } else { mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_ERROR); diff --git a/src/systemcmds/tests/test_dataman.c b/src/systemcmds/tests/test_dataman.c index 7de87b476..5b121e34e 100644 --- a/src/systemcmds/tests/test_dataman.c +++ b/src/systemcmds/tests/test_dataman.c @@ -89,7 +89,7 @@ task_main(int argc, char *argv[]) unsigned hash = i ^ my_id; unsigned len = (hash & 63) + 2; - int ret = dm_write(DM_KEY_WAYPOINTS, hash, DM_PERSIST_IN_FLIGHT_RESET, buffer, len); + int ret = dm_write(DM_KEY_WAYPOINTS_OFFBOARD, hash, DM_PERSIST_IN_FLIGHT_RESET, buffer, len); warnx("ret: %d", ret); if (ret != len) { warnx("%d write failed, index %d, length %d", my_id, hash, len); @@ -103,7 +103,7 @@ task_main(int argc, char *argv[]) for (unsigned i = 0; i < NUM_MISSIONS_SUPPORTED; i++) { unsigned hash = i ^ my_id; unsigned len2, len = (hash & 63) + 2; - if ((len2 = dm_read(DM_KEY_WAYPOINTS, hash, buffer, sizeof(buffer))) < 2) { + if ((len2 = dm_read(DM_KEY_WAYPOINTS_OFFBOARD, hash, buffer, sizeof(buffer))) < 2) { warnx("%d read failed length test, index %d", my_id, hash); goto fail; } @@ -163,7 +163,7 @@ int test_dataman(int argc, char *argv[]) free(sems); dm_restart(DM_INIT_REASON_IN_FLIGHT); for (i = 0; i < NUM_MISSIONS_SUPPORTED; i++) { - if (dm_read(DM_KEY_WAYPOINTS, i, buffer, sizeof(buffer)) != 0) + if (dm_read(DM_KEY_WAYPOINTS_OFFBOARD, i, buffer, sizeof(buffer)) != 0) break; } if (i >= NUM_MISSIONS_SUPPORTED) { @@ -173,7 +173,7 @@ int test_dataman(int argc, char *argv[]) } dm_restart(DM_INIT_REASON_POWER_ON); for (i = 0; i < NUM_MISSIONS_SUPPORTED; i++) { - if (dm_read(DM_KEY_WAYPOINTS, i, buffer, sizeof(buffer)) != 0) { + if (dm_read(DM_KEY_WAYPOINTS_OFFBOARD, i, buffer, sizeof(buffer)) != 0) { warnx("Restart power-on failed"); return -1; } -- cgit v1.2.3 From e685c65365511341edb7cb4eded8d645643114a6 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sat, 14 Dec 2013 20:55:03 +0100 Subject: Navigator: Use state table for main FSM --- src/modules/navigator/navigator_main.cpp | 1049 +++++++++++++++++------------- src/modules/systemlib/state_table.h | 75 +++ 2 files changed, 660 insertions(+), 464 deletions(-) create mode 100644 src/modules/systemlib/state_table.h diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index e2e2949e2..d258aa8a6 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -38,6 +38,7 @@ * Implementation of the main navigation state machine. * * Handles missions, geo fencing and failsafe navigation behavior. + * Published the mission item triplet for the position controller. */ #include @@ -66,21 +67,15 @@ #include #include #include -#include +#include #include #include +#include #include #include #include -typedef enum { - NAVIGATION_MODE_NONE, - NAVIGATION_MODE_LOITER, - NAVIGATION_MODE_WAYPOINT, - NAVIGATION_MODE_LOITER_WAYPOINT, - NAVIGATION_MODE_RTL, - NAVIGATION_MODE_LOITER_RTL -} navigation_mode_t; + /* oddly, ERROR is not defined for c++ */ #ifdef ERROR @@ -95,7 +90,7 @@ static const int ERROR = -1; */ extern "C" __EXPORT int navigator_main(int argc, char *argv[]); -class Navigator +class Navigator : public StateTable { public: /** @@ -141,7 +136,7 @@ private: int _home_pos_sub; /**< home position subscription */ int _vstatus_sub; /**< vehicle status subscription */ int _params_sub; /**< notification of parameter updates */ - int _mission_sub; /**< notification of mission updates */ + int _offboard_mission_sub; /**< notification of offboard mission updates */ int _onboard_mission_sub; /**< notification of onboard mission updates */ int _capabilities_sub; /**< notification of vehicle capabilities updates */ @@ -157,22 +152,26 @@ private: perf_counter_t _loop_perf; /**< loop performance counter */ - unsigned _mission_item_count; /** number of mission items copied */ - unsigned _onboard_mission_item_count; /** number of onboard mission items copied */ - struct fence_s _fence; /**< storage for fence vertices */ + struct fence_s _fence; /**< storage for fence vertices */ bool _fence_valid; /**< flag if fence is valid */ bool _inside_fence; /**< vehicle is inside fence */ struct navigation_capabilities_s _nav_caps; + unsigned _current_offboard_mission_index; + unsigned _current_onboard_mission_index; + unsigned _offboard_mission_item_count; /** number of offboard mission items copied */ + unsigned _onboard_mission_item_count; /** number of onboard mission items copied */ + + enum { + MISSION_TYPE_NONE, + MISSION_TYPE_ONBOARD, + MISSION_TYPE_OFFBOARD, + } _mission_type; + bool _waypoint_position_reached; bool _waypoint_yaw_reached; uint64_t _time_first_inside_orbit; - bool _mission_item_reached; - - navigation_mode_t _mode; - unsigned _current_mission_index; - unsigned _current_onboard_mission_index; struct { float min_altitude; @@ -187,22 +186,69 @@ private: } _parameter_handles; /**< handles for parameters */ + enum Event { + EVENT_NONE_REQUESTED, + EVENT_LOITER_REQUESTED, + EVENT_MISSION_REQUESTED, + EVENT_RTL_REQUESTED, + EVENT_MISSION_FINISHED, + EVENT_MISSION_CHANGED, + EVENT_HOME_POSITION_CHANGED, + MAX_EVENT + }; + + enum State { + STATE_INIT, + STATE_NONE, + STATE_LOITER, + STATE_MISSION, + STATE_MISSION_LOITER, + STATE_RTL, + STATE_RTL_LOITER, + MAX_STATE + }; + + /** + * State machine transition table + */ + static StateTable::Tran const myTable[MAX_STATE][MAX_EVENT]; /** * Update our local parameter cache. */ - int parameters_update(); + void parameters_update(); + + /** + * Retrieve global position + */ + void global_position_update(); /** - * Retrieve mission. - */ - void mission_update(); + * Retrieve home position + */ + void home_position_update(); /** - * Retrieve onboard mission. - */ + * Retreive navigation capabilities + */ + void navigation_capabilities_update(); + + /** + * Retrieve offboard mission. + */ + void offboard_mission_update(); + + /** + * Retrieve onboard mission. + */ void onboard_mission_update(); + /** + * Retrieve vehicle status + */ + void vehicle_status_update(); + + /** * Shim for calling task_main from task_create. */ @@ -219,31 +265,52 @@ private: bool fence_valid(const struct fence_s &fence); - void set_mode(navigation_mode_t new_nav_mode); - - bool mission_possible(); - - bool onboard_mission_possible(); - - int set_waypoint_mission_item(dm_item_t dm_item, unsigned mission_item_index, struct mission_item_s *new_mission_item); + /** + * Functions that are triggered when a new state is entered. + */ + void start_none(); + void start_loiter(); + void start_mission(); + void start_mission_loiter(); + void start_rtl(); + void start_rtl_loiter(); - void publish_mission_item_triplet(); + /** + * Guards offboard mission + */ + bool offboard_mission_available(unsigned relative_index); - void publish_mission_result(); + /** + * Guards onboard mission + */ + bool onboard_mission_available(unsigned relative_index); - int advance_current_mission_item(); + /** + * Check if current mission item has been reached. + */ + bool mission_item_reached(); - void reset_mission_item_reached(); + /** + * Move to next waypoint + */ + int advance_mission(); - void check_mission_item_reached(); + /** + * Helper function to set a mission item + */ + int set_mission_item(unsigned relative_index, struct mission_item_s *mission_item) ; - void report_mission_reached(); + /** + * Helper function to set a loiter item + */ + void set_loiter_item(mission_item_s *new_loiter_position); - void start_waypoint(); + /** + * Publish a new mission item triplet for position controller + */ + void publish_mission_item_triplet(); - void start_loiter(mission_item_s *new_loiter_position); - void start_rtl(); /** * Compare two mission items if they are equivalent @@ -266,11 +333,13 @@ static const int ERROR = -1; Navigator *g_navigator; } -Navigator::Navigator() : +Navigator::Navigator() : + +/* state machine transition table */ + StateTable(&myTable[0][0], MAX_STATE, MAX_EVENT), _task_should_exit(false), _navigator_task(-1), - _mavlink_fd(-1), /* subscriptions */ @@ -278,7 +347,7 @@ Navigator::Navigator() : _home_pos_sub(-1), _vstatus_sub(-1), _params_sub(-1), - _mission_sub(-1), + _offboard_mission_sub(-1), _onboard_mission_sub(-1), _capabilities_sub(-1), @@ -289,21 +358,20 @@ Navigator::Navigator() : /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "navigator")), + /* states */ - _mission_item_count(0), - _onboard_mission_item_count(0), _fence_valid(false), _inside_fence(true), + _current_offboard_mission_index(0), + _current_onboard_mission_index(0), + _offboard_mission_item_count(0), + _onboard_mission_item_count(0), _waypoint_position_reached(false), _waypoint_yaw_reached(false), - _time_first_inside_orbit(0), - _mission_item_reached(false), - _mode(NAVIGATION_MODE_NONE), - _current_mission_index(0), - _current_onboard_mission_index(0) + _time_first_inside_orbit(0) { - _global_pos.valid = false; memset(&_fence, 0, sizeof(_fence)); + _parameter_handles.min_altitude = param_find("NAV_MIN_ALT"); _parameter_handles.loiter_radius = param_find("NAV_LOITER_RAD"); _parameter_handles.onboard_mission_enabled = param_find("NAV_ONB_MIS_EN"); @@ -317,8 +385,8 @@ Navigator::Navigator() : memset(&_mission_result, 0, sizeof(struct mission_result_s)); - /* fetch initial values */ - parameters_update(); + /* Initialize state machine */ + myState = STATE_INIT; } Navigator::~Navigator() @@ -346,35 +414,53 @@ Navigator::~Navigator() navigator::g_navigator = nullptr; } -int +void Navigator::parameters_update() { + /* read from param to clear updated flag */ + struct parameter_update_s update; + orb_copy(ORB_ID(parameter_update), _params_sub, &update); param_get(_parameter_handles.min_altitude, &(_parameters.min_altitude)); param_get(_parameter_handles.loiter_radius, &(_parameters.loiter_radius)); param_get(_parameter_handles.onboard_mission_enabled, &(_parameters.onboard_mission_enabled)); +} - return OK; +void +Navigator::global_position_update() +{ + /* load local copies */ + orb_copy(ORB_ID(vehicle_global_position), _global_pos_sub, &_global_pos); } void -Navigator::mission_update() +Navigator::home_position_update() { - struct mission_s mission; - if (orb_copy(ORB_ID(mission), _mission_sub, &mission) == OK) { + orb_copy(ORB_ID(home_position), _home_pos_sub, &_home_pos); +} - _mission_item_count = mission.count; - _current_mission_index = mission.current_index; +void +Navigator::navigation_capabilities_update() +{ + orb_copy(ORB_ID(navigation_capabilities), _capabilities_sub, &_nav_caps); +} + + +void +Navigator::offboard_mission_update() +{ + struct mission_s offboard_mission; + if (orb_copy(ORB_ID(mission), _offboard_mission_sub, &offboard_mission) == OK) { + + _offboard_mission_item_count = offboard_mission.count; + + if (offboard_mission.current_index != -1) { + _current_offboard_mission_index = offboard_mission.current_index; + } } else { - _mission_item_count = 0; - _current_mission_index = 0; - } - if ((!mission_possible() || !onboard_mission_possible()) && _mode == NAVIGATION_MODE_WAYPOINT) { - set_mode(NAVIGATION_MODE_LOITER); - } - else if (_mode == NAVIGATION_MODE_WAYPOINT) { - start_waypoint(); + _offboard_mission_item_count = 0; + _current_offboard_mission_index = 0; } } @@ -385,18 +471,23 @@ Navigator::onboard_mission_update() if (orb_copy(ORB_ID(onboard_mission), _onboard_mission_sub, &onboard_mission) == OK) { _onboard_mission_item_count = onboard_mission.count; - _current_onboard_mission_index = onboard_mission.current_index; + + if (onboard_mission.current_index != -1) { + _current_onboard_mission_index = onboard_mission.current_index; + } } else { _onboard_mission_item_count = 0; _current_onboard_mission_index = 0; } +} - if ((!mission_possible() || !onboard_mission_possible()) && _mode == NAVIGATION_MODE_WAYPOINT) { - set_mode(NAVIGATION_MODE_LOITER); - } - else if (_mode == NAVIGATION_MODE_WAYPOINT) { - start_waypoint(); +void +Navigator::vehicle_status_update() +{ + /* try to load initial states */ + if (orb_copy(ORB_ID(vehicle_status), _vstatus_sub, &_vstatus) != OK) { + _vstatus.arming_state = ARMING_STATE_STANDBY; /* in case the commander is not be running */ } } @@ -414,37 +505,34 @@ Navigator::task_main() warnx("Initializing.."); fflush(stdout); + _fence_valid = load_fence(GEOFENCE_MAX_VERTICES); + /* * do subscriptions */ _global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); - _mission_sub = orb_subscribe(ORB_ID(mission)); + _offboard_mission_sub = orb_subscribe(ORB_ID(mission)); _onboard_mission_sub = orb_subscribe(ORB_ID(onboard_mission)); _capabilities_sub = orb_subscribe(ORB_ID(navigation_capabilities)); _vstatus_sub = orb_subscribe(ORB_ID(vehicle_status)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); _home_pos_sub = orb_subscribe(ORB_ID(home_position)); - // Load initial states - if (orb_copy(ORB_ID(vehicle_status), _vstatus_sub, &_vstatus) != OK) { - _vstatus.arming_state = ARMING_STATE_STANDBY; // for testing... commander may not be running - } - - mission_update(); + + /* copy all topics first time */ + parameters_update(); + global_position_update(); + home_position_update(); + navigation_capabilities_update(); + offboard_mission_update(); onboard_mission_update(); + vehicle_status_update(); /* rate limit vehicle status updates to 5Hz */ orb_set_interval(_vstatus_sub, 200); /* rate limit position updates to 50 Hz */ orb_set_interval(_global_pos_sub, 20); - parameters_update(); - - _fence_valid = load_fence(GEOFENCE_MAX_VERTICES); - - /* load the craft capabilities */ - orb_copy(ORB_ID(navigation_capabilities), _capabilities_sub, &_nav_caps); - /* wakeup source(s) */ struct pollfd fds[7]; @@ -457,7 +545,7 @@ Navigator::task_main() fds[2].events = POLLIN; fds[3].fd = _capabilities_sub; fds[3].events = POLLIN; - fds[4].fd = _mission_sub; + fds[4].fd = _offboard_mission_sub; fds[4].events = POLLIN; fds[5].fd = _onboard_mission_sub; fds[5].events = POLLIN; @@ -485,8 +573,8 @@ Navigator::task_main() /* only update vehicle status if it changed */ if (fds[6].revents & POLLIN) { - /* read from param to clear updated flag */ - orb_copy(ORB_ID(vehicle_status), _vstatus_sub, &_vstatus); + + vehicle_status_update(); /* Evaluate state machine from commander and set the navigator mode accordingly */ if (_vstatus.main_state == MAIN_STATE_AUTO) { @@ -497,135 +585,96 @@ Navigator::task_main() case NAVIGATION_STATE_ALTHOLD: case NAVIGATION_STATE_VECTOR: - set_mode(NAVIGATION_MODE_NONE); + /* don't publish a mission item triplet */ + dispatch(EVENT_NONE_REQUESTED); break; case NAVIGATION_STATE_AUTO_READY: case NAVIGATION_STATE_AUTO_TAKEOFF: + case NAVIGATION_STATE_AUTO_MISSION: - /* TODO probably not needed since takeoff WPs will just be passed on */ - //set_mode(NAVIGATION_MODE_TAKEOFF); + /* try mission if none is available, fallback to loiter instead */ + if (onboard_mission_available(0) || offboard_mission_available(0)) { + dispatch(EVENT_MISSION_REQUESTED); + } else { + dispatch(EVENT_LOITER_REQUESTED); + } break; case NAVIGATION_STATE_AUTO_LOITER: - set_mode(NAVIGATION_MODE_LOITER); + dispatch(EVENT_LOITER_REQUESTED); break; - case NAVIGATION_STATE_AUTO_MISSION: - - if (mission_possible() || onboard_mission_possible()) { - /* Start mission or onboard mission if available */ - set_mode(NAVIGATION_MODE_WAYPOINT); - } else { - /* else fallback to loiter */ - set_mode(NAVIGATION_MODE_LOITER); - } - break; case NAVIGATION_STATE_AUTO_RTL: - set_mode(NAVIGATION_MODE_RTL); + dispatch(EVENT_RTL_REQUESTED); break; case NAVIGATION_STATE_AUTO_LAND: /* TODO add this */ - //set_mode(NAVIGATION_MODE_LAND); + break; default: - warnx("Navigation state not supported"); + warnx("ERROR: Navigation state not supported"); break; } } else { - set_mode(NAVIGATION_MODE_NONE); + /* not in AUTO */ + dispatch(EVENT_NONE_REQUESTED); + } + + /* XXX Hack to get mavlink output going, try opening the fd with 5Hz */ + if (_mavlink_fd < 0) { + /* try to open the mavlink log device every once in a while */ + _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); } } + /* only update parameters if it changed */ if (fds[0].revents & POLLIN) { - /* read from param to clear updated flag */ - struct parameter_update_s update; - orb_copy(ORB_ID(parameter_update), _params_sub, &update); - /* update parameters from storage */ parameters_update(); - /* note that these new parameters won't be in effect until a mission triplet is published again */ } /* only update craft capabilities if they have changed */ if (fds[3].revents & POLLIN) { - orb_copy(ORB_ID(navigation_capabilities), _capabilities_sub, &_nav_caps); + navigation_capabilities_update(); } if (fds[4].revents & POLLIN) { - mission_update(); + offboard_mission_update(); + // XXX check if mission really changed + dispatch(EVENT_MISSION_CHANGED); } if (fds[5].revents & POLLIN) { onboard_mission_update(); + // XXX check if mission really changed + dispatch(EVENT_MISSION_CHANGED); } if (fds[2].revents & POLLIN) { - orb_copy(ORB_ID(home_position), _home_pos_sub, &_home_pos); + home_position_update(); + // XXX check if home position really changed + dispatch(EVENT_HOME_POSITION_CHANGED); } /* only run controller if position changed */ if (fds[1].revents & POLLIN) { - - /* XXX Hack to get mavlink output going */ - if (_mavlink_fd < 0) { - /* try to open the mavlink log device every once in a while */ - _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); - } - - /* load local copies */ - orb_copy(ORB_ID(vehicle_global_position), _global_pos_sub, &_global_pos); - - /* do stuff according to mode */ - switch (_mode) { - case NAVIGATION_MODE_NONE: - case NAVIGATION_MODE_LOITER: - case NAVIGATION_MODE_LOITER_WAYPOINT: - case NAVIGATION_MODE_LOITER_RTL: - break; - - case NAVIGATION_MODE_WAYPOINT: - - check_mission_item_reached(); - - if (_mission_item_reached) { - - - - if (onboard_mission_possible()) { - mavlink_log_info(_mavlink_fd, "[navigator] reached onboard WP %d", _current_onboard_mission_index); - } else { - mavlink_log_info(_mavlink_fd, "[navigator] reached WP %d", _current_mission_index); - report_mission_reached(); - } - - if (advance_current_mission_item() != OK) { - set_mode(NAVIGATION_MODE_LOITER_WAYPOINT); - } - } - break; - - case NAVIGATION_MODE_RTL: - - check_mission_item_reached(); - - if (_mission_item_reached) { - mavlink_log_info(_mavlink_fd, "[navigator] reached RTL position"); - set_mode(NAVIGATION_MODE_LOITER_RTL); - } - break; - default: - warnx("navigation mode not supported"); - break; + global_position_update(); + /* only check if waypoint has been reached in Mission or RTL mode */ + if (mission_item_reached()) { + /* try to advance mission */ + if (advance_mission() != OK) { + dispatch(EVENT_MISSION_FINISHED); + } } } perf_end(_loop_perf); @@ -668,15 +717,57 @@ Navigator::status() (double)_global_pos.alt, (double)_global_pos.relative_alt); warnx("Ground velocity in m/s, x %5.5f, y %5.5f, z %5.5f", (double)_global_pos.vx, (double)_global_pos.vy, (double)_global_pos.vz); - warnx("Compass heading in degrees %5.5f", (double)_global_pos.yaw * 57.2957795); + warnx("Compass heading in degrees %5.5f", (double)(_global_pos.yaw * M_RAD_TO_DEG_F)); } if (_fence_valid) { warnx("Geofence is valid"); warnx("Vertex longitude latitude"); for (unsigned i = 0; i < _fence.count; i++) warnx("%6u %9.5f %8.5f", i, (double)_fence.vertices[i].lon, (double)_fence.vertices[i].lat); - } else + } else { warnx("Geofence not set"); + } + + switch (myState) { + case STATE_INIT: + warnx("State: Init"); + break; + case STATE_NONE: + warnx("State: None"); + break; + case STATE_LOITER: + warnx("State: Loiter"); + break; + case STATE_MISSION: + warnx("State: Mission"); + switch (_mission_type) { + case MISSION_TYPE_ONBOARD: + warnx("Mission type: Onboard"); + break; + case MISSION_TYPE_OFFBOARD: + warnx("Mission type: Offboard"); + break; + case MISSION_TYPE_NONE: + default: + warnx("ERROR: Mission type unsupported"); + break; + } + warnx("Onboard mission: %d of %d", _current_onboard_mission_index, _onboard_mission_item_count); + warnx("Offboard mission: %d of %d", _current_offboard_mission_index, _offboard_mission_item_count); + break; + case STATE_MISSION_LOITER: + warnx("State: Loiter after Mission"); + break; + case STATE_RTL: + warnx("State: RTL"); + break; + case STATE_RTL_LOITER: + warnx("State: Loiter after RTL"); + break; + default: + warnx("State: Unknown"); + break; + } } void @@ -767,262 +858,360 @@ Navigator::fence_point(int argc, char *argv[]) errx(1, "can't store fence point"); } -void -Navigator::set_mode(navigation_mode_t new_nav_mode) -{ - if (new_nav_mode == _mode) { - /* no change, return */ - return; - } - switch (new_nav_mode) { - case NAVIGATION_MODE_NONE: - // warnx("Set mode NONE"); - _mode = new_nav_mode; - break; +StateTable::Tran const Navigator::myTable[MAX_STATE][MAX_EVENT] = { + { + /* STATE_INIT */ + /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), STATE_NONE}, + /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), STATE_LOITER}, + /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), STATE_MISSION}, + /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), STATE_RTL}, + /* EVENT_MISSION_FINISHED */ {NO_ACTION, STATE_INIT}, + /* EVENT_MISSION_CHANGED */ {NO_ACTION, STATE_INIT}, + /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, STATE_INIT}, + }, + { + /* STATE_NONE */ + /* EVENT_NONE_REQUESTED */ {NO_ACTION, STATE_NONE}, + /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), STATE_LOITER}, + /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), STATE_MISSION}, + /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), STATE_RTL}, + /* EVENT_MISSION_FINISHED */ {NO_ACTION, STATE_NONE}, + /* EVENT_MISSION_CHANGED */ {NO_ACTION, STATE_NONE}, + /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, STATE_NONE}, + }, + { + /* STATE_LOITER */ + /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), STATE_NONE}, + /* EVENT_LOITER_REQUESTED */ {NO_ACTION, STATE_LOITER}, + /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), STATE_MISSION}, + /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), STATE_RTL}, + /* EVENT_MISSION_FINISHED */ {NO_ACTION, STATE_LOITER}, + /* EVENT_MISSION_CHANGED */ {NO_ACTION, STATE_LOITER}, + /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, STATE_LOITER}, + }, + { + /* STATE_MISSION */ + /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), STATE_NONE}, + /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), STATE_LOITER}, + /* EVENT_MISSION_REQUESTED */ {NO_ACTION, STATE_MISSION}, + /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), STATE_RTL}, + /* EVENT_MISSION_FINISHED */ {ACTION(&Navigator::start_mission_loiter), STATE_MISSION_LOITER}, + /* EVENT_MISSION_CHANGED */ {ACTION(&Navigator::start_mission), STATE_MISSION}, + /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, STATE_MISSION}, + }, + { + /* STATE_MISSION_LOITER */ + /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), STATE_NONE}, + /* EVENT_LOITER_REQUESTED */ {NO_ACTION, STATE_MISSION_LOITER}, + /* EVENT_MISSION_REQUESTED */ {NO_ACTION, STATE_MISSION_LOITER}, + /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), STATE_RTL}, + /* EVENT_MISSION_FINISHED */ {NO_ACTION, STATE_MISSION_LOITER}, + /* EVENT_MISSION_CHANGED */ {ACTION(&Navigator::start_mission), STATE_MISSION}, + /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, STATE_MISSION_LOITER}, + }, + { + /* STATE_RTL */ + /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), STATE_NONE}, + /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), STATE_LOITER}, + /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), STATE_MISSION}, + /* EVENT_RTL_REQUESTED */ {NO_ACTION, STATE_RTL}, + /* EVENT_MISSION_FINISHED */ {ACTION(&Navigator::start_rtl_loiter), STATE_RTL_LOITER}, + /* EVENT_MISSION_CHANGED */ {NO_ACTION, STATE_RTL}, + /* EVENT_HOME_POSITION_CHANGED */ {ACTION(&Navigator::start_loiter), STATE_RTL}, + }, + { + /* STATE_RTL_LOITER */ + /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), STATE_NONE}, + /* EVENT_LOITER_REQUESTED */ {NO_ACTION, STATE_RTL_LOITER}, + /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), STATE_MISSION}, + /* EVENT_RTL_REQUESTED */ {NO_ACTION, STATE_RTL_LOITER}, + /* EVENT_MISSION_FINISHED */ {NO_ACTION, STATE_RTL_LOITER}, + /* EVENT_MISSION_CHANGED */ {NO_ACTION, STATE_RTL_LOITER}, + /* EVENT_HOME_POSITION_CHANGED */ {ACTION(&Navigator::start_loiter), STATE_LOITER}, + }, +}; - case NAVIGATION_MODE_LOITER: - - /* decide depending on previous navigation mode */ - switch (_mode) { - case NAVIGATION_MODE_NONE: - - case NAVIGATION_MODE_WAYPOINT: - case NAVIGATION_MODE_RTL: { - - /* use current position and loiter around it */ - mission_item_s global_position_mission_item; - global_position_mission_item.lat = (double)_global_pos.lat / 1e7; - global_position_mission_item.lon = (double)_global_pos.lon / 1e7; - - /* XXX get rid of ugly conversion for home position altitude */ - float global_min_alt = _parameters.min_altitude + (float)_home_pos.alt/1e3f; - - /* Use current altitude if above min altitude set by parameter */ - if (_global_pos.alt < global_min_alt) { - global_position_mission_item.altitude = global_min_alt; - mavlink_log_info(_mavlink_fd, "[navigator] loiter %.1fm higher", (double)(global_min_alt - _global_pos.alt)); - } else { - global_position_mission_item.altitude = _global_pos.alt; - mavlink_log_info(_mavlink_fd, "[navigator] loiter here"); - } - start_loiter(&global_position_mission_item); - _mode = new_nav_mode; - - break; - } - case NAVIGATION_MODE_LOITER_WAYPOINT: - case NAVIGATION_MODE_LOITER_RTL: - /* already loitering, just continue */ - _mode = new_nav_mode; - // warnx("continue loiter"); - break; +void +Navigator::start_none() +{ + _mission_item_triplet.previous_valid = false; + _mission_item_triplet.current_valid = false; + _mission_item_triplet.next_valid = false; - case NAVIGATION_MODE_LOITER: - default: - // warnx("previous navigation mode not supported"); - break; - } - break; + publish_mission_item_triplet(); +} - case NAVIGATION_MODE_WAYPOINT: +void +Navigator::start_loiter() +{ + struct mission_item_s loiter_item; - /* Start mission if there is one available */ - start_waypoint(); - _mode = new_nav_mode; - mavlink_log_info(_mavlink_fd, "[navigator] start waypoint mission"); - break; + loiter_item.lat = (double)_global_pos.lat / 1e7; + loiter_item.lon = (double)_global_pos.lon / 1e7; + loiter_item.yaw = 0.0f; - case NAVIGATION_MODE_LOITER_WAYPOINT: + /* XXX get rid of ugly conversion for home position altitude */ + float global_min_alt = _parameters.min_altitude + (float)_home_pos.alt/1e3f; - start_loiter(&_mission_item_triplet.current); - _mode = new_nav_mode; - mavlink_log_info(_mavlink_fd, "[navigator] loiter at WP %d", _current_mission_index-1); - break; + /* Use current altitude if above min altitude set by parameter */ + if (_global_pos.alt < global_min_alt) { + loiter_item.altitude = global_min_alt; + mavlink_log_info(_mavlink_fd, "[navigator] loiter %.1fm higher", (double)(global_min_alt - _global_pos.alt)); + } else { + loiter_item.altitude = _global_pos.alt; + mavlink_log_info(_mavlink_fd, "[navigator] loiter here"); + } - case NAVIGATION_MODE_RTL: + set_loiter_item(&loiter_item); - /* decide depending on previous navigation mode */ - switch (_mode) { - case NAVIGATION_MODE_NONE: - case NAVIGATION_MODE_LOITER: - case NAVIGATION_MODE_WAYPOINT: - case NAVIGATION_MODE_LOITER_WAYPOINT: + publish_mission_item_triplet(); +} - /* start RTL here */ - start_rtl(); - _mode = new_nav_mode; - mavlink_log_info(_mavlink_fd, "[navigator] start RTL"); - break; - case NAVIGATION_MODE_LOITER_RTL: - /* already loitering after RTL, just continue */ - // warnx("stay in loiter after RTL"); - break; +void +Navigator::start_mission() +{ + /* leave previous mission item as isas is */ - case NAVIGATION_MODE_RTL: - default: - warnx("previous navigation mode not supported"); - break; - } - break; + if(set_mission_item(0, &_mission_item_triplet.current) == OK) { + _mission_item_triplet.current_valid = true; + } else { + _mission_item_triplet.current_valid = false; + warnx("ERROR: current WP can't be set"); + } - case NAVIGATION_MODE_LOITER_RTL: + if(set_mission_item(1, &_mission_item_triplet.next) == OK) { + _mission_item_triplet.next_valid = true; + } else { + _mission_item_triplet.next_valid = false; + } - /* TODO: get rid of this conversion */ - mission_item_s home_position_mission_item; - home_position_mission_item.lat = (double)_home_pos.lat / 1e7; - home_position_mission_item.lon = (double)_home_pos.lon / 1e7; - home_position_mission_item.altitude = _home_pos.alt / 1e3f + _parameters.min_altitude; - start_loiter(&home_position_mission_item); - mavlink_log_info(_mavlink_fd, "[navigator] loiter after RTL"); - _mode = new_nav_mode; + switch (_mission_type) { + case MISSION_TYPE_ONBOARD: + mavlink_log_info(_mavlink_fd, "[navigator] heading to onboard WP %d", _current_onboard_mission_index); + break; + case MISSION_TYPE_OFFBOARD: + mavlink_log_info(_mavlink_fd, "[navigator] heading to offboard WP %d", _current_offboard_mission_index); + break; + case MISSION_TYPE_NONE: + default: + warnx("ERROR: Mission type unsupported"); break; } -} -bool -Navigator::mission_possible() -{ - return _mission_item_count > 0 && - !(_current_mission_index >= _mission_item_count); + publish_mission_item_triplet(); } -bool -Navigator::onboard_mission_possible() -{ - return _onboard_mission_item_count > 0 && - !(_current_onboard_mission_index >= _onboard_mission_item_count) && - _parameters.onboard_mission_enabled; -} + int -Navigator::set_waypoint_mission_item(dm_item_t dm_item, unsigned mission_item_index, struct mission_item_s *new_mission_item) +Navigator::advance_mission() { - struct mission_item_s mission_item; + switch (_mission_type) { + case MISSION_TYPE_ONBOARD: + warnx("advance onboard before: %d", _current_onboard_mission_index); + _current_onboard_mission_index++; + warnx("advance onboard after: %d", _current_onboard_mission_index); + break; + case MISSION_TYPE_OFFBOARD: + warnx("advance offboard before: %d", _current_offboard_mission_index); + _current_offboard_mission_index++; + warnx("advance offboard after: %d", _current_offboard_mission_index); + break; + case MISSION_TYPE_NONE: + default: + warnx("ERROR: Mission type unsupported"); + return ERROR; + } - if (dm_read(dm_item, mission_item_index, &mission_item, sizeof(struct mission_item_s)) != sizeof(struct mission_item_s)) { + /* if there is no more mission available, don't advance and switch to loiter at current WP */ + if (!_mission_item_triplet.next_valid) { + warnx("no next valid"); return ERROR; } - memcpy(new_mission_item, &mission_item, sizeof(struct mission_item_s)); + /* copy current mission to previous item */ + memcpy(&_mission_item_triplet.previous, &_mission_item_triplet.current, sizeof(mission_item_s)); + _mission_item_triplet.previous_valid = _mission_item_triplet.current_valid; - if (new_mission_item->nav_cmd == NAV_CMD_RETURN_TO_LAUNCH) { - /* if it is a RTL waypoint, append the home position */ - new_mission_item->lat = (double)_home_pos.lat / 1e7; - new_mission_item->lon = (double)_home_pos.lon / 1e7; - new_mission_item->altitude = (float)_home_pos.alt / 1e3f + _parameters.min_altitude; - new_mission_item->loiter_radius = _parameters.loiter_radius; // TODO: get rid of magic number - new_mission_item->radius = 50.0f; // TODO: get rid of magic number + if(set_mission_item(0, &_mission_item_triplet.current) == OK) { + _mission_item_triplet.current_valid = true; + + } else { + /* should never ever happen */ + _mission_item_triplet.current_valid = false; + warnx("no current available"); + return ERROR; + } + + if(set_mission_item(1, &_mission_item_triplet.next) == OK) { + _mission_item_triplet.next_valid = true; + + } else { + _mission_item_triplet.next_valid = false; } + publish_mission_item_triplet(); return OK; } void -Navigator::publish_mission_item_triplet() +Navigator::start_mission_loiter() { - /* lazily publish the mission triplet only once available */ - if (_triplet_pub > 0) { - /* publish the mission triplet */ - orb_publish(ORB_ID(mission_item_triplet), _triplet_pub, &_mission_item_triplet); - - } else { - /* advertise and publish */ - _triplet_pub = orb_advertise(ORB_ID(mission_item_triplet), &_mission_item_triplet); + /* make sure the current WP is valid */ + if (!_mission_item_triplet.current_valid) { + warnx("ERROR: cannot switch to offboard mission loiter"); + return; } -} -void -Navigator::publish_mission_result() -{ - /* lazily publish the mission result only once available */ - if (_mission_result_pub > 0) { - /* publish the mission result */ - orb_publish(ORB_ID(mission_result), _mission_result_pub, &_mission_result); + set_loiter_item(&_mission_item_triplet.current); - } else { - /* advertise and publish */ - _mission_result_pub = orb_advertise(ORB_ID(mission_result), &_mission_result); + switch (_mission_type) { + case MISSION_TYPE_ONBOARD: + mavlink_log_info(_mavlink_fd, "[navigator] loiter at onboard WP %d", _current_onboard_mission_index-1); + break; + case MISSION_TYPE_OFFBOARD: + mavlink_log_info(_mavlink_fd, "[navigator] loiter at offboard WP %d", _current_offboard_mission_index-1); + break; + case MISSION_TYPE_NONE: + default: + warnx("ERROR: Mission type unsupported"); + break; } } -int -Navigator::advance_current_mission_item() +void +Navigator::start_rtl() { - reset_mission_item_reached(); - // warnx("advancing from %d to %d", _current_mission_index, _current_mission_index+1); + /* discard all mission item and insert RTL item */ + _mission_item_triplet.previous_valid = false; + _mission_item_triplet.current_valid = true; + _mission_item_triplet.next_valid = false; - /* ultimately this index will be == _mission_item_count and this flags the mission as completed */ - - /* if there is no more mission available, don't advance and return */ - if (!_mission_item_triplet.next_valid) { - // warnx("no next available"); - return ERROR; - } + _mission_item_triplet.current.lat = (double)_home_pos.lat / 1e7; + _mission_item_triplet.current.lon = (double)_home_pos.lon / 1e7; + _mission_item_triplet.current.altitude = (float)_home_pos.alt / 1e3f + _parameters.min_altitude; + _mission_item_triplet.current.yaw = 0.0f; + _mission_item_triplet.current.nav_cmd = NAV_CMD_RETURN_TO_LAUNCH; + _mission_item_triplet.current.loiter_direction = 1; + _mission_item_triplet.current.loiter_radius = _parameters.loiter_radius; // TODO: get rid of magic number + _mission_item_triplet.current.radius = 50.0f; // TODO: get rid of magic number + _mission_item_triplet.current.autocontinue = false; + _mission_item_triplet.current_valid = true; - /* copy current mission to previous item */ - memcpy(&_mission_item_triplet.previous, &_mission_item_triplet.current, sizeof(mission_item_s)); - _mission_item_triplet.previous_valid = _mission_item_triplet.current_valid; + publish_mission_item_triplet(); - /* copy the next to current */ - memcpy(&_mission_item_triplet.current, &_mission_item_triplet.next, sizeof(mission_item_s)); - _mission_item_triplet.current_valid = _mission_item_triplet.next_valid; + mavlink_log_info(_mavlink_fd, "[navigator] return to launch"); +} - int ret = ERROR; - if (onboard_mission_possible()) { - _current_onboard_mission_index++; - ret = set_waypoint_mission_item(DM_KEY_WAYPOINTS_ONBOARD, _current_onboard_mission_index + 1, &_mission_item_triplet.next); - } else if (mission_possible()) { - _current_mission_index++; - ret = set_waypoint_mission_item(DM_KEY_WAYPOINTS, _current_mission_index + 1, &_mission_item_triplet.next); - } else { - warnx("Error: nothing to advance"); - } +void +Navigator::start_rtl_loiter() +{ + mission_item_s home_position_mission_item; + home_position_mission_item.lat = (double)_home_pos.lat / 1e7; + home_position_mission_item.lon = (double)_home_pos.lon / 1e7; + home_position_mission_item.altitude = _home_pos.alt / 1e3f + _parameters.min_altitude; - if(ret == OK) { - _mission_item_triplet.next_valid = true; - } - else { - _mission_item_triplet.next_valid = false; - } + set_loiter_item(&home_position_mission_item); - publish_mission_item_triplet(); + mavlink_log_info(_mavlink_fd, "[navigator] loiter after RTL"); +} - return OK; +bool +Navigator::offboard_mission_available(unsigned relative_index) +{ + return _offboard_mission_item_count > _current_offboard_mission_index + relative_index; } +bool +Navigator::onboard_mission_available(unsigned relative_index) +{ + return _onboard_mission_item_count > _current_onboard_mission_index + relative_index && _parameters.onboard_mission_enabled; +} -void -Navigator::reset_mission_item_reached() +int +Navigator::set_mission_item(unsigned relative_index, struct mission_item_s *mission_item) { - /* reset all states */ - _waypoint_position_reached = false; - _waypoint_yaw_reached = false; - _time_first_inside_orbit = 0; + struct mission_item_s new_mission_item; + + /* try onboard mission first */ + if (onboard_mission_available(relative_index)) { + if (_mission_type != MISSION_TYPE_ONBOARD && relative_index == 1) { + relative_index--; + } + if (dm_read(DM_KEY_WAYPOINTS_ONBOARD, _current_onboard_mission_index + relative_index, &new_mission_item, sizeof(struct mission_item_s)) != sizeof(struct mission_item_s)) { + /* not supposed to happen unless the datamanager can't access the SD card, etc. */ + _mission_type = MISSION_TYPE_NONE; + return ERROR; + } + /* base the mission type on the current mission item, not on future ones */ + if (relative_index == 0) { + _mission_type = MISSION_TYPE_ONBOARD; + } + /* otherwise fallback to offboard */ + } else if (offboard_mission_available(relative_index)) { - _mission_item_reached = false; + warnx("fallback try offboard: %d / %d", _current_offboard_mission_index + relative_index, _offboard_mission_item_count); - _mission_result.mission_reached = false; - _mission_result.mission_index = 0; + if (_mission_type != MISSION_TYPE_OFFBOARD && relative_index == 1) { + relative_index--; + } + + if (dm_read(DM_KEY_WAYPOINTS_OFFBOARD, _current_offboard_mission_index + relative_index, &new_mission_item, sizeof(struct mission_item_s)) != sizeof(struct mission_item_s)) { + /* not supposed to happen unless the datamanager can't access the SD card, etc. */ + _mission_type = MISSION_TYPE_NONE; + warnx("failed"); + return ERROR; + } + /* base the mission type on the current mission item, not on future ones */ + if (relative_index == 0) { + _mission_type = MISSION_TYPE_OFFBOARD; + } + } else { + /* happens when no more mission items can be added as a next item */ + return ERROR; + } + + if (new_mission_item.nav_cmd == NAV_CMD_RETURN_TO_LAUNCH) { + /* if it is a RTL waypoint, append the home position */ + new_mission_item.lat = (double)_home_pos.lat / 1e7; + new_mission_item.lon = (double)_home_pos.lon / 1e7; + new_mission_item.altitude = (float)_home_pos.alt / 1e3f + _parameters.min_altitude; + new_mission_item.loiter_radius = _parameters.loiter_radius; // TODO: get rid of magic number + new_mission_item.radius = 50.0f; // TODO: get rid of magic number + } + + memcpy(mission_item, &new_mission_item, sizeof(struct mission_item_s)); + + return OK; } -void -Navigator::check_mission_item_reached() +bool +Navigator::mission_item_reached() { - /* don't check if mission item is already reached */ - if (_mission_item_reached) { - return; + /* only check if there is actually a mission item to check */ + if (!_mission_item_triplet.current_valid) { + return false; } - /* don't try to reach the landing mission, just stay in that mode */ + /* don't try to reach the landing mission, just stay in that mode, XXX maybe add another state for this */ if (_mission_item_triplet.current.nav_cmd == NAV_CMD_LAND) { - return; + return false; } + /* XXX TODO count turns */ + if ((_mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_TURN_COUNT || + _mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || + _mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_UNLIMITED) && + _mission_item_triplet.current.loiter_radius > 0.01f) { + + return false; + } + uint64_t now = hrt_absolute_time(); float orbit; @@ -1030,12 +1219,6 @@ Navigator::check_mission_item_reached() orbit = _mission_item_triplet.current.radius; - } else if ((_mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_TURN_COUNT || - _mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || - _mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_UNLIMITED) && - _mission_item_triplet.current.loiter_radius > 0.01f) { - - orbit = _mission_item_triplet.current.loiter_radius; } else { // XXX set default orbit via param @@ -1098,73 +1281,19 @@ Navigator::check_mission_item_reached() if ((now - _time_first_inside_orbit >= (uint64_t)_mission_item_triplet.current.time_inside * 1e6) || _mission_item_triplet.current.nav_cmd == NAV_CMD_TAKEOFF) { - _mission_item_reached = true; + _time_first_inside_orbit = 0; + _waypoint_yaw_reached = false; + _waypoint_position_reached = false; + return true; } } + return false; } void -Navigator::report_mission_reached() -{ - _mission_result.mission_reached = true; - _mission_result.mission_index = _current_mission_index; - - publish_mission_result(); -} - -void -Navigator::start_waypoint() +Navigator::set_loiter_item(struct mission_item_s *new_loiter_position) { - reset_mission_item_reached(); - - // if (_current_mission_index > 0) { - // set_waypoint_mission_item(_current_mission_index - 1, &_mission_item_triplet.previous); - // _mission_item_triplet.previous_valid = true; - // } else { - // _mission_item_triplet.previous_valid = false; - // } - _mission_item_triplet.previous_valid = false; - - int ret = ERROR; - - if (onboard_mission_possible()) { - set_waypoint_mission_item(DM_KEY_WAYPOINTS_ONBOARD, _current_onboard_mission_index, &_mission_item_triplet.current); - mavlink_log_info(_mavlink_fd, "[navigator] heading to onboard WP %d", _current_onboard_mission_index); - ret = set_waypoint_mission_item(DM_KEY_WAYPOINTS_ONBOARD, _current_onboard_mission_index + 1, &_mission_item_triplet.next); - } else if (mission_possible()) { - set_waypoint_mission_item(DM_KEY_WAYPOINTS, _current_mission_index, &_mission_item_triplet.current); - mavlink_log_info(_mavlink_fd, "[navigator] heading to WP %d", _current_mission_index); - ret = set_waypoint_mission_item(DM_KEY_WAYPOINTS, _current_mission_index + 1, &_mission_item_triplet.next); - } - - _mission_item_triplet.current_valid = true; - - // if (_mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_UNLIMITED) { - - // if the current mission is to loiter unlimited, don't bother about a next mission item - // _mission_item_triplet.next_valid = false; - // } else { - /* if we are not loitering yet, try to add the next mission item */ - // set_waypoint_mission_item(_current_mission_index + 1, &_mission_item_triplet.next); - // _mission_item_triplet.next_valid = true; - // } - - if(ret == OK) { - _mission_item_triplet.next_valid = true; - } - else { - _mission_item_triplet.next_valid = false; - } - - publish_mission_item_triplet(); -} - -void -Navigator::start_loiter(mission_item_s *new_loiter_position) -{ - //reset_mission_item_reached(); - _mission_item_triplet.previous_valid = false; _mission_item_triplet.current_valid = true; _mission_item_triplet.next_valid = false; @@ -1184,27 +1313,38 @@ Navigator::start_loiter(mission_item_s *new_loiter_position) } void -Navigator::start_rtl() +Navigator::publish_mission_item_triplet() { - reset_mission_item_reached(); + /* lazily publish the mission triplet only once available */ + if (_triplet_pub > 0) { + /* publish the mission triplet */ + orb_publish(ORB_ID(mission_item_triplet), _triplet_pub, &_mission_item_triplet); - /* discard all mission item and insert RTL item */ - _mission_item_triplet.previous_valid = false; - _mission_item_triplet.current_valid = true; - _mission_item_triplet.next_valid = false; + } else { + /* advertise and publish */ + _triplet_pub = orb_advertise(ORB_ID(mission_item_triplet), &_mission_item_triplet); + } +} - _mission_item_triplet.current.lat = (double)_home_pos.lat / 1e7; - _mission_item_triplet.current.lon = (double)_home_pos.lon / 1e7; - _mission_item_triplet.current.altitude = (float)_home_pos.alt / 1e3f + _parameters.min_altitude; - _mission_item_triplet.current.yaw = 0.0f; - _mission_item_triplet.current.nav_cmd = NAV_CMD_RETURN_TO_LAUNCH; - _mission_item_triplet.current.loiter_direction = 1; - _mission_item_triplet.current.loiter_radius = _parameters.loiter_radius; // TODO: get rid of magic number - _mission_item_triplet.current.radius = 50.0f; // TODO: get rid of magic number - _mission_item_triplet.current.autocontinue = false; - _mission_item_triplet.current_valid = true; - publish_mission_item_triplet(); +bool Navigator::cmp_mission_item_equivalent(const struct mission_item_s a, const struct mission_item_s b) { + if (fabsf(a.altitude_is_relative - b.altitude_is_relative) < FLT_EPSILON && + fabsf(a.lat - b.lat) < FLT_EPSILON && + fabsf(a.lon - b.lon) < FLT_EPSILON && + fabsf(a.altitude - b.altitude) < FLT_EPSILON && + fabsf(a.yaw - b.yaw) < FLT_EPSILON && + fabsf(a.loiter_radius - b.loiter_radius) < FLT_EPSILON && + fabsf(a.loiter_direction - b.loiter_direction) < FLT_EPSILON && + fabsf(a.nav_cmd - b.nav_cmd) < FLT_EPSILON && + fabsf(a.radius - b.radius) < FLT_EPSILON && + fabsf(a.time_inside - b.time_inside) < FLT_EPSILON && + fabsf(a.autocontinue - b.autocontinue) < FLT_EPSILON && + fabsf(a.index - b.index) < FLT_EPSILON) { + return true; + } else { + warnx("a.index %d, b.index %d", a.index, b.index); + return false; + } } @@ -1260,22 +1400,3 @@ int navigator_main(int argc, char *argv[]) return 0; } -bool Navigator::cmp_mission_item_equivalent(const struct mission_item_s a, const struct mission_item_s b) { - if (fabsf(a.altitude_is_relative - b.altitude_is_relative) < FLT_EPSILON && - fabsf(a.lat - b.lat) < FLT_EPSILON && - fabsf(a.lon - b.lon) < FLT_EPSILON && - fabsf(a.altitude - b.altitude) < FLT_EPSILON && - fabsf(a.yaw - b.yaw) < FLT_EPSILON && - fabsf(a.loiter_radius - b.loiter_radius) < FLT_EPSILON && - fabsf(a.loiter_direction - b.loiter_direction) < FLT_EPSILON && - fabsf(a.nav_cmd - b.nav_cmd) < FLT_EPSILON && - fabsf(a.radius - b.radius) < FLT_EPSILON && - fabsf(a.time_inside - b.time_inside) < FLT_EPSILON && - fabsf(a.autocontinue - b.autocontinue) < FLT_EPSILON && - fabsf(a.index - b.index) < FLT_EPSILON) { - return true; - } else { - warnx("a.index %d, b.index %d", a.index, b.index); - return false; - } -} diff --git a/src/modules/systemlib/state_table.h b/src/modules/systemlib/state_table.h new file mode 100644 index 000000000..f2709d29f --- /dev/null +++ b/src/modules/systemlib/state_table.h @@ -0,0 +1,75 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file state_table.h + * + * Finite-State-Machine helper class for state table + */ + +#ifndef __SYSTEMLIB_STATE_TABLE_H +#define __SYSTEMLIB_STATE_TABLE_H + +class StateTable +{ +public: + typedef void (StateTable::*Action)(); + struct Tran { + Action action; + unsigned nextState; + }; + + StateTable(Tran const *table, unsigned nStates, unsigned nSignals) + : myTable(table), myNsignals(nSignals), myNstates(nStates) {} + + #define NO_ACTION &StateTable::doNothing + #define ACTION(_target) static_cast(_target) + + virtual ~StateTable() {} + + void dispatch(unsigned const sig) { + register Tran const *t = myTable + myState*myNsignals + sig; + (this->*(t->action))(); + + myState = t->nextState; + } + void doNothing() {} +protected: + unsigned myState; +private: + Tran const *myTable; + unsigned myNsignals; + unsigned myNstates; +}; + +#endif \ No newline at end of file -- cgit v1.2.3 From 9d4ba6e4f64d631c67a419518a8398debade4641 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 16 Dec 2013 16:59:24 +0100 Subject: Navigator: Moved mission stuff in separate class --- src/modules/navigator/module.mk | 3 +- src/modules/navigator/navigator_main.cpp | 343 ++++++++++------------------ src/modules/navigator/navigator_mission.cpp | 234 +++++++++++++++++++ src/modules/navigator/navigator_mission.h | 95 ++++++++ 4 files changed, 458 insertions(+), 217 deletions(-) create mode 100644 src/modules/navigator/navigator_mission.cpp create mode 100644 src/modules/navigator/navigator_mission.h diff --git a/src/modules/navigator/module.mk b/src/modules/navigator/module.mk index 7f7443c43..fc59c956a 100644 --- a/src/modules/navigator/module.mk +++ b/src/modules/navigator/module.mk @@ -38,6 +38,7 @@ MODULE_COMMAND = navigator SRCS = navigator_main.cpp \ - navigator_params.c + navigator_params.c \ + navigator_mission.cpp INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index d258aa8a6..d93ecc7cd 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -75,6 +75,7 @@ #include #include +#include "navigator_mission.h" /* oddly, ERROR is not defined for c++ */ @@ -99,7 +100,7 @@ public: Navigator(); /** - * Destructor, also kills the sensors task. + * Destructor, also kills the navigators task. */ ~Navigator(); @@ -158,16 +159,7 @@ private: struct navigation_capabilities_s _nav_caps; - unsigned _current_offboard_mission_index; - unsigned _current_onboard_mission_index; - unsigned _offboard_mission_item_count; /** number of offboard mission items copied */ - unsigned _onboard_mission_item_count; /** number of onboard mission items copied */ - - enum { - MISSION_TYPE_NONE, - MISSION_TYPE_ONBOARD, - MISSION_TYPE_OFFBOARD, - } _mission_type; + class Mission _mission; bool _waypoint_position_reached; bool _waypoint_yaw_reached; @@ -293,17 +285,12 @@ private: /** * Move to next waypoint */ - int advance_mission(); - - /** - * Helper function to set a mission item - */ - int set_mission_item(unsigned relative_index, struct mission_item_s *mission_item) ; + void advance_mission(); /** - * Helper function to set a loiter item + * Helper function to get a loiter item */ - void set_loiter_item(mission_item_s *new_loiter_position); + void get_loiter_item(mission_item_s *new_loiter_position); /** * Publish a new mission item triplet for position controller @@ -319,6 +306,8 @@ private: * @return true if equivalent, false otherwise */ bool cmp_mission_item_equivalent(const struct mission_item_s a, const struct mission_item_s b); + + void add_home_pos_to_rtl(struct mission_item_s *new_mission_item); }; namespace navigator @@ -362,10 +351,7 @@ Navigator::Navigator() : /* states */ _fence_valid(false), _inside_fence(true), - _current_offboard_mission_index(0), - _current_onboard_mission_index(0), - _offboard_mission_item_count(0), - _onboard_mission_item_count(0), + _mission(), _waypoint_position_reached(false), _waypoint_yaw_reached(false), _time_first_inside_orbit(0) @@ -424,6 +410,8 @@ Navigator::parameters_update() param_get(_parameter_handles.min_altitude, &(_parameters.min_altitude)); param_get(_parameter_handles.loiter_radius, &(_parameters.loiter_radius)); param_get(_parameter_handles.onboard_mission_enabled, &(_parameters.onboard_mission_enabled)); + + _mission.set_onboard_mission_allowed((bool)_parameter_handles.onboard_mission_enabled); } void @@ -452,15 +440,12 @@ Navigator::offboard_mission_update() struct mission_s offboard_mission; if (orb_copy(ORB_ID(mission), _offboard_mission_sub, &offboard_mission) == OK) { - _offboard_mission_item_count = offboard_mission.count; - - if (offboard_mission.current_index != -1) { - _current_offboard_mission_index = offboard_mission.current_index; - } + _mission.set_current_offboard_mission_index(offboard_mission.current_index); + _mission.set_offboard_mission_count(offboard_mission.count); } else { - _offboard_mission_item_count = 0; - _current_offboard_mission_index = 0; + _mission.set_current_offboard_mission_index(0); + _mission.set_offboard_mission_count(0); } } @@ -468,17 +453,14 @@ void Navigator::onboard_mission_update() { struct mission_s onboard_mission; - if (orb_copy(ORB_ID(onboard_mission), _onboard_mission_sub, &onboard_mission) == OK) { + if (orb_copy(ORB_ID(mission), _onboard_mission_sub, &onboard_mission) == OK) { - _onboard_mission_item_count = onboard_mission.count; - - if (onboard_mission.current_index != -1) { - _current_onboard_mission_index = onboard_mission.current_index; - } + _mission.set_current_onboard_mission_index(onboard_mission.current_index); + _mission.set_onboard_mission_count(onboard_mission.count); } else { - _onboard_mission_item_count = 0; - _current_onboard_mission_index = 0; + _mission.set_current_onboard_mission_index(0); + _mission.set_onboard_mission_count(0); } } @@ -594,7 +576,7 @@ Navigator::task_main() case NAVIGATION_STATE_AUTO_MISSION: /* try mission if none is available, fallback to loiter instead */ - if (onboard_mission_available(0) || offboard_mission_available(0)) { + if (_mission.current_mission_available()) { dispatch(EVENT_MISSION_REQUESTED); } else { dispatch(EVENT_LOITER_REQUESTED); @@ -671,8 +653,22 @@ Navigator::task_main() global_position_update(); /* only check if waypoint has been reached in Mission or RTL mode */ if (mission_item_reached()) { - /* try to advance mission */ - if (advance_mission() != OK) { + + if (_vstatus.main_state == MAIN_STATE_AUTO && + (_vstatus.navigation_state == NAVIGATION_STATE_AUTO_READY || + _vstatus.navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF || + _vstatus.navigation_state == NAVIGATION_STATE_AUTO_MISSION)) { + + /* advance by one mission item */ + _mission.move_to_next(); + + /* if no more mission items available send this to state machine and start loiter at the last WP */ + if (_mission.current_mission_available()) { + advance_mission(); + } else { + dispatch(EVENT_MISSION_FINISHED); + } + } else { dispatch(EVENT_MISSION_FINISHED); } } @@ -740,20 +736,6 @@ Navigator::status() break; case STATE_MISSION: warnx("State: Mission"); - switch (_mission_type) { - case MISSION_TYPE_ONBOARD: - warnx("Mission type: Onboard"); - break; - case MISSION_TYPE_OFFBOARD: - warnx("Mission type: Offboard"); - break; - case MISSION_TYPE_NONE: - default: - warnx("ERROR: Mission type unsupported"); - break; - } - warnx("Onboard mission: %d of %d", _current_onboard_mission_index, _onboard_mission_item_count); - warnx("Offboard mission: %d of %d", _current_offboard_mission_index, _offboard_mission_item_count); break; case STATE_MISSION_LOITER: warnx("State: Loiter after Mission"); @@ -946,26 +928,28 @@ Navigator::start_none() void Navigator::start_loiter() { - struct mission_item_s loiter_item; + _mission_item_triplet.previous_valid = false; + _mission_item_triplet.current_valid = true; + _mission_item_triplet.next_valid = false; - loiter_item.lat = (double)_global_pos.lat / 1e7; - loiter_item.lon = (double)_global_pos.lon / 1e7; - loiter_item.yaw = 0.0f; + _mission_item_triplet.current.lat = (double)_global_pos.lat / 1e7; + _mission_item_triplet.current.lon = (double)_global_pos.lon / 1e7; + _mission_item_triplet.current.yaw = 0.0f; + + get_loiter_item(&_mission_item_triplet.current); /* XXX get rid of ugly conversion for home position altitude */ float global_min_alt = _parameters.min_altitude + (float)_home_pos.alt/1e3f; /* Use current altitude if above min altitude set by parameter */ if (_global_pos.alt < global_min_alt) { - loiter_item.altitude = global_min_alt; + _mission_item_triplet.current.altitude = global_min_alt; mavlink_log_info(_mavlink_fd, "[navigator] loiter %.1fm higher", (double)(global_min_alt - _global_pos.alt)); } else { - loiter_item.altitude = _global_pos.alt; + _mission_item_triplet.current.altitude = _global_pos.alt; mavlink_log_info(_mavlink_fd, "[navigator] loiter here"); } - set_loiter_item(&loiter_item); - publish_mission_item_triplet(); } @@ -975,86 +959,86 @@ Navigator::start_mission() { /* leave previous mission item as isas is */ - if(set_mission_item(0, &_mission_item_triplet.current) == OK) { + int ret; + bool onboard; + unsigned index; + + ret = _mission.get_current_mission_item(&_mission_item_triplet.current, &onboard, &index); + + if (ret == OK) { + + add_home_pos_to_rtl(&_mission_item_triplet.current); _mission_item_triplet.current_valid = true; + + if (onboard) { + mavlink_log_info(_mavlink_fd, "[navigator] heading to onboard WP %d", index); + } else { + mavlink_log_info(_mavlink_fd, "[navigator] heading to offboard WP %d", index); + } } else { + /* since a mission is not started without WPs available, this is not supposed to happen */ _mission_item_triplet.current_valid = false; warnx("ERROR: current WP can't be set"); } - if(set_mission_item(1, &_mission_item_triplet.next) == OK) { + ret = _mission.get_next_mission_item(&_mission_item_triplet.next); + + if (ret == OK) { + + add_home_pos_to_rtl(&_mission_item_triplet.next); _mission_item_triplet.next_valid = true; } else { + /* this will fail for the last WP */ _mission_item_triplet.next_valid = false; } - switch (_mission_type) { - case MISSION_TYPE_ONBOARD: - mavlink_log_info(_mavlink_fd, "[navigator] heading to onboard WP %d", _current_onboard_mission_index); - break; - case MISSION_TYPE_OFFBOARD: - mavlink_log_info(_mavlink_fd, "[navigator] heading to offboard WP %d", _current_offboard_mission_index); - break; - case MISSION_TYPE_NONE: - default: - warnx("ERROR: Mission type unsupported"); - break; - } - publish_mission_item_triplet(); } -int +void Navigator::advance_mission() { - switch (_mission_type) { - case MISSION_TYPE_ONBOARD: - warnx("advance onboard before: %d", _current_onboard_mission_index); - _current_onboard_mission_index++; - warnx("advance onboard after: %d", _current_onboard_mission_index); - break; - case MISSION_TYPE_OFFBOARD: - warnx("advance offboard before: %d", _current_offboard_mission_index); - _current_offboard_mission_index++; - warnx("advance offboard after: %d", _current_offboard_mission_index); - break; - case MISSION_TYPE_NONE: - default: - warnx("ERROR: Mission type unsupported"); - return ERROR; - } - - /* if there is no more mission available, don't advance and switch to loiter at current WP */ - if (!_mission_item_triplet.next_valid) { - warnx("no next valid"); - return ERROR; - } - /* copy current mission to previous item */ memcpy(&_mission_item_triplet.previous, &_mission_item_triplet.current, sizeof(mission_item_s)); _mission_item_triplet.previous_valid = _mission_item_triplet.current_valid; - if(set_mission_item(0, &_mission_item_triplet.current) == OK) { + int ret; + bool onboard; + unsigned index; + + ret = _mission.get_current_mission_item(&_mission_item_triplet.current, &onboard, &index); + + if (ret == OK) { + + add_home_pos_to_rtl(&_mission_item_triplet.current); _mission_item_triplet.current_valid = true; - + + if (onboard) { + mavlink_log_info(_mavlink_fd, "[navigator] heading to onboard WP %d", index); + } else { + mavlink_log_info(_mavlink_fd, "[navigator] heading to offboard WP %d", index); + } } else { - /* should never ever happen */ + /* since a mission is not advanced without WPs available, this is not supposed to happen */ _mission_item_triplet.current_valid = false; - warnx("no current available"); - return ERROR; + warnx("ERROR: current WP can't be set"); } - - if(set_mission_item(1, &_mission_item_triplet.next) == OK) { + + ret = _mission.get_next_mission_item(&_mission_item_triplet.next); + + if (ret == OK) { + + add_home_pos_to_rtl(&_mission_item_triplet.next); + _mission_item_triplet.next_valid = true; - } else { + /* this will fail for the last WP */ _mission_item_triplet.next_valid = false; } publish_mission_item_triplet(); - return OK; } void @@ -1063,23 +1047,13 @@ Navigator::start_mission_loiter() /* make sure the current WP is valid */ if (!_mission_item_triplet.current_valid) { warnx("ERROR: cannot switch to offboard mission loiter"); - return; } - set_loiter_item(&_mission_item_triplet.current); + get_loiter_item(&_mission_item_triplet.current); - switch (_mission_type) { - case MISSION_TYPE_ONBOARD: - mavlink_log_info(_mavlink_fd, "[navigator] loiter at onboard WP %d", _current_onboard_mission_index-1); - break; - case MISSION_TYPE_OFFBOARD: - mavlink_log_info(_mavlink_fd, "[navigator] loiter at offboard WP %d", _current_offboard_mission_index-1); - break; - case MISSION_TYPE_NONE: - default: - warnx("ERROR: Mission type unsupported"); - break; - } + publish_mission_item_triplet(); + + mavlink_log_info(_mavlink_fd, "[navigator] loiter at last WP"); } void @@ -1111,83 +1085,19 @@ Navigator::start_rtl() void Navigator::start_rtl_loiter() { - mission_item_s home_position_mission_item; - home_position_mission_item.lat = (double)_home_pos.lat / 1e7; - home_position_mission_item.lon = (double)_home_pos.lon / 1e7; - home_position_mission_item.altitude = _home_pos.alt / 1e3f + _parameters.min_altitude; - - set_loiter_item(&home_position_mission_item); - - mavlink_log_info(_mavlink_fd, "[navigator] loiter after RTL"); -} - -bool -Navigator::offboard_mission_available(unsigned relative_index) -{ - return _offboard_mission_item_count > _current_offboard_mission_index + relative_index; -} - -bool -Navigator::onboard_mission_available(unsigned relative_index) -{ - return _onboard_mission_item_count > _current_onboard_mission_index + relative_index && _parameters.onboard_mission_enabled; -} - -int -Navigator::set_mission_item(unsigned relative_index, struct mission_item_s *mission_item) -{ - struct mission_item_s new_mission_item; - - /* try onboard mission first */ - if (onboard_mission_available(relative_index)) { - if (_mission_type != MISSION_TYPE_ONBOARD && relative_index == 1) { - relative_index--; - } - if (dm_read(DM_KEY_WAYPOINTS_ONBOARD, _current_onboard_mission_index + relative_index, &new_mission_item, sizeof(struct mission_item_s)) != sizeof(struct mission_item_s)) { - /* not supposed to happen unless the datamanager can't access the SD card, etc. */ - _mission_type = MISSION_TYPE_NONE; - return ERROR; - } - /* base the mission type on the current mission item, not on future ones */ - if (relative_index == 0) { - _mission_type = MISSION_TYPE_ONBOARD; - } - /* otherwise fallback to offboard */ - } else if (offboard_mission_available(relative_index)) { - - warnx("fallback try offboard: %d / %d", _current_offboard_mission_index + relative_index, _offboard_mission_item_count); - - if (_mission_type != MISSION_TYPE_OFFBOARD && relative_index == 1) { - relative_index--; - } + _mission_item_triplet.previous_valid = false; + _mission_item_triplet.current_valid = true; + _mission_item_triplet.next_valid = false; - if (dm_read(DM_KEY_WAYPOINTS_OFFBOARD, _current_offboard_mission_index + relative_index, &new_mission_item, sizeof(struct mission_item_s)) != sizeof(struct mission_item_s)) { - /* not supposed to happen unless the datamanager can't access the SD card, etc. */ - _mission_type = MISSION_TYPE_NONE; - warnx("failed"); - return ERROR; - } - /* base the mission type on the current mission item, not on future ones */ - if (relative_index == 0) { - _mission_type = MISSION_TYPE_OFFBOARD; - } - } else { - /* happens when no more mission items can be added as a next item */ - return ERROR; - } + _mission_item_triplet.current.lat = (double)_home_pos.lat / 1e7; + _mission_item_triplet.current.lon = (double)_home_pos.lon / 1e7; + _mission_item_triplet.current.altitude = _home_pos.alt / 1e3f + _parameters.min_altitude; + + get_loiter_item(&_mission_item_triplet.current); - if (new_mission_item.nav_cmd == NAV_CMD_RETURN_TO_LAUNCH) { - /* if it is a RTL waypoint, append the home position */ - new_mission_item.lat = (double)_home_pos.lat / 1e7; - new_mission_item.lon = (double)_home_pos.lon / 1e7; - new_mission_item.altitude = (float)_home_pos.alt / 1e3f + _parameters.min_altitude; - new_mission_item.loiter_radius = _parameters.loiter_radius; // TODO: get rid of magic number - new_mission_item.radius = 50.0f; // TODO: get rid of magic number - } + publish_mission_item_triplet(); - memcpy(mission_item, &new_mission_item, sizeof(struct mission_item_s)); - - return OK; + mavlink_log_info(_mavlink_fd, "[navigator] loiter after RTL"); } bool @@ -1292,24 +1202,13 @@ Navigator::mission_item_reached() } void -Navigator::set_loiter_item(struct mission_item_s *new_loiter_position) +Navigator::get_loiter_item(struct mission_item_s *new_loiter_position) { - _mission_item_triplet.previous_valid = false; - _mission_item_triplet.current_valid = true; - _mission_item_triplet.next_valid = false; - - _mission_item_triplet.current.nav_cmd = NAV_CMD_LOITER_UNLIMITED; - _mission_item_triplet.current.loiter_direction = 1; - _mission_item_triplet.current.loiter_radius = _parameters.loiter_radius; // TODO: get rid of magic number - _mission_item_triplet.current.radius = 50.0f; // TODO: get rid of magic number - _mission_item_triplet.current.autocontinue = false; - - _mission_item_triplet.current.lat = new_loiter_position->lat; - _mission_item_triplet.current.lon = new_loiter_position->lon; - _mission_item_triplet.current.altitude = new_loiter_position->altitude; - _mission_item_triplet.current.yaw = new_loiter_position->yaw; - - publish_mission_item_triplet(); + new_loiter_position->nav_cmd = NAV_CMD_LOITER_UNLIMITED; + new_loiter_position->loiter_direction = 1; + new_loiter_position->loiter_radius = _parameters.loiter_radius; // TODO: get rid of magic number + new_loiter_position->radius = 50.0f; // TODO: get rid of magic number + new_loiter_position->autocontinue = false; } void @@ -1400,3 +1299,15 @@ int navigator_main(int argc, char *argv[]) return 0; } +void +Navigator::add_home_pos_to_rtl(struct mission_item_s *new_mission_item) +{ + if (new_mission_item->nav_cmd == NAV_CMD_RETURN_TO_LAUNCH) { + /* if it is a RTL waypoint, append the home position */ + new_mission_item->lat = (double)_home_pos.lat / 1e7; + new_mission_item->lon = (double)_home_pos.lon / 1e7; + new_mission_item->altitude = (float)_home_pos.alt / 1e3f + _parameters.min_altitude; + new_mission_item->loiter_radius = _parameters.loiter_radius; // TODO: get rid of magic number + new_mission_item->radius = 50.0f; // TODO: get rid of magic number + } +} \ No newline at end of file diff --git a/src/modules/navigator/navigator_mission.cpp b/src/modules/navigator/navigator_mission.cpp new file mode 100644 index 000000000..993f8f133 --- /dev/null +++ b/src/modules/navigator/navigator_mission.cpp @@ -0,0 +1,234 @@ +/**************************************************************************** + * + * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * Author: @author Julian Oes + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ +/** + * @file navigator_mission.cpp + * Helper class to access missions + */ + +// #include +// #include +// #include +// #include + +#include +#include +#include "navigator_mission.h" + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; + + +Mission::Mission() : + + _current_offboard_mission_index(0), + _current_onboard_mission_index(0), + _offboard_mission_item_count(0), + _onboard_mission_item_count(0), + _onboard_mission_allowed(false), + _current_mission_type(MISSION_TYPE_NONE) +{} + +Mission::~Mission() +{ + +} + +void +Mission::set_current_offboard_mission_index(int new_index) +{ + if (new_index != -1) { + _current_offboard_mission_index = (unsigned)new_index; + } +} + +void +Mission::set_current_onboard_mission_index(int new_index) +{ + if (new_index != -1) { + _current_onboard_mission_index = (unsigned)new_index; + } +} + +void +Mission::set_offboard_mission_count(unsigned new_count) +{ + _offboard_mission_item_count = new_count; +} + +void +Mission::set_onboard_mission_count(unsigned new_count) +{ + _onboard_mission_item_count = new_count; +} + +void +Mission::set_onboard_mission_allowed(bool allowed) +{ + _onboard_mission_allowed = allowed; +} + +bool +Mission::current_mission_available() +{ + return (current_onboard_mission_available() || current_offboard_mission_available()); + +} + +bool +Mission::next_mission_available() +{ + return (next_onboard_mission_available() || next_offboard_mission_available()); +} + +int +Mission::get_current_mission_item(struct mission_item_s *new_mission_item, bool *onboard, unsigned *index) +{ + /* try onboard mission first */ + if (current_onboard_mission_available()) { + + const ssize_t len = sizeof(struct mission_item_s); + if (dm_read(DM_KEY_WAYPOINTS_ONBOARD, _current_onboard_mission_index, new_mission_item, len) != len) { + /* not supposed to happen unless the datamanager can't access the SD card, etc. */ + return ERROR; + } + _current_mission_type = MISSION_TYPE_ONBOARD; + *onboard = true; + *index = _current_onboard_mission_index; + + /* otherwise fallback to offboard */ + } else if (current_offboard_mission_available()) { + + const ssize_t len = sizeof(struct mission_item_s); + if (dm_read(DM_KEY_WAYPOINTS_OFFBOARD, _current_offboard_mission_index, new_mission_item, len) != len) { + /* not supposed to happen unless the datamanager can't access the SD card, etc. */ + _current_mission_type = MISSION_TYPE_NONE; + return ERROR; + } + _current_mission_type = MISSION_TYPE_OFFBOARD; + *onboard = false; + *index = _current_offboard_mission_index; + + } else { + /* happens when no more mission items can be added as a next item */ + _current_mission_type = MISSION_TYPE_NONE; + return ERROR; + } + + return OK; +} + +int +Mission::get_next_mission_item(struct mission_item_s *new_mission_item) +{ + /* try onboard mission first */ + if (next_onboard_mission_available()) { + + const ssize_t len = sizeof(struct mission_item_s); + if (dm_read(DM_KEY_WAYPOINTS_ONBOARD, _current_onboard_mission_index + 1, new_mission_item, len) != len) { + /* not supposed to happen unless the datamanager can't access the SD card, etc. */ + return ERROR; + } + + /* otherwise fallback to offboard */ + } else if (next_offboard_mission_available()) { + + const ssize_t len = sizeof(struct mission_item_s); + if (dm_read(DM_KEY_WAYPOINTS_OFFBOARD, _current_offboard_mission_index + 1, new_mission_item, len) != len) { + /* not supposed to happen unless the datamanager can't access the SD card, etc. */ + return ERROR; + } + + } else { + /* happens when no more mission items can be added as a next item */ + return ERROR; + } + + return OK; +} + + +bool +Mission::current_onboard_mission_available() +{ + return _onboard_mission_item_count > _current_onboard_mission_index && _onboard_mission_allowed; +} + +bool +Mission::current_offboard_mission_available() +{ + return _offboard_mission_item_count > _current_offboard_mission_index; +} + +bool +Mission::next_onboard_mission_available() +{ + unsigned next = 0; + + if (_current_mission_type != MISSION_TYPE_ONBOARD) { + next = 1; + } + + return _onboard_mission_item_count > (_current_onboard_mission_index + next) && _onboard_mission_allowed; +} + +bool +Mission::next_offboard_mission_available() +{ + unsigned next = 0; + + if (_current_mission_type != MISSION_TYPE_OFFBOARD) { + next = 1; + } + + return _offboard_mission_item_count > (_current_offboard_mission_index + next); +} + +void +Mission::move_to_next() +{ + switch (_current_mission_type) { + case MISSION_TYPE_ONBOARD: + _current_onboard_mission_index++; + break; + case MISSION_TYPE_OFFBOARD: + _current_offboard_mission_index++; + break; + case MISSION_TYPE_NONE: + default: + break; + } +} \ No newline at end of file diff --git a/src/modules/navigator/navigator_mission.h b/src/modules/navigator/navigator_mission.h new file mode 100644 index 000000000..e8e476382 --- /dev/null +++ b/src/modules/navigator/navigator_mission.h @@ -0,0 +1,95 @@ +/**************************************************************************** + * + * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * Author: @author Julian Oes + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ +/** + * @file navigator_mission.h + * Helper class to access missions + */ + +#ifndef NAVIGATOR_MISSION_H +#define NAVIGATOR_MISSION_H + +#include + + +class __EXPORT Mission +{ +public: + /** + * Constructor + */ + Mission(); + + /** + * Destructor, also kills the sensors task. + */ + ~Mission(); + + void set_current_offboard_mission_index(int new_index); + void set_current_onboard_mission_index(int new_index); + void set_offboard_mission_count(unsigned new_count); + void set_onboard_mission_count(unsigned new_count); + + void set_onboard_mission_allowed(bool allowed); + + bool current_mission_available(); + bool next_mission_available(); + + int get_current_mission_item(struct mission_item_s *mission_item, bool *onboard, unsigned *index); + int get_next_mission_item(struct mission_item_s *mission_item); + + void move_to_next(); + + void add_home_pos(struct mission_item_s *new_mission_item); + +private: + bool current_onboard_mission_available(); + bool current_offboard_mission_available(); + bool next_onboard_mission_available(); + bool next_offboard_mission_available(); + + unsigned _current_offboard_mission_index; + unsigned _current_onboard_mission_index; + unsigned _offboard_mission_item_count; /** number of offboard mission items available */ + unsigned _onboard_mission_item_count; /** number of onboard mission items available */ + + bool _onboard_mission_allowed; + + enum { + MISSION_TYPE_NONE, + MISSION_TYPE_ONBOARD, + MISSION_TYPE_OFFBOARD, + } _current_mission_type; +}; + +#endif \ No newline at end of file -- cgit v1.2.3 From d422d443eebbba24d785f96de5b74fab143e6652 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sat, 29 Mar 2014 12:14:57 +0100 Subject: bottle_drop: started rewrite in C++ --- src/modules/bottle_drop/bottle_drop.cpp | 198 ++++++++++++++++++++++++++++++++ src/modules/bottle_drop/module.mk | 4 +- 2 files changed, 200 insertions(+), 2 deletions(-) create mode 100644 src/modules/bottle_drop/bottle_drop.cpp diff --git a/src/modules/bottle_drop/bottle_drop.cpp b/src/modules/bottle_drop/bottle_drop.cpp new file mode 100644 index 000000000..7672ace9e --- /dev/null +++ b/src/modules/bottle_drop/bottle_drop.cpp @@ -0,0 +1,198 @@ +/**************************************************************************** + * + * Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file bottle_drop.cpp + * + * Bottle drop module for Outback Challenge 2014, Team Swiss Fang + * + * @author Dominik Juchli + * @author Julian Oes + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + + +/** + * bottle_drop app start / stop handling function + * + * @ingroup apps + */ +extern "C" __EXPORT int bottle_drop_main(int argc, char *argv[]); + +class BottleDrop +{ +public: + /** + * Constructor + */ + BottleDrop(); + + /** + * Destructor, also kills task. + */ + ~BottleDrop(); + + /** + * Start the task. + * + * @return OK on success. + */ + int start(); + + /** + * Display status. + */ + void status(); + +private: + bool _task_should_exit; /**< if true, sensor task should exit */ + int _main_task; /**< task handle for sensor task */ + + int _mavlink_fd; +}; + +namespace bottle_drop +{ +BottleDrop *g_bottle_drop; +} + +BottleDrop::BottleDrop() : + + _task_should_exit(false), + _main_task(-1), + _mavlink_fd(-1) +{ +} + +BottleDrop::~BottleDrop() +{ + if (_main_task != -1) { + + /* task wakes up every 100ms or so at the longest */ + _task_should_exit = true; + + /* wait for a second for the task to quit at our request */ + unsigned i = 0; + + do { + /* wait 20ms */ + usleep(20000); + + /* if we have given up, kill it */ + if (++i > 50) { + task_delete(_main_task); + break; + } + } while (_main_task != -1); + } + + bottle_drop::g_bottle_drop = nullptr; +} + +int BottleDrop::start() +{ +} + + +void BottleDrop::status() +{ +} + +static void usage() +{ + errx(1, "usage: bottle_drop {start|stop|status}"); +} + +int bottle_drop_main(int argc, char *argv[]) +{ + if (argc < 2) { + usage(); + } + + if (!strcmp(argv[1], "start")) { + + if (bottle_drop::g_bottle_drop != nullptr) { + errx(1, "already running"); + } + + bottle_drop::g_bottle_drop = new BottleDrop; + + if (bottle_drop::g_bottle_drop == nullptr) { + errx(1, "alloc failed"); + } + + if (OK != bottle_drop::g_bottle_drop->start()) { + delete bottle_drop::g_bottle_drop; + bottle_drop::g_bottle_drop = nullptr; + err(1, "start failed"); + } + + return 0; + } + + if (bottle_drop::g_bottle_drop == nullptr) + errx(1, "not running"); + + if (!strcmp(argv[1], "stop")) { + delete bottle_drop::g_bottle_drop; + bottle_drop::g_bottle_drop = nullptr; + + } else if (!strcmp(argv[1], "status")) { + bottle_drop::g_bottle_drop->status(); + } else { + usage(); + } + + return 0; +} diff --git a/src/modules/bottle_drop/module.mk b/src/modules/bottle_drop/module.mk index 222858b27..bb1f6c6b5 100644 --- a/src/modules/bottle_drop/module.mk +++ b/src/modules/bottle_drop/module.mk @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. +# Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions @@ -37,4 +37,4 @@ MODULE_COMMAND = bottle_drop -SRCS = bottle_drop.c +SRCS = bottle_drop.cpp -- cgit v1.2.3 From 9da8e249fd2d1504b01571965c4dd5acdbf77ff1 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sat, 29 Mar 2014 20:36:39 +0100 Subject: bottle_drop: added simple commands to drop bottle --- src/modules/bottle_drop/bottle_drop.cpp | 209 ++++++++++++++++++++++++++++++-- 1 file changed, 202 insertions(+), 7 deletions(-) diff --git a/src/modules/bottle_drop/bottle_drop.cpp b/src/modules/bottle_drop/bottle_drop.cpp index 7672ace9e..1dcad2144 100644 --- a/src/modules/bottle_drop/bottle_drop.cpp +++ b/src/modules/bottle_drop/bottle_drop.cpp @@ -55,7 +55,9 @@ #include #include #include -#include +#include +#include +#include #include #include #include @@ -96,10 +98,30 @@ public: void status(); private: - bool _task_should_exit; /**< if true, sensor task should exit */ - int _main_task; /**< task handle for sensor task */ - + bool _task_should_exit; /**< if true, task should exit */ + int _main_task; /**< handle for task */ int _mavlink_fd; + + int _command_sub; + struct vehicle_command_s _command; + + orb_advert_t _actuator_pub; + struct actuator_controls_s _actuators; + + bool _open_door; + bool _drop; + hrt_abstime _doors_opened; + + void task_main(); + + void handle_command(struct vehicle_command_s *cmd); + + void answer_command(struct vehicle_command_s *cmd, enum VEHICLE_CMD_RESULT result); + + /** + * Shim for calling task_main from task_create. + */ + static void task_main_trampoline(int argc, char *argv[]); }; namespace bottle_drop @@ -111,7 +133,14 @@ BottleDrop::BottleDrop() : _task_should_exit(false), _main_task(-1), - _mavlink_fd(-1) + _mavlink_fd(-1), + _command_sub(-1), + _command({}), + _actuator_pub(-1), + _actuators({}), + _open_door(false), + _drop(false), + _doors_opened(0) { } @@ -140,13 +169,179 @@ BottleDrop::~BottleDrop() bottle_drop::g_bottle_drop = nullptr; } -int BottleDrop::start() +int +BottleDrop::start() +{ + ASSERT(_main_task == -1); + + /* start the task */ + _main_task = task_spawn_cmd("bottle_drop", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 2048, + (main_t)&BottleDrop::task_main_trampoline, + nullptr); + + if (_main_task < 0) { + warn("task start failed"); + return -errno; + } + + return OK; +} + + +void +BottleDrop::status() +{ + warnx("Doors: %s", _open_door ? "OPEN" : "CLOSED"); + warnx("Dropping: %s", _drop ? "YES" : "NO"); +} + +void +BottleDrop::task_main() +{ + /* inform about start */ + warnx("Initializing.."); + fflush(stdout); + + _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); + mavlink_log_info(_mavlink_fd, "[bottle_drop] started"); + + _command_sub = orb_subscribe(ORB_ID(vehicle_command)); + + /* wakeup source(s) */ + struct pollfd fds[1]; + + /* Setup of loop */ + fds[0].fd = _command_sub; + fds[0].events = POLLIN; + + while (!_task_should_exit) { + + /* wait for up to 100ms for data */ + int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 50); + + /* this is undesirable but not much we can do - might want to flag unhappy status */ + if (pret < 0) { + warn("poll error %d, %d", pret, errno); + continue; + } + + /* vehicle commands updated */ + if (fds[0].revents & POLLIN) { + orb_copy(ORB_ID(vehicle_command), _command_sub, &_command); + handle_command(&_command); + } + + /* update door actuators */ + if (_open_door) { + _actuators.control[0] = -1.0f; + _actuators.control[1] = 1.0f; + + if (_doors_opened == 0) { + _doors_opened = hrt_absolute_time(); + } + } else { + _actuators.control[0] = 0.5f; + _actuators.control[1] = -0.5f; + + _doors_opened = 0; + } + + /* update drop actuator, wait 0.5s until the doors are open before dropping */ + if (_drop && _doors_opened != 0 && hrt_elapsed_time(&_doors_opened) > 500000) { + _actuators.control[2] = 0.5f; + } else { + _actuators.control[2] = -0.5f; + } + + /* 2s after drop, reset and close everything again */ + if (_drop && _doors_opened != 0 && hrt_elapsed_time(&_doors_opened) > 2000000) { + _open_door = false; + _drop = false; + } + + /* lazily publish _actuators only once available */ + if (_actuator_pub > 0) { + orb_publish(ORB_ID(actuator_controls_1), _actuator_pub, &_actuators); + + } else { + _actuator_pub = orb_advertise(ORB_ID(actuator_controls_1), &_actuators); + } + } + + warnx("exiting."); + + _main_task = -1; + _exit(0); +} + +void +BottleDrop::handle_command(struct vehicle_command_s *cmd) { + switch (cmd->command) { + case VEHICLE_CMD_CUSTOM_0: + + /* + * param1 and param2 set to 1: open and drop + * param1 set to 1: open + * else: close (and don't drop) + */ + if (cmd->param1 > 0.5f && cmd->param2 > 0.5f) { + _open_door = true; + _drop = true; + mavlink_log_info(_mavlink_fd, "#audio: drop bottle"); + } + else if (cmd->param1 > 0.5f) { + _open_door = true; + _drop = false; + mavlink_log_info(_mavlink_fd, "#audio: open doors"); + } else { + _open_door = false; + _drop = false; + mavlink_log_info(_mavlink_fd, "#audio: close doors"); + } + + answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); + break; + default: + break; + } } +void +BottleDrop::answer_command(struct vehicle_command_s *cmd, enum VEHICLE_CMD_RESULT result) +{ + switch (result) { + case VEHICLE_CMD_RESULT_ACCEPTED: + break; + + case VEHICLE_CMD_RESULT_DENIED: + mavlink_log_critical(_mavlink_fd, "#audio: command denied: %u", cmd->command); + break; + + case VEHICLE_CMD_RESULT_FAILED: + mavlink_log_critical(_mavlink_fd, "#audio: command failed: %u", cmd->command); + break; + + case VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED: + mavlink_log_critical(_mavlink_fd, "#audio: command temporarily rejected: %u", cmd->command); + break; + + case VEHICLE_CMD_RESULT_UNSUPPORTED: + mavlink_log_critical(_mavlink_fd, "#audio: command unsupported: %u", cmd->command); + break; + + default: + break; + } +} -void BottleDrop::status() +void +BottleDrop::task_main_trampoline(int argc, char *argv[]) { + bottle_drop::g_bottle_drop->task_main(); } static void usage() -- cgit v1.2.3 From 947b09a1209e5fd0c1e6e6cbe6a5820d15c83809 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sat, 29 Mar 2014 20:37:16 +0100 Subject: commander: don't report unsupported commands --- src/modules/commander/commander.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index cf7ba757e..46ca04661 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -588,8 +588,8 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe break; default: - /* warn about unsupported commands */ - answer_command(*cmd, VEHICLE_CMD_RESULT_UNSUPPORTED); + /* don't warn about unsupported commands, maybe another app supports it */ +// answer_command(*cmd, VEHICLE_CMD_RESULT_UNSUPPORTED); break; } -- cgit v1.2.3 From 5e51812c8b1cb9af57145eeb85705e1f914aa77c Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sat, 29 Mar 2014 20:37:58 +0100 Subject: fw_att_control: workaround, don't publish to actuator_1 --- src/modules/fw_att_control/fw_att_control_main.cpp | 24 +++++++++++----------- 1 file changed, 12 insertions(+), 12 deletions(-) diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index f139c25f4..111f56a90 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -832,18 +832,18 @@ FixedwingAttitudeControl::task_main() _actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators); } - if (_actuators_1_pub > 0) { - /* publish the attitude setpoint */ - orb_publish(ORB_ID(actuator_controls_1), _actuators_1_pub, &_actuators_airframe); -// warnx("%.2f %.2f %.2f %.2f %.2f %.2f %.2f %.2f", -// (double)_actuators_airframe.control[0], (double)_actuators_airframe.control[1], (double)_actuators_airframe.control[2], -// (double)_actuators_airframe.control[3], (double)_actuators_airframe.control[4], (double)_actuators_airframe.control[5], -// (double)_actuators_airframe.control[6], (double)_actuators_airframe.control[7]); - - } else { - /* advertise and publish */ - _actuators_1_pub = orb_advertise(ORB_ID(actuator_controls_1), &_actuators_airframe); - } +// if (_actuators_1_pub > 0) { +// /* publish the attitude setpoint */ +// orb_publish(ORB_ID(actuator_controls_1), _actuators_1_pub, &_actuators_airframe); +//// warnx("%.2f %.2f %.2f %.2f %.2f %.2f %.2f %.2f", +//// (double)_actuators_airframe.control[0], (double)_actuators_airframe.control[1], (double)_actuators_airframe.control[2], +//// (double)_actuators_airframe.control[3], (double)_actuators_airframe.control[4], (double)_actuators_airframe.control[5], +//// (double)_actuators_airframe.control[6], (double)_actuators_airframe.control[7]); +// +// } else { +// /* advertise and publish */ +// _actuators_1_pub = orb_advertise(ORB_ID(actuator_controls_1), &_actuators_airframe); +// } } -- cgit v1.2.3 From 1d75f3eb8a1dba5b919c417b27a44e16ddcb0d32 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sat, 29 Mar 2014 20:38:27 +0100 Subject: vehicle_command topic: added CUSOTM_0 as seen in QGC --- src/modules/uORB/topics/vehicle_command.h | 1 + 1 file changed, 1 insertion(+) diff --git a/src/modules/uORB/topics/vehicle_command.h b/src/modules/uORB/topics/vehicle_command.h index e6e4743c6..9f1bbb317 100644 --- a/src/modules/uORB/topics/vehicle_command.h +++ b/src/modules/uORB/topics/vehicle_command.h @@ -53,6 +53,7 @@ */ enum VEHICLE_CMD { + VEHICLE_CMD_CUSTOM_0=0, /* XXX: added because this exists in QGroundControl */ VEHICLE_CMD_NAV_WAYPOINT=16, /* Navigate to MISSION. |Hold time in decimal seconds. (ignored by fixed wing, time to stay at MISSION for rotary wing)| Acceptance radius in meters (if the sphere with this radius is hit, the MISSION counts as reached)| 0 to pass through the WP, if > 0 radius in meters to pass by WP. Positive value for clockwise orbit, negative value for counter-clockwise orbit. Allows trajectory control.| Desired yaw angle at MISSION (rotary wing)| Latitude| Longitude| Altitude| */ VEHICLE_CMD_NAV_LOITER_UNLIM=17, /* Loiter around this MISSION an unlimited amount of time |Empty| Empty| Radius around MISSION, in meters. If positive loiter clockwise, else counter-clockwise| Desired yaw angle.| Latitude| Longitude| Altitude| */ VEHICLE_CMD_NAV_LOITER_TURNS=18, /* Loiter around this MISSION for X turns |Turns| Empty| Radius around MISSION, in meters. If positive loiter clockwise, else counter-clockwise| Desired yaw angle.| Latitude| Longitude| Altitude| */ -- cgit v1.2.3 From eb4e250da88cf519850a53266fffbd06fd4a0872 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sun, 30 Mar 2014 15:00:37 +0200 Subject: Startup script: added viper script --- ROMFS/px4fmu_common/init.d/3035_viper | 10 ++++++++++ ROMFS/px4fmu_common/init.d/rc.autostart | 5 +++++ 2 files changed, 15 insertions(+) create mode 100644 ROMFS/px4fmu_common/init.d/3035_viper diff --git a/ROMFS/px4fmu_common/init.d/3035_viper b/ROMFS/px4fmu_common/init.d/3035_viper new file mode 100644 index 000000000..9c8f8a585 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/3035_viper @@ -0,0 +1,10 @@ +#!nsh +# +# Viper +# +# Simon Wilks +# + +sh /etc/init.d/rc.fw_defaults + +set MIXER FMU_FX79 \ No newline at end of file diff --git a/ROMFS/px4fmu_common/init.d/rc.autostart b/ROMFS/px4fmu_common/init.d/rc.autostart index 5ec735d39..6c1329c10 100644 --- a/ROMFS/px4fmu_common/init.d/rc.autostart +++ b/ROMFS/px4fmu_common/init.d/rc.autostart @@ -103,6 +103,11 @@ then sh /etc/init.d/3034_fx79 fi +if param compare SYS_AUTOSTART 3035 35 +then + sh /etc/init.d/3035_viper +fi + if param compare SYS_AUTOSTART 3100 then sh /etc/init.d/3100_tbs_caipirinha -- cgit v1.2.3 From 64148a9e2af255357df8aeae06a4ad2ef972ffec Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sun, 30 Mar 2014 15:01:07 +0200 Subject: bottle_drop: changed servo travels to match Simon's viper --- src/modules/bottle_drop/bottle_drop.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/modules/bottle_drop/bottle_drop.cpp b/src/modules/bottle_drop/bottle_drop.cpp index 1dcad2144..ce760adc3 100644 --- a/src/modules/bottle_drop/bottle_drop.cpp +++ b/src/modules/bottle_drop/bottle_drop.cpp @@ -243,21 +243,21 @@ BottleDrop::task_main() _doors_opened = hrt_absolute_time(); } } else { - _actuators.control[0] = 0.5f; - _actuators.control[1] = -0.5f; + _actuators.control[0] = 1.0f; + _actuators.control[1] = -1.0f; _doors_opened = 0; } /* update drop actuator, wait 0.5s until the doors are open before dropping */ if (_drop && _doors_opened != 0 && hrt_elapsed_time(&_doors_opened) > 500000) { - _actuators.control[2] = 0.5f; - } else { _actuators.control[2] = -0.5f; + } else { + _actuators.control[2] = 0.5f; } - /* 2s after drop, reset and close everything again */ - if (_drop && _doors_opened != 0 && hrt_elapsed_time(&_doors_opened) > 2000000) { + /* 20s after drop, reset and close everything again */ + if (_drop && _doors_opened != 0 && hrt_elapsed_time(&_doors_opened) > 20000000) { _open_door = false; _drop = false; } -- cgit v1.2.3 From f24c5184e8c1716795f10980a8c3b383c463facd Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sun, 30 Mar 2014 15:01:31 +0200 Subject: bottle_drop: hack to start bottle drop --- ROMFS/px4fmu_common/init.d/rc.fw_apps | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/ROMFS/px4fmu_common/init.d/rc.fw_apps b/ROMFS/px4fmu_common/init.d/rc.fw_apps index 429abc5ec..bb17d44eb 100644 --- a/ROMFS/px4fmu_common/init.d/rc.fw_apps +++ b/ROMFS/px4fmu_common/init.d/rc.fw_apps @@ -13,3 +13,7 @@ fw_att_pos_estimator start # fw_att_control start fw_pos_control_l1 start + +bottle_drop start +fmu mode_pwm +mixer load /dev/px4fmu /etc/mixers/FMU_pass.mix \ No newline at end of file -- cgit v1.2.3 From 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(-) 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(-) 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(+) 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(-) 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(-) 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 2c74babafcc3eae4bd513185395ac754ada6d86a Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 8 Jul 2014 11:16:19 +0200 Subject: fix merge of mavlink submodule --- mavlink/include/mavlink/v1.0 | 1 + 1 file changed, 1 insertion(+) create mode 160000 mavlink/include/mavlink/v1.0 diff --git a/mavlink/include/mavlink/v1.0 b/mavlink/include/mavlink/v1.0 new file mode 160000 index 000000000..3711190d2 --- /dev/null +++ b/mavlink/include/mavlink/v1.0 @@ -0,0 +1 @@ +Subproject commit 3711190d23d9928ea0687e00621c8d9ecf145f50 -- 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(-) 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(-) 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 af4d5c0a3c985007958e512db977832bc93b2553 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 10 Jul 2014 13:14:39 +0200 Subject: Added initial bits for ST24 decoding --- src/lib/rc/st24.c | 66 +++++++++++++++++++++++++ src/lib/rc/st24.h | 140 ++++++++++++++++++++++++++++++++++++++++++++++++++++++ 2 files changed, 206 insertions(+) create mode 100644 src/lib/rc/st24.c create mode 100644 src/lib/rc/st24.h diff --git a/src/lib/rc/st24.c b/src/lib/rc/st24.c new file mode 100644 index 000000000..8287a0453 --- /dev/null +++ b/src/lib/rc/st24.c @@ -0,0 +1,66 @@ +/**************************************************************************** + * + * Copyright (c) 2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/* + * @file st24.h + * + * RC protocol implementation for Yuneec ST24 transmitter. + * + * @author Lorenz Meier + */ + +uint8_t st24_common_crc8(uint8_t *ptr, uint8_t len) +{ + uint8_t i, crc ; + crc = 0; + + while (len--) { + for (i = 0x80; i != 0; i >>= 1) { + if ((crc & 0x80) != 0) { + crc <<= 1; + crc ^= 0x07; + + } else { + crc <<= 1; + } + + if ((*ptr & i) != 0) { + crc ^= 0x07; + } + } + + ptr++; + } + + return (crc); +} diff --git a/src/lib/rc/st24.h b/src/lib/rc/st24.h new file mode 100644 index 000000000..1c92bad43 --- /dev/null +++ b/src/lib/rc/st24.h @@ -0,0 +1,140 @@ +/**************************************************************************** + * + * Copyright (c) 2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file st24.h + * + * RC protocol definition for Yuneec ST24 transmitter + * + * @author Lorenz Meier + */ + +#define ST24_DATA_LEN_MAX 64 + +enum { + ST24_PACKET_TYPE_CHANNELDATA12 = 0, + ST24_PACKET_TYPE_CHANNELDATA24, + ST24_PACKET_TYPE_TRANSMITTERGPSDATA +} ST24_PACKET_TYPE; + +#pragma pack(push, 1) +typedef struct { + uint8_t header1; ///< 0x55 for a valid packet + uint8_t header2; ///< 0x55 for a valid packet + uint8_t length; ///< length includes type, data, and crc = sizeof(type)+sizeof(data[payload_len])+sizeof(crc8) + uint8_t type; ///< from enum ST24_PACKET_TYPE + uint8_t st24_data[ST24_DATA_LEN_MAX]; + uint8_t crc8; ///< crc8 checksum, calculated by st24_common_crc8 and including fields length, type and st24_data +} ReceiverFcPacket; + +/** + * RC Channel data (12 channels). + * + * This is incoming from the ST24 + */ +typedef struct { + uint16_t t; ///< packet counter or clock + uint8_t rssi ///< signal strength + uint8_t packet_count; ///< Number of UART packets sent since reception of last RF frame (this tells something about age / rate) + uint8_t channel[18]; ///< channel data, 12 channels (12 bit numbers) +} ChannelData12; + +/** + * RC Channel data (12 channels). + * + */ +typedef struct { + uint16_t t; ///< packet counter or clock + uint8_t rssi ///< signal strength + uint8_t packet_count; ///< Number of UART packets sent since reception of last RF frame (this tells something about age / rate) + uint8_t channel[36]; ///< channel data, 24 channels (12 bit numbers) +} ChannelData24; + +/** + * Telemetry packet + * + * This is outgoing to the ST24 + * + * imuStatus: + * 8 bit total + * bits 0-2 for status + * - value 0 is FAILED + * - value 1 is INITIALIZING + * - value 2 is RUNNING + * - values 3 through 7 are reserved + * bits 3-7 are status for sensors (0 or 1) + * - mpu6050 + * - accelerometer + * - primary gyro x + * - primary gyro y + * - primary gyro z + * + * pressCompassStatus + * 8 bit total + * bits 0-3 for compass status + * - value 0 is FAILED + * - value 1 is INITIALIZING + * - value 2 is RUNNING + * - value 3 - 15 are reserved + * bits 4-7 for pressure status + * - value 0 is FAILED + * - value 1 is INITIALIZING + * - value 2 is RUNNING + * - value 3 - 15 are reserved + * + */ +typedef struct { + uint16_t t; ///< packet counter or clock + int32_t lat; ///< lattitude (degrees) +/- 90 deg + int32_t lon; ///< longitude (degrees) +/- 180 deg + int32_t alt; ///< 0.01m resolution, altitude (meters) + int16_t vx, vy, vz; ///< velocity 0.01m res, +/-320.00 North-East- Down + uint8_t nsat; /// Date: Thu, 10 Jul 2014 13:21:29 +0200 Subject: More protocol decoding skeleton code --- src/lib/rc/module.mk | 40 ++++++++++++++++++++++++++++++++++++++++ src/lib/rc/st24.c | 3 +++ src/lib/rc/st24.h | 14 +++++++++++++- 3 files changed, 56 insertions(+), 1 deletion(-) create mode 100644 src/lib/rc/module.mk diff --git a/src/lib/rc/module.mk b/src/lib/rc/module.mk new file mode 100644 index 000000000..e089c6965 --- /dev/null +++ b/src/lib/rc/module.mk @@ -0,0 +1,40 @@ +############################################################################ +# +# Copyright (c) 2014 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +# +# Yuntec ST24 transmitter protocol decoder +# + +SRCS = st24.c + +MAXOPTIMIZATION = -Os diff --git a/src/lib/rc/st24.c b/src/lib/rc/st24.c index 8287a0453..bae28c85e 100644 --- a/src/lib/rc/st24.c +++ b/src/lib/rc/st24.c @@ -64,3 +64,6 @@ uint8_t st24_common_crc8(uint8_t *ptr, uint8_t len) return (crc); } + + +uint8_t st24_decode(uint8_t byte, uint8_t *rssi, uint8_t* rx_count, uint16_t *channels, uint16_t max_chan_count) \ No newline at end of file diff --git a/src/lib/rc/st24.h b/src/lib/rc/st24.h index 1c92bad43..3621e8506 100644 --- a/src/lib/rc/st24.h +++ b/src/lib/rc/st24.h @@ -137,4 +137,16 @@ typedef struct { * @param len number of bytes to accumulate in the checksum * @return the checksum of these bytes over len */ -__EXPORT uint8_t st24_common_crc8(uint8_t *ptr, uint8_t len); +uint8_t st24_common_crc8(uint8_t *ptr, uint8_t len); + +/** + * Decoder for ST24 protocol + * + * @param byte current char to read + * @param rssi pointer to a byte where the RSSI value is written back to + * @param rx_count pointer to a byte where the receive count of packets signce last wireless frame is written back to + * @param channels pointer to a datastructure of size max_chan_count where channel values (12 bit) are written back to + * @param max_chan_count maximum channels to decode - if more channels are decoded, the last n are skipped and success (0) is returned + * @return 0 for success (a decoded packet), 1 for no packet yet (accumulating), 3 for out of sync, 4 for checksum error + */ +__EXPORT uint8_t st24_decode(uint8_t byte, uint8_t *rssi, uint8_t* rx_count, uint16_t *channels, uint16_t max_chan_count); -- cgit v1.2.3 From 25a8f2d8056deca5049a8acc27c77e5e17760167 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 10 Jul 2014 14:30:34 +0200 Subject: ST24 decoding skeleton --- src/lib/rc/st24.c | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/lib/rc/st24.c b/src/lib/rc/st24.c index bae28c85e..addbcd899 100644 --- a/src/lib/rc/st24.c +++ b/src/lib/rc/st24.c @@ -66,4 +66,6 @@ uint8_t st24_common_crc8(uint8_t *ptr, uint8_t len) } -uint8_t st24_decode(uint8_t byte, uint8_t *rssi, uint8_t* rx_count, uint16_t *channels, uint16_t max_chan_count) \ No newline at end of file +uint8_t st24_decode(uint8_t byte, uint8_t *rssi, uint8_t* rx_count, uint16_t *channels, uint16_t max_chan_count) { + +} \ No newline at end of file -- 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(+) 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(-) 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(-) 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(-) 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(-) 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(-) 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(+) 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(+) 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(+) 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(-) 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 7baa337d9bcf7077a5e5080e951899cb595f6ff6 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 19 Jul 2014 19:25:53 +0200 Subject: flight termination on geofence violation --- src/modules/commander/commander.cpp | 8 ++++++++ src/modules/navigator/navigator_main.cpp | 10 ++++++++++ src/modules/uORB/topics/position_setpoint_triplet.h | 1 + 3 files changed, 19 insertions(+) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 04450a44f..522c6e886 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1244,6 +1244,14 @@ int commander_thread_main(int argc, char *argv[]) if (updated) { orb_copy(ORB_ID(position_setpoint_triplet), pos_sp_triplet_sub, &pos_sp_triplet); + + /* Check for geofence violation */ + if (pos_sp_triplet.geofence_violated) { + //XXX: make this configurable to select different actions (e.g. navigation modes) + /* this will only trigger if geofence is activated via param and a geofence file is present */ + armed.force_failsafe = true; + status_changed = true; + } // no reset is done here on purpose, on geofence violation we want to stay in flighttermination } if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) { diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 331a9a728..ba46bd568 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -335,6 +335,11 @@ Navigator::task_main() /* Check geofence violation */ if (!_geofence.inside(&_global_pos)) { + /* inform other apps via the sp triplet */ + _pos_sp_triplet.geofence_violated = true; + if (_pos_sp_triplet.geofence_violated != true) { + _pos_sp_triplet_updated = true; + } /* Issue a warning about the geofence violation once */ if (!_geofence_violation_warning_sent) { @@ -342,6 +347,11 @@ Navigator::task_main() _geofence_violation_warning_sent = true; } } else { + /* inform other apps via the sp triplet */ + _pos_sp_triplet.geofence_violated = false; + if (_pos_sp_triplet.geofence_violated != false) { + _pos_sp_triplet_updated = true; + } /* Reset the _geofence_violation_warning_sent field */ _geofence_violation_warning_sent = false; } diff --git a/src/modules/uORB/topics/position_setpoint_triplet.h b/src/modules/uORB/topics/position_setpoint_triplet.h index 4a1932180..4e8c6c53e 100644 --- a/src/modules/uORB/topics/position_setpoint_triplet.h +++ b/src/modules/uORB/topics/position_setpoint_triplet.h @@ -97,6 +97,7 @@ struct position_setpoint_triplet_s struct position_setpoint_s next; unsigned nav_state; /**< report the navigation state */ + bool geofence_violated; /**< true if the geofence is violated */ }; /** -- cgit v1.2.3 From b5ef8fd2cd54d180b5debe362a4c1f07f64394af Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 19 Jul 2014 20:03:37 +0200 Subject: create empty datalinkloss class for OBC Currently the class does the same as the RTL class. It is now possible whichclass is sued in the navigator to handle datalink loss via a parameter --- src/modules/navigator/datalinkloss.cpp | 317 ++++++++++++++++++++++++++++ src/modules/navigator/datalinkloss.h | 95 +++++++++ src/modules/navigator/datalinkloss_params.c | 98 +++++++++ src/modules/navigator/module.mk | 4 +- src/modules/navigator/navigator.h | 4 + src/modules/navigator/navigator_main.cpp | 13 +- src/modules/navigator/navigator_params.c | 11 + 7 files changed, 539 insertions(+), 3 deletions(-) create mode 100644 src/modules/navigator/datalinkloss.cpp create mode 100644 src/modules/navigator/datalinkloss.h create mode 100644 src/modules/navigator/datalinkloss_params.c diff --git a/src/modules/navigator/datalinkloss.cpp b/src/modules/navigator/datalinkloss.cpp new file mode 100644 index 000000000..17e85b284 --- /dev/null +++ b/src/modules/navigator/datalinkloss.cpp @@ -0,0 +1,317 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ +/** + * @file datalinkloss.cpp + * Helper class for Data Link Loss Mode acording to the OBC rules + * + * @author Thomas Gubler + */ + +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include + +#include "navigator.h" +#include "datalinkloss.h" + +#define DELAY_SIGMA 0.01f + +DataLinkLoss::DataLinkLoss(Navigator *navigator, const char *name) : + MissionBlock(navigator, name), + _rtl_state(RTL_STATE_NONE), + _param_return_alt(this, "RETURN_ALT"), + _param_descend_alt(this, "DESCEND_ALT"), + _param_land_delay(this, "LAND_DELAY") +{ + /* load initial params */ + updateParams(); + /* initial reset */ + on_inactive(); +} + +DataLinkLoss::~DataLinkLoss() +{ +} + +void +DataLinkLoss::on_inactive() +{ + /* reset RTL state only if setpoint moved */ + if (!_navigator->get_can_loiter_at_sp()) { + _rtl_state = RTL_STATE_NONE; + } +} + +void +DataLinkLoss::on_activation() +{ + /* decide where to enter the RTL procedure when we switch into it */ + if (_rtl_state == RTL_STATE_NONE) { + /* for safety reasons don't go into RTL if landed */ + if (_navigator->get_vstatus()->condition_landed) { + _rtl_state = RTL_STATE_LANDED; + mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: no RTL when landed"); + + /* if lower than return altitude, climb up first */ + } else if (_navigator->get_global_position()->alt < _navigator->get_home_position()->alt + + _param_return_alt.get()) { + _rtl_state = RTL_STATE_CLIMB; + + /* otherwise go straight to return */ + } else { + /* set altitude setpoint to current altitude */ + _rtl_state = RTL_STATE_RETURN; + _mission_item.altitude_is_relative = false; + _mission_item.altitude = _navigator->get_global_position()->alt; + } + } + + set_rtl_item(); +} + +void +DataLinkLoss::on_active() +{ + if (_rtl_state != RTL_STATE_LANDED && is_mission_item_reached()) { + advance_rtl(); + set_rtl_item(); + } +} + +void +DataLinkLoss::set_rtl_item() +{ + struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); + + /* make sure we have the latest params */ + updateParams(); + + set_previous_pos_setpoint(); + _navigator->set_can_loiter_at_sp(false); + + switch (_rtl_state) { + case RTL_STATE_CLIMB: { + float climb_alt = _navigator->get_home_position()->alt + _param_return_alt.get(); + + _mission_item.lat = _navigator->get_global_position()->lat; + _mission_item.lon = _navigator->get_global_position()->lon; + _mission_item.altitude_is_relative = false; + _mission_item.altitude = climb_alt; + _mission_item.yaw = NAN; + _mission_item.loiter_radius = _navigator->get_loiter_radius(); + _mission_item.loiter_direction = 1; + _mission_item.nav_cmd = NAV_CMD_WAYPOINT; + _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); + _mission_item.time_inside = 0.0f; + _mission_item.pitch_min = 0.0f; + _mission_item.autocontinue = true; + _mission_item.origin = ORIGIN_ONBOARD; + + mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: climb to %d meters above home", + (int)(climb_alt - _navigator->get_home_position()->alt)); + break; + } + + case RTL_STATE_RETURN: { + _mission_item.lat = _navigator->get_home_position()->lat; + _mission_item.lon = _navigator->get_home_position()->lon; + // don't change altitude + + if (pos_sp_triplet->previous.valid) { + /* if previous setpoint is valid then use it to calculate heading to home */ + _mission_item.yaw = get_bearing_to_next_waypoint( + pos_sp_triplet->previous.lat, pos_sp_triplet->previous.lon, + _mission_item.lat, _mission_item.lon); + + } else { + /* else use current position */ + _mission_item.yaw = get_bearing_to_next_waypoint( + _navigator->get_global_position()->lat, _navigator->get_global_position()->lon, + _mission_item.lat, _mission_item.lon); + } + _mission_item.loiter_radius = _navigator->get_loiter_radius(); + _mission_item.loiter_direction = 1; + _mission_item.nav_cmd = NAV_CMD_WAYPOINT; + _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); + _mission_item.time_inside = 0.0f; + _mission_item.pitch_min = 0.0f; + _mission_item.autocontinue = true; + _mission_item.origin = ORIGIN_ONBOARD; + + mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: return at %d meters above home", + (int)(_mission_item.altitude - _navigator->get_home_position()->alt)); + break; + } + + case RTL_STATE_DESCEND: { + _mission_item.lat = _navigator->get_home_position()->lat; + _mission_item.lon = _navigator->get_home_position()->lon; + _mission_item.altitude_is_relative = false; + _mission_item.altitude = _navigator->get_home_position()->alt + _param_descend_alt.get(); + _mission_item.yaw = NAN; + _mission_item.loiter_radius = _navigator->get_loiter_radius(); + _mission_item.loiter_direction = 1; + _mission_item.nav_cmd = NAV_CMD_LOITER_TIME_LIMIT; + _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); + _mission_item.time_inside = 0.0f; + _mission_item.pitch_min = 0.0f; + _mission_item.autocontinue = false; + _mission_item.origin = ORIGIN_ONBOARD; + + mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: descend to %d meters above home", + (int)(_mission_item.altitude - _navigator->get_home_position()->alt)); + break; + } + + case RTL_STATE_LOITER: { + bool autoland = _param_land_delay.get() > -DELAY_SIGMA; + + _mission_item.lat = _navigator->get_home_position()->lat; + _mission_item.lon = _navigator->get_home_position()->lon; + _mission_item.altitude_is_relative = false; + _mission_item.altitude = _navigator->get_home_position()->alt + _param_descend_alt.get(); + _mission_item.yaw = NAN; + _mission_item.loiter_radius = _navigator->get_loiter_radius(); + _mission_item.loiter_direction = 1; + _mission_item.nav_cmd = autoland ? NAV_CMD_LOITER_TIME_LIMIT : NAV_CMD_LOITER_UNLIMITED; + _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); + _mission_item.time_inside = _param_land_delay.get() < 0.0f ? 0.0f : _param_land_delay.get(); + _mission_item.pitch_min = 0.0f; + _mission_item.autocontinue = autoland; + _mission_item.origin = ORIGIN_ONBOARD; + + _navigator->set_can_loiter_at_sp(true); + + if (autoland) { + mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: loiter %.1fs", (double)_mission_item.time_inside); + + } else { + mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: completed, loiter"); + } + break; + } + + case RTL_STATE_LAND: { + _mission_item.lat = _navigator->get_home_position()->lat; + _mission_item.lon = _navigator->get_home_position()->lon; + _mission_item.altitude_is_relative = false; + _mission_item.altitude = _navigator->get_home_position()->alt; + _mission_item.yaw = NAN; + _mission_item.loiter_radius = _navigator->get_loiter_radius(); + _mission_item.loiter_direction = 1; + _mission_item.nav_cmd = NAV_CMD_LAND; + _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); + _mission_item.time_inside = 0.0f; + _mission_item.pitch_min = 0.0f; + _mission_item.autocontinue = true; + _mission_item.origin = ORIGIN_ONBOARD; + + mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: land at home"); + break; + } + + case RTL_STATE_LANDED: { + _mission_item.lat = _navigator->get_home_position()->lat; + _mission_item.lon = _navigator->get_home_position()->lon; + _mission_item.altitude_is_relative = false; + _mission_item.altitude = _navigator->get_home_position()->alt; + _mission_item.yaw = NAN; + _mission_item.loiter_radius = _navigator->get_loiter_radius(); + _mission_item.loiter_direction = 1; + _mission_item.nav_cmd = NAV_CMD_IDLE; + _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); + _mission_item.time_inside = 0.0f; + _mission_item.pitch_min = 0.0f; + _mission_item.autocontinue = true; + _mission_item.origin = ORIGIN_ONBOARD; + + mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: completed, landed"); + break; + } + + default: + break; + } + + reset_mission_item_reached(); + + /* convert mission item to current position setpoint and make it valid */ + mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current); + pos_sp_triplet->next.valid = false; + + _navigator->set_position_setpoint_triplet_updated(); +} + +void +DataLinkLoss::advance_rtl() +{ + switch (_rtl_state) { + case RTL_STATE_CLIMB: + _rtl_state = RTL_STATE_RETURN; + break; + + case RTL_STATE_RETURN: + _rtl_state = RTL_STATE_DESCEND; + break; + + case RTL_STATE_DESCEND: + /* only go to land if autoland is enabled */ + if (_param_land_delay.get() < -DELAY_SIGMA || _param_land_delay.get() > DELAY_SIGMA) { + _rtl_state = RTL_STATE_LOITER; + + } else { + _rtl_state = RTL_STATE_LAND; + } + break; + + case RTL_STATE_LOITER: + _rtl_state = RTL_STATE_LAND; + break; + + case RTL_STATE_LAND: + _rtl_state = RTL_STATE_LANDED; + break; + + default: + break; + } +} diff --git a/src/modules/navigator/datalinkloss.h b/src/modules/navigator/datalinkloss.h new file mode 100644 index 000000000..242cfac8d --- /dev/null +++ b/src/modules/navigator/datalinkloss.h @@ -0,0 +1,95 @@ +/*************************************************************************** + * + * Copyright (c) 2013-2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ +/** + * @file datalinkloss.cpp + * Helper class for Data Link Loss Mode acording to the OBC rules + * + * @author Thomas Gubler + */ + +#ifndef NAVIGATOR_DATALINKLOSS_H +#define NAVIGATOR_DATALINKLOSS_H + +#include +#include + +#include +#include +#include +#include + +#include "navigator_mode.h" +#include "mission_block.h" + +class Navigator; + +class DataLinkLoss : public MissionBlock +{ +public: + DataLinkLoss(Navigator *navigator, const char *name); + + ~DataLinkLoss(); + + virtual void on_inactive(); + + virtual void on_activation(); + + virtual void on_active(); + +private: + /** + * Set the RTL item + */ + void set_rtl_item(); + + /** + * Move to next RTL item + */ + void advance_rtl(); + + enum RTLState { + RTL_STATE_NONE = 0, + RTL_STATE_CLIMB, + RTL_STATE_RETURN, + RTL_STATE_DESCEND, + RTL_STATE_LOITER, + RTL_STATE_LAND, + RTL_STATE_LANDED, + } _rtl_state; + + control::BlockParamFloat _param_return_alt; + control::BlockParamFloat _param_descend_alt; + control::BlockParamFloat _param_land_delay; +}; + +#endif diff --git a/src/modules/navigator/datalinkloss_params.c b/src/modules/navigator/datalinkloss_params.c new file mode 100644 index 000000000..bfe6ce7e1 --- /dev/null +++ b/src/modules/navigator/datalinkloss_params.c @@ -0,0 +1,98 @@ +/**************************************************************************** + * + * Copyright (c) 2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * AS IS AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file rtl_params.c + * + * Parameters for RTL + * + * @author Julian Oes + */ + +#include + +#include + +/* + * RTL parameters, accessible via MAVLink + */ + +/** + * Loiter radius after RTL (FW only) + * + * Default value of loiter radius after RTL (fixedwing only). + * + * @unit meters + * @min 0.0 + * @group RTL + */ +PARAM_DEFINE_FLOAT(RTL_LOITER_RAD, 50.0f); + +/** + * RTL altitude + * + * Altitude to fly back in RTL in meters + * + * @unit meters + * @min 0 + * @max 1 + * @group RTL + */ +PARAM_DEFINE_FLOAT(RTL_RETURN_ALT, 100); + + +/** + * RTL loiter altitude + * + * Stay at this altitude above home position after RTL descending. + * Land (i.e. slowly descend) from this altitude if autolanding allowed. + * + * @unit meters + * @min 0 + * @max 100 + * @group RTL + */ +PARAM_DEFINE_FLOAT(RTL_DESCEND_ALT, 20); + +/** + * RTL delay + * + * Delay after descend before landing in RTL mode. + * If set to -1 the system will not land but loiter at NAV_LAND_ALT. + * + * @unit seconds + * @min -1 + * @max + * @group RTL + */ +PARAM_DEFINE_FLOAT(RTL_LAND_DELAY, -1.0f); diff --git a/src/modules/navigator/module.mk b/src/modules/navigator/module.mk index b50198996..a1e42ec38 100644 --- a/src/modules/navigator/module.mk +++ b/src/modules/navigator/module.mk @@ -49,7 +49,9 @@ SRCS = navigator_main.cpp \ offboard.cpp \ mission_feasibility_checker.cpp \ geofence.cpp \ - geofence_params.c + geofence_params.c \ + datalinkloss.cpp \ + datalinkloss_params.c INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index 8edbb63b3..d0b2ed841 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -35,6 +35,7 @@ * Helper class to access missions * @author Julian Oes * @author Anton Babushkin + * @author Thomas Gubler */ #ifndef NAVIGATOR_H @@ -57,6 +58,7 @@ #include "loiter.h" #include "rtl.h" #include "offboard.h" +#include "datalinkloss.h" #include "geofence.h" /** @@ -165,6 +167,7 @@ private: Loiter _loiter; /**< class that handles loiter */ RTL _rtl; /**< class that handles RTL */ Offboard _offboard; /**< class that handles offboard */ + DataLinkLoss _dataLinkLoss; /**< class that handles offboard */ NavigatorMode *_navigation_mode_array[NAVIGATOR_MODE_ARRAY_SIZE]; /**< array of navigation modes */ @@ -173,6 +176,7 @@ private: control::BlockParamFloat _param_loiter_radius; /**< loiter radius for fixedwing */ control::BlockParamFloat _param_acceptance_radius; /**< acceptance for takeoff */ + control::BlockParamInt _param_datalinkloss_obc; /**< if true: obc mode on data link loss enabled */ /** * Retrieve global position */ diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index ba46bd568..1ce6770c9 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -40,6 +40,7 @@ * @author Jean Cyr * @author Julian Oes * @author Anton Babushkin + * @author Thomas Gubler */ #include @@ -125,10 +126,12 @@ Navigator::Navigator() : _loiter(this, "LOI"), _rtl(this, "RTL"), _offboard(this, "OFF"), + _dataLinkLoss(this, "DLL"), _can_loiter_at_sp(false), _pos_sp_triplet_updated(false), _param_loiter_radius(this, "LOITER_RAD"), - _param_acceptance_radius(this, "ACC_RAD") + _param_acceptance_radius(this, "ACC_RAD"), + _param_datalinkloss_obc(this, "DLL_OBC") { /* Create a list of our possible navigation types */ _navigation_mode_array[0] = &_mission; @@ -376,7 +379,13 @@ Navigator::task_main() _navigation_mode = &_rtl; break; case NAVIGATION_STATE_AUTO_RTGS: - _navigation_mode = &_rtl; /* TODO: change this to something else */ + /* Use complex data link loss mode only when enabled via param + * otherwise use rtl */ + if (_param_datalinkloss_obc.get() != 0) { + _navigation_mode = &_dataLinkLoss; + } else { + _navigation_mode = &_rtl; /* TODO: change this to something else */ + } break; case NAVIGATION_STATE_LAND: case NAVIGATION_STATE_TERMINATION: diff --git a/src/modules/navigator/navigator_params.c b/src/modules/navigator/navigator_params.c index 084afe340..afaf1c3c3 100644 --- a/src/modules/navigator/navigator_params.c +++ b/src/modules/navigator/navigator_params.c @@ -37,6 +37,7 @@ * Parameters for navigator in general * * @author Julian Oes + * @author Thomas Gubler */ #include @@ -64,3 +65,13 @@ PARAM_DEFINE_FLOAT(NAV_LOITER_RAD, 50.0f); * @group Mission */ PARAM_DEFINE_FLOAT(NAV_ACC_RAD, 25.0f); + +/** + * Set OBC mode for data link loss + * + * If set to 1 the behaviour on data link loss is set to a mode according to the OBC rules + * + * @min 0 + * @group Mission + */ +PARAM_DEFINE_INT32(NAV_DLL_OBC, 0); -- cgit v1.2.3 From 8739308999b410ac8e2a92cf3e5fa63c5e18f5ba Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 20 Jul 2014 17:40:26 +0200 Subject: WIP, datalinkloss: implementing basic behavior --- src/modules/navigator/datalinkloss.cpp | 228 +++++++--------------------- src/modules/navigator/datalinkloss.h | 33 ++-- src/modules/navigator/datalinkloss_params.c | 91 +++++++---- 3 files changed, 130 insertions(+), 222 deletions(-) diff --git a/src/modules/navigator/datalinkloss.cpp b/src/modules/navigator/datalinkloss.cpp index 17e85b284..2bd80165d 100644 --- a/src/modules/navigator/datalinkloss.cpp +++ b/src/modules/navigator/datalinkloss.cpp @@ -57,10 +57,14 @@ DataLinkLoss::DataLinkLoss(Navigator *navigator, const char *name) : MissionBlock(navigator, name), - _rtl_state(RTL_STATE_NONE), - _param_return_alt(this, "RETURN_ALT"), - _param_descend_alt(this, "DESCEND_ALT"), - _param_land_delay(this, "LAND_DELAY") + _dll_state(DLL_STATE_NONE), + _param_commsholdwaittime(this, "CH_T"), + _param_commsholdlat(this, "CH_LAT"), + _param_commsholdlon(this, "CH_LON"), + _param_commsholdalt(this, "CH_ALT"), + _param_airfieldhomelat(this, "AH_LAT"), + _param_airfieldhomelon(this, "AH_LON"), + _param_airfieldhomealt(this, "AH_ALT") { /* load initial params */ updateParams(); @@ -77,7 +81,7 @@ DataLinkLoss::on_inactive() { /* reset RTL state only if setpoint moved */ if (!_navigator->get_can_loiter_at_sp()) { - _rtl_state = RTL_STATE_NONE; + _dll_state = DLL_STATE_NONE; } } @@ -85,40 +89,40 @@ void DataLinkLoss::on_activation() { /* decide where to enter the RTL procedure when we switch into it */ - if (_rtl_state == RTL_STATE_NONE) { - /* for safety reasons don't go into RTL if landed */ - if (_navigator->get_vstatus()->condition_landed) { - _rtl_state = RTL_STATE_LANDED; - mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: no RTL when landed"); - - /* if lower than return altitude, climb up first */ - } else if (_navigator->get_global_position()->alt < _navigator->get_home_position()->alt - + _param_return_alt.get()) { - _rtl_state = RTL_STATE_CLIMB; - - /* otherwise go straight to return */ - } else { - /* set altitude setpoint to current altitude */ - _rtl_state = RTL_STATE_RETURN; - _mission_item.altitude_is_relative = false; - _mission_item.altitude = _navigator->get_global_position()->alt; - } - } - - set_rtl_item(); + //if (_rtl_state == RTL_STATE_NONE) { + //[> for safety reasons don't go into RTL if landed <] + //if (_navigator->get_vstatus()->condition_landed) { + //_rtl_state = RTL_STATE_LANDED; + //mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: no RTL when landed"); + + //[> if lower than return altitude, climb up first <] + //} else if (_navigator->get_global_position()->alt < _navigator->get_home_position()->alt + //+ _param_return_alt.get()) { + //_rtl_state = RTL_STATE_CLIMB; + + //[> otherwise go straight to return <] + //} else { + //[> set altitude setpoint to current altitude <] + //_rtl_state = RTL_STATE_RETURN; + //_mission_item.altitude_is_relative = false; + //_mission_item.altitude = _navigator->get_global_position()->alt; + //} + //} + _dll_state = DLL_STATE_FLYTOCOMMSHOLDWP; + set_dll_item(); } void DataLinkLoss::on_active() { - if (_rtl_state != RTL_STATE_LANDED && is_mission_item_reached()) { - advance_rtl(); - set_rtl_item(); + if (is_mission_item_reached()) { + advance_dll(); + set_dll_item(); } } void -DataLinkLoss::set_rtl_item() +DataLinkLoss::set_dll_item() { struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); @@ -128,146 +132,43 @@ DataLinkLoss::set_rtl_item() set_previous_pos_setpoint(); _navigator->set_can_loiter_at_sp(false); - switch (_rtl_state) { - case RTL_STATE_CLIMB: { - float climb_alt = _navigator->get_home_position()->alt + _param_return_alt.get(); - - _mission_item.lat = _navigator->get_global_position()->lat; - _mission_item.lon = _navigator->get_global_position()->lon; - _mission_item.altitude_is_relative = false; - _mission_item.altitude = climb_alt; - _mission_item.yaw = NAN; - _mission_item.loiter_radius = _navigator->get_loiter_radius(); - _mission_item.loiter_direction = 1; - _mission_item.nav_cmd = NAV_CMD_WAYPOINT; - _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); - _mission_item.time_inside = 0.0f; - _mission_item.pitch_min = 0.0f; - _mission_item.autocontinue = true; - _mission_item.origin = ORIGIN_ONBOARD; - - mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: climb to %d meters above home", - (int)(climb_alt - _navigator->get_home_position()->alt)); - break; - } - - case RTL_STATE_RETURN: { - _mission_item.lat = _navigator->get_home_position()->lat; - _mission_item.lon = _navigator->get_home_position()->lon; - // don't change altitude - - if (pos_sp_triplet->previous.valid) { - /* if previous setpoint is valid then use it to calculate heading to home */ - _mission_item.yaw = get_bearing_to_next_waypoint( - pos_sp_triplet->previous.lat, pos_sp_triplet->previous.lon, - _mission_item.lat, _mission_item.lon); - - } else { - /* else use current position */ - _mission_item.yaw = get_bearing_to_next_waypoint( - _navigator->get_global_position()->lat, _navigator->get_global_position()->lon, - _mission_item.lat, _mission_item.lon); - } - _mission_item.loiter_radius = _navigator->get_loiter_radius(); - _mission_item.loiter_direction = 1; - _mission_item.nav_cmd = NAV_CMD_WAYPOINT; - _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); - _mission_item.time_inside = 0.0f; - _mission_item.pitch_min = 0.0f; - _mission_item.autocontinue = true; - _mission_item.origin = ORIGIN_ONBOARD; - - mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: return at %d meters above home", - (int)(_mission_item.altitude - _navigator->get_home_position()->alt)); - break; - } - - case RTL_STATE_DESCEND: { - _mission_item.lat = _navigator->get_home_position()->lat; - _mission_item.lon = _navigator->get_home_position()->lon; + switch (_dll_state) { + case DLL_STATE_FLYTOCOMMSHOLDWP: { + _mission_item.lat = (double)(_param_commsholdlat.get()) * 1.0e-7; + _mission_item.lon = (double)(_param_commsholdlon.get()) * 1.0e-7; _mission_item.altitude_is_relative = false; - _mission_item.altitude = _navigator->get_home_position()->alt + _param_descend_alt.get(); + _mission_item.altitude = (double)(_param_commsholdalt.get()); _mission_item.yaw = NAN; _mission_item.loiter_radius = _navigator->get_loiter_radius(); _mission_item.loiter_direction = 1; _mission_item.nav_cmd = NAV_CMD_LOITER_TIME_LIMIT; _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); - _mission_item.time_inside = 0.0f; - _mission_item.pitch_min = 0.0f; - _mission_item.autocontinue = false; - _mission_item.origin = ORIGIN_ONBOARD; - - mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: descend to %d meters above home", - (int)(_mission_item.altitude - _navigator->get_home_position()->alt)); - break; - } - - case RTL_STATE_LOITER: { - bool autoland = _param_land_delay.get() > -DELAY_SIGMA; - - _mission_item.lat = _navigator->get_home_position()->lat; - _mission_item.lon = _navigator->get_home_position()->lon; - _mission_item.altitude_is_relative = false; - _mission_item.altitude = _navigator->get_home_position()->alt + _param_descend_alt.get(); - _mission_item.yaw = NAN; - _mission_item.loiter_radius = _navigator->get_loiter_radius(); - _mission_item.loiter_direction = 1; - _mission_item.nav_cmd = autoland ? NAV_CMD_LOITER_TIME_LIMIT : NAV_CMD_LOITER_UNLIMITED; - _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); - _mission_item.time_inside = _param_land_delay.get() < 0.0f ? 0.0f : _param_land_delay.get(); - _mission_item.pitch_min = 0.0f; - _mission_item.autocontinue = autoland; - _mission_item.origin = ORIGIN_ONBOARD; - - _navigator->set_can_loiter_at_sp(true); - - if (autoland) { - mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: loiter %.1fs", (double)_mission_item.time_inside); - - } else { - mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: completed, loiter"); - } - break; - } - - case RTL_STATE_LAND: { - _mission_item.lat = _navigator->get_home_position()->lat; - _mission_item.lon = _navigator->get_home_position()->lon; - _mission_item.altitude_is_relative = false; - _mission_item.altitude = _navigator->get_home_position()->alt; - _mission_item.yaw = NAN; - _mission_item.loiter_radius = _navigator->get_loiter_radius(); - _mission_item.loiter_direction = 1; - _mission_item.nav_cmd = NAV_CMD_LAND; - _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); - _mission_item.time_inside = 0.0f; + _mission_item.time_inside = _param_commsholdwaittime.get() < 0.0f ? 0.0f : _param_commsholdwaittime.get(); _mission_item.pitch_min = 0.0f; _mission_item.autocontinue = true; _mission_item.origin = ORIGIN_ONBOARD; - mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: land at home"); + _navigator->set_can_loiter_at_sp(true); break; } - - case RTL_STATE_LANDED: { - _mission_item.lat = _navigator->get_home_position()->lat; - _mission_item.lon = _navigator->get_home_position()->lon; + case DLL_STATE_FLYTOAIRFIELDHOMEWP: { + _mission_item.lat = (double)(_param_airfieldhomelat.get()) * 1.0e-7; + _mission_item.lon = (double)(_param_airfieldhomelon.get()) * 1.0e-7; _mission_item.altitude_is_relative = false; - _mission_item.altitude = _navigator->get_home_position()->alt; + _mission_item.altitude = (double)(_param_airfieldhomealt.get()); _mission_item.yaw = NAN; _mission_item.loiter_radius = _navigator->get_loiter_radius(); _mission_item.loiter_direction = 1; - _mission_item.nav_cmd = NAV_CMD_IDLE; + _mission_item.nav_cmd = NAV_CMD_LOITER_UNLIMITED; _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); - _mission_item.time_inside = 0.0f; + _mission_item.time_inside = _param_commsholdwaittime.get() < 0.0f ? 0.0f : _param_commsholdwaittime.get(); _mission_item.pitch_min = 0.0f; _mission_item.autocontinue = true; _mission_item.origin = ORIGIN_ONBOARD; - mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: completed, landed"); + _navigator->set_can_loiter_at_sp(true); break; } - default: break; } @@ -282,35 +183,16 @@ DataLinkLoss::set_rtl_item() } void -DataLinkLoss::advance_rtl() +DataLinkLoss::advance_dll() { - switch (_rtl_state) { - case RTL_STATE_CLIMB: - _rtl_state = RTL_STATE_RETURN; + switch (_dll_state) { + case DLL_STATE_NONE: + _dll_state = DLL_STATE_FLYTOCOMMSHOLDWP; break; - - case RTL_STATE_RETURN: - _rtl_state = RTL_STATE_DESCEND; + case DLL_STATE_FLYTOCOMMSHOLDWP: + //XXX check here if time is over are over + _dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP; break; - - case RTL_STATE_DESCEND: - /* only go to land if autoland is enabled */ - if (_param_land_delay.get() < -DELAY_SIGMA || _param_land_delay.get() > DELAY_SIGMA) { - _rtl_state = RTL_STATE_LOITER; - - } else { - _rtl_state = RTL_STATE_LAND; - } - break; - - case RTL_STATE_LOITER: - _rtl_state = RTL_STATE_LAND; - break; - - case RTL_STATE_LAND: - _rtl_state = RTL_STATE_LANDED; - break; - default: break; } diff --git a/src/modules/navigator/datalinkloss.h b/src/modules/navigator/datalinkloss.h index 242cfac8d..101c88a25 100644 --- a/src/modules/navigator/datalinkloss.h +++ b/src/modules/navigator/datalinkloss.h @@ -68,28 +68,27 @@ public: private: /** - * Set the RTL item + * Set the DLL item */ - void set_rtl_item(); + void set_dll_item(); /** - * Move to next RTL item + * Move to next DLL item */ - void advance_rtl(); + void advance_dll(); - enum RTLState { - RTL_STATE_NONE = 0, - RTL_STATE_CLIMB, - RTL_STATE_RETURN, - RTL_STATE_DESCEND, - RTL_STATE_LOITER, - RTL_STATE_LAND, - RTL_STATE_LANDED, - } _rtl_state; + enum DLLState { + DLL_STATE_NONE = 0, + DLL_STATE_FLYTOCOMMSHOLDWP = 1, + DLL_STATE_FLYTOAIRFIELDHOMEWP = 2, + } _dll_state; - control::BlockParamFloat _param_return_alt; - control::BlockParamFloat _param_descend_alt; - control::BlockParamFloat _param_land_delay; + control::BlockParamFloat _param_commsholdwaittime; + control::BlockParamInt _param_commsholdlat; // * 1e7 + control::BlockParamInt _param_commsholdlon; // * 1e7 + control::BlockParamFloat _param_commsholdalt; + control::BlockParamInt _param_airfieldhomelat; // * 1e7 + control::BlockParamInt _param_airfieldhomelon; // * 1e7 + control::BlockParamFloat _param_airfieldhomealt; }; - #endif diff --git a/src/modules/navigator/datalinkloss_params.c b/src/modules/navigator/datalinkloss_params.c index bfe6ce7e1..a836fc8ca 100644 --- a/src/modules/navigator/datalinkloss_params.c +++ b/src/modules/navigator/datalinkloss_params.c @@ -32,11 +32,11 @@ ****************************************************************************/ /** - * @file rtl_params.c + * @file datalinkloss_params.c * - * Parameters for RTL + * Parameters for DLL * - * @author Julian Oes + * @author Thomas Gubler */ #include @@ -44,55 +44,82 @@ #include /* - * RTL parameters, accessible via MAVLink + * Data Link Loss parameters, accessible via MAVLink */ /** - * Loiter radius after RTL (FW only) + * Comms hold wait time * - * Default value of loiter radius after RTL (fixedwing only). + * The amount of time in seconds the system should wait at the comms hold waypoint * - * @unit meters + * @unit seconds * @min 0.0 - * @group RTL + * @group DLL */ -PARAM_DEFINE_FLOAT(RTL_LOITER_RAD, 50.0f); +PARAM_DEFINE_FLOAT(NAV_DLL_CH_T, 120.0f); /** - * RTL altitude + * Comms hold Lat * - * Altitude to fly back in RTL in meters + * Latitude of comms hold waypoint * - * @unit meters - * @min 0 - * @max 1 - * @group RTL + * @unit degrees * 1e7 + * @min 0.0 + * @group DLL */ -PARAM_DEFINE_FLOAT(RTL_RETURN_ALT, 100); +PARAM_DEFINE_INT32(NAV_DLL_CH_LAT, 266072120); +/** + * Comms hold Lon + * + * Longitude of comms hold waypoint + * + * @unit degrees * 1e7 + * @min 0.0 + * @group DLL + */ +PARAM_DEFINE_INT32(NAV_DLL_CH_LON, 1518453890); /** - * RTL loiter altitude + * Comms hold alt * - * Stay at this altitude above home position after RTL descending. - * Land (i.e. slowly descend) from this altitude if autolanding allowed. + * Altitude of comms hold waypoint * - * @unit meters - * @min 0 - * @max 100 - * @group RTL + * @unit m + * @min 0.0 + * @group DLL */ -PARAM_DEFINE_FLOAT(RTL_DESCEND_ALT, 20); +PARAM_DEFINE_FLOAT(NAV_DLL_CH_ALT, 600.0f); /** - * RTL delay + * Airfield home Lat * - * Delay after descend before landing in RTL mode. - * If set to -1 the system will not land but loiter at NAV_LAND_ALT. + * Latitude of airfield home waypoint * - * @unit seconds - * @min -1 - * @max - * @group RTL + * @unit degrees * 1e7 + * @min 0.0 + * @group DLL + */ +PARAM_DEFINE_INT32(NAV_DLL_AH_LAT, 265847810); + +/** + * Airfield home Lon + * + * Longitude of airfield home waypoint + * + * @unit degrees * 1e7 + * @min 0.0 + * @group DLL + */ +PARAM_DEFINE_INT32(NAV_DLL_AH_LON, 1518423250); + +/** + * Airfield home alt + * + * Altitude of airfield home waypoint + * + * @unit m + * @min 0.0 + * @group DLL */ -PARAM_DEFINE_FLOAT(RTL_LAND_DELAY, -1.0f); +PARAM_DEFINE_FLOAT(NAV_DLL_AH_ALT, 600.0f); -- cgit v1.2.3 From dcf114aa65273d5d5ce522565fc364fc347ba3fe Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 20 Jul 2014 17:53:04 +0200 Subject: data link loss timeout as param --- src/modules/commander/commander.cpp | 8 +++++--- src/modules/commander/commander_params.c | 12 ++++++++++++ 2 files changed, 17 insertions(+), 3 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 7d4d677d0..8eba64c1d 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -127,7 +127,6 @@ extern struct system_load_s system_load; #define POSITION_TIMEOUT (600 * 1000) /**< consider the local or global position estimate invalid after 600ms */ #define FAILSAFE_DEFAULT_TIMEOUT (3 * 1000 * 1000) /**< hysteresis time - the failsafe will trigger after 3 seconds in this state */ #define RC_TIMEOUT 500000 -#define DL_TIMEOUT 5 * 1000* 1000 #define OFFBOARD_TIMEOUT 500000 #define DIFFPRESS_TIMEOUT 2000000 @@ -650,6 +649,7 @@ int commander_thread_main(int argc, char *argv[]) param_t _param_takeoff_alt = param_find("NAV_TAKEOFF_ALT"); param_t _param_enable_parachute = param_find("NAV_PARACHUTE_EN"); param_t _param_enable_datalink_loss = param_find("COM_DL_LOSS_EN"); + param_t _param_datalink_loss_timeout = param_find("COM_DL_LOSS_T"); /* welcome user */ warnx("starting"); @@ -924,6 +924,7 @@ int commander_thread_main(int argc, char *argv[]) transition_result_t arming_ret; int32_t datalink_loss_enabled = false; + int32_t datalink_loss_timeout = 10; /* check which state machines for changes, clear "changed" flag */ bool arming_state_changed = false; @@ -983,6 +984,7 @@ int commander_thread_main(int argc, char *argv[]) param_get(_param_takeoff_alt, &takeoff_alt); param_get(_param_enable_parachute, ¶chute_enabled); param_get(_param_enable_datalink_loss, &datalink_loss_enabled); + param_get(_param_datalink_loss_timeout, &datalink_loss_timeout); } orb_check(sp_man_sub, &updated); @@ -1023,7 +1025,7 @@ int commander_thread_main(int argc, char *argv[]) if (mavlink_fd && telemetry_last_heartbeat[i] == 0 && telemetry.heartbeat_time > 0 && - hrt_elapsed_time(&telemetry.heartbeat_time) < DL_TIMEOUT) { + hrt_elapsed_time(&telemetry.heartbeat_time) < datalink_loss_timeout) { (void)rc_calibration_check(mavlink_fd); } @@ -1448,7 +1450,7 @@ int commander_thread_main(int argc, char *argv[]) /* data links check */ bool have_link = false; for (int i = 0; i < TELEMETRY_STATUS_ORB_ID_NUM; i++) { - if (telemetry_last_heartbeat[i] != 0 && hrt_elapsed_time(&telemetry_last_heartbeat[i]) < DL_TIMEOUT) { + if (telemetry_last_heartbeat[i] != 0 && hrt_elapsed_time(&telemetry_last_heartbeat[i]) < datalink_loss_timeout) { /* handle the case where data link was regained */ if (telemetry_lost[i]) { mavlink_log_critical(mavlink_fd, "data link %i regained", i); diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index 4750f9d5c..25effbd21 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -95,3 +95,15 @@ PARAM_DEFINE_FLOAT(BAT_CAPACITY, -1.0f); * @max 1 */ PARAM_DEFINE_INT32(COM_DL_LOSS_EN, 0); + +/** + * Datalink timeout threshold + * + * After this amount of seconds the data link lost mode triggers + * + * @group commander + * @unit second + * @min 0 + * @max 1000 + */ +PARAM_DEFINE_INT32(COM_DL_LOSS_T, 10); -- cgit v1.2.3 From 86b9e367a6cc5791f83df3223190a470798c00ff Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 20 Jul 2014 18:23:41 +0200 Subject: introduce data link lost counter --- src/modules/commander/commander.cpp | 1 + src/modules/uORB/topics/vehicle_status.h | 1 + 2 files changed, 2 insertions(+) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 8eba64c1d..46065fef1 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1477,6 +1477,7 @@ int commander_thread_main(int argc, char *argv[]) if (!status.data_link_lost) { mavlink_log_critical(mavlink_fd, "ALL DATA LINKS LOST"); status.data_link_lost = true; + status.data_link_lost_counter++; status_changed = true; } } diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index b46c00b75..eda1dfaec 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -201,6 +201,7 @@ struct vehicle_status_s { bool rc_input_blocked; /**< set if RC input should be ignored */ bool data_link_lost; /**< datalink to GCS lost */ + uint8_t data_link_lost_counter; /**< counts unique data link lost events */ bool offboard_control_signal_found_once; bool offboard_control_signal_lost; -- cgit v1.2.3 From a9a8f1435fa798b289aa4e4af9312041abdbcf94 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 21 Jul 2014 23:56:32 +0200 Subject: abort comm loss mode if counter above param and return home directly --- src/modules/commander/commander_params.c | 5 ++-- src/modules/navigator/datalinkloss.cpp | 14 ++++++++--- src/modules/navigator/datalinkloss.h | 37 ++++++++++++++++------------- src/modules/navigator/datalinkloss_params.c | 11 +++++++++ 4 files changed, 44 insertions(+), 23 deletions(-) diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index 25effbd21..980a7a2bb 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -94,10 +94,9 @@ PARAM_DEFINE_FLOAT(BAT_CAPACITY, -1.0f); * @min 0 * @max 1 */ -PARAM_DEFINE_INT32(COM_DL_LOSS_EN, 0); +PARAM_DEFINE_INT32(DL_LOSS_EN, 0); -/** - * Datalink timeout threshold + /** Datalink timeout threshold * * After this amount of seconds the data link lost mode triggers * diff --git a/src/modules/navigator/datalinkloss.cpp b/src/modules/navigator/datalinkloss.cpp index 2bd80165d..a98e21139 100644 --- a/src/modules/navigator/datalinkloss.cpp +++ b/src/modules/navigator/datalinkloss.cpp @@ -57,14 +57,16 @@ DataLinkLoss::DataLinkLoss(Navigator *navigator, const char *name) : MissionBlock(navigator, name), - _dll_state(DLL_STATE_NONE), + _vehicleStatus(&getSubscriptions(), ORB_ID(vehicle_status), 100), _param_commsholdwaittime(this, "CH_T"), _param_commsholdlat(this, "CH_LAT"), _param_commsholdlon(this, "CH_LON"), _param_commsholdalt(this, "CH_ALT"), _param_airfieldhomelat(this, "AH_LAT"), _param_airfieldhomelon(this, "AH_LON"), - _param_airfieldhomealt(this, "AH_ALT") + _param_airfieldhomealt(this, "AH_ALT"), + _param_numberdatalinklosses(this, "DLL_N"), + _dll_state(DLL_STATE_NONE) { /* load initial params */ updateParams(); @@ -187,7 +189,13 @@ DataLinkLoss::advance_dll() { switch (_dll_state) { case DLL_STATE_NONE: - _dll_state = DLL_STATE_FLYTOCOMMSHOLDWP; + /* Check the number of data link losses. If above home fly home directly */ + updateSubscriptions(); + if (_vehicleStatus.data_link_lost_counter > _param_numberdatalinklosses.get()) { + _dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP; + } else { + _dll_state = DLL_STATE_FLYTOCOMMSHOLDWP; + } break; case DLL_STATE_FLYTOCOMMSHOLDWP: //XXX check here if time is over are over diff --git a/src/modules/navigator/datalinkloss.h b/src/modules/navigator/datalinkloss.h index 101c88a25..650cc7bc5 100644 --- a/src/modules/navigator/datalinkloss.h +++ b/src/modules/navigator/datalinkloss.h @@ -43,10 +43,7 @@ #include #include -#include -#include -#include -#include +#include #include "navigator_mode.h" #include "mission_block.h" @@ -67,6 +64,25 @@ public: virtual void on_active(); private: + /* Subscriptions */ + uORB::Subscription _vehicleStatus; + + /* Params */ + control::BlockParamFloat _param_commsholdwaittime; + control::BlockParamInt _param_commsholdlat; // * 1e7 + control::BlockParamInt _param_commsholdlon; // * 1e7 + control::BlockParamFloat _param_commsholdalt; + control::BlockParamInt _param_airfieldhomelat; // * 1e7 + control::BlockParamInt _param_airfieldhomelon; // * 1e7 + control::BlockParamFloat _param_airfieldhomealt; + control::BlockParamInt _param_numberdatalinklosses; + + enum DLLState { + DLL_STATE_NONE = 0, + DLL_STATE_FLYTOCOMMSHOLDWP = 1, + DLL_STATE_FLYTOAIRFIELDHOMEWP = 2, + } _dll_state; + /** * Set the DLL item */ @@ -77,18 +93,5 @@ private: */ void advance_dll(); - enum DLLState { - DLL_STATE_NONE = 0, - DLL_STATE_FLYTOCOMMSHOLDWP = 1, - DLL_STATE_FLYTOAIRFIELDHOMEWP = 2, - } _dll_state; - - control::BlockParamFloat _param_commsholdwaittime; - control::BlockParamInt _param_commsholdlat; // * 1e7 - control::BlockParamInt _param_commsholdlon; // * 1e7 - control::BlockParamFloat _param_commsholdalt; - control::BlockParamInt _param_airfieldhomelat; // * 1e7 - control::BlockParamInt _param_airfieldhomelon; // * 1e7 - control::BlockParamFloat _param_airfieldhomealt; }; #endif diff --git a/src/modules/navigator/datalinkloss_params.c b/src/modules/navigator/datalinkloss_params.c index a836fc8ca..038c80a1a 100644 --- a/src/modules/navigator/datalinkloss_params.c +++ b/src/modules/navigator/datalinkloss_params.c @@ -123,3 +123,14 @@ PARAM_DEFINE_INT32(NAV_DLL_AH_LON, 1518423250); * @group DLL */ PARAM_DEFINE_FLOAT(NAV_DLL_AH_ALT, 600.0f); + +/** + * Number of allowed Datalink timeouts + * + * After more than this number of data link timeouts the aircraft returns home directly + * + * @group commander + * @min 0 + * @max 1000 + */ +PARAM_DEFINE_INT32(NAV_DLL_N, 2); -- 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(-) 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 034d0a6a610c8838d6c43b51a4401ce818b77624 Mon Sep 17 00:00:00 2001 From: Luis Rodrigues Date: Wed, 23 Jul 2014 18:19:49 +0200 Subject: driver for the TeraRangerOne I2C ranger finder --- makefiles/config_px4fmu-v1_default.mk | 1 + makefiles/config_px4fmu-v2_default.mk | 1 + src/drivers/trone/module.mk | 40 ++ src/drivers/trone/trone.cpp | 913 ++++++++++++++++++++++++++++++++++ 4 files changed, 955 insertions(+) create mode 100644 src/drivers/trone/module.mk create mode 100644 src/drivers/trone/trone.cpp diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk index cc0958c29..70c6f7df5 100644 --- a/makefiles/config_px4fmu-v1_default.mk +++ b/makefiles/config_px4fmu-v1_default.mk @@ -26,6 +26,7 @@ MODULES += drivers/hmc5883 MODULES += drivers/ms5611 MODULES += drivers/mb12xx MODULES += drivers/ll40ls +MODULES += drivers/trone MODULES += drivers/gps MODULES += drivers/hil MODULES += drivers/hott/hott_telemetry diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index a498a1b40..2fcdd6b39 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -29,6 +29,7 @@ MODULES += drivers/ms5611 MODULES += drivers/mb12xx MODULES += drivers/sf0x MODULES += drivers/ll40ls +MODULES += drivers/trone MODULES += drivers/gps MODULES += drivers/hil MODULES += drivers/hott/hott_telemetry diff --git a/src/drivers/trone/module.mk b/src/drivers/trone/module.mk new file mode 100644 index 000000000..d8edf17cc --- /dev/null +++ b/src/drivers/trone/module.mk @@ -0,0 +1,40 @@ +############################################################################ +# +# Copyright (c) 2013 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +# +# Makefile to build the TeraRanger One range finder driver +# + +MODULE_COMMAND = trone + +SRCS = trone.cpp diff --git a/src/drivers/trone/trone.cpp b/src/drivers/trone/trone.cpp new file mode 100644 index 000000000..2f2f692a1 --- /dev/null +++ b/src/drivers/trone/trone.cpp @@ -0,0 +1,913 @@ +/**************************************************************************** + * + * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file trone.cpp + * @author Luis Rodrigues + * + * Driver for the TeraRanger One range finders connected via I2C. + */ + +#include + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include + +#include +#include +#include + +#include +#include + +#include + +/* Configuration Constants */ +#define TRONE_BUS PX4_I2C_BUS_EXPANSION +#define TRONE_BASEADDR 0x30 /* 7-bit address */ +#define TRONE_DEVICE_PATH "/dev/trone" + +/* TRONE Registers addresses */ + +#define TRONE_MEASURE_REG 0x00 /* Measure range register */ + +/* Device limits */ +#define TRONE_MIN_DISTANCE (0.20f) +#define TRONE_MAX_DISTANCE (14.00f) + +#define TRONE_CONVERSION_INTERVAL 50000 /* 50ms */ + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; + +#ifndef CONFIG_SCHED_WORKQUEUE +# error This requires CONFIG_SCHED_WORKQUEUE. +#endif + +class TRONE : public device::I2C +{ +public: + TRONE(int bus = TRONE_BUS, int address = TRONE_BASEADDR); + virtual ~TRONE(); + + virtual int init(); + + virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); + virtual int ioctl(struct file *filp, int cmd, unsigned long arg); + + /** + * Diagnostics - print some basic information about the driver. + */ + void print_info(); + +protected: + virtual int probe(); + +private: + float _min_distance; + float _max_distance; + work_s _work; + RingBuffer *_reports; + bool _sensor_ok; + int _measure_ticks; + bool _collect_phase; + int _class_instance; + + orb_advert_t _range_finder_topic; + + perf_counter_t _sample_perf; + perf_counter_t _comms_errors; + perf_counter_t _buffer_overflows; + + /** + * Test whether the device supported by the driver is present at a + * specific address. + * + * @param address The I2C bus address to probe. + * @return True if the device is present. + */ + int probe_address(uint8_t address); + + /** + * Initialise the automatic measurement state machine and start it. + * + * @note This function is called at open and error time. It might make sense + * to make it more aggressive about resetting the bus in case of errors. + */ + void start(); + + /** + * Stop the automatic measurement state machine. + */ + void stop(); + + /** + * Set the min and max distance thresholds if you want the end points of the sensors + * range to be brought in at all, otherwise it will use the defaults TRONE_MIN_DISTANCE + * and TRONE_MAX_DISTANCE + */ + void set_minimum_distance(float min); + void set_maximum_distance(float max); + float get_minimum_distance(); + float get_maximum_distance(); + + /** + * Perform a poll cycle; collect from the previous measurement + * and start a new one. + */ + void cycle(); + int measure(); + int collect(); + /** + * Static trampoline from the workq context; because we don't have a + * generic workq wrapper yet. + * + * @param arg Instance pointer for the driver that is polling. + */ + static void cycle_trampoline(void *arg); + + +}; + +static const uint8_t crc_table[] = { + 0x00, 0x07, 0x0e, 0x09, 0x1c, 0x1b, 0x12, 0x15, 0x38, 0x3f, 0x36, 0x31, + 0x24, 0x23, 0x2a, 0x2d, 0x70, 0x77, 0x7e, 0x79, 0x6c, 0x6b, 0x62, 0x65, + 0x48, 0x4f, 0x46, 0x41, 0x54, 0x53, 0x5a, 0x5d, 0xe0, 0xe7, 0xee, 0xe9, + 0xfc, 0xfb, 0xf2, 0xf5, 0xd8, 0xdf, 0xd6, 0xd1, 0xc4, 0xc3, 0xca, 0xcd, + 0x90, 0x97, 0x9e, 0x99, 0x8c, 0x8b, 0x82, 0x85, 0xa8, 0xaf, 0xa6, 0xa1, + 0xb4, 0xb3, 0xba, 0xbd, 0xc7, 0xc0, 0xc9, 0xce, 0xdb, 0xdc, 0xd5, 0xd2, + 0xff, 0xf8, 0xf1, 0xf6, 0xe3, 0xe4, 0xed, 0xea, 0xb7, 0xb0, 0xb9, 0xbe, + 0xab, 0xac, 0xa5, 0xa2, 0x8f, 0x88, 0x81, 0x86, 0x93, 0x94, 0x9d, 0x9a, + 0x27, 0x20, 0x29, 0x2e, 0x3b, 0x3c, 0x35, 0x32, 0x1f, 0x18, 0x11, 0x16, + 0x03, 0x04, 0x0d, 0x0a, 0x57, 0x50, 0x59, 0x5e, 0x4b, 0x4c, 0x45, 0x42, + 0x6f, 0x68, 0x61, 0x66, 0x73, 0x74, 0x7d, 0x7a, 0x89, 0x8e, 0x87, 0x80, + 0x95, 0x92, 0x9b, 0x9c, 0xb1, 0xb6, 0xbf, 0xb8, 0xad, 0xaa, 0xa3, 0xa4, + 0xf9, 0xfe, 0xf7, 0xf0, 0xe5, 0xe2, 0xeb, 0xec, 0xc1, 0xc6, 0xcf, 0xc8, + 0xdd, 0xda, 0xd3, 0xd4, 0x69, 0x6e, 0x67, 0x60, 0x75, 0x72, 0x7b, 0x7c, + 0x51, 0x56, 0x5f, 0x58, 0x4d, 0x4a, 0x43, 0x44, 0x19, 0x1e, 0x17, 0x10, + 0x05, 0x02, 0x0b, 0x0c, 0x21, 0x26, 0x2f, 0x28, 0x3d, 0x3a, 0x33, 0x34, + 0x4e, 0x49, 0x40, 0x47, 0x52, 0x55, 0x5c, 0x5b, 0x76, 0x71, 0x78, 0x7f, + 0x6a, 0x6d, 0x64, 0x63, 0x3e, 0x39, 0x30, 0x37, 0x22, 0x25, 0x2c, 0x2b, + 0x06, 0x01, 0x08, 0x0f, 0x1a, 0x1d, 0x14, 0x13, 0xae, 0xa9, 0xa0, 0xa7, + 0xb2, 0xb5, 0xbc, 0xbb, 0x96, 0x91, 0x98, 0x9f, 0x8a, 0x8d, 0x84, 0x83, + 0xde, 0xd9, 0xd0, 0xd7, 0xc2, 0xc5, 0xcc, 0xcb, 0xe6, 0xe1, 0xe8, 0xef, + 0xfa, 0xfd, 0xf4, 0xf3 +}; + +uint8_t crc8(uint8_t *p, uint8_t len){ + uint16_t i; + uint16_t crc = 0x0; + + while (len--) { + i = (crc ^ *p++) & 0xFF; + crc = (crc_table[i] ^ (crc << 8)) & 0xFF; + } + + return crc & 0xFF; +} + +/* + * Driver 'main' command. + */ +extern "C" __EXPORT int trone_main(int argc, char *argv[]); + +TRONE::TRONE(int bus, int address) : + I2C("TRONE", TRONE_DEVICE_PATH, bus, address, 100000), + _min_distance(TRONE_MIN_DISTANCE), + _max_distance(TRONE_MAX_DISTANCE), + _reports(nullptr), + _sensor_ok(false), + _measure_ticks(0), + _collect_phase(false), + _class_instance(-1), + _range_finder_topic(-1), + _sample_perf(perf_alloc(PC_ELAPSED, "trone_read")), + _comms_errors(perf_alloc(PC_COUNT, "trone_comms_errors")), + _buffer_overflows(perf_alloc(PC_COUNT, "trone_buffer_overflows")) +{ + // up the retries since the device misses the first measure attempts + I2C::_retries = 3; + + // enable debug() calls + _debug_enabled = false; + + // work_cancel in the dtor will explode if we don't do this... + memset(&_work, 0, sizeof(_work)); +} + +TRONE::~TRONE() +{ + /* make sure we are truly inactive */ + stop(); + + /* free any existing reports */ + if (_reports != nullptr) { + delete _reports; + } + + if (_class_instance != -1) { + unregister_class_devname(RANGE_FINDER_DEVICE_PATH, _class_instance); + } + + // free perf counters + perf_free(_sample_perf); + perf_free(_comms_errors); + perf_free(_buffer_overflows); +} + +int +TRONE::init() +{ + int ret = ERROR; + + /* do I2C init (and probe) first */ + if (I2C::init() != OK) { + goto out; + } + + /* allocate basic report buffers */ + _reports = new RingBuffer(2, sizeof(range_finder_report)); + + if (_reports == nullptr) { + goto out; + } + + _class_instance = register_class_devname(RANGE_FINDER_DEVICE_PATH); + + if (_class_instance == CLASS_DEVICE_PRIMARY) { + /* get a publish handle on the range finder topic */ + struct range_finder_report rf_report; + measure(); + _reports->get(&rf_report); + _range_finder_topic = orb_advertise(ORB_ID(sensor_range_finder), &rf_report); + + if (_range_finder_topic < 0) { + debug("failed to create sensor_range_finder object. Did you start uOrb?"); + } + } + + ret = OK; + /* sensor is ok, but we don't really know if it is within range */ + _sensor_ok = true; +out: + return ret; +} + +int +TRONE::probe() +{ + return measure(); +} + +void +TRONE::set_minimum_distance(float min) +{ + _min_distance = min; +} + +void +TRONE::set_maximum_distance(float max) +{ + _max_distance = max; +} + +float +TRONE::get_minimum_distance() +{ + return _min_distance; +} + +float +TRONE::get_maximum_distance() +{ + return _max_distance; +} + +int +TRONE::ioctl(struct file *filp, int cmd, unsigned long arg) +{ + switch (cmd) { + + case SENSORIOCSPOLLRATE: { + switch (arg) { + + /* switching to manual polling */ + case SENSOR_POLLRATE_MANUAL: + stop(); + _measure_ticks = 0; + return OK; + + /* external signalling (DRDY) not supported */ + case SENSOR_POLLRATE_EXTERNAL: + + /* zero would be bad */ + case 0: + return -EINVAL; + + /* set default/max polling rate */ + case SENSOR_POLLRATE_MAX: + case SENSOR_POLLRATE_DEFAULT: { + /* do we need to start internal polling? */ + bool want_start = (_measure_ticks == 0); + + /* set interval for next measurement to minimum legal value */ + _measure_ticks = USEC2TICK(TRONE_CONVERSION_INTERVAL); + + /* if we need to start the poll state machine, do it */ + if (want_start) { + start(); + } + + return OK; + } + + /* adjust to a legal polling interval in Hz */ + default: { + /* do we need to start internal polling? */ + bool want_start = (_measure_ticks == 0); + + /* convert hz to tick interval via microseconds */ + unsigned ticks = USEC2TICK(1000000 / arg); + + /* check against maximum rate */ + if (ticks < USEC2TICK(TRONE_CONVERSION_INTERVAL)) { + return -EINVAL; + } + + /* update interval for next measurement */ + _measure_ticks = ticks; + + /* if we need to start the poll state machine, do it */ + if (want_start) { + start(); + } + + return OK; + } + } + } + + case SENSORIOCGPOLLRATE: + if (_measure_ticks == 0) { + return SENSOR_POLLRATE_MANUAL; + } + + return (1000 / _measure_ticks); + + case SENSORIOCSQUEUEDEPTH: { + /* lower bound is mandatory, upper bound is a sanity check */ + if ((arg < 1) || (arg > 100)) { + return -EINVAL; + } + + irqstate_t flags = irqsave(); + + if (!_reports->resize(arg)) { + irqrestore(flags); + return -ENOMEM; + } + + irqrestore(flags); + + return OK; + } + + case SENSORIOCGQUEUEDEPTH: + return _reports->size(); + + case SENSORIOCRESET: + /* XXX implement this */ + return -EINVAL; + + case RANGEFINDERIOCSETMINIUMDISTANCE: { + set_minimum_distance(*(float *)arg); + return 0; + } + break; + + case RANGEFINDERIOCSETMAXIUMDISTANCE: { + set_maximum_distance(*(float *)arg); + return 0; + } + break; + + default: + /* give it to the superclass */ + return I2C::ioctl(filp, cmd, arg); + } +} + +ssize_t +TRONE::read(struct file *filp, char *buffer, size_t buflen) +{ + unsigned count = buflen / sizeof(struct range_finder_report); + struct range_finder_report *rbuf = reinterpret_cast(buffer); + int ret = 0; + + /* buffer must be large enough */ + if (count < 1) { + return -ENOSPC; + } + + /* if automatic measurement is enabled */ + if (_measure_ticks > 0) { + + /* + * While there is space in the caller's buffer, and reports, copy them. + * Note that we may be pre-empted by the workq thread while we are doing this; + * we are careful to avoid racing with them. + */ + while (count--) { + if (_reports->get(rbuf)) { + ret += sizeof(*rbuf); + rbuf++; + } + } + + /* if there was no data, warn the caller */ + return ret ? ret : -EAGAIN; + } + + /* manual measurement - run one conversion */ + do { + _reports->flush(); + + /* trigger a measurement */ + if (OK != measure()) { + ret = -EIO; + break; + } + + /* wait for it to complete */ + usleep(TRONE_CONVERSION_INTERVAL); + + /* run the collection phase */ + if (OK != collect()) { + ret = -EIO; + break; + } + + /* state machine will have generated a report, copy it out */ + if (_reports->get(rbuf)) { + ret = sizeof(*rbuf); + } + + } while (0); + + return ret; +} + +int +TRONE::measure() +{ + int ret; + + /* + * Send the command to begin a measurement. + */ + const uint8_t cmd = TRONE_MEASURE_REG; + ret = transfer(&cmd, sizeof(cmd), nullptr, 0); + + if (OK != ret) { + perf_count(_comms_errors); + log("i2c::transfer returned %d", ret); + return ret; + } + + ret = OK; + + return ret; +} + +int +TRONE::collect() +{ + int ret = -EIO; + + /* read from the sensor */ + uint8_t val[3] = {0, 0, 0}; + + perf_begin(_sample_perf); + + ret = transfer(nullptr, 0, &val[0], 3); + + if (ret < 0) { + log("error reading from sensor: %d", ret); + perf_count(_comms_errors); + perf_end(_sample_perf); + return ret; + } + + uint16_t distance = (val[0] << 8) | val[1]; + float si_units = distance * 0.001f; /* mm to m */ + struct range_finder_report report; + + /* this should be fairly close to the end of the measurement, so the best approximation of the time */ + report.timestamp = hrt_absolute_time(); + report.error_count = perf_event_count(_comms_errors); + report.distance = si_units; + report.valid = crc8(val, 2) == val[2] && si_units > get_minimum_distance() && si_units < get_maximum_distance() ? 1 : 0; + + + /* publish it, if we are the primary */ + if (_range_finder_topic >= 0) { + orb_publish(ORB_ID(sensor_range_finder), _range_finder_topic, &report); + } + + if (_reports->force(&report)) { + perf_count(_buffer_overflows); + } + + /* notify anyone waiting for data */ + poll_notify(POLLIN); + + ret = OK; + + perf_end(_sample_perf); + return ret; +} + +void +TRONE::start() +{ + /* reset the report ring and state machine */ + _collect_phase = false; + _reports->flush(); + + /* schedule a cycle to start things */ + work_queue(HPWORK, &_work, (worker_t)&TRONE::cycle_trampoline, this, 1); + + /* notify about state change */ + struct subsystem_info_s info = { + true, + true, + true, + SUBSYSTEM_TYPE_RANGEFINDER + }; + static orb_advert_t pub = -1; + + if (pub > 0) { + orb_publish(ORB_ID(subsystem_info), pub, &info); + + } else { + pub = orb_advertise(ORB_ID(subsystem_info), &info); + } +} + +void +TRONE::stop() +{ + work_cancel(HPWORK, &_work); +} + +void +TRONE::cycle_trampoline(void *arg) +{ + TRONE *dev = (TRONE *)arg; + + dev->cycle(); +} + +void +TRONE::cycle() +{ + /* collection phase? */ + if (_collect_phase) { + + /* perform collection */ + if (OK != collect()) { + log("collection error"); + /* restart the measurement state machine */ + start(); + return; + } + + /* next phase is measurement */ + _collect_phase = false; + + /* + * Is there a collect->measure gap? + */ + if (_measure_ticks > USEC2TICK(TRONE_CONVERSION_INTERVAL)) { + + /* schedule a fresh cycle call when we are ready to measure again */ + work_queue(HPWORK, + &_work, + (worker_t)&TRONE::cycle_trampoline, + this, + _measure_ticks - USEC2TICK(TRONE_CONVERSION_INTERVAL)); + + return; + } + } + + /* measurement phase */ + if (OK != measure()) { + log("measure error"); + } + + /* next phase is collection */ + _collect_phase = true; + + /* schedule a fresh cycle call when the measurement is done */ + work_queue(HPWORK, + &_work, + (worker_t)&TRONE::cycle_trampoline, + this, + USEC2TICK(TRONE_CONVERSION_INTERVAL)); +} + +void +TRONE::print_info() +{ + perf_print_counter(_sample_perf); + perf_print_counter(_comms_errors); + perf_print_counter(_buffer_overflows); + printf("poll interval: %u ticks\n", _measure_ticks); + _reports->print_info("report queue"); +} + +/** + * Local functions in support of the shell command. + */ +namespace trone +{ + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +const int ERROR = -1; + +TRONE *g_dev; + +void start(); +void stop(); +void test(); +void reset(); +void info(); + +/** + * Start the driver. + */ +void +start() +{ + int fd; + + if (g_dev != nullptr) { + errx(1, "already started"); + } + + /* create the driver */ + g_dev = new TRONE(TRONE_BUS); + + + if (g_dev == nullptr) { + goto fail; + } + + if (OK != g_dev->init()) { + goto fail; + } + + /* set the poll rate to default, starts automatic data collection */ + fd = open(TRONE_DEVICE_PATH, O_RDONLY); + + if (fd < 0) { + goto fail; + } + + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { + goto fail; + } + + exit(0); + +fail: + + if (g_dev != nullptr) { + delete g_dev; + g_dev = nullptr; + } + + errx(1, "driver start failed"); +} + +/** + * Stop the driver + */ +void stop() +{ + if (g_dev != nullptr) { + delete g_dev; + g_dev = nullptr; + + } else { + errx(1, "driver not running"); + } + + exit(0); +} + +/** + * Perform some basic functional tests on the driver; + * make sure we can collect data from the sensor in polled + * and automatic modes. + */ +void +test() +{ + struct range_finder_report report; + ssize_t sz; + int ret; + + int fd = open(TRONE_DEVICE_PATH, O_RDONLY); + + if (fd < 0) { + err(1, "%s open failed (try 'trone start' if the driver is not running", TRONE_DEVICE_PATH); + } + + /* do a simple demand read */ + sz = read(fd, &report, sizeof(report)); + + if (sz != sizeof(report)) { + err(1, "immediate read failed"); + } + + warnx("single read"); + warnx("measurement: %0.2f m", (double)report.distance); + warnx("time: %lld", report.timestamp); + + /* start the sensor polling at 2Hz */ + if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) { + errx(1, "failed to set 2Hz poll rate"); + } + + /* read the sensor 50x and report each value */ + for (unsigned i = 0; i < 50; i++) { + struct pollfd fds; + + /* wait for data to be ready */ + fds.fd = fd; + fds.events = POLLIN; + ret = poll(&fds, 1, 2000); + + if (ret != 1) { + errx(1, "timed out waiting for sensor data"); + } + + /* now go get it */ + sz = read(fd, &report, sizeof(report)); + + if (sz != sizeof(report)) { + err(1, "periodic read failed"); + } + + warnx("periodic read %u", i); + warnx("valid %u", report.valid); + warnx("measurement: %0.3f", (double)report.distance); + warnx("time: %lld", report.timestamp); + } + + /* reset the sensor polling to default rate */ + if (OK != ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT)) { + errx(1, "failed to set default poll rate"); + } + + errx(0, "PASS"); +} + +/** + * Reset the driver. + */ +void +reset() +{ + int fd = open(TRONE_DEVICE_PATH, O_RDONLY); + + if (fd < 0) { + err(1, "failed "); + } + + if (ioctl(fd, SENSORIOCRESET, 0) < 0) { + err(1, "driver reset failed"); + } + + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { + err(1, "driver poll restart failed"); + } + + exit(0); +} + +/** + * Print a little info about the driver. + */ +void +info() +{ + if (g_dev == nullptr) { + errx(1, "driver not running"); + } + + printf("state @ %p\n", g_dev); + g_dev->print_info(); + + exit(0); +} + +} // namespace + +int +trone_main(int argc, char *argv[]) +{ + /* + * Start/load the driver. + */ + if (!strcmp(argv[1], "start")) { + trone::start(); + } + + /* + * Stop the driver + */ + if (!strcmp(argv[1], "stop")) { + trone::stop(); + } + + /* + * Test the driver/device. + */ + if (!strcmp(argv[1], "test")) { + trone::test(); + } + + /* + * Reset the driver. + */ + if (!strcmp(argv[1], "reset")) { + trone::reset(); + } + + /* + * Print driver information. + */ + if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status")) { + trone::info(); + } + + errx(1, "unrecognized command, try 'start', 'test', 'reset' or 'info'"); +} -- cgit v1.2.3 From 24f380137ecb91fb9647e22e1d29c13da5fc0357 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 23 Jul 2014 22:58:19 +0200 Subject: add method to block fallback to mission failsafe navigation modes can use a flag in mission_result to tell the commander to not switch back to mission --- src/modules/commander/commander.cpp | 5 ++-- src/modules/commander/state_machine_helper.cpp | 3 ++- src/modules/commander/state_machine_helper.h | 2 +- src/modules/navigator/datalinkloss.cpp | 2 ++ src/modules/navigator/mission.cpp | 32 ++++++-------------------- src/modules/navigator/mission.h | 8 ------- src/modules/navigator/navigator.h | 13 ++++++++++- src/modules/navigator/navigator_main.cpp | 22 ++++++++++++++++++ src/modules/navigator/navigator_mode.cpp | 3 +++ src/modules/uORB/topics/mission_result.h | 1 + 10 files changed, 53 insertions(+), 38 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 5c89e0123..cb09a68e3 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1461,7 +1461,7 @@ int commander_thread_main(int argc, char *argv[]) for (int i = 0; i < TELEMETRY_STATUS_ORB_ID_NUM; i++) { if (telemetry_last_heartbeat[i] != 0 && hrt_elapsed_time(&telemetry_last_heartbeat[i]) < datalink_loss_timeout) { /* handle the case where data link was regained */ - if (telemetry_lost[i]) { + if (telemetry_lost[i]) {//XXX also add hysteresis here mavlink_log_critical(mavlink_fd, "data link %i regained", i); telemetry_lost[i] = false; } @@ -1545,7 +1545,8 @@ int commander_thread_main(int argc, char *argv[]) /* now set navigation state according to failsafe and main state */ bool nav_state_changed = set_nav_state(&status, (bool)datalink_loss_enabled, - mission_result.finished); + mission_result.finished, + mission_result.stay_in_failsafe); // TODO handle mode changes by commands if (main_state_changed) { diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 7b26e3e8c..4e1cfb987 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -435,7 +435,8 @@ transition_result_t hil_state_transition(hil_state_t new_state, int status_pub, /** * Check failsafe and main status and set navigation status for navigator accordingly */ -bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_enabled, const bool mission_finished) +bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_enabled, const bool mission_finished, + const bool stay_in_failsafe) { navigation_state_t nav_state_old = status->nav_state; diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h index bb1b87e71..4285d2977 100644 --- a/src/modules/commander/state_machine_helper.h +++ b/src/modules/commander/state_machine_helper.h @@ -63,7 +63,7 @@ transition_result_t main_state_transition(struct vehicle_status_s *current_state transition_result_t hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_state, const int mavlink_fd); -bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_enabled, const bool mission_finished); +bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_enabled, const bool mission_finished, const bool stay_in_failsafe); int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd); diff --git a/src/modules/navigator/datalinkloss.cpp b/src/modules/navigator/datalinkloss.cpp index a98e21139..52f263aff 100644 --- a/src/modules/navigator/datalinkloss.cpp +++ b/src/modules/navigator/datalinkloss.cpp @@ -200,6 +200,8 @@ DataLinkLoss::advance_dll() case DLL_STATE_FLYTOCOMMSHOLDWP: //XXX check here if time is over are over _dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP; + _navigator->get_mission_result()->stay_in_failsafe = true; + _navigator->publish_mission_result(); break; default: break; diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index ba766cd10..7ce0e2f89 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -68,8 +68,6 @@ Mission::Mission(Navigator *navigator, const char *name) : _current_offboard_mission_index(-1), _need_takeoff(true), _takeoff(false), - _mission_result_pub(-1), - _mission_result({0}), _mission_type(MISSION_TYPE_NONE), _inited(false), _dist_1wp_ok(false) @@ -577,18 +575,18 @@ void Mission::report_mission_item_reached() { if (_mission_type == MISSION_TYPE_OFFBOARD) { - _mission_result.reached = true; - _mission_result.seq_reached = _current_offboard_mission_index; + _navigator->get_mission_result()->reached = true; + _navigator->get_mission_result()->seq_reached = _current_offboard_mission_index; } - publish_mission_result(); + _navigator->publish_mission_result(); } void Mission::report_current_offboard_mission_item() { warnx("current offboard mission index: %d", _current_offboard_mission_index); - _mission_result.seq_current = _current_offboard_mission_index; - publish_mission_result(); + _navigator->get_mission_result()->seq_current = _current_offboard_mission_index; + _navigator->publish_mission_result(); save_offboard_mission_state(); } @@ -596,23 +594,7 @@ Mission::report_current_offboard_mission_item() void Mission::report_mission_finished() { - _mission_result.finished = true; - publish_mission_result(); + _navigator->get_mission_result()->finished = true; + _navigator->publish_mission_result(); } -void -Mission::publish_mission_result() -{ - /* lazily publish the mission result only once available */ - if (_mission_result_pub > 0) { - /* publish mission result */ - orb_publish(ORB_ID(mission_result), _mission_result_pub, &_mission_result); - - } else { - /* advertise and publish */ - _mission_result_pub = orb_advertise(ORB_ID(mission_result), &_mission_result); - } - /* reset reached bool */ - _mission_result.reached = false; - _mission_result.finished = false; -} diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h index 4da6a1155..1b8c8c874 100644 --- a/src/modules/navigator/mission.h +++ b/src/modules/navigator/mission.h @@ -128,11 +128,6 @@ private: */ void report_mission_finished(); - /** - * Publish the mission result so commander and mavlink know what is going on - */ - void publish_mission_result(); - control::BlockParamInt _param_onboard_enabled; control::BlockParamFloat _param_takeoff_alt; control::BlockParamFloat _param_dist_1wp; @@ -145,9 +140,6 @@ private: bool _need_takeoff; bool _takeoff; - orb_advert_t _mission_result_pub; - struct mission_result_s _mission_result; - enum { MISSION_TYPE_NONE, MISSION_TYPE_ONBOARD, diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index d0b2ed841..363877bb8 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -52,6 +52,7 @@ #include #include #include +#include #include "navigator_mode.h" #include "mission.h" @@ -102,6 +103,11 @@ public: */ void load_fence_from_file(const char *filename); + /** + * Publish the mission result so commander and mavlink know what is going on + */ + void publish_mission_result(); + /** * Setters */ @@ -115,7 +121,9 @@ public: struct vehicle_control_mode_s* get_control_mode() { return &_control_mode; } struct vehicle_global_position_s* get_global_position() { return &_global_pos; } struct home_position_s* get_home_position() { return &_home_pos; } - struct position_setpoint_triplet_s* get_position_setpoint_triplet() { return &_pos_sp_triplet; } + struct position_setpoint_triplet_s* get_position_setpoint_triplet() { return &_pos_sp_triplet; } + struct mission_result_s* get_mission_result() { return &_mission_result; } + int get_onboard_mission_sub() { return _onboard_mission_sub; } int get_offboard_mission_sub() { return _offboard_mission_sub; } int get_offboard_control_sp_sub() { return _offboard_control_sp_sub; } @@ -143,6 +151,7 @@ private: int _param_update_sub; /**< param update subscription */ orb_advert_t _pos_sp_triplet_pub; /**< publish position setpoint triplet */ + orb_advert_t _mission_result_pub; vehicle_status_s _vstatus; /**< vehicle status */ vehicle_control_mode_s _control_mode; /**< vehicle control mode */ @@ -152,6 +161,8 @@ private: navigation_capabilities_s _nav_caps; /**< navigation capabilities */ position_setpoint_triplet_s _pos_sp_triplet; /**< triplet of position setpoints */ + mission_result_s _mission_result; + bool _mission_item_valid; /**< flags if the current mission item is valid */ perf_counter_t _loop_perf; /**< loop performance counter */ diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 1ce6770c9..77124e8f6 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -108,6 +108,7 @@ Navigator::Navigator() : _offboard_mission_sub(-1), _param_update_sub(-1), _pos_sp_triplet_pub(-1), + _mission_result_pub(-1), _vstatus{}, _control_mode{}, _global_pos{}, @@ -115,6 +116,7 @@ Navigator::Navigator() : _mission_item{}, _nav_caps{}, _pos_sp_triplet{}, + _mission_result{}, _mission_item_valid(false), _loop_perf(perf_alloc(PC_ELAPSED, "navigator")), _geofence{}, @@ -370,6 +372,9 @@ Navigator::task_main() _can_loiter_at_sp = false; break; case NAVIGATION_STATE_AUTO_MISSION: + /* Some failsafe modes prohibit the fallback to mission + * usually this is done after some time to make sure + * that the full failsafe operation is performed */ _navigation_mode = &_mission; break; case NAVIGATION_STATE_AUTO_LOITER: @@ -553,3 +558,20 @@ int navigator_main(int argc, char *argv[]) return 0; } + +void +Navigator::publish_mission_result() +{ + /* lazily publish the mission result only once available */ + if (_mission_result_pub > 0) { + /* publish mission result */ + orb_publish(ORB_ID(mission_result), _mission_result_pub, &_mission_result); + + } else { + /* advertise and publish */ + _mission_result_pub = orb_advertise(ORB_ID(mission_result), &_mission_result); + } + /* reset reached bool */ + _mission_result.reached = false; + _mission_result.finished = false; +} diff --git a/src/modules/navigator/navigator_mode.cpp b/src/modules/navigator/navigator_mode.cpp index f43215665..3c6754c55 100644 --- a/src/modules/navigator/navigator_mode.cpp +++ b/src/modules/navigator/navigator_mode.cpp @@ -63,6 +63,9 @@ NavigatorMode::run(bool active) { if (_first_run) { /* first run */ _first_run = false; + /* Reset stay in failsafe flag */ + _navigator->get_mission_result()->stay_in_failsafe = true; + _navigator->publish_mission_result(); on_activation(); } else { diff --git a/src/modules/uORB/topics/mission_result.h b/src/modules/uORB/topics/mission_result.h index beb797e62..65ddfb4ad 100644 --- a/src/modules/uORB/topics/mission_result.h +++ b/src/modules/uORB/topics/mission_result.h @@ -57,6 +57,7 @@ struct mission_result_s unsigned seq_current; /**< Sequence of the current mission item */ bool reached; /**< true if mission has been reached */ bool finished; /**< true if mission has been completed */ + bool stay_in_failsafe; /**< true if the commander should not switch out of the failsafe mode*/ }; /** -- cgit v1.2.3 From 756ea3019ec3a50623d846c019c6474488273a34 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 23 Jul 2014 23:00:34 +0200 Subject: datalink loss: fix type error --- src/modules/navigator/datalinkloss.cpp | 24 ++---------------------- 1 file changed, 2 insertions(+), 22 deletions(-) diff --git a/src/modules/navigator/datalinkloss.cpp b/src/modules/navigator/datalinkloss.cpp index 52f263aff..91bb5b110 100644 --- a/src/modules/navigator/datalinkloss.cpp +++ b/src/modules/navigator/datalinkloss.cpp @@ -90,26 +90,6 @@ DataLinkLoss::on_inactive() void DataLinkLoss::on_activation() { - /* decide where to enter the RTL procedure when we switch into it */ - //if (_rtl_state == RTL_STATE_NONE) { - //[> for safety reasons don't go into RTL if landed <] - //if (_navigator->get_vstatus()->condition_landed) { - //_rtl_state = RTL_STATE_LANDED; - //mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: no RTL when landed"); - - //[> if lower than return altitude, climb up first <] - //} else if (_navigator->get_global_position()->alt < _navigator->get_home_position()->alt - //+ _param_return_alt.get()) { - //_rtl_state = RTL_STATE_CLIMB; - - //[> otherwise go straight to return <] - //} else { - //[> set altitude setpoint to current altitude <] - //_rtl_state = RTL_STATE_RETURN; - //_mission_item.altitude_is_relative = false; - //_mission_item.altitude = _navigator->get_global_position()->alt; - //} - //} _dll_state = DLL_STATE_FLYTOCOMMSHOLDWP; set_dll_item(); } @@ -139,7 +119,7 @@ DataLinkLoss::set_dll_item() _mission_item.lat = (double)(_param_commsholdlat.get()) * 1.0e-7; _mission_item.lon = (double)(_param_commsholdlon.get()) * 1.0e-7; _mission_item.altitude_is_relative = false; - _mission_item.altitude = (double)(_param_commsholdalt.get()); + _mission_item.altitude = _param_commsholdalt.get(); _mission_item.yaw = NAN; _mission_item.loiter_radius = _navigator->get_loiter_radius(); _mission_item.loiter_direction = 1; @@ -157,7 +137,7 @@ DataLinkLoss::set_dll_item() _mission_item.lat = (double)(_param_airfieldhomelat.get()) * 1.0e-7; _mission_item.lon = (double)(_param_airfieldhomelon.get()) * 1.0e-7; _mission_item.altitude_is_relative = false; - _mission_item.altitude = (double)(_param_airfieldhomealt.get()); + _mission_item.altitude = _param_airfieldhomealt.get(); _mission_item.yaw = NAN; _mission_item.loiter_radius = _navigator->get_loiter_radius(); _mission_item.loiter_direction = 1; -- cgit v1.2.3 From f10d1277a5e795256a247cb6f130cefba12c5ffc Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 23 Jul 2014 23:07:11 +0200 Subject: navigator: fix comment --- src/modules/navigator/navigator.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index 363877bb8..fc0d47b38 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -178,7 +178,7 @@ private: Loiter _loiter; /**< class that handles loiter */ RTL _rtl; /**< class that handles RTL */ Offboard _offboard; /**< class that handles offboard */ - DataLinkLoss _dataLinkLoss; /**< class that handles offboard */ + DataLinkLoss _dataLinkLoss; /**< class that handles the OBC datalink loss mode */ NavigatorMode *_navigation_mode_array[NAVIGATOR_MODE_ARRAY_SIZE]; /**< array of navigation modes */ -- cgit v1.2.3 From a35814d15b1317f73f325e98f0500f5fd1233583 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 24 Jul 2014 08:57:58 +0200 Subject: dl loss: correct timeout, add hysteresis also for regain --- src/modules/commander/commander.cpp | 21 ++++++++++++++++----- src/modules/commander/commander_params.c | 18 +++++++++++++++--- 2 files changed, 31 insertions(+), 8 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index cb09a68e3..fe1974c88 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -650,6 +650,7 @@ int commander_thread_main(int argc, char *argv[]) param_t _param_enable_parachute = param_find("NAV_PARACHUTE_EN"); param_t _param_enable_datalink_loss = param_find("COM_DL_LOSS_EN"); param_t _param_datalink_loss_timeout = param_find("COM_DL_LOSS_T"); + param_t _param_datalink_regain_timeout = param_find("COM_DL_REG_T"); /* welcome user */ warnx("starting"); @@ -841,11 +842,13 @@ int commander_thread_main(int argc, char *argv[]) /* Subscribe to telemetry status topics */ int telemetry_subs[TELEMETRY_STATUS_ORB_ID_NUM]; uint64_t telemetry_last_heartbeat[TELEMETRY_STATUS_ORB_ID_NUM]; + uint64_t telemetry_last_dl_loss[TELEMETRY_STATUS_ORB_ID_NUM]; bool telemetry_lost[TELEMETRY_STATUS_ORB_ID_NUM]; for (int i = 0; i < TELEMETRY_STATUS_ORB_ID_NUM; i++) { telemetry_subs[i] = orb_subscribe(telemetry_status_orb_id[i]); telemetry_last_heartbeat[i] = 0; + telemetry_last_dl_loss[i] = 0; telemetry_lost[i] = true; } @@ -930,6 +933,7 @@ int commander_thread_main(int argc, char *argv[]) int32_t datalink_loss_enabled = false; int32_t datalink_loss_timeout = 10; + int32_t datalink_regain_timeout = 0; /* check which state machines for changes, clear "changed" flag */ bool arming_state_changed = false; @@ -990,6 +994,7 @@ int commander_thread_main(int argc, char *argv[]) param_get(_param_enable_parachute, ¶chute_enabled); param_get(_param_enable_datalink_loss, &datalink_loss_enabled); param_get(_param_datalink_loss_timeout, &datalink_loss_timeout); + param_get(_param_datalink_regain_timeout, &datalink_regain_timeout); } orb_check(sp_man_sub, &updated); @@ -1030,7 +1035,7 @@ int commander_thread_main(int argc, char *argv[]) if (mavlink_fd && telemetry_last_heartbeat[i] == 0 && telemetry.heartbeat_time > 0 && - hrt_elapsed_time(&telemetry.heartbeat_time) < datalink_loss_timeout) { + hrt_elapsed_time(&telemetry.heartbeat_time) < datalink_loss_timeout * 1e6) { (void)rc_calibration_check(mavlink_fd); } @@ -1459,15 +1464,21 @@ int commander_thread_main(int argc, char *argv[]) /* data links check */ bool have_link = false; for (int i = 0; i < TELEMETRY_STATUS_ORB_ID_NUM; i++) { - if (telemetry_last_heartbeat[i] != 0 && hrt_elapsed_time(&telemetry_last_heartbeat[i]) < datalink_loss_timeout) { - /* handle the case where data link was regained */ - if (telemetry_lost[i]) {//XXX also add hysteresis here + if (telemetry_last_heartbeat[i] != 0 && + hrt_elapsed_time(&telemetry_last_heartbeat[i]) < datalink_loss_timeout * 1e6) { + /* handle the case where data link was regained, + * accept datalink as healthy only after datalink_regain_timeout seconds + * */ + if (telemetry_lost[i] && + hrt_elapsed_time(&telemetry_last_dl_loss[i]) > datalink_regain_timeout * 1e6) { + mavlink_log_critical(mavlink_fd, "data link %i regained", i); telemetry_lost[i] = false; + have_link = true; } - have_link = true; } else { + telemetry_last_dl_loss[i] = hrt_absolute_time(); if (!telemetry_lost[i]) { mavlink_log_critical(mavlink_fd, "data link %i lost", i); telemetry_lost[i] = true; diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index f73ae71f3..3d1e231c6 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -106,13 +106,25 @@ PARAM_DEFINE_FLOAT(BAT_CAPACITY, -1.0f); */ PARAM_DEFINE_INT32(DL_LOSS_EN, 0); - /** Datalink timeout threshold + /** Datalink loss time threshold * - * After this amount of seconds the data link lost mode triggers + * After this amount of seconds without datalink the data link lost mode triggers * * @group commander * @unit second * @min 0 - * @max 1000 + * @max 30 */ PARAM_DEFINE_INT32(COM_DL_LOSS_T, 10); + +/** Datalink regain time threshold + * + * After a data link loss: after this this amount of seconds with a healthy datalink the 'datalink loss' + * flag is set back to false + * + * @group commander + * @unit second + * @min 0 + * @max 30 + */ +PARAM_DEFINE_INT32(COM_DL_REG_T, 0); -- cgit v1.2.3 From bffa9e3fa83b84b51b2446291682519433fd3fff Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 24 Jul 2014 19:27:40 +0200 Subject: navigator: add skeleton of FW engine failure mode --- src/modules/navigator/datalinkloss.h | 2 +- src/modules/navigator/enginefailure.cpp | 147 +++++++++++++++++++++++++++++++ src/modules/navigator/enginefailure.h | 83 +++++++++++++++++ src/modules/navigator/module.mk | 1 + src/modules/navigator/navigator.h | 3 + src/modules/navigator/navigator_main.cpp | 1 + 6 files changed, 236 insertions(+), 1 deletion(-) create mode 100644 src/modules/navigator/enginefailure.cpp create mode 100644 src/modules/navigator/enginefailure.h diff --git a/src/modules/navigator/datalinkloss.h b/src/modules/navigator/datalinkloss.h index 650cc7bc5..5a46b5700 100644 --- a/src/modules/navigator/datalinkloss.h +++ b/src/modules/navigator/datalinkloss.h @@ -31,7 +31,7 @@ * ****************************************************************************/ /** - * @file datalinkloss.cpp + * @file datalinkloss.h * Helper class for Data Link Loss Mode acording to the OBC rules * * @author Thomas Gubler diff --git a/src/modules/navigator/enginefailure.cpp b/src/modules/navigator/enginefailure.cpp new file mode 100644 index 000000000..ea654c5fd --- /dev/null +++ b/src/modules/navigator/enginefailure.cpp @@ -0,0 +1,147 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + + /* @file enginefailure.cpp + * Helper class for a fixedwing engine failure mode + * + * @author Thomas Gubler + */ +#include +#include +#include +#include + +#include +#include +#include + +#include +#include + +#include "navigator.h" +#include "enginefailure.h" + +#define DELAY_SIGMA 0.01f + +EngineFailure::EngineFailure(Navigator *navigator, const char *name) : + MissionBlock(navigator, name), + _ef_state(EF_STATE_NONE) +{ + /* load initial params */ + updateParams(); + /* initial reset */ + on_inactive(); +} + +EngineFailure::~EngineFailure() +{ +} + +void +EngineFailure::on_inactive() +{ + _ef_state = EF_STATE_NONE; +} + +void +EngineFailure::on_activation() +{ + _ef_state = EF_STATE_LOITERDOWN; + set_ef_item(); +} + +void +EngineFailure::on_active() +{ + if (is_mission_item_reached()) { + advance_ef(); + set_ef_item(); + } +} + +void +EngineFailure::set_ef_item() +{ + struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); + + /* make sure we have the latest params */ + updateParams(); + + set_previous_pos_setpoint(); + _navigator->set_can_loiter_at_sp(false); + + switch (_ef_state) { + case EF_STATE_LOITERDOWN: { + //XXX create mission item at ground (below?) here + + //_mission_item.lat = (double)(_param_airfieldhomelat.get()) * 1.0e-7; + //_mission_item.lon = (double)(_param_airfieldhomelon.get()) * 1.0e-7; + //_mission_item.altitude_is_relative = false; + //_mission_item.altitude = _param_airfieldhomealt.get(); + //_mission_item.yaw = NAN; + //_mission_item.loiter_radius = _navigator->get_loiter_radius(); + //_mission_item.loiter_direction = 1; + //_mission_item.nav_cmd = NAV_CMD_LOITER_UNLIMITED; + //_mission_item.acceptance_radius = _navigator->get_acceptance_radius(); + //_mission_item.time_inside = _param_commsholdwaittime.get() < 0.0f ? 0.0f : _param_commsholdwaittime.get(); + //_mission_item.pitch_min = 0.0f; + //_mission_item.autocontinue = true; + //_mission_item.origin = ORIGIN_ONBOARD; + + //_navigator->set_can_loiter_at_sp(true); + break; + } + default: + break; + } + + reset_mission_item_reached(); + + /* convert mission item to current position setpoint and make it valid */ + mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current); + pos_sp_triplet->next.valid = false; + + _navigator->set_position_setpoint_triplet_updated(); +} + +void +EngineFailure::advance_ef() +{ + switch (_ef_state) { + case EF_STATE_NONE: + _ef_state = EF_STATE_LOITERDOWN; + break; + default: + break; + } +} diff --git a/src/modules/navigator/enginefailure.h b/src/modules/navigator/enginefailure.h new file mode 100644 index 000000000..2c48c2fce --- /dev/null +++ b/src/modules/navigator/enginefailure.h @@ -0,0 +1,83 @@ +/*************************************************************************** + * + * Copyright (c) 2013-2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ +/** + * @file enginefailure.h + * Helper class for a fixedwing engine failure mode + * + * @author Thomas Gubler + */ + +#ifndef NAVIGATOR_ENGINEFAILURE_H +#define NAVIGATOR_ENGINEFAILURE_H + +#include +#include + +#include + +#include "navigator_mode.h" +#include "mission_block.h" + +class Navigator; + +class EngineFailure : public MissionBlock +{ +public: + EngineFailure(Navigator *navigator, const char *name); + + ~EngineFailure(); + + virtual void on_inactive(); + + virtual void on_activation(); + + virtual void on_active(); + +private: + enum EFState { + EF_STATE_NONE = 0, + EF_STATE_LOITERDOWN = 1, + } _ef_state; + + /** + * Set the DLL item + */ + void set_ef_item(); + + /** + * Move to next EF item + */ + void advance_ef(); + +}; +#endif diff --git a/src/modules/navigator/module.mk b/src/modules/navigator/module.mk index a1e42ec38..0539087df 100644 --- a/src/modules/navigator/module.mk +++ b/src/modules/navigator/module.mk @@ -51,6 +51,7 @@ SRCS = navigator_main.cpp \ geofence.cpp \ geofence_params.c \ datalinkloss.cpp \ + enginefailure.cpp \ datalinkloss_params.c INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index fc0d47b38..fe6639dfe 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -60,6 +60,7 @@ #include "rtl.h" #include "offboard.h" #include "datalinkloss.h" +#include "enginefailure.h" #include "geofence.h" /** @@ -179,6 +180,8 @@ private: RTL _rtl; /**< class that handles RTL */ Offboard _offboard; /**< class that handles offboard */ DataLinkLoss _dataLinkLoss; /**< class that handles the OBC datalink loss mode */ + EngineFailure _engineFailure; /**< class that handles the engine failure mode + (FW only!) */ NavigatorMode *_navigation_mode_array[NAVIGATOR_MODE_ARRAY_SIZE]; /**< array of navigation modes */ diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 77124e8f6..21ab691ff 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -129,6 +129,7 @@ Navigator::Navigator() : _rtl(this, "RTL"), _offboard(this, "OFF"), _dataLinkLoss(this, "DLL"), + _engineFailure(this, "EF"), _can_loiter_at_sp(false), _pos_sp_triplet_updated(false), _param_loiter_radius(this, "LOITER_RAD"), -- cgit v1.2.3 From bc4d7952f384fc079cd30327a5b4e32da90bbcb6 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 24 Jul 2014 21:22:20 +0200 Subject: dl loss: don't set unnecessary value --- src/modules/navigator/datalinkloss.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/src/modules/navigator/datalinkloss.cpp b/src/modules/navigator/datalinkloss.cpp index 91bb5b110..19f335633 100644 --- a/src/modules/navigator/datalinkloss.cpp +++ b/src/modules/navigator/datalinkloss.cpp @@ -143,7 +143,6 @@ DataLinkLoss::set_dll_item() _mission_item.loiter_direction = 1; _mission_item.nav_cmd = NAV_CMD_LOITER_UNLIMITED; _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); - _mission_item.time_inside = _param_commsholdwaittime.get() < 0.0f ? 0.0f : _param_commsholdwaittime.get(); _mission_item.pitch_min = 0.0f; _mission_item.autocontinue = true; _mission_item.origin = ORIGIN_ONBOARD; -- cgit v1.2.3 From a08b7a9f359cc0cf9c6d0631bb844b54f50adcab Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 24 Jul 2014 21:22:40 +0200 Subject: enginefailure: set mission item --- src/modules/navigator/enginefailure.cpp | 30 +++++++++++++++--------------- 1 file changed, 15 insertions(+), 15 deletions(-) diff --git a/src/modules/navigator/enginefailure.cpp b/src/modules/navigator/enginefailure.cpp index ea654c5fd..de567f0dc 100644 --- a/src/modules/navigator/enginefailure.cpp +++ b/src/modules/navigator/enginefailure.cpp @@ -104,21 +104,21 @@ EngineFailure::set_ef_item() case EF_STATE_LOITERDOWN: { //XXX create mission item at ground (below?) here - //_mission_item.lat = (double)(_param_airfieldhomelat.get()) * 1.0e-7; - //_mission_item.lon = (double)(_param_airfieldhomelon.get()) * 1.0e-7; - //_mission_item.altitude_is_relative = false; - //_mission_item.altitude = _param_airfieldhomealt.get(); - //_mission_item.yaw = NAN; - //_mission_item.loiter_radius = _navigator->get_loiter_radius(); - //_mission_item.loiter_direction = 1; - //_mission_item.nav_cmd = NAV_CMD_LOITER_UNLIMITED; - //_mission_item.acceptance_radius = _navigator->get_acceptance_radius(); - //_mission_item.time_inside = _param_commsholdwaittime.get() < 0.0f ? 0.0f : _param_commsholdwaittime.get(); - //_mission_item.pitch_min = 0.0f; - //_mission_item.autocontinue = true; - //_mission_item.origin = ORIGIN_ONBOARD; - - //_navigator->set_can_loiter_at_sp(true); + _mission_item.lat = _navigator->get_global_position()->lat; + _mission_item.lon = _navigator->get_global_position()->lon; + _mission_item.altitude_is_relative = false; + //XXX setting altitude to a very low value, evaluate other options + _mission_item.altitude = _navigator->get_home_position()->alt - 1000.0f; + _mission_item.yaw = NAN; + _mission_item.loiter_radius = _navigator->get_loiter_radius(); + _mission_item.loiter_direction = 1; + _mission_item.nav_cmd = NAV_CMD_LOITER_UNLIMITED; + _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); + _mission_item.pitch_min = 0.0f; + _mission_item.autocontinue = true; + _mission_item.origin = ORIGIN_ONBOARD; + + _navigator->set_can_loiter_at_sp(true); break; } default: -- cgit v1.2.3 From 303664d94af4949bcc7f1c3393f82eb242a60c5e Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 24 Jul 2014 21:49:22 +0200 Subject: add landengfail nav state --- src/modules/navigator/navigator_main.cpp | 3 +++ src/modules/uORB/topics/vehicle_status.h | 1 + 2 files changed, 4 insertions(+) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 21ab691ff..043d883b2 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -393,6 +393,9 @@ Navigator::task_main() _navigation_mode = &_rtl; /* TODO: change this to something else */ } break; + case NAVIGATION_STATE_AUTO_LANDENGFAIL: + _navigation_mode = &_engineFailure; + break; case NAVIGATION_STATE_LAND: case NAVIGATION_STATE_TERMINATION: case NAVIGATION_STATE_OFFBOARD: diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index eda1dfaec..c2926dce8 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -103,6 +103,7 @@ typedef enum { NAVIGATION_STATE_AUTO_LOITER, /**< Auto loiter mode */ NAVIGATION_STATE_AUTO_RTL, /**< Auto return to launch mode */ NAVIGATION_STATE_AUTO_RTGS, /**< Auto return to groundstation on data link loss */ + NAVIGATION_STATE_AUTO_LANDENGFAIL, /**< Auto land on engine failure */ NAVIGATION_STATE_ACRO, /**< Acro mode */ NAVIGATION_STATE_LAND, /**< Land mode */ NAVIGATION_STATE_DESCEND, /**< Descend mode (no position control) */ -- cgit v1.2.3 From 1b2f9070ea063255aca4a882e4adda7f89e082e7 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 24 Jul 2014 22:29:04 +0200 Subject: engine_failure flag Added engine_failure flag to behicle status, alsoset_nav_state in the state machine helper takes this flag into account --- src/modules/commander/state_machine_helper.cpp | 19 ++++++++++++++----- src/modules/uORB/topics/vehicle_status.h | 6 ++++-- 2 files changed, 18 insertions(+), 7 deletions(-) diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 4e1cfb987..bc6803596 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -490,9 +490,12 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en case MAIN_STATE_AUTO_MISSION: /* go into failsafe + * - if we have an engine failure * - if either the datalink is enabled and lost as well as RC is lost * - if there is no datalink and the mission is finished */ - if (((status->data_link_lost && data_link_loss_enabled) && status->rc_signal_lost) || + if (status->engine_failure) { + status->nav_state = NAVIGATION_STATE_AUTO_LANDENGFAIL; + } else if (((status->data_link_lost && data_link_loss_enabled) && status->rc_signal_lost) || (!data_link_loss_enabled && status->rc_signal_lost && mission_finished)) { status->failsafe = true; @@ -532,8 +535,10 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en break; case MAIN_STATE_AUTO_LOITER: - /* go into failsafe if datalink and RC is lost */ - if ((status->data_link_lost && data_link_loss_enabled) && status->rc_signal_lost) { + /* go into failsafe on a engine failure or if datalink and RC is lost */ + if (status->engine_failure) { + status->nav_state = NAVIGATION_STATE_AUTO_LANDENGFAIL; + } else if ((status->data_link_lost && data_link_loss_enabled) && status->rc_signal_lost) { status->failsafe = true; if (status->condition_global_position_valid && status->condition_home_position_valid) { @@ -586,8 +591,12 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en break; case MAIN_STATE_AUTO_RTL: - /* require global position and home */ - if ((!status->condition_global_position_valid || !status->condition_home_position_valid)) { + /* require global position and home, also go into failsafe on an engine failure */ + + if (status->engine_failure) { + status->nav_state = NAVIGATION_STATE_AUTO_LANDENGFAIL; + } else if ((!status->condition_global_position_valid || + !status->condition_home_position_valid)) { status->failsafe = true; if (status->condition_local_position_valid) { diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index c2926dce8..a47488621 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -201,8 +201,10 @@ struct vehicle_status_s { bool rc_signal_lost; /**< true if RC reception lost */ bool rc_input_blocked; /**< set if RC input should be ignored */ - bool data_link_lost; /**< datalink to GCS lost */ - uint8_t data_link_lost_counter; /**< counts unique data link lost events */ + bool data_link_lost; /**< datalink to GCS lost */ + uint8_t data_link_lost_counter; /**< counts unique data link lost events */ + + bool engine_failure; /** Set to true if an engine failure is detected */ bool offboard_control_signal_found_once; bool offboard_control_signal_lost; -- cgit v1.2.3 From 351598626054e5d853e8768d2f5d04226510c12a Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 24 Jul 2014 23:44:38 +0200 Subject: define gps loss failsafe nav state --- src/modules/uORB/topics/vehicle_status.h | 1 + 1 file changed, 1 insertion(+) diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index a47488621..e0072b52d 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -104,6 +104,7 @@ typedef enum { NAVIGATION_STATE_AUTO_RTL, /**< Auto return to launch mode */ NAVIGATION_STATE_AUTO_RTGS, /**< Auto return to groundstation on data link loss */ NAVIGATION_STATE_AUTO_LANDENGFAIL, /**< Auto land on engine failure */ + NAVIGATION_STATE_AUTO_LANDGPSFAIL, /**< Auto land on gps failure (e.g. open loop loiter down) */ NAVIGATION_STATE_ACRO, /**< Acro mode */ NAVIGATION_STATE_LAND, /**< Land mode */ NAVIGATION_STATE_DESCEND, /**< Descend mode (no position control) */ -- cgit v1.2.3 From db5d668439be63e4c8fd7dab49b81c5e162ee095 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 27 Jul 2014 00:55:43 +0200 Subject: add simplistic gps failure detection --- src/modules/commander/commander.cpp | 16 ++++++++++++++++ src/modules/uORB/topics/vehicle_status.h | 1 + 2 files changed, 17 insertions(+) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index fe1974c88..50beb5da8 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1135,6 +1135,22 @@ int commander_thread_main(int argc, char *argv[]) check_valid(global_position.timestamp, POSITION_TIMEOUT, eph_epv_good, &(status.condition_global_position_valid), &status_changed); /* check if GPS fix is ok */ + if (gps_position.fix_type >= 3 && //XXX check eph and epv ? + hrt_elapsed_time(&gps_position.timestamp_position) < FAILSAFE_DEFAULT_TIMEOUT) { + /* handle the case where gps was regained */ + if (status.gps_failure) { + status.gps_failure = false; + status_changed = true; + mavlink_log_critical(mavlink_fd, "gps regained"); + } + } else { + if (!status.gps_failure) { + status.gps_failure = true; + status_changed = true; + mavlink_log_critical(mavlink_fd, "gps fix lost"); + } + } + /* update home position */ if (!status.condition_home_position_valid && status.condition_global_position_valid && !armed.armed && diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index 707abb545..0751b57fe 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -206,6 +206,7 @@ struct vehicle_status_s { uint8_t data_link_lost_counter; /**< counts unique data link lost events */ bool engine_failure; /** Set to true if an engine failure is detected */ + bool gps_failure; /** Set to true if a gps failure is detected */ bool offboard_control_signal_found_once; bool offboard_control_signal_lost; -- cgit v1.2.3 From ab80459b326aaef5ce9e2d7cae04613af6653c66 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 11 Aug 2014 08:56:06 +0200 Subject: Fixed whitespace in rcS --- ROMFS/px4fmu_common/init.d/rcS | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 19d42dfa5..24b2a299a 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -235,7 +235,7 @@ then tone_alarm $TUNE_OUT_ERROR fi fi - + # # Set default output if not set # -- cgit v1.2.3 From b0fe5ae1ba1f6bbdb0fff1b450c0bf0a032095ec Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 11 Aug 2014 08:58:39 +0200 Subject: Bring back nominal state of fw attitude controller --- src/modules/fw_att_control/fw_att_control_main.cpp | 24 +++++++++++----------- 1 file changed, 12 insertions(+), 12 deletions(-) diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index e9480336a..0cea13cc4 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -910,18 +910,18 @@ FixedwingAttitudeControl::task_main() _actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators); } -// if (_actuators_1_pub > 0) { -// /* publish the attitude setpoint */ -// orb_publish(ORB_ID(actuator_controls_1), _actuators_1_pub, &_actuators_airframe); -//// warnx("%.2f %.2f %.2f %.2f %.2f %.2f %.2f %.2f", -//// (double)_actuators_airframe.control[0], (double)_actuators_airframe.control[1], (double)_actuators_airframe.control[2], -//// (double)_actuators_airframe.control[3], (double)_actuators_airframe.control[4], (double)_actuators_airframe.control[5], -//// (double)_actuators_airframe.control[6], (double)_actuators_airframe.control[7]); -// -// } else { -// /* advertise and publish */ -// _actuators_1_pub = orb_advertise(ORB_ID(actuator_controls_1), &_actuators_airframe); -// } + if (_actuators_1_pub > 0) { + /* publish the attitude setpoint */ + orb_publish(ORB_ID(actuator_controls_1), _actuators_1_pub, &_actuators_airframe); +// warnx("%.2f %.2f %.2f %.2f %.2f %.2f %.2f %.2f", +// (double)_actuators_airframe.control[0], (double)_actuators_airframe.control[1], (double)_actuators_airframe.control[2], +// (double)_actuators_airframe.control[3], (double)_actuators_airframe.control[4], (double)_actuators_airframe.control[5], +// (double)_actuators_airframe.control[6], (double)_actuators_airframe.control[7]); + + } else { + /* advertise and publish */ + _actuators_1_pub = orb_advertise(ORB_ID(actuator_controls_1), &_actuators_airframe); + } } -- cgit v1.2.3 From 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(-) 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 01a25547717736ef1692ffac4364dac1e75fb848 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 11 Aug 2014 14:51:49 +0200 Subject: Viper mixer: Set failsafe values so we are not starting to command actuators --- ROMFS/px4fmu_common/init.d/3035_viper | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/ROMFS/px4fmu_common/init.d/3035_viper b/ROMFS/px4fmu_common/init.d/3035_viper index a4c1e832d..cb2b26c3c 100644 --- a/ROMFS/px4fmu_common/init.d/3035_viper +++ b/ROMFS/px4fmu_common/init.d/3035_viper @@ -8,3 +8,8 @@ sh /etc/init.d/rc.fw_defaults set MIXER Viper + +pwm failsafe -c 5 -p 1000 +pwm failsafe -c 6 -p 1000 +pwm failsafe -c 7 -p 1000 +pwm failsafe -c 8 -p 1000 -- cgit v1.2.3 From c81c94d74f049fac409d97e5dcafb3b3afa72ab6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 11 Aug 2014 14:52:07 +0200 Subject: Fix control group in Viper mixer --- ROMFS/px4fmu_common/mixers/Viper.mix | 13 +++++-------- 1 file changed, 5 insertions(+), 8 deletions(-) diff --git a/ROMFS/px4fmu_common/mixers/Viper.mix b/ROMFS/px4fmu_common/mixers/Viper.mix index 79c4447be..b05a8c8b0 100755 --- a/ROMFS/px4fmu_common/mixers/Viper.mix +++ b/ROMFS/px4fmu_common/mixers/Viper.mix @@ -52,21 +52,18 @@ M: 1 O: 10000 10000 0 -10000 10000 S: 0 3 0 20000 -10000 -10000 10000 -Gimbal / flaps / payload mixer for last four channels +Inputs to the mixer come from channel group 2 (payload), channels 0 +(bay servo 1), 1 (bay servo 2) and 3 (drop release). ----------------------------------------------------- M: 1 O: 10000 10000 0 -10000 10000 -S: 0 4 10000 10000 0 -10000 10000 +S: 2 4 10000 10000 0 -10000 10000 M: 1 O: 10000 10000 0 -10000 10000 -S: 0 5 10000 10000 0 -10000 10000 +S: 2 5 10000 10000 0 -10000 10000 M: 1 O: 10000 10000 0 -10000 10000 -S: 0 6 10000 10000 0 -10000 10000 - -M: 1 -O: 10000 10000 0 -10000 10000 -S: 0 7 10000 10000 0 -10000 10000 +S: 2 6 10000 10000 0 -10000 10000 -- cgit v1.2.3 From f47cf9a002cc2fbe27e922adfdedf111f4b28093 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 11 Aug 2014 14:52:45 +0200 Subject: Bottle drop: C++ify full code base, fix transformation assumptions and other things, ready for integration testing --- src/modules/bottle_drop/bottle_drop.c | 477 +------------------------------ src/modules/bottle_drop/bottle_drop.cpp | 492 +++++++++++++++++++++++++++++--- src/modules/bottle_drop/module.mk | 3 +- 3 files changed, 451 insertions(+), 521 deletions(-) diff --git a/src/modules/bottle_drop/bottle_drop.c b/src/modules/bottle_drop/bottle_drop.c index 1911329d0..b86782075 100644 --- a/src/modules/bottle_drop/bottle_drop.c +++ b/src/modules/bottle_drop/bottle_drop.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. + * Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -33,487 +33,16 @@ /** * @file bottle_drop.c - * bottle_drop application - * + * Bottle drop parameters + * * @author Dominik Juchli */ #include -#include -#include -#include -#include -#include -#include -#include -#include -#include - - -#include -#include #include -#include -#include - -#include -#include -#include -#include -#include -#include - -#include - PARAM_DEFINE_FLOAT(BD_HEIGHT, 60.0f); PARAM_DEFINE_FLOAT(BD_GPROPERTIES, 0.03f); PARAM_DEFINE_FLOAT(BD_TURNRADIUS, 70.0f); PARAM_DEFINE_FLOAT(BD_PRECISION, 1.0f); PARAM_DEFINE_INT32(BD_APPROVAL, 0); - - - - -static bool thread_should_exit = false; /**< daemon exit flag */ -static bool thread_running = false; /**< daemon status flag */ -static int daemon_task; /**< Handle of daemon task / thread */ - -static bool drop = false; - -/** - * daemon management function. - */ -__EXPORT int bottle_drop_main(int argc, char *argv[]); - -/** - * Mainloop of daemon. - */ -int bottle_drop_thread_main(int argc, char *argv[]); - -/** - * Print the correct usage. - */ -static void usage(const char *reason); - -static void -usage(const char *reason) -{ - if (reason) - warnx("%s\n", reason); - errx(1, "usage: daemon {start|stop|status} [-p ]\n\n"); -} - -/** - * The daemon app only briefly exists to start - * the background job. The stack size assigned in the - * Makefile does only apply to this management task. - * - * The actual stack size should be set in the call - * to task_create(). - */ -int bottle_drop_main(int argc, char *argv[]) -{ - if (argc < 1) - usage("missing command"); - - if (!strcmp(argv[1], "start")) { - - if (thread_running) { - warnx("daemon already running\n"); - /* this is not an error */ - exit(0); - } - - thread_should_exit = false; - daemon_task = task_spawn_cmd("bottle_drop", - SCHED_DEFAULT, - SCHED_PRIORITY_DEFAULT, - 4096, - bottle_drop_thread_main, - (argv) ? (const char **)&argv[2] : (const char **)NULL); - exit(0); - } - - if (!strcmp(argv[1], "drop")) { - drop = true; - exit(0); - } - - if (!strcmp(argv[1], "stop")) { - thread_should_exit = true; - exit(0); - } - - if (!strcmp(argv[1], "status")) { - if (thread_running) { - warnx("\trunning\n"); - } else { - warnx("\tnot started\n"); - } - exit(0); - } - - usage("unrecognized command"); - exit(1); -} - -int bottle_drop_thread_main(int argc, char *argv[]) { - - warnx("starting\n"); - - bool updated = false; - - float height; // height at which the normal should be dropped NED - float z_0; // ground properties - float turn_radius; // turn radius of the UAV - float precision; // Expected precision of the UAV - bool drop_approval; // if approval is given = true, otherwise = false - - thread_running = true; - - /* XXX TODO: create, publish and read in wind speed topic */ - struct wind_speed_s { - float vx; // m/s - float vy; // m/s - float altitude; // m - } wind_speed; - - wind_speed.vx = 4.2f; - wind_speed.vy = 0.0f; - wind_speed.altitude = 62.0f; - - - /* XXX TODO: create, publish and read in target position in NED*/ - struct position_s { - double lat; //degrees 1E7 - double lon; //degrees 1E7 - float alt; //m - } target_position, drop_position; - - target_position.lat = 47.385806; - target_position.lon = 8.589093; - target_position.alt = 0.0f; - - - // constant - float g = 9.81f; // constant of gravity [m/s^2] - float m = 0.5f; // mass of bottle [kg] - float rho = 1.2f; // air density [kg/m^3] - float A = (powf(0.063f, 2.0f)/4.0f*M_PI_F); // Bottle cross section [m^2] - float dt = 0.01f; // step size [s] - float dt2 = 0.05f; // step size 2 [s] - - // Has to be estimated by experiment - float cd = 0.86f; // Drag coefficient for a cylinder with a d/l ratio of 1/3 [] - float t_signal = 0.084f; // Time span between sending the signal and the bottle top reaching level height with the bottom of the plane [s] - float t_door = 0.7f; // The time the system needs to open the door + safety, is also the time the palyload needs to safely escape the shaft [s] - - - // Definition - float h_0; // height over target - float az; // acceleration in z direction[m/s^2] - float vz; // velocity in z direction [m/s] - float z; // fallen distance [m] - float h; // height over target [m] - float ax; // acceleration in x direction [m/s^2] - float vx; // ground speed in x direction [m/s] - float x; // traveled distance in x direction [m] - float vw; // wind speed [m/s] - float vrx; // relative velocity in x direction [m/s] - float v; // relative speed vector [m/s] - float Fd; // Drag force [N] - float Fdx; // Drag force in x direction [N] - float Fdz; // Drag force in z direction [N] - float vr; // absolute wind speed [m/s] - float x_drop, y_drop; // coordinates of the drop point in reference to the target (projection of NED) - float x_t,y_t; // coordinates of the target in reference to the target x_t = 0, y_t = 0 (projection of NED) - float x_l,y_l; // local position in projected coordinates - float x_f,y_f; // to-be position of the UAV after dt2 seconds in projected coordinates - double x_f_NED, y_f_NED; // to-be position of the UAV after dt2 seconds in NED - float distance_open_door; // The distance the UAV travels during its doors open [m] - float distance_real = 0; // The distance between the UAVs position and the drop point [m] - float future_distance = 0; // The distance between the UAVs to-be position and the drop point [m] - - // states - bool state_door = false; // Doors are closed = false, open = true - bool state_drop = false; // Drop occurred = true, Drop din't occur = false - bool state_run = false; // A drop was attempted = true, the drop is still in progress = false - - unsigned counter = 0; - - param_t param_height = param_find("BD_HEIGHT"); - param_t param_gproperties = param_find("BD_GPROPERTIES"); - param_t param_turn_radius = param_find("BD_TURNRADIUS"); - param_t param_precision = param_find("BD_PRECISION"); - param_t param_approval = param_find("BD_APPROVAL"); - - - param_get(param_approval, &drop_approval); - param_get(param_precision, &precision); - param_get(param_turn_radius, &turn_radius); - param_get(param_height, &height); - param_get(param_gproperties, &z_0); - - - struct vehicle_attitude_s att; - memset(&att, 0, sizeof(att)); - int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude)); - orb_set_interval(vehicle_attitude_sub, 20); - - struct vehicle_global_position_s globalpos; - memset(&globalpos, 0, sizeof(globalpos)); - int vehicle_global_position_sub = orb_subscribe(ORB_ID(vehicle_global_position)); - - struct parameter_update_s update; - memset(&update, 0, sizeof(update)); - int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update)); - - struct actuator_controls_s actuators; - memset(&actuators, 0, sizeof(actuators)); - orb_advert_t actuator_pub = orb_advertise(ORB_ID(actuator_controls_1), &actuators); - - struct mission_item_s flight_vector_s; - struct mission_item_s flight_vector_e; - - flight_vector_s.nav_cmd = NAV_CMD_WAYPOINT; - flight_vector_s.radius = 50; // TODO: make parameter - flight_vector_s.autocontinue = true; - flight_vector_e.nav_cmd = NAV_CMD_WAYPOINT; - flight_vector_e.radius = 50; // TODO: make parameter - flight_vector_e.autocontinue = true; - - struct mission_s onboard_mission; - memset(&onboard_mission, 0, sizeof(onboard_mission)); - - orb_advert_t onboard_mission_pub = orb_advertise(ORB_ID(onboard_mission), &onboard_mission); - - struct pollfd fds[] = { - { .fd = vehicle_attitude_sub, .events = POLLIN } - }; - - double latitude; - double longitude; - - - while (!thread_should_exit) { - -// warnx("in while!\n"); - // values from -1 to 1 - - int ret = poll(fds, 1, 500); - - if (ret < 0) { - /* poll error, count it in perf */ - warnx("poll error"); - - } else if (ret > 0) { - - orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &att); - - orb_check(vehicle_global_position_sub, &updated); - if (updated){ - /* copy global position */ - orb_copy(ORB_ID(vehicle_global_position), vehicle_global_position_sub, &globalpos); - - latitude = (double)globalpos.lat / 1e7; - longitude = (double)globalpos.lon / 1e7; - } - - orb_check(parameter_update_sub, &updated); - if (updated){ - /* copy global position */ - orb_copy(ORB_ID(parameter_update), parameter_update_sub, &update); - - /* update all parameters */ - param_get(param_height, &height); - param_get(param_gproperties, &z_0); - param_get(param_turn_radius, &turn_radius); - param_get(param_approval, &drop_approval); - param_get(param_precision, &precision); - - - } - - // Initialization - - - vr = sqrtf(powf(wind_speed.vx,2) + powf(wind_speed.vy,2)); // absolute wind speed [m/s] - distance_open_door = fabsf(t_door * globalpos.vx); - - - //warnx("absolut wind speed = %.4f", vr); //////////////////////////////////////////////////////////////////// DEBUGGING - - - //warnx("Initialization complete\n"); //////////////////////////////////////////////////////////////////// DEBUGGING - - - if (drop_approval && !state_drop) - { - //warnx("approval given\n"); //////////////////////////////////////////////////////////////////// DEBUGGING - // drop here - //open_now = true; - //drop = false; - //drop_start = hrt_absolute_time(); - - - if (counter % 50 == 0) { - - az = g; // acceleration in z direction[m/s^2] - vz = 0; // velocity in z direction [m/s] - z = 0; // fallen distance [m] - h_0 = globalpos.alt - target_position.alt; // height over target at start[m] - h = h_0; // height over target [m] - ax = 0; // acceleration in x direction [m/s^2] - vx = sqrtf(powf(globalpos.vx,2.0f) + powf(globalpos.vy,2.0f)); // ground speed in x direction [m/s] - x = 0; // traveled distance in x direction [m] - vw = 0; // wind speed [m/s] - vrx = 0; // relative velocity in x direction [m/s] - v = sqrtf(powf(globalpos.vx,2.0f) + powf(globalpos.vy,2.0f)); // relative speed vector [m/s] - Fd = 0; // Drag force [N] - Fdx = 0; // Drag force in x direction [N] - Fdz = 0; // Drag force in z direction [N] - - - // Compute the distance the bottle will travel after it is dropped in body frame coordinates --> x - while( h > 0.05f) - { - // z-direction - vz = vz + az*dt; - z = z + vz*dt; - h = h_0 - z; - - // x-direction - vw = vr*logf(h/z_0)/logf(wind_speed.altitude/z_0); - vx = vx + ax*dt; - x = x + vx*dt; - vrx = vx + vw; - - //Drag force - v = sqrtf(powf(vz,2.0f) + powf(vrx,2.0f)); - Fd = 0.5f*rho*A*cd*powf(v,2.0f); - Fdx = Fd*vrx/v; - Fdz = Fd*vz/v; - - //acceleration - az = g - Fdz/m; - ax = -Fdx/m; - } - // Compute Drop point - x = sqrtf(powf(globalpos.vx,2.0f) + powf(globalpos.vy,2.0f))*t_signal + x; - map_projection_init(target_position.lat, target_position.lon); - } - - map_projection_project(target_position.lat, target_position.lon, &x_t, &y_t); - if( vr < 0.001f) // if there is no wind, an arbitrarily direction is chosen - { - vr = 1; - wind_speed.vx = 1; - wind_speed.vy = 0; - } - x_drop = x_t + x*wind_speed.vx/vr; - y_drop = y_t + x*wind_speed.vy/vr; - map_projection_reproject(x_drop, y_drop, &drop_position.lat, &drop_position.lon); - drop_position.alt = height; - //warnx("drop point: lat = %.7f , lon = %.7f", drop_position.lat, drop_position.lon); //////////////////////////////////////////////////////////////////// DEBUGGING - - - - // Compute flight vector - map_projection_reproject(x_drop + 2*turn_radius*wind_speed.vx/vr, y_drop + 2*turn_radius*wind_speed.vy/vr, &(flight_vector_s.lat), &(flight_vector_s.lon)); - flight_vector_s.altitude = height; - map_projection_reproject(x_drop - turn_radius*wind_speed.vx/vr, y_drop - turn_radius*wind_speed.vy/vr, &flight_vector_e.lat, &flight_vector_e.lon); - flight_vector_e.altitude = height; - //warnx("Flight vector: starting point = %.7f %.7f , end point = %.7f %.7f", flight_vector_s.lat,flight_vector_s.lon,flight_vector_e.lat,flight_vector_e.lon); //////////////////////////////////////////////////////////////////// DEBUGGING - - // Drop Cancellation if terms are not met - - // warnx("latitude:%.2f", latitude); - // warnx("longitude:%.2f", longitude); - // warnx("drop_position.lat:%.2f", drop_position.lat); - // warnx("drop_position.lon:%.2f", drop_position.lon); - - distance_real = fabsf(get_distance_to_next_waypoint(latitude, longitude, drop_position.lat, drop_position.lon)); - map_projection_project(latitude, longitude, &x_l, &y_l); - x_f = x_l + globalpos.vx*dt2; - y_f = y_l + globalpos.vy*dt2; - map_projection_reproject(x_f, y_f, &x_f_NED, &y_f_NED); - future_distance = fabsf(get_distance_to_next_waypoint(x_f_NED, y_f_NED, drop_position.lat, drop_position.lon)); - //warnx("position to-be: = %.7f %.7f" ,x_f_NED, y_f_NED ); //////////////////////////////////////////////////////////////////// DEBUGGING - - // if (counter % 10 ==0) { - // warnx("x: %.4f", x); - // warnx("drop_position.lat: %.4f, drop_position.lon: %.4f", drop_position.lat, drop_position.lon); - // warnx("latitude %.4f, longitude: %.4f", latitude, longitude); - // warnx("future_distance: %.2f, precision: %.2f", future_distance, precision); - // } - - /* Save WPs in datamanager */ - const size_t len = sizeof(struct mission_item_s); - - if (dm_write(DM_KEY_WAYPOINTS_ONBOARD, 0, DM_PERSIST_IN_FLIGHT_RESET, &flight_vector_s, len) != len) { - warnx("ERROR: could not save onboard WP"); - } - - if (dm_write(DM_KEY_WAYPOINTS_ONBOARD, 1, DM_PERSIST_IN_FLIGHT_RESET, &flight_vector_e, len) != len) { - warnx("ERROR: could not save onboard WP"); - } - - onboard_mission.count = 2; - - if (state_run && !state_drop) { - onboard_mission.current_index = 0; - } else { - onboard_mission.current_index = -1; - } - - if (counter % 10 ==0) - warnx("Distance real: %.2f, distance_open_door: %.2f, angle to wind: %.2f", distance_real, distance_open_door, fabsf(_wrap_pi(globalpos.yaw-atan2f(wind_speed.vy,wind_speed.vx)+M_PI_F))); - - orb_publish(ORB_ID(onboard_mission), onboard_mission_pub, &onboard_mission); - - } - - if(isfinite(distance_real) && distance_real < distance_open_door && drop_approval) - { - actuators.control[0] = -1.0f; // open door - actuators.control[1] = 1.0f; - state_door = true; - warnx("open doors"); - } - else - { // closed door and locked survival kit - actuators.control[0] = 0.5f; - actuators.control[1] = -0.5f; - actuators.control[2] = -0.5f; - state_door = false; - } - if(isfinite(distance_real) && distance_real < precision && distance_real < future_distance && state_door) // Drop only if the distance between drop point and actual position is getting larger again - { - if(fabsf(_wrap_pi(globalpos.yaw-atan2f(wind_speed.vy,wind_speed.vx)+M_PI_F)) < 10.0f/180.0f*M_PI_F) // if flight trajectory deviates more than 10 degrees from calculated path, it will no drop - { - actuators.control[2] = 0.5f; - state_drop = true; - state_run = true; - warnx("dropping now"); - } - else - { - state_run = true; - } - } - - actuators.timestamp = hrt_absolute_time(); - orb_publish(ORB_ID(actuator_controls_1), actuator_pub, &actuators); - - counter++; - } - } - - warnx("exiting.\n"); - - thread_running = false; - - - return 0; -} diff --git a/src/modules/bottle_drop/bottle_drop.cpp b/src/modules/bottle_drop/bottle_drop.cpp index ce760adc3..84c7900ce 100644 --- a/src/modules/bottle_drop/bottle_drop.cpp +++ b/src/modules/bottle_drop/bottle_drop.cpp @@ -57,10 +57,13 @@ #include #include #include +#include +#include #include #include #include #include +#include #include #include @@ -72,7 +75,7 @@ */ extern "C" __EXPORT int bottle_drop_main(int argc, char *argv[]); -class BottleDrop +class BottleDrop { public: /** @@ -101,17 +104,26 @@ private: bool _task_should_exit; /**< if true, task should exit */ int _main_task; /**< handle for task */ int _mavlink_fd; - + int _command_sub; + int _wind_estimate_sub; struct vehicle_command_s _command; orb_advert_t _actuator_pub; struct actuator_controls_s _actuators; + bool drop_approval; bool _open_door; bool _drop; hrt_abstime _doors_opened; + struct position_s { + double lat; //degrees + double lon; //degrees + float alt; //m + bool initialized; + } _target_position, _drop_position; + void task_main(); void handle_command(struct vehicle_command_s *cmd); @@ -135,12 +147,13 @@ BottleDrop::BottleDrop() : _main_task(-1), _mavlink_fd(-1), _command_sub(-1), - _command({}), - _actuator_pub(-1), - _actuators({}), - _open_door(false), - _drop(false), - _doors_opened(0) + _wind_estimate_sub(-1), + _command {}, + _actuator_pub(-1), + _actuators {}, + _open_door(false), + _drop(false), + _doors_opened(0) { } @@ -176,11 +189,11 @@ BottleDrop::start() /* start the task */ _main_task = task_spawn_cmd("bottle_drop", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 5, - 2048, - (main_t)&BottleDrop::task_main_trampoline, - nullptr); + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 2048, + (main_t)&BottleDrop::task_main_trampoline, + nullptr); if (_main_task < 0) { warn("task start failed"); @@ -201,14 +214,112 @@ BottleDrop::status() void BottleDrop::task_main() { - /* inform about start */ - warnx("Initializing.."); - fflush(stdout); _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); mavlink_log_info(_mavlink_fd, "[bottle_drop] started"); _command_sub = orb_subscribe(ORB_ID(vehicle_command)); + _wind_estimate_sub = orb_subscribe(ORB_ID(wind_estimate)); + + bool updated = false; + + float height; // height at which the normal should be dropped NED + float z_0; // ground properties + float turn_radius; // turn radius of the UAV + float precision; // Expected precision of the UAV + + // XXX we do not measure the exact ground altitude yet, but eventually we have to + float ground_distance = 70.0f; + + // constant + float g = 9.81f; // constant of gravity [m/s^2] + float m = 0.5f; // mass of bottle [kg] + float rho = 1.2f; // air density [kg/m^3] + float A = (powf(0.063f, 2.0f) / 4.0f * M_PI_F); // Bottle cross section [m^2] + float dt = 0.01f; // step size [s] + float dt2 = 0.05f; // step size 2 [s] + + // Has to be estimated by experiment + float cd = 0.86f; // Drag coefficient for a cylinder with a d/l ratio of 1/3 [] + float t_signal = + 0.084f; // Time span between sending the signal and the bottle top reaching level height with the bottom of the plane [s] + float t_door = + 0.7f; // The time the system needs to open the door + safety, is also the time the palyload needs to safely escape the shaft [s] + + + // Definition + float h_0; // height over target + float az; // acceleration in z direction[m/s^2] + float vz; // velocity in z direction [m/s] + float z; // fallen distance [m] + float h; // height over target [m] + float ax; // acceleration in x direction [m/s^2] + float vx; // ground speed in x direction [m/s] + float x; // traveled distance in x direction [m] + float vw; // wind speed [m/s] + float vrx; // relative velocity in x direction [m/s] + float v; // relative speed vector [m/s] + float Fd; // Drag force [N] + float Fdx; // Drag force in x direction [N] + float Fdz; // Drag force in z direction [N] + float x_drop, y_drop; // coordinates of the drop point in reference to the target (projection of NED) + float x_t, y_t; // coordinates of the target in reference to the target x_t = 0, y_t = 0 (projection of NED) + float x_l, y_l; // local position in projected coordinates + float x_f, y_f; // to-be position of the UAV after dt2 seconds in projected coordinates + double x_f_NED, y_f_NED; // to-be position of the UAV after dt2 seconds in NED + float distance_open_door; // The distance the UAV travels during its doors open [m] + float distance_real = 0; // The distance between the UAVs position and the drop point [m] + float future_distance = 0; // The distance between the UAVs to-be position and the drop point [m] + + // states + bool state_door = false; // Doors are closed = false, open = true + bool state_drop = false; // Drop occurred = true, Drop din't occur = false + bool state_run = false; // A drop was attempted = true, the drop is still in progress = false + + unsigned counter = 0; + + param_t param_height = param_find("BD_HEIGHT"); + param_t param_gproperties = param_find("BD_GPROPERTIES"); + param_t param_turn_radius = param_find("BD_TURNRADIUS"); + param_t param_precision = param_find("BD_PRECISION"); + + + param_get(param_precision, &precision); + param_get(param_turn_radius, &turn_radius); + param_get(param_height, &height); + param_get(param_gproperties, &z_0); + + struct vehicle_global_position_s globalpos; + memset(&globalpos, 0, sizeof(globalpos)); + int vehicle_global_position_sub = orb_subscribe(ORB_ID(vehicle_global_position)); + + struct parameter_update_s update; + memset(&update, 0, sizeof(update)); + int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update)); + + struct actuator_controls_s actuators; + memset(&actuators, 0, sizeof(actuators)); + + struct mission_item_s flight_vector_s; + struct mission_item_s flight_vector_e; + + flight_vector_s.nav_cmd = NAV_CMD_WAYPOINT; + flight_vector_s.acceptance_radius = 50; // TODO: make parameter + flight_vector_s.autocontinue = true; + flight_vector_e.nav_cmd = NAV_CMD_WAYPOINT; + flight_vector_e.acceptance_radius = 50; // TODO: make parameter + flight_vector_e.autocontinue = true; + + struct mission_s onboard_mission; + memset(&onboard_mission, 0, sizeof(onboard_mission)); + orb_advert_t onboard_mission_pub = -1; + + double latitude; + double longitude; + + struct wind_estimate_s wind; + + map_projection_reference_s ref; /* wakeup source(s) */ struct pollfd fds[1]; @@ -234,40 +345,246 @@ BottleDrop::task_main() handle_command(&_command); } - /* update door actuators */ - if (_open_door) { - _actuators.control[0] = -1.0f; - _actuators.control[1] = 1.0f; + /* switch to faster updates during the drop */ + while (_drop) { - if (_doors_opened == 0) { - _doors_opened = hrt_absolute_time(); + /* 20s after drop, reset and close everything again */ + if (_drop && _doors_opened != 0 && hrt_elapsed_time(&_doors_opened) > 20000000) { + _open_door = false; + _drop = false; } - } else { - _actuators.control[0] = 1.0f; - _actuators.control[1] = -1.0f; - _doors_opened = 0; - } + orb_check(_wind_estimate_sub, &updated); - /* update drop actuator, wait 0.5s until the doors are open before dropping */ - if (_drop && _doors_opened != 0 && hrt_elapsed_time(&_doors_opened) > 500000) { - _actuators.control[2] = -0.5f; - } else { - _actuators.control[2] = 0.5f; - } + if (updated) { + orb_copy(ORB_ID(wind_estimate), _wind_estimate_sub, &wind); + } - /* 20s after drop, reset and close everything again */ - if (_drop && _doors_opened != 0 && hrt_elapsed_time(&_doors_opened) > 20000000) { - _open_door = false; - _drop = false; - } + orb_check(vehicle_global_position_sub, &updated); - /* lazily publish _actuators only once available */ - if (_actuator_pub > 0) { - orb_publish(ORB_ID(actuator_controls_1), _actuator_pub, &_actuators); + if (updated) { + /* copy global position */ + orb_copy(ORB_ID(vehicle_global_position), vehicle_global_position_sub, &globalpos); - } else { - _actuator_pub = orb_advertise(ORB_ID(actuator_controls_1), &_actuators); + latitude = (double)globalpos.lat / 1e7; + longitude = (double)globalpos.lon / 1e7; + } + + orb_check(parameter_update_sub, &updated); + + if (updated) { + /* copy global position */ + orb_copy(ORB_ID(parameter_update), parameter_update_sub, &update); + + /* update all parameters */ + param_get(param_height, &height); + param_get(param_gproperties, &z_0); + param_get(param_turn_radius, &turn_radius); + param_get(param_precision, &precision); + } + + // Initialization + + float windspeed_norm = sqrtf(wind.windspeed_north * wind.windspeed_north + wind.windspeed_east * wind.windspeed_east); + float groundspeed_body = sqrtf(globalpos.vel_n * globalpos.vel_n + globalpos.vel_e * globalpos.vel_e); + + distance_open_door = fabsf(t_door * groundspeed_body); + + + //warnx("absolut wind speed = %.4f", vr); //////////////////////////////////////////////////////////////////// DEBUGGING + + + //warnx("Initialization complete\n"); //////////////////////////////////////////////////////////////////// DEBUGGING + + + if (drop_approval && !state_drop) { + //warnx("approval given\n"); //////////////////////////////////////////////////////////////////// DEBUGGING + // drop here + //open_now = true; + //drop = false; + //drop_start = hrt_absolute_time(); + + // Update drop point at 10 Hz + if (counter % 10 == 0) { + + az = g; // acceleration in z direction[m/s^2] + vz = 0; // velocity in z direction [m/s] + z = 0; // fallen distance [m] + h_0 = globalpos.alt - _target_position.alt; // height over target at start[m] + h = h_0; // height over target [m] + ax = 0; // acceleration in x direction [m/s^2] + vx = groundspeed_body;// XXX project // ground speed in x direction [m/s] + x = 0; // traveled distance in x direction [m] + vw = 0; // wind speed [m/s] + vrx = 0; // relative velocity in x direction [m/s] + v = groundspeed_body; // relative speed vector [m/s] + Fd = 0; // Drag force [N] + Fdx = 0; // Drag force in x direction [N] + Fdz = 0; // Drag force in z direction [N] + + + // Compute the distance the bottle will travel after it is dropped in body frame coordinates --> x + while (h > 0.05f) { + // z-direction + vz = vz + az * dt; + z = z + vz * dt; + h = h_0 - z; + + // x-direction + vw = windspeed_norm * logf(h / z_0) / logf(ground_distance / z_0); + vx = vx + ax * dt; + x = x + vx * dt; + vrx = vx + vw; + + //Drag force + v = sqrtf(vz * vz + vrx * vrx); + Fd = 0.5f * rho * A * cd * powf(v, 2.0f); + Fdx = Fd * vrx / v; + Fdz = Fd * vz / v; + + //acceleration + az = g - Fdz / m; + ax = -Fdx / m; + } + + // Compute Drop point + x = groundspeed_body * t_signal + x; + map_projection_init(&ref, _target_position.lat, _target_position.lon); + } + + map_projection_project(&ref, _target_position.lat, _target_position.lon, &x_t, &y_t); + + float wind_direction_n, wind_direction_e; + + if (windspeed_norm < 0.5f) { // If there is no wind, an arbitrarily direction is chosen + wind_direction_n = 1.0f; + wind_direction_e = 0.0f; + + } else { + wind_direction_n = wind.windspeed_north / windspeed_norm; + wind_direction_e = wind.windspeed_east / windspeed_norm; + } + + x_drop = x_t + x * wind_direction_n; + y_drop = y_t + x * wind_direction_e; + map_projection_reproject(&ref, x_drop, y_drop, &_drop_position.lat, &_drop_position.lon); + _drop_position.alt = height; + //warnx("drop point: lat = %.7f , lon = %.7f", drop_position.lat, drop_position.lon); //////////////////////////////////////////////////////////////////// DEBUGGING + + + + // Compute flight vector + map_projection_reproject(&ref, x_drop + 2 * turn_radius * wind_direction_n, y_drop + 2 * turn_radius * wind_direction_n, + &(flight_vector_s.lat), &(flight_vector_s.lon)); + flight_vector_s.altitude = height; + map_projection_reproject(&ref, x_drop - turn_radius * wind_direction_e, y_drop - turn_radius * wind_direction_e, + &flight_vector_e.lat, &flight_vector_e.lon); + flight_vector_e.altitude = height; + //warnx("Flight vector: starting point = %.7f %.7f , end point = %.7f %.7f", flight_vector_s.lat,flight_vector_s.lon,flight_vector_e.lat,flight_vector_e.lon); //////////////////////////////////////////////////////////////////// DEBUGGING + + // Drop Cancellation if terms are not met + + // warnx("latitude:%.2f", latitude); + // warnx("longitude:%.2f", longitude); + // warnx("drop_position.lat:%.2f", drop_position.lat); + // warnx("drop_position.lon:%.2f", drop_position.lon); + + distance_real = fabsf(get_distance_to_next_waypoint(latitude, longitude, _drop_position.lat, _drop_position.lon)); + map_projection_project(&ref, latitude, longitude, &x_l, &y_l); + x_f = x_l + globalpos.vel_n * dt2; + y_f = y_l + globalpos.vel_e * dt2; + map_projection_reproject(&ref, x_f, y_f, &x_f_NED, &y_f_NED); + future_distance = fabsf(get_distance_to_next_waypoint(x_f_NED, y_f_NED, _drop_position.lat, _drop_position.lon)); + //warnx("position to-be: = %.7f %.7f" ,x_f_NED, y_f_NED ); //////////////////////////////////////////////////////////////////// DEBUGGING + + // if (counter % 10 ==0) { + // warnx("x: %.4f", x); + // warnx("drop_position.lat: %.4f, drop_position.lon: %.4f", drop_position.lat, drop_position.lon); + // warnx("latitude %.4f, longitude: %.4f", latitude, longitude); + // warnx("future_distance: %.2f, precision: %.2f", future_distance, precision); + // } + + /* Save WPs in datamanager */ + const size_t len = sizeof(struct mission_item_s); + + if (dm_write(DM_KEY_WAYPOINTS_ONBOARD, 0, DM_PERSIST_IN_FLIGHT_RESET, &flight_vector_s, len) != len) { + warnx("ERROR: could not save onboard WP"); + } + + if (dm_write(DM_KEY_WAYPOINTS_ONBOARD, 1, DM_PERSIST_IN_FLIGHT_RESET, &flight_vector_e, len) != len) { + warnx("ERROR: could not save onboard WP"); + } + + onboard_mission.count = 2; + + if (state_run && !state_drop) { + onboard_mission.current_seq = 0; + + } else { + onboard_mission.current_seq = -1; + } + + if (counter % 10 == 0) + warnx("Distance real: %.2f, distance_open_door: %.2f, angle to wind: %.2f", (double)distance_real, + (double)distance_open_door, + (double)(_wrap_pi(globalpos.yaw - atan2f(wind.windspeed_north, wind.windspeed_east)))); + + if (onboard_mission_pub > 0) { + orb_publish(ORB_ID(onboard_mission), onboard_mission_pub, &onboard_mission); + + } else { + onboard_mission_pub = orb_advertise(ORB_ID(onboard_mission), &onboard_mission); + } + + } + + if (isfinite(distance_real) && distance_real < distance_open_door && drop_approval) { + actuators.control[0] = -1.0f; // open door + actuators.control[1] = 1.0f; + state_door = true; + warnx("open doors"); + + } else { + // closed door and locked survival kit + actuators.control[0] = 0.5f; + actuators.control[1] = -0.5f; + actuators.control[2] = -0.5f; + state_door = false; + } + + if (isfinite(distance_real) && distance_real < precision && distance_real < future_distance + && state_door) { // Drop only if the distance between drop point and actual position is getting larger again + // XXX this check needs to be carefully validated - right now we prefer to drop if we're close to the goal + + // if(fabsf(_wrap_pi(globalpos.yaw-atan2f(wind.windspeed_north,wind_speed.windspeed_east)+M_PI_F)) < 80.0f/180.0f*M_PI_F) // if flight trajectory deviates more than 80 degrees from calculated path, it will no drop + // { + actuators.control[2] = 0.5f; + state_drop = true; + state_run = true; + warnx("dropping now"); + // } + // else + // { + // state_run = true; + // } + } + + actuators.timestamp = hrt_absolute_time(); + + // lazily publish _actuators only once available + if (_actuator_pub > 0) { + orb_publish(ORB_ID(actuator_controls_2), _actuator_pub, &actuators); + + } else { + _actuator_pub = orb_advertise(ORB_ID(actuator_controls_2), &actuators); + } + + counter++; + + // update_actuators(); + + // run at roughly 100 Hz + usleep(1000); } } @@ -277,6 +594,43 @@ BottleDrop::task_main() _exit(0); } +// XXX this is out of sync with the C port +// void +// BottleDrop::update_actuators() +// { +// _actuators.timestamp = hrt_absolute_time(); + +// // update door actuators +// if (_open_door) { +// _actuators.control[0] = -1.0f; +// _actuators.control[1] = 1.0f; + +// if (_doors_opened == 0) { +// _doors_opened = hrt_absolute_time(); +// } +// } else { +// _actuators.control[0] = 1.0f; +// _actuators.control[1] = -1.0f; + +// _doors_opened = 0; +// } + +// // update drop actuator, wait 0.5s until the doors are open before dropping +// if (_drop && _doors_opened != 0 && hrt_elapsed_time(&_doors_opened) > 500000) { +// _actuators.control[2] = -0.5f; +// } else { +// _actuators.control[2] = 0.5f; +// } + +// // lazily publish _actuators only once available +// if (_actuator_pub > 0) { +// orb_publish(ORB_ID(actuator_controls_2), _actuator_pub, &_actuators); + +// } else { +// _actuator_pub = orb_advertise(ORB_ID(actuator_controls_2), &_actuators); +// } +// } + void BottleDrop::handle_command(struct vehicle_command_s *cmd) { @@ -292,11 +646,12 @@ BottleDrop::handle_command(struct vehicle_command_s *cmd) _open_door = true; _drop = true; mavlink_log_info(_mavlink_fd, "#audio: drop bottle"); - } - else if (cmd->param1 > 0.5f) { + + } else if (cmd->param1 > 0.5f) { _open_door = true; _drop = false; mavlink_log_info(_mavlink_fd, "#audio: open doors"); + } else { _open_door = false; _drop = false; @@ -305,6 +660,49 @@ BottleDrop::handle_command(struct vehicle_command_s *cmd) answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); break; + + case VEHICLE_CMD_PAYLOAD_PREPARE_DEPLOY: + + switch ((int)(cmd->param1 + 0.5f)) { + case 0: + drop_approval = false; + break; + + case 1: + drop_approval = true; + break; + + default: + drop_approval = false; + break; + } + + // XXX check all fields + _target_position.lat = cmd->param5; + _target_position.lon = cmd->param6; + _target_position.alt = cmd->param7; + answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); + break; + + case VEHICLE_CMD_PAYLOAD_CONTROL_DEPLOY: + switch ((int)(cmd->param1 + 0.5f)) { + case 0: + drop_approval = false; + break; + + case 1: + drop_approval = true; + break; + + default: + drop_approval = false; + break; + // XXX handle other values + } + + answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); + break; + default: break; } @@ -376,8 +774,9 @@ int bottle_drop_main(int argc, char *argv[]) return 0; } - if (bottle_drop::g_bottle_drop == nullptr) + if (bottle_drop::g_bottle_drop == nullptr) { errx(1, "not running"); + } if (!strcmp(argv[1], "stop")) { delete bottle_drop::g_bottle_drop; @@ -385,6 +784,7 @@ int bottle_drop_main(int argc, char *argv[]) } else if (!strcmp(argv[1], "status")) { bottle_drop::g_bottle_drop->status(); + } else { usage(); } diff --git a/src/modules/bottle_drop/module.mk b/src/modules/bottle_drop/module.mk index bb1f6c6b5..61ac89e38 100644 --- a/src/modules/bottle_drop/module.mk +++ b/src/modules/bottle_drop/module.mk @@ -37,4 +37,5 @@ MODULE_COMMAND = bottle_drop -SRCS = bottle_drop.cpp +SRCS = bottle_drop.cpp \ + bottle_drop.c -- cgit v1.2.3 From f0458af28b3414cd43276c08c029be7900d0d9cf Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 11 Aug 2014 14:53:04 +0200 Subject: Extend vehicle commands as needed --- src/modules/uORB/topics/vehicle_command.h | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/src/modules/uORB/topics/vehicle_command.h b/src/modules/uORB/topics/vehicle_command.h index 15cdc517f..44aa50572 100644 --- a/src/modules/uORB/topics/vehicle_command.h +++ b/src/modules/uORB/topics/vehicle_command.h @@ -53,6 +53,9 @@ * but can contain additional ones. */ enum VEHICLE_CMD { + VEHICLE_CMD_CUSTOM_0 = 0, /* test command */ + VEHICLE_CMD_CUSTOM_1 = 1, /* test command */ + VEHICLE_CMD_CUSTOM_2 = 2, /* test command */ VEHICLE_CMD_NAV_WAYPOINT = 16, /* Navigate to MISSION. |Hold time in decimal seconds. (ignored by fixed wing, time to stay at MISSION for rotary wing)| Acceptance radius in meters (if the sphere with this radius is hit, the MISSION counts as reached)| 0 to pass through the WP, if > 0 radius in meters to pass by WP. Positive value for clockwise orbit, negative value for counter-clockwise orbit. Allows trajectory control.| Desired yaw angle at MISSION (rotary wing)| Latitude| Longitude| Altitude| */ VEHICLE_CMD_NAV_LOITER_UNLIM = 17, /* Loiter around this MISSION an unlimited amount of time |Empty| Empty| Radius around MISSION, in meters. If positive loiter clockwise, else counter-clockwise| Desired yaw angle.| Latitude| Longitude| Altitude| */ VEHICLE_CMD_NAV_LOITER_TURNS = 18, /* Loiter around this MISSION for X turns |Turns| Empty| Radius around MISSION, in meters. If positive loiter clockwise, else counter-clockwise| Desired yaw angle.| Latitude| Longitude| Altitude| */ @@ -117,8 +120,8 @@ struct vehicle_command_s { float param2; /**< Parameter 2, as defined by MAVLink VEHICLE_CMD enum. */ float param3; /**< Parameter 3, as defined by MAVLink VEHICLE_CMD enum. */ float param4; /**< Parameter 4, as defined by MAVLink VEHICLE_CMD enum. */ - float param5; /**< Parameter 5, as defined by MAVLink VEHICLE_CMD enum. */ - float param6; /**< Parameter 6, as defined by MAVLink VEHICLE_CMD enum. */ + double param5; /**< Parameter 5, as defined by MAVLink VEHICLE_CMD enum. */ + double param6; /**< Parameter 6, as defined by MAVLink VEHICLE_CMD enum. */ float param7; /**< Parameter 7, as defined by MAVLink VEHICLE_CMD enum. */ enum VEHICLE_CMD command; /**< Command ID, as defined MAVLink by VEHICLE_CMD enum. */ uint8_t target_system; /**< System which should execute the command */ -- cgit v1.2.3 From f4cf94b08427bfda10919de1cee6b58171ed5e50 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 12 Aug 2014 08:21:06 +0200 Subject: Improved rcS handling, added failsafe flag. Needs further testing for USB stability --- ROMFS/px4fmu_common/init.d/3035_viper | 2 ++ ROMFS/px4fmu_common/init.d/rc.interface | 5 +++++ ROMFS/px4fmu_common/init.d/rc.sensors | 5 +++++ ROMFS/px4fmu_common/init.d/rcS | 9 ++++----- 4 files changed, 16 insertions(+), 5 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/3035_viper b/ROMFS/px4fmu_common/init.d/3035_viper index a4c1e832d..3714b612f 100644 --- a/ROMFS/px4fmu_common/init.d/3035_viper +++ b/ROMFS/px4fmu_common/init.d/3035_viper @@ -8,3 +8,5 @@ sh /etc/init.d/rc.fw_defaults set MIXER Viper + +set FAILSAFE "-c567 -p 1000" diff --git a/ROMFS/px4fmu_common/init.d/rc.interface b/ROMFS/px4fmu_common/init.d/rc.interface index 1de0abb58..e44cd0953 100644 --- a/ROMFS/px4fmu_common/init.d/rc.interface +++ b/ROMFS/px4fmu_common/init.d/rc.interface @@ -77,4 +77,9 @@ then pwm max -c $PWM_OUTPUTS -p $PWM_MAX fi fi + + if [ $FAILSAFE != none ] + then + pwm failsafe -d $OUTPUT_DEV $FAILSAFE + fi fi diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors index ecb408a54..739df7ac0 100644 --- a/ROMFS/px4fmu_common/init.d/rc.sensors +++ b/ROMFS/px4fmu_common/init.d/rc.sensors @@ -68,6 +68,11 @@ else fi fi +# Check for flow sensor +if px4flow start +then +fi + # # Start the sensor collection task. # IMPORTANT: this also loads param offsets diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index c9e6a27ca..c1948460d 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -66,6 +66,9 @@ then # sercon + # Try to get an USB console + nshterm /dev/ttyACM0 & + # # Start the ORB (first app to start) # @@ -96,11 +99,9 @@ then # if rgbled start then - echo "[init] RGB Led" else if blinkm start then - echo "[init] BlinkM" blinkm systemstate fi fi @@ -129,6 +130,7 @@ then set LOAD_DEFAULT_APPS yes set GPS yes set GPS_FAKE no + set FAILSAFE none # # Set DO_AUTOCONFIG flag to use it in AUTOSTART scripts @@ -279,9 +281,6 @@ then fi fi - # Try to get an USB console - nshterm /dev/ttyACM0 & - # # Start the datamanager (and do not abort boot if it fails) # -- cgit v1.2.3 From 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(-) 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(-) 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 309a718db41ea683502792199d8dc9fa6fbdeabc Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 12 Aug 2014 17:57:14 +0200 Subject: Put payload outputs for AERT mixer onto right actuator group --- ROMFS/px4fmu_common/mixers/FMU_AERT.mix | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/ROMFS/px4fmu_common/mixers/FMU_AERT.mix b/ROMFS/px4fmu_common/mixers/FMU_AERT.mix index 0ec663e35..7fed488af 100644 --- a/ROMFS/px4fmu_common/mixers/FMU_AERT.mix +++ b/ROMFS/px4fmu_common/mixers/FMU_AERT.mix @@ -64,21 +64,22 @@ O: 10000 10000 0 -10000 10000 S: 0 3 0 20000 -10000 -10000 10000 -Gimbal / flaps / payload mixer for last four channels +Gimbal / flaps / payload mixer for last four channels, +using the payload control group ----------------------------------------------------- M: 1 O: 10000 10000 0 -10000 10000 -S: 0 4 10000 10000 0 -10000 10000 +S: 2 0 10000 10000 0 -10000 10000 M: 1 O: 10000 10000 0 -10000 10000 -S: 0 5 10000 10000 0 -10000 10000 +S: 2 1 10000 10000 0 -10000 10000 M: 1 O: 10000 10000 0 -10000 10000 -S: 0 6 10000 10000 0 -10000 10000 +S: 2 2 10000 10000 0 -10000 10000 M: 1 O: 10000 10000 0 -10000 10000 -S: 0 7 10000 10000 0 -10000 10000 +S: 2 3 10000 10000 0 -10000 10000 -- cgit v1.2.3 From d6c018d14b283f67cc5d1f96d8bc6de3ccd73326 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 12 Aug 2014 17:57:47 +0200 Subject: Bottle drop iteration - commandline tool for component testing --- src/modules/bottle_drop/bottle_drop.c | 48 ------- src/modules/bottle_drop/bottle_drop.cpp | 188 +++++++++++++++++---------- src/modules/bottle_drop/bottle_drop_params.c | 48 +++++++ src/modules/bottle_drop/module.mk | 2 +- 4 files changed, 168 insertions(+), 118 deletions(-) delete mode 100644 src/modules/bottle_drop/bottle_drop.c create mode 100644 src/modules/bottle_drop/bottle_drop_params.c diff --git a/src/modules/bottle_drop/bottle_drop.c b/src/modules/bottle_drop/bottle_drop.c deleted file mode 100644 index b86782075..000000000 --- a/src/modules/bottle_drop/bottle_drop.c +++ /dev/null @@ -1,48 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file bottle_drop.c - * Bottle drop parameters - * - * @author Dominik Juchli - */ - -#include -#include - -PARAM_DEFINE_FLOAT(BD_HEIGHT, 60.0f); -PARAM_DEFINE_FLOAT(BD_GPROPERTIES, 0.03f); -PARAM_DEFINE_FLOAT(BD_TURNRADIUS, 70.0f); -PARAM_DEFINE_FLOAT(BD_PRECISION, 1.0f); -PARAM_DEFINE_INT32(BD_APPROVAL, 0); diff --git a/src/modules/bottle_drop/bottle_drop.cpp b/src/modules/bottle_drop/bottle_drop.cpp index 84c7900ce..e73f36163 100644 --- a/src/modules/bottle_drop/bottle_drop.cpp +++ b/src/modules/bottle_drop/bottle_drop.cpp @@ -100,6 +100,10 @@ public: */ void status(); + void open_bay(); + void close_bay(); + void drop(); + private: bool _task_should_exit; /**< if true, task should exit */ int _main_task; /**< handle for task */ @@ -116,6 +120,7 @@ private: bool _open_door; bool _drop; hrt_abstime _doors_opened; + hrt_abstime _drop_time; struct position_s { double lat; //degrees @@ -130,6 +135,11 @@ private: void answer_command(struct vehicle_command_s *cmd, enum VEHICLE_CMD_RESULT result); + /** + * Set the actuators + */ + int actuators_publish(); + /** * Shim for calling task_main from task_create. */ @@ -153,7 +163,8 @@ BottleDrop::BottleDrop() : _actuators {}, _open_door(false), _drop(false), - _doors_opened(0) + _doors_opened(0), + _drop_time(0) { } @@ -211,6 +222,82 @@ BottleDrop::status() warnx("Dropping: %s", _drop ? "YES" : "NO"); } +void +BottleDrop::open_bay() +{ + _actuators.control[0] = -1.0f; + _actuators.control[1] = 1.0f; + + if (_doors_opened == 0) { + _doors_opened = hrt_absolute_time(); + } + warnx("open doors"); + + actuators_publish(); +} + +void +BottleDrop::close_bay() +{ + // closed door and locked survival kit + _actuators.control[0] = 1.0f; + _actuators.control[1] = -1.0f; + + _doors_opened = 0; + + actuators_publish(); +} + +void +BottleDrop::drop() +{ + + // update drop actuator, wait 0.5s until the doors are open before dropping + hrt_abstime starttime = hrt_absolute_time(); + + // force the door open if we have to + if (_doors_opened == 0) { + open_bay(); + warnx("bay not ready, forced open"); + } + + while (hrt_elapsed_time(&_doors_opened) < 400000 && hrt_elapsed_time(&starttime) < 2000000) { + usleep(50000); + warnx("delayed by door!"); + } + + if (_drop && _doors_opened != 0 && hrt_elapsed_time(&_doors_opened) < 500000) { + _actuators.control[2] = -0.5f; + } else { + _actuators.control[2] = 0.5f; + } + + _drop_time = hrt_absolute_time(); + + warnx("dropping now"); + + actuators_publish(); +} + +int +BottleDrop::actuators_publish() +{ + _actuators.timestamp = hrt_absolute_time(); + + // lazily publish _actuators only once available + if (_actuator_pub > 0) { + return orb_publish(ORB_ID(actuator_controls_2), _actuator_pub, &_actuators); + + } else { + _actuator_pub = orb_advertise(ORB_ID(actuator_controls_2), &_actuators); + if (_actuator_pub > 0) { + return OK; + } else { + return -1; + } + } +} + void BottleDrop::task_main() { @@ -272,7 +359,6 @@ BottleDrop::task_main() float future_distance = 0; // The distance between the UAVs to-be position and the drop point [m] // states - bool state_door = false; // Doors are closed = false, open = true bool state_drop = false; // Drop occurred = true, Drop din't occur = false bool state_run = false; // A drop was attempted = true, the drop is still in progress = false @@ -297,9 +383,6 @@ BottleDrop::task_main() memset(&update, 0, sizeof(update)); int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update)); - struct actuator_controls_s actuators; - memset(&actuators, 0, sizeof(actuators)); - struct mission_item_s flight_vector_s; struct mission_item_s flight_vector_e; @@ -314,6 +397,7 @@ BottleDrop::task_main() memset(&onboard_mission, 0, sizeof(onboard_mission)); orb_advert_t onboard_mission_pub = -1; + bool position_initialized = false; double latitude; double longitude; @@ -328,6 +412,9 @@ BottleDrop::task_main() fds[0].fd = _command_sub; fds[0].events = POLLIN; + // Whatever state the bay is in, we want it closed on startup + close_bay(); + while (!_task_should_exit) { /* wait for up to 100ms for data */ @@ -345,6 +432,19 @@ BottleDrop::task_main() handle_command(&_command); } + orb_check(vehicle_global_position_sub, &updated); + if (updated) { + /* copy global position */ + orb_copy(ORB_ID(vehicle_global_position), vehicle_global_position_sub, &globalpos); + + latitude = (double)globalpos.lat / 1e7; + longitude = (double)globalpos.lon / 1e7; + } + + if (!position_initialized) { + continue; + } + /* switch to faster updates during the drop */ while (_drop) { @@ -505,7 +605,7 @@ BottleDrop::task_main() // } /* Save WPs in datamanager */ - const size_t len = sizeof(struct mission_item_s); + const ssize_t len = sizeof(struct mission_item_s); if (dm_write(DM_KEY_WAYPOINTS_ONBOARD, 0, DM_PERSIST_IN_FLIGHT_RESET, &flight_vector_s, len) != len) { warnx("ERROR: could not save onboard WP"); @@ -539,29 +639,17 @@ BottleDrop::task_main() } if (isfinite(distance_real) && distance_real < distance_open_door && drop_approval) { - actuators.control[0] = -1.0f; // open door - actuators.control[1] = 1.0f; - state_door = true; - warnx("open doors"); - + open_bay(); } else { - // closed door and locked survival kit - actuators.control[0] = 0.5f; - actuators.control[1] = -0.5f; - actuators.control[2] = -0.5f; - state_door = false; + close_bay(); } - if (isfinite(distance_real) && distance_real < precision && distance_real < future_distance - && state_door) { // Drop only if the distance between drop point and actual position is getting larger again + if (isfinite(distance_real) && distance_real < precision && distance_real < future_distance) { // Drop only if the distance between drop point and actual position is getting larger again // XXX this check needs to be carefully validated - right now we prefer to drop if we're close to the goal // if(fabsf(_wrap_pi(globalpos.yaw-atan2f(wind.windspeed_north,wind_speed.windspeed_east)+M_PI_F)) < 80.0f/180.0f*M_PI_F) // if flight trajectory deviates more than 80 degrees from calculated path, it will no drop // { - actuators.control[2] = 0.5f; - state_drop = true; - state_run = true; - warnx("dropping now"); + drop(); // } // else // { @@ -569,16 +657,6 @@ BottleDrop::task_main() // } } - actuators.timestamp = hrt_absolute_time(); - - // lazily publish _actuators only once available - if (_actuator_pub > 0) { - orb_publish(ORB_ID(actuator_controls_2), _actuator_pub, &actuators); - - } else { - _actuator_pub = orb_advertise(ORB_ID(actuator_controls_2), &actuators); - } - counter++; // update_actuators(); @@ -594,43 +672,6 @@ BottleDrop::task_main() _exit(0); } -// XXX this is out of sync with the C port -// void -// BottleDrop::update_actuators() -// { -// _actuators.timestamp = hrt_absolute_time(); - -// // update door actuators -// if (_open_door) { -// _actuators.control[0] = -1.0f; -// _actuators.control[1] = 1.0f; - -// if (_doors_opened == 0) { -// _doors_opened = hrt_absolute_time(); -// } -// } else { -// _actuators.control[0] = 1.0f; -// _actuators.control[1] = -1.0f; - -// _doors_opened = 0; -// } - -// // update drop actuator, wait 0.5s until the doors are open before dropping -// if (_drop && _doors_opened != 0 && hrt_elapsed_time(&_doors_opened) > 500000) { -// _actuators.control[2] = -0.5f; -// } else { -// _actuators.control[2] = 0.5f; -// } - -// // lazily publish _actuators only once available -// if (_actuator_pub > 0) { -// orb_publish(ORB_ID(actuator_controls_2), _actuator_pub, &_actuators); - -// } else { -// _actuator_pub = orb_advertise(ORB_ID(actuator_controls_2), &_actuators); -// } -// } - void BottleDrop::handle_command(struct vehicle_command_s *cmd) { @@ -785,6 +826,15 @@ int bottle_drop_main(int argc, char *argv[]) } else if (!strcmp(argv[1], "status")) { bottle_drop::g_bottle_drop->status(); + } else if (!strcmp(argv[1], "drop")) { + bottle_drop::g_bottle_drop->drop(); + + } else if (!strcmp(argv[1], "open")) { + bottle_drop::g_bottle_drop->open_bay(); + + } else if (!strcmp(argv[1], "close")) { + bottle_drop::g_bottle_drop->close_bay(); + } else { usage(); } diff --git a/src/modules/bottle_drop/bottle_drop_params.c b/src/modules/bottle_drop/bottle_drop_params.c new file mode 100644 index 000000000..0f60b58e4 --- /dev/null +++ b/src/modules/bottle_drop/bottle_drop_params.c @@ -0,0 +1,48 @@ +/**************************************************************************** + * + * Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file bottle_drop_params.c + * Bottle drop parameters + * + * @author Dominik Juchli + */ + +#include +#include + +PARAM_DEFINE_FLOAT(BD_HEIGHT, 60.0f); +PARAM_DEFINE_FLOAT(BD_GPROPERTIES, 0.03f); +PARAM_DEFINE_FLOAT(BD_TURNRADIUS, 70.0f); +PARAM_DEFINE_FLOAT(BD_PRECISION, 1.0f); +PARAM_DEFINE_INT32(BD_APPROVAL, 0); diff --git a/src/modules/bottle_drop/module.mk b/src/modules/bottle_drop/module.mk index 61ac89e38..6b18fff55 100644 --- a/src/modules/bottle_drop/module.mk +++ b/src/modules/bottle_drop/module.mk @@ -38,4 +38,4 @@ MODULE_COMMAND = bottle_drop SRCS = bottle_drop.cpp \ - bottle_drop.c + bottle_drop_params.c -- cgit v1.2.3 From 1525341cad376c04a1e495b1ac4ac2cd870b30a2 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 14 Aug 2014 13:12:02 +0200 Subject: Disable CONFIG_ARCH_IRQPRIO in all NuttX configs --- NuttX | 2 +- nuttx-configs/aerocore/nsh/defconfig | 2 +- nuttx-configs/px4fmu-v1/nsh/defconfig | 2 +- nuttx-configs/px4fmu-v2/nsh/defconfig | 2 +- nuttx-configs/px4io-v1/nsh/defconfig | 1 - nuttx-configs/px4io-v2/nsh/defconfig | 1 - 6 files changed, 4 insertions(+), 6 deletions(-) diff --git a/NuttX b/NuttX index 088146b90..41fffa0df 160000 --- a/NuttX +++ b/NuttX @@ -1 +1 @@ -Subproject commit 088146b90eee5b614ea6386a64dae343a49a5172 +Subproject commit 41fffa0df192e4a26d2325c7ac5d9b5d7ba0211c diff --git a/nuttx-configs/aerocore/nsh/defconfig b/nuttx-configs/aerocore/nsh/defconfig index 8d0bae7b9..317194f68 100644 --- a/nuttx-configs/aerocore/nsh/defconfig +++ b/nuttx-configs/aerocore/nsh/defconfig @@ -314,7 +314,7 @@ CONFIG_STM32_I2CTIMEOTICKS=500 # CONFIG_ARCH_NOINTC is not set # CONFIG_ARCH_VECNOTIRQ is not set CONFIG_ARCH_DMA=y -CONFIG_ARCH_IRQPRIO=y +# CONFIG_ARCH_IRQPRIO is not set # CONFIG_CUSTOM_STACK is not set # CONFIG_ADDRENV is not set CONFIG_ARCH_HAVE_VFORK=y diff --git a/nuttx-configs/px4fmu-v1/nsh/defconfig b/nuttx-configs/px4fmu-v1/nsh/defconfig index a651faffa..d7697cd67 100644 --- a/nuttx-configs/px4fmu-v1/nsh/defconfig +++ b/nuttx-configs/px4fmu-v1/nsh/defconfig @@ -309,7 +309,7 @@ CONFIG_STM32_I2CTIMEOTICKS=500 # CONFIG_ARCH_NOINTC is not set # CONFIG_ARCH_VECNOTIRQ is not set CONFIG_ARCH_DMA=y -CONFIG_ARCH_IRQPRIO=y +# CONFIG_ARCH_IRQPRIO is not set # CONFIG_CUSTOM_STACK is not set # CONFIG_ADDRENV is not set CONFIG_ARCH_HAVE_VFORK=y diff --git a/nuttx-configs/px4fmu-v2/nsh/defconfig b/nuttx-configs/px4fmu-v2/nsh/defconfig index a55c95a29..bd956f7bf 100644 --- a/nuttx-configs/px4fmu-v2/nsh/defconfig +++ b/nuttx-configs/px4fmu-v2/nsh/defconfig @@ -350,7 +350,7 @@ CONFIG_SDIO_PRI=128 # CONFIG_ARCH_NOINTC is not set # CONFIG_ARCH_VECNOTIRQ is not set CONFIG_ARCH_DMA=y -CONFIG_ARCH_IRQPRIO=y +# CONFIG_ARCH_IRQPRIO is not set # CONFIG_CUSTOM_STACK is not set # CONFIG_ADDRENV is not set CONFIG_ARCH_HAVE_VFORK=y diff --git a/nuttx-configs/px4io-v1/nsh/defconfig b/nuttx-configs/px4io-v1/nsh/defconfig index 3785e0367..512c6a0f2 100755 --- a/nuttx-configs/px4io-v1/nsh/defconfig +++ b/nuttx-configs/px4io-v1/nsh/defconfig @@ -83,7 +83,6 @@ CONFIG_ARCH_BOARD="px4io-v1" CONFIG_BOARD_LOOPSPERMSEC=2000 CONFIG_DRAM_SIZE=0x00002000 CONFIG_DRAM_START=0x20000000 -CONFIG_ARCH_IRQPRIO=y CONFIG_ARCH_INTERRUPTSTACK=n CONFIG_ARCH_STACKDUMP=y CONFIG_ARCH_BOOTLOADER=n diff --git a/nuttx-configs/px4io-v2/nsh/defconfig b/nuttx-configs/px4io-v2/nsh/defconfig index e6563e587..02b51e3d7 100755 --- a/nuttx-configs/px4io-v2/nsh/defconfig +++ b/nuttx-configs/px4io-v2/nsh/defconfig @@ -79,7 +79,6 @@ CONFIG_ARCH_BOARD_PX4IO_V2=y CONFIG_BOARD_LOOPSPERMSEC=2000 CONFIG_DRAM_SIZE=0x00002000 CONFIG_DRAM_START=0x20000000 -CONFIG_ARCH_IRQPRIO=y CONFIG_ARCH_INTERRUPTSTACK=n CONFIG_ARCH_STACKDUMP=y CONFIG_ARCH_BOOTLOADER=n -- cgit v1.2.3 From 0c78794e5729d17a4649b567899cf9492a498c2b Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Thu, 14 Aug 2014 16:43:49 +0200 Subject: Fix the index numbers and command max travel in the app for the drop servo. --- ROMFS/px4fmu_common/mixers/Viper.mix | 8 +++++--- src/modules/bottle_drop/bottle_drop.cpp | 4 ++-- 2 files changed, 7 insertions(+), 5 deletions(-) diff --git a/ROMFS/px4fmu_common/mixers/Viper.mix b/ROMFS/px4fmu_common/mixers/Viper.mix index b05a8c8b0..5a0381bd8 100755 --- a/ROMFS/px4fmu_common/mixers/Viper.mix +++ b/ROMFS/px4fmu_common/mixers/Viper.mix @@ -58,12 +58,14 @@ Inputs to the mixer come from channel group 2 (payload), channels 0 M: 1 O: 10000 10000 0 -10000 10000 -S: 2 4 10000 10000 0 -10000 10000 +S: 2 0 10000 10000 0 -10000 10000 M: 1 O: 10000 10000 0 -10000 10000 -S: 2 5 10000 10000 0 -10000 10000 +S: 2 1 10000 10000 0 -10000 10000 M: 1 O: 10000 10000 0 -10000 10000 -S: 2 6 10000 10000 0 -10000 10000 +S: 2 2 -10000 -10000 0 -10000 10000 + + diff --git a/src/modules/bottle_drop/bottle_drop.cpp b/src/modules/bottle_drop/bottle_drop.cpp index e73f36163..e5acc3c7e 100644 --- a/src/modules/bottle_drop/bottle_drop.cpp +++ b/src/modules/bottle_drop/bottle_drop.cpp @@ -267,9 +267,9 @@ BottleDrop::drop() } if (_drop && _doors_opened != 0 && hrt_elapsed_time(&_doors_opened) < 500000) { - _actuators.control[2] = -0.5f; + _actuators.control[2] = -1.0f; } else { - _actuators.control[2] = 0.5f; + _actuators.control[2] = 1.0f; } _drop_time = hrt_absolute_time(); -- cgit v1.2.3 From 9ee6ab366d2a7c6f24e2f08021da9cd861663bdc Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 14 Aug 2014 17:30:05 +0200 Subject: add flighttermination circuit breaker --- src/modules/systemlib/circuit_breaker.c | 13 +++++++++++++ src/modules/systemlib/circuit_breaker.h | 1 + 2 files changed, 14 insertions(+) diff --git a/src/modules/systemlib/circuit_breaker.c b/src/modules/systemlib/circuit_breaker.c index 64317a18a..8a0b51b71 100644 --- a/src/modules/systemlib/circuit_breaker.c +++ b/src/modules/systemlib/circuit_breaker.c @@ -95,6 +95,19 @@ PARAM_DEFINE_INT32(CBRK_IO_SAFETY, 0); */ PARAM_DEFINE_INT32(CBRK_AIRSPD_CHK, 0); +/** + * Circuit breaker for flight termination + * + * Setting this parameter to 121212 will disable the flight termination action. + * --> The IO driver will not do flight termination if requested by the FMU + * WARNING: ENABLING THIS CIRCUIT BREAKER IS AT OWN RISK + * + * @min 0 + * @max 121212 + * @group Circuit Breaker + */ +PARAM_DEFINE_INT32(CBRK_FLIGHTTERMINATION, 0); + bool circuit_breaker_enabled(const char* breaker, int32_t magic) { int32_t val; diff --git a/src/modules/systemlib/circuit_breaker.h b/src/modules/systemlib/circuit_breaker.h index b60584608..54c4fced6 100644 --- a/src/modules/systemlib/circuit_breaker.h +++ b/src/modules/systemlib/circuit_breaker.h @@ -53,6 +53,7 @@ #define CBRK_RATE_CTRL_KEY 140253 #define CBRK_IO_SAFETY_KEY 22027 #define CBRK_AIRSPD_CHK_KEY 162128 +#define CBRK_FLIGHTTERMINATION_KEY 121212 #include -- cgit v1.2.3 From 0f2b66fa8b3782c965966e9c0b0b6f16b80d7f38 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 14 Aug 2014 22:38:48 +0200 Subject: failsafe: enable support for commands --- src/modules/commander/commander.cpp | 25 +++++++++++++++++++++++++ src/modules/commander/state_machine_helper.cpp | 11 ++++++++++- src/modules/navigator/navigator_main.cpp | 2 +- src/modules/uORB/topics/vehicle_status.h | 5 ++++- 4 files changed, 40 insertions(+), 3 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index c6f56d5e3..b635a22cb 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -557,6 +557,31 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s armed_local->force_failsafe = false; warnx("disabling failsafe"); } + /* param2 is currently used for other failsafe modes */ + status_local->engine_failure_cmd = false; + status_local->data_link_lost_cmd = false; + status_local->gps_failure_cmd = false; + status_local->rc_signal_lost_cmd = false; + if ((int)cmd->param2 <= 0) { + /* reset all commanded failure modes */ + warnx("revert to normal mode"); + } else if ((int)cmd->param2 == 1) { + /* trigger engine failure mode */ + status_local->engine_failure_cmd = true; + warnx("engine failure mode commanded"); + } else if ((int)cmd->param2 == 2) { + /* trigger data link loss mode */ + status_local->data_link_lost_cmd = true; + warnx("data link loss mode commanded"); + } else if ((int)cmd->param2 == 3) { + /* trigger gps loss mode */ + status_local->gps_failure_cmd = true; + warnx("gps loss mode commanded"); + } else if ((int)cmd->param2 == 4) { + /* trigger rc loss mode */ + status_local->rc_signal_lost_cmd = true; + warnx("rc loss mode commanded"); + } cmd_result = VEHICLE_CMD_RESULT_ACCEPTED; } break; diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index ecfe62e03..157e35ef8 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -491,10 +491,19 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en case MAIN_STATE_AUTO_MISSION: /* go into failsafe + * - if commanded to do so * - if we have an engine failure * - if either the datalink is enabled and lost as well as RC is lost * - if there is no datalink and the mission is finished */ - if (status->engine_failure) { + if (status->engine_failure_cmd) { + status->nav_state = NAVIGATION_STATE_AUTO_LANDENGFAIL; + } else if (status->data_link_lost_cmd) { + status->nav_state = NAVIGATION_STATE_AUTO_RTGS; + //} else if (status->gps_failure_cmd) { + //status->nav_state = NAVIGATION_STATE_AUTO_***; + } else if (status->rc_signal_lost_cmd) { + status->nav_state = NAVIGATION_STATE_AUTO_RTGS; //XXX + } else if (status->engine_failure) { status->nav_state = NAVIGATION_STATE_AUTO_LANDENGFAIL; } else if (((status->data_link_lost && data_link_loss_enabled) && status->rc_signal_lost) || (!data_link_loss_enabled && status->rc_signal_lost && mission_finished)) { diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 043d883b2..4af37fa3e 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -384,7 +384,7 @@ Navigator::task_main() case NAVIGATION_STATE_AUTO_RTL: _navigation_mode = &_rtl; break; - case NAVIGATION_STATE_AUTO_RTGS: + case NAVIGATION_STATE_AUTO_RTGS: //XXX OBC: differentiate between rc loss and dl loss here /* Use complex data link loss mode only when enabled via param * otherwise use rtl */ if (_param_datalinkloss_obc.get() != 0) { diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index 301503b82..b465f8407 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -200,13 +200,16 @@ struct vehicle_status_s { bool rc_signal_found_once; bool rc_signal_lost; /**< true if RC reception lost */ + bool rc_signal_lost_cmd; /**< true if RC lost mode is commanded */ bool rc_input_blocked; /**< set if RC input should be ignored */ bool data_link_lost; /**< datalink to GCS lost */ + bool data_link_lost_cmd; /**< datalink to GCS lost mode commanded */ uint8_t data_link_lost_counter; /**< counts unique data link lost events */ - bool engine_failure; /** Set to true if an engine failure is detected */ + bool engine_failure_cmd; /** Set to true if an engine failure mode is commanded */ bool gps_failure; /** Set to true if a gps failure is detected */ + bool gps_failure_cmd; /** Set to true if a gps failure mode is commanded */ bool offboard_control_signal_found_once; bool offboard_control_signal_lost; -- cgit v1.2.3 From 6fe0482b277cd7120878787171c735e2d424c1c6 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 15 Aug 2014 00:07:38 +0200 Subject: failsafe: make warnx more clear --- src/modules/commander/commander.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index b635a22cb..4f996b526 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -552,10 +552,10 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s if (cmd->param1 > 0.5f) { //XXX update state machine? armed_local->force_failsafe = true; - warnx("forcing failsafe"); + warnx("forcing failsafe (termination)"); } else { armed_local->force_failsafe = false; - warnx("disabling failsafe"); + warnx("disabling failsafe (termination)"); } /* param2 is currently used for other failsafe modes */ status_local->engine_failure_cmd = false; -- cgit v1.2.3 From 85d8781bc32fc25b30350aab518a8b9823cce661 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 15 Aug 2014 01:26:04 +0200 Subject: datalinkloss: improve logic --- src/modules/navigator/datalinkloss.cpp | 34 +++++++++++++++++++++++----------- 1 file changed, 23 insertions(+), 11 deletions(-) diff --git a/src/modules/navigator/datalinkloss.cpp b/src/modules/navigator/datalinkloss.cpp index 19f335633..6c5012bfd 100644 --- a/src/modules/navigator/datalinkloss.cpp +++ b/src/modules/navigator/datalinkloss.cpp @@ -58,14 +58,14 @@ DataLinkLoss::DataLinkLoss(Navigator *navigator, const char *name) : MissionBlock(navigator, name), _vehicleStatus(&getSubscriptions(), ORB_ID(vehicle_status), 100), - _param_commsholdwaittime(this, "CH_T"), - _param_commsholdlat(this, "CH_LAT"), - _param_commsholdlon(this, "CH_LON"), - _param_commsholdalt(this, "CH_ALT"), - _param_airfieldhomelat(this, "AH_LAT"), - _param_airfieldhomelon(this, "AH_LON"), - _param_airfieldhomealt(this, "AH_ALT"), - _param_numberdatalinklosses(this, "DLL_N"), + _param_commsholdwaittime(this, "NAV_DLL_CH_T", false), + _param_commsholdlat(this, "NAV_DLL_CH_LAT", false), + _param_commsholdlon(this, "NAV_DLL_CH_LON", false), + _param_commsholdalt(this, "NAV_DLL_CH_ALT", false), + _param_airfieldhomelat(this, "NAV_DLL_AH_LAT", false), + _param_airfieldhomelon(this, "NAV_DLL_AH_LON", false), + _param_airfieldhomealt(this, "NAV_DLL_AH_ALT", false), + _param_numberdatalinklosses(this, "NAV_DLL_N", false), _dll_state(DLL_STATE_NONE) { /* load initial params */ @@ -81,7 +81,7 @@ DataLinkLoss::~DataLinkLoss() void DataLinkLoss::on_inactive() { - /* reset RTL state only if setpoint moved */ + /* reset DLL state only if setpoint moved */ if (!_navigator->get_can_loiter_at_sp()) { _dll_state = DLL_STATE_NONE; } @@ -90,7 +90,8 @@ DataLinkLoss::on_inactive() void DataLinkLoss::on_activation() { - _dll_state = DLL_STATE_FLYTOCOMMSHOLDWP; + _dll_state = DLL_STATE_NONE; + advance_dll(); set_dll_item(); } @@ -131,6 +132,10 @@ DataLinkLoss::set_dll_item() _mission_item.origin = ORIGIN_ONBOARD; _navigator->set_can_loiter_at_sp(true); + warnx("mission item: lat %.7f lon %.7f alt %.3f", + _mission_item.lat, + _mission_item.lon, + (double)_mission_item.altitude); break; } case DLL_STATE_FLYTOAIRFIELDHOMEWP: { @@ -159,6 +164,10 @@ DataLinkLoss::set_dll_item() /* convert mission item to current position setpoint and make it valid */ mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current); pos_sp_triplet->next.valid = false; + warnx("triplet current: lat %.7f lon %.7f alt %.3f", + pos_sp_triplet->current.lat, + pos_sp_triplet->current.lon, + (double)pos_sp_triplet->current.alt); _navigator->set_position_setpoint_triplet_updated(); } @@ -166,18 +175,21 @@ DataLinkLoss::set_dll_item() void DataLinkLoss::advance_dll() { + warnx("dll_state %u", _dll_state); switch (_dll_state) { case DLL_STATE_NONE: /* Check the number of data link losses. If above home fly home directly */ updateSubscriptions(); if (_vehicleStatus.data_link_lost_counter > _param_numberdatalinklosses.get()) { + warnx("too many data link losses, fly to airfield home"); _dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP; } else { + warnx("fly to comms hold"); _dll_state = DLL_STATE_FLYTOCOMMSHOLDWP; } break; case DLL_STATE_FLYTOCOMMSHOLDWP: - //XXX check here if time is over are over + warnx("fly to airfield home"); _dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP; _navigator->get_mission_result()->stay_in_failsafe = true; _navigator->publish_mission_result(); -- cgit v1.2.3 From 8bc7d7f43dd350c19993cd12b514a6a0b5366b3c Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 15 Aug 2014 01:26:43 +0200 Subject: navigator: correctly use all navigator modes --- src/modules/navigator/navigator.h | 3 +-- src/modules/navigator/navigator_main.cpp | 2 ++ 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index fe6639dfe..536977972 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -65,9 +65,8 @@ /** * Number of navigation modes that need on_active/on_inactive calls - * Currently: mission, loiter, and rtl */ -#define NAVIGATOR_MODE_ARRAY_SIZE 4 +#define NAVIGATOR_MODE_ARRAY_SIZE 6 class Navigator : public control::SuperBlock { diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 4af37fa3e..18bfc2cec 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -141,6 +141,8 @@ Navigator::Navigator() : _navigation_mode_array[1] = &_loiter; _navigation_mode_array[2] = &_rtl; _navigation_mode_array[3] = &_offboard; + _navigation_mode_array[4] = &_dataLinkLoss; + _navigation_mode_array[5] = &_engineFailure; updateParams(); } -- cgit v1.2.3 From b71778d7e1d03adf8b1366982f0e86753ac072be Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 15 Aug 2014 01:41:48 +0200 Subject: datalinkloss: change default param value --- src/modules/navigator/datalinkloss_params.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/navigator/datalinkloss_params.c b/src/modules/navigator/datalinkloss_params.c index 038c80a1a..77a8763cb 100644 --- a/src/modules/navigator/datalinkloss_params.c +++ b/src/modules/navigator/datalinkloss_params.c @@ -67,7 +67,7 @@ PARAM_DEFINE_FLOAT(NAV_DLL_CH_T, 120.0f); * @min 0.0 * @group DLL */ -PARAM_DEFINE_INT32(NAV_DLL_CH_LAT, 266072120); +PARAM_DEFINE_INT32(NAV_DLL_CH_LAT, -266072120); /** * Comms hold Lon @@ -100,7 +100,7 @@ PARAM_DEFINE_FLOAT(NAV_DLL_CH_ALT, 600.0f); * @min 0.0 * @group DLL */ -PARAM_DEFINE_INT32(NAV_DLL_AH_LAT, 265847810); +PARAM_DEFINE_INT32(NAV_DLL_AH_LAT, -265847810); /** * Airfield home Lon -- 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(-) 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 8dbe6a6dc0d753a0f092fc2d4be508e48429c008 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 15 Aug 2014 14:35:45 +0200 Subject: px4io driver: use flighttermination circuit breaker --- src/drivers/px4io/px4io.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 32069cf09..afb03789f 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1169,7 +1169,8 @@ PX4IO::io_set_arming_state() clear |= PX4IO_P_SETUP_ARMING_LOCKDOWN; } - if (armed.force_failsafe) { + /* Do not set failsafe if circuit breaker is enabled */ + if (armed.force_failsafe && !circuit_breaker_enabled("CBRK_FLIGHTTERMINATION", CBRK_FLIGHTTERMINATION_KEY)) { set |= PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE; } else { clear |= PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE; -- cgit v1.2.3 From 20ceba48cfc4e306f2dc4f5f98d0730730cd3233 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 15 Aug 2014 20:34:04 +0200 Subject: Do not modify startup where not absolutely required --- ROMFS/px4fmu_common/init.d/rc.fw_apps | 2 -- 1 file changed, 2 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.fw_apps b/ROMFS/px4fmu_common/init.d/rc.fw_apps index 767c54629..c97b3477f 100644 --- a/ROMFS/px4fmu_common/init.d/rc.fw_apps +++ b/ROMFS/px4fmu_common/init.d/rc.fw_apps @@ -15,5 +15,3 @@ fw_att_control start fw_pos_control_l1 start bottle_drop start -fmu mode_pwm -mixer load /dev/px4fmu /etc/mixers/FMU_pass.mix -- cgit v1.2.3 From 5406ba78a8de82c49aa9ea8e033c82670950259d Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 15 Aug 2014 20:53:03 +0200 Subject: make navigator mode a child of navigator --- src/modules/navigator/datalinkloss.cpp | 19 +++++++++++-------- src/modules/navigator/mission.cpp | 6 +++--- src/modules/navigator/navigator_mode.cpp | 2 +- src/modules/navigator/rtl.cpp | 6 +++--- 4 files changed, 18 insertions(+), 15 deletions(-) diff --git a/src/modules/navigator/datalinkloss.cpp b/src/modules/navigator/datalinkloss.cpp index 6c5012bfd..2efca0a94 100644 --- a/src/modules/navigator/datalinkloss.cpp +++ b/src/modules/navigator/datalinkloss.cpp @@ -58,14 +58,14 @@ DataLinkLoss::DataLinkLoss(Navigator *navigator, const char *name) : MissionBlock(navigator, name), _vehicleStatus(&getSubscriptions(), ORB_ID(vehicle_status), 100), - _param_commsholdwaittime(this, "NAV_DLL_CH_T", false), - _param_commsholdlat(this, "NAV_DLL_CH_LAT", false), - _param_commsholdlon(this, "NAV_DLL_CH_LON", false), - _param_commsholdalt(this, "NAV_DLL_CH_ALT", false), - _param_airfieldhomelat(this, "NAV_DLL_AH_LAT", false), - _param_airfieldhomelon(this, "NAV_DLL_AH_LON", false), - _param_airfieldhomealt(this, "NAV_DLL_AH_ALT", false), - _param_numberdatalinklosses(this, "NAV_DLL_N", false), + _param_commsholdwaittime(this, "CH_T"), + _param_commsholdlat(this, "CH_LAT"), + _param_commsholdlon(this, "CH_LON"), + _param_commsholdalt(this, "CH_ALT"), + _param_airfieldhomelat(this, "AH_LAT"), + _param_airfieldhomelon(this, "AH_LON"), + _param_airfieldhomealt(this, "AH_ALT"), + _param_numberdatalinklosses(this, "N"), _dll_state(DLL_STATE_NONE) { /* load initial params */ @@ -182,14 +182,17 @@ DataLinkLoss::advance_dll() updateSubscriptions(); if (_vehicleStatus.data_link_lost_counter > _param_numberdatalinklosses.get()) { warnx("too many data link losses, fly to airfield home"); + mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: too many DL losses, fly to home"); _dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP; } else { warnx("fly to comms hold"); + mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: fly to comms hold"); _dll_state = DLL_STATE_FLYTOCOMMSHOLDWP; } break; case DLL_STATE_FLYTOCOMMSHOLDWP: warnx("fly to airfield home"); + mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: fly to home"); _dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP; _navigator->get_mission_result()->stay_in_failsafe = true; _navigator->publish_mission_result(); diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index c76192f66..79865e2ae 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -59,9 +59,9 @@ Mission::Mission(Navigator *navigator, const char *name) : MissionBlock(navigator, name), - _param_onboard_enabled(this, "ONBOARD_EN"), - _param_takeoff_alt(this, "TAKEOFF_ALT"), - _param_dist_1wp(this, "DIST_1WP"), + _param_onboard_enabled(this, "MIS_ONBOARD_EN", false), + _param_takeoff_alt(this, "MIS_TAKEOFF_ALT", false), + _param_dist_1wp(this, "MIS_DIST_1WP", false), _onboard_mission({0}), _offboard_mission({0}), _current_onboard_mission_index(-1), diff --git a/src/modules/navigator/navigator_mode.cpp b/src/modules/navigator/navigator_mode.cpp index 3c6754c55..d91f7ab18 100644 --- a/src/modules/navigator/navigator_mode.cpp +++ b/src/modules/navigator/navigator_mode.cpp @@ -43,7 +43,7 @@ #include "navigator.h" NavigatorMode::NavigatorMode(Navigator *navigator, const char *name) : - SuperBlock(NULL, name), + SuperBlock(navigator, name), _navigator(navigator), _first_run(true) { diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index 142a73409..b6c4b8fdf 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -58,9 +58,9 @@ RTL::RTL(Navigator *navigator, const char *name) : MissionBlock(navigator, name), _rtl_state(RTL_STATE_NONE), - _param_return_alt(this, "RETURN_ALT"), - _param_descend_alt(this, "DESCEND_ALT"), - _param_land_delay(this, "LAND_DELAY") + _param_return_alt(this, "RTL_RETURN_ALT", false), + _param_descend_alt(this, "RTL_DESCEND_ALT", false), + _param_land_delay(this, "RTL_LAND_DELAY", false) { /* load initial params */ updateParams(); -- cgit v1.2.3 From 2791a7097686d327e91ac31c20716bb602011d65 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 15 Aug 2014 20:57:21 +0200 Subject: remove warnx --- src/modules/navigator/datalinkloss.cpp | 8 -------- 1 file changed, 8 deletions(-) diff --git a/src/modules/navigator/datalinkloss.cpp b/src/modules/navigator/datalinkloss.cpp index 2efca0a94..7b9a7b151 100644 --- a/src/modules/navigator/datalinkloss.cpp +++ b/src/modules/navigator/datalinkloss.cpp @@ -132,10 +132,6 @@ DataLinkLoss::set_dll_item() _mission_item.origin = ORIGIN_ONBOARD; _navigator->set_can_loiter_at_sp(true); - warnx("mission item: lat %.7f lon %.7f alt %.3f", - _mission_item.lat, - _mission_item.lon, - (double)_mission_item.altitude); break; } case DLL_STATE_FLYTOAIRFIELDHOMEWP: { @@ -164,10 +160,6 @@ DataLinkLoss::set_dll_item() /* convert mission item to current position setpoint and make it valid */ mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current); pos_sp_triplet->next.valid = false; - warnx("triplet current: lat %.7f lon %.7f alt %.3f", - pos_sp_triplet->current.lat, - pos_sp_triplet->current.lon, - (double)pos_sp_triplet->current.alt); _navigator->set_position_setpoint_triplet_updated(); } -- cgit v1.2.3 From f8f72d665d2a0f07d14bb9c27293423e715486c4 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 15 Aug 2014 23:40:53 +0200 Subject: Last drop fixes and adjustments --- src/modules/bottle_drop/bottle_drop.cpp | 121 +++++++++++++++------------ src/modules/bottle_drop/bottle_drop_params.c | 4 +- 2 files changed, 67 insertions(+), 58 deletions(-) diff --git a/src/modules/bottle_drop/bottle_drop.cpp b/src/modules/bottle_drop/bottle_drop.cpp index e5acc3c7e..4367d1343 100644 --- a/src/modules/bottle_drop/bottle_drop.cpp +++ b/src/modules/bottle_drop/bottle_drop.cpp @@ -112,16 +112,20 @@ private: int _command_sub; int _wind_estimate_sub; struct vehicle_command_s _command; + struct vehicle_global_position_s _global_pos; + map_projection_reference_s ref; orb_advert_t _actuator_pub; struct actuator_controls_s _actuators; - bool drop_approval; + bool _drop_approval; bool _open_door; bool _drop; hrt_abstime _doors_opened; hrt_abstime _drop_time; + float _alt_clearance; + struct position_s { double lat; //degrees double lon; //degrees @@ -159,12 +163,16 @@ BottleDrop::BottleDrop() : _command_sub(-1), _wind_estimate_sub(-1), _command {}, - _actuator_pub(-1), - _actuators {}, - _open_door(false), - _drop(false), - _doors_opened(0), - _drop_time(0) + _global_pos {}, + ref {}, + _actuator_pub(-1), + _actuators {}, + _drop_approval(false), + _open_door(false), + _drop(false), + _doors_opened(0), + _drop_time(0), + _alt_clearance(0) { } @@ -310,7 +318,6 @@ BottleDrop::task_main() bool updated = false; - float height; // height at which the normal should be dropped NED float z_0; // ground properties float turn_radius; // turn radius of the UAV float precision; // Expected precision of the UAV @@ -319,12 +326,12 @@ BottleDrop::task_main() float ground_distance = 70.0f; // constant - float g = 9.81f; // constant of gravity [m/s^2] + float g = CONSTANTS_ONE_G; // constant of gravity [m/s^2] float m = 0.5f; // mass of bottle [kg] float rho = 1.2f; // air density [kg/m^3] - float A = (powf(0.063f, 2.0f) / 4.0f * M_PI_F); // Bottle cross section [m^2] - float dt = 0.01f; // step size [s] - float dt2 = 0.05f; // step size 2 [s] + float A = ((0.063f * 0.063f) / 4.0f * M_PI_F); // Bottle cross section [m^2] + float dt_freefall_prediction = 0.01f; // step size of the free fall prediction [s] + float dt_runs = 0.05f; // step size 2 [s] // Has to be estimated by experiment float cd = 0.86f; // Drag coefficient for a cylinder with a d/l ratio of 1/3 [] @@ -352,8 +359,8 @@ BottleDrop::task_main() float x_drop, y_drop; // coordinates of the drop point in reference to the target (projection of NED) float x_t, y_t; // coordinates of the target in reference to the target x_t = 0, y_t = 0 (projection of NED) float x_l, y_l; // local position in projected coordinates - float x_f, y_f; // to-be position of the UAV after dt2 seconds in projected coordinates - double x_f_NED, y_f_NED; // to-be position of the UAV after dt2 seconds in NED + float x_f, y_f; // to-be position of the UAV after dt_runs seconds in projected coordinates + double x_f_NED, y_f_NED; // to-be position of the UAV after dt_runs seconds in NED float distance_open_door; // The distance the UAV travels during its doors open [m] float distance_real = 0; // The distance between the UAVs position and the drop point [m] float future_distance = 0; // The distance between the UAVs to-be position and the drop point [m] @@ -364,7 +371,6 @@ BottleDrop::task_main() unsigned counter = 0; - param_t param_height = param_find("BD_HEIGHT"); param_t param_gproperties = param_find("BD_GPROPERTIES"); param_t param_turn_radius = param_find("BD_TURNRADIUS"); param_t param_precision = param_find("BD_PRECISION"); @@ -372,11 +378,8 @@ BottleDrop::task_main() param_get(param_precision, &precision); param_get(param_turn_radius, &turn_radius); - param_get(param_height, &height); param_get(param_gproperties, &z_0); - struct vehicle_global_position_s globalpos; - memset(&globalpos, 0, sizeof(globalpos)); int vehicle_global_position_sub = orb_subscribe(ORB_ID(vehicle_global_position)); struct parameter_update_s update; @@ -389,6 +392,7 @@ BottleDrop::task_main() flight_vector_s.nav_cmd = NAV_CMD_WAYPOINT; flight_vector_s.acceptance_radius = 50; // TODO: make parameter flight_vector_s.autocontinue = true; + flight_vector_e.nav_cmd = NAV_CMD_WAYPOINT; flight_vector_e.acceptance_radius = 50; // TODO: make parameter flight_vector_e.autocontinue = true; @@ -397,14 +401,11 @@ BottleDrop::task_main() memset(&onboard_mission, 0, sizeof(onboard_mission)); orb_advert_t onboard_mission_pub = -1; - bool position_initialized = false; double latitude; double longitude; struct wind_estimate_s wind; - map_projection_reference_s ref; - /* wakeup source(s) */ struct pollfd fds[1]; @@ -435,16 +436,21 @@ BottleDrop::task_main() orb_check(vehicle_global_position_sub, &updated); if (updated) { /* copy global position */ - orb_copy(ORB_ID(vehicle_global_position), vehicle_global_position_sub, &globalpos); + orb_copy(ORB_ID(vehicle_global_position), vehicle_global_position_sub, &_global_pos); - latitude = (double)globalpos.lat / 1e7; - longitude = (double)globalpos.lon / 1e7; + latitude = (double)_global_pos.lat / 1e7; + longitude = (double)_global_pos.lon / 1e7; } - if (!position_initialized) { + if (_global_pos.timestamp == 0) { continue; } + const unsigned sleeptime_us = 10000; + + hrt_abstime last_run = hrt_absolute_time(); + dt_runs = 1e6f / sleeptime_us; + /* switch to faster updates during the drop */ while (_drop) { @@ -464,10 +470,10 @@ BottleDrop::task_main() if (updated) { /* copy global position */ - orb_copy(ORB_ID(vehicle_global_position), vehicle_global_position_sub, &globalpos); + orb_copy(ORB_ID(vehicle_global_position), vehicle_global_position_sub, &_global_pos); - latitude = (double)globalpos.lat / 1e7; - longitude = (double)globalpos.lon / 1e7; + latitude = (double)_global_pos.lat / 1e7; + longitude = (double)_global_pos.lon / 1e7; } orb_check(parameter_update_sub, &updated); @@ -477,7 +483,6 @@ BottleDrop::task_main() orb_copy(ORB_ID(parameter_update), parameter_update_sub, &update); /* update all parameters */ - param_get(param_height, &height); param_get(param_gproperties, &z_0); param_get(param_turn_radius, &turn_radius); param_get(param_precision, &precision); @@ -486,7 +491,7 @@ BottleDrop::task_main() // Initialization float windspeed_norm = sqrtf(wind.windspeed_north * wind.windspeed_north + wind.windspeed_east * wind.windspeed_east); - float groundspeed_body = sqrtf(globalpos.vel_n * globalpos.vel_n + globalpos.vel_e * globalpos.vel_e); + float groundspeed_body = sqrtf(_global_pos.vel_n * _global_pos.vel_n + _global_pos.vel_e * _global_pos.vel_e); distance_open_door = fabsf(t_door * groundspeed_body); @@ -497,7 +502,7 @@ BottleDrop::task_main() //warnx("Initialization complete\n"); //////////////////////////////////////////////////////////////////// DEBUGGING - if (drop_approval && !state_drop) { + if (_drop_approval && !state_drop) { //warnx("approval given\n"); //////////////////////////////////////////////////////////////////// DEBUGGING // drop here //open_now = true; @@ -510,7 +515,7 @@ BottleDrop::task_main() az = g; // acceleration in z direction[m/s^2] vz = 0; // velocity in z direction [m/s] z = 0; // fallen distance [m] - h_0 = globalpos.alt - _target_position.alt; // height over target at start[m] + h_0 = _global_pos.alt - _target_position.alt; // height over target at start[m] h = h_0; // height over target [m] ax = 0; // acceleration in x direction [m/s^2] vx = groundspeed_body;// XXX project // ground speed in x direction [m/s] @@ -526,14 +531,14 @@ BottleDrop::task_main() // Compute the distance the bottle will travel after it is dropped in body frame coordinates --> x while (h > 0.05f) { // z-direction - vz = vz + az * dt; - z = z + vz * dt; + vz = vz + az * dt_freefall_prediction; + z = z + vz * dt_freefall_prediction; h = h_0 - z; // x-direction vw = windspeed_norm * logf(h / z_0) / logf(ground_distance / z_0); - vx = vx + ax * dt; - x = x + vx * dt; + vx = vx + ax * dt_freefall_prediction; + x = x + vx * dt_freefall_prediction; vrx = vx + vw; //Drag force @@ -547,12 +552,12 @@ BottleDrop::task_main() ax = -Fdx / m; } - // Compute Drop point + // Compute drop vector x = groundspeed_body * t_signal + x; - map_projection_init(&ref, _target_position.lat, _target_position.lon); } - map_projection_project(&ref, _target_position.lat, _target_position.lon, &x_t, &y_t); + x_t = 0.0f; + y_t = 0.0f; float wind_direction_n, wind_direction_e; @@ -568,7 +573,6 @@ BottleDrop::task_main() x_drop = x_t + x * wind_direction_n; y_drop = y_t + x * wind_direction_e; map_projection_reproject(&ref, x_drop, y_drop, &_drop_position.lat, &_drop_position.lon); - _drop_position.alt = height; //warnx("drop point: lat = %.7f , lon = %.7f", drop_position.lat, drop_position.lon); //////////////////////////////////////////////////////////////////// DEBUGGING @@ -576,10 +580,10 @@ BottleDrop::task_main() // Compute flight vector map_projection_reproject(&ref, x_drop + 2 * turn_radius * wind_direction_n, y_drop + 2 * turn_radius * wind_direction_n, &(flight_vector_s.lat), &(flight_vector_s.lon)); - flight_vector_s.altitude = height; + flight_vector_s.altitude = _drop_position.alt + _alt_clearance; map_projection_reproject(&ref, x_drop - turn_radius * wind_direction_e, y_drop - turn_radius * wind_direction_e, &flight_vector_e.lat, &flight_vector_e.lon); - flight_vector_e.altitude = height; + flight_vector_e.altitude = _drop_position.alt + _alt_clearance; //warnx("Flight vector: starting point = %.7f %.7f , end point = %.7f %.7f", flight_vector_s.lat,flight_vector_s.lon,flight_vector_e.lat,flight_vector_e.lon); //////////////////////////////////////////////////////////////////// DEBUGGING // Drop Cancellation if terms are not met @@ -591,8 +595,8 @@ BottleDrop::task_main() distance_real = fabsf(get_distance_to_next_waypoint(latitude, longitude, _drop_position.lat, _drop_position.lon)); map_projection_project(&ref, latitude, longitude, &x_l, &y_l); - x_f = x_l + globalpos.vel_n * dt2; - y_f = y_l + globalpos.vel_e * dt2; + x_f = x_l + _global_pos.vel_n * dt_runs; + y_f = y_l + _global_pos.vel_e * dt_runs; map_projection_reproject(&ref, x_f, y_f, &x_f_NED, &y_f_NED); future_distance = fabsf(get_distance_to_next_waypoint(x_f_NED, y_f_NED, _drop_position.lat, _drop_position.lon)); //warnx("position to-be: = %.7f %.7f" ,x_f_NED, y_f_NED ); //////////////////////////////////////////////////////////////////// DEBUGGING @@ -627,7 +631,7 @@ BottleDrop::task_main() if (counter % 10 == 0) warnx("Distance real: %.2f, distance_open_door: %.2f, angle to wind: %.2f", (double)distance_real, (double)distance_open_door, - (double)(_wrap_pi(globalpos.yaw - atan2f(wind.windspeed_north, wind.windspeed_east)))); + (double)(_wrap_pi(_global_pos.yaw - atan2f(wind.windspeed_north, wind.windspeed_east)))); if (onboard_mission_pub > 0) { orb_publish(ORB_ID(onboard_mission), onboard_mission_pub, &onboard_mission); @@ -638,7 +642,7 @@ BottleDrop::task_main() } - if (isfinite(distance_real) && distance_real < distance_open_door && drop_approval) { + if (isfinite(distance_real) && distance_real < distance_open_door && _drop_approval) { open_bay(); } else { close_bay(); @@ -647,7 +651,7 @@ BottleDrop::task_main() if (isfinite(distance_real) && distance_real < precision && distance_real < future_distance) { // Drop only if the distance between drop point and actual position is getting larger again // XXX this check needs to be carefully validated - right now we prefer to drop if we're close to the goal - // if(fabsf(_wrap_pi(globalpos.yaw-atan2f(wind.windspeed_north,wind_speed.windspeed_east)+M_PI_F)) < 80.0f/180.0f*M_PI_F) // if flight trajectory deviates more than 80 degrees from calculated path, it will no drop + // if(fabsf(_wrap_pi(_global_pos.yaw-atan2f(wind.windspeed_north,wind_speed.windspeed_east)+M_PI_F)) < 80.0f/180.0f*M_PI_F) // if flight trajectory deviates more than 80 degrees from calculated path, it will no drop // { drop(); // } @@ -662,7 +666,10 @@ BottleDrop::task_main() // update_actuators(); // run at roughly 100 Hz - usleep(1000); + usleep(sleeptime_us); + + dt_runs = 1e6f / hrt_elapsed_time(&last_run); + last_run = hrt_absolute_time(); } } @@ -706,37 +713,41 @@ BottleDrop::handle_command(struct vehicle_command_s *cmd) switch ((int)(cmd->param1 + 0.5f)) { case 0: - drop_approval = false; + _drop_approval = false; break; case 1: - drop_approval = true; + _drop_approval = true; break; default: - drop_approval = false; + _drop_approval = false; + warnx("param1 val unknown"); break; } - // XXX check all fields + // XXX check all fields (2-3) + _alt_clearance = cmd->param4; _target_position.lat = cmd->param5; _target_position.lon = cmd->param6; _target_position.alt = cmd->param7; + _target_position.initialized; + map_projection_init(&ref, _target_position.lat, _target_position.lon); answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); break; case VEHICLE_CMD_PAYLOAD_CONTROL_DEPLOY: switch ((int)(cmd->param1 + 0.5f)) { case 0: - drop_approval = false; + _drop_approval = false; break; case 1: - drop_approval = true; + _drop_approval = true; break; default: - drop_approval = false; + _drop_approval = false; break; // XXX handle other values } diff --git a/src/modules/bottle_drop/bottle_drop_params.c b/src/modules/bottle_drop/bottle_drop_params.c index 0f60b58e4..239f25884 100644 --- a/src/modules/bottle_drop/bottle_drop_params.c +++ b/src/modules/bottle_drop/bottle_drop_params.c @@ -41,8 +41,6 @@ #include #include -PARAM_DEFINE_FLOAT(BD_HEIGHT, 60.0f); PARAM_DEFINE_FLOAT(BD_GPROPERTIES, 0.03f); -PARAM_DEFINE_FLOAT(BD_TURNRADIUS, 70.0f); +PARAM_DEFINE_FLOAT(BD_TURNRADIUS, 90.0f); PARAM_DEFINE_FLOAT(BD_PRECISION, 1.0f); -PARAM_DEFINE_INT32(BD_APPROVAL, 0); -- cgit v1.2.3 From 61d052ede751274969869e53e98b1d8b2004a128 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 15 Aug 2014 23:41:57 +0200 Subject: Fix param value --- src/modules/bottle_drop/bottle_drop_params.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/bottle_drop/bottle_drop_params.c b/src/modules/bottle_drop/bottle_drop_params.c index 239f25884..22e9baf8a 100644 --- a/src/modules/bottle_drop/bottle_drop_params.c +++ b/src/modules/bottle_drop/bottle_drop_params.c @@ -43,4 +43,4 @@ PARAM_DEFINE_FLOAT(BD_GPROPERTIES, 0.03f); PARAM_DEFINE_FLOAT(BD_TURNRADIUS, 90.0f); -PARAM_DEFINE_FLOAT(BD_PRECISION, 1.0f); +PARAM_DEFINE_FLOAT(BD_PRECISION, 10.0f); -- cgit v1.2.3 From 35daef948bb6dac06900d7bc74aa09fe35aceabd Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 16 Aug 2014 10:52:01 +0200 Subject: fix datalink loss detection logic --- src/modules/commander/commander.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 28aba759f..8a12e16ca 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1534,6 +1534,10 @@ int commander_thread_main(int argc, char *argv[]) mavlink_log_critical(mavlink_fd, "data link %i regained", i); telemetry_lost[i] = false; have_link = true; + } else if (!telemetry_lost[i]) { + /* telemetry was healthy also in last iteration + * we don't have to check a timeout */ + have_link = true; } } else { -- cgit v1.2.3 From ee254be845f16531990f4574ca7505ed359c4a61 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 16 Aug 2014 11:16:27 +0200 Subject: navigator: skip takeoff if already above takeoff altitude --- src/modules/navigator/mission.cpp | 67 +++++++++++++++++++++++---------------- src/modules/navigator/mission.h | 4 +-- 2 files changed, 41 insertions(+), 30 deletions(-) diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index c0e37a3ed..98a4340e1 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -107,6 +107,7 @@ Mission::on_inactive() update_offboard_mission(); } + /* require takeoff after non-loiter or landing */ if (!_navigator->get_can_loiter_at_sp() || _navigator->get_vstatus()->condition_landed) { _need_takeoff = true; } @@ -399,7 +400,7 @@ Mission::set_mission_items() takeoff_alt += _navigator->get_home_position()->alt; } - /* perform takeoff at least to NAV_TAKEOFF_ALT above home/ground, even if first waypoint is lower */ + /* takeoff to at least NAV_TAKEOFF_ALT above home/ground, even if first waypoint is lower */ if (_navigator->get_vstatus()->condition_landed) { takeoff_alt = fmaxf(takeoff_alt, _navigator->get_global_position()->alt + _param_takeoff_alt.get()); @@ -407,43 +408,53 @@ Mission::set_mission_items() takeoff_alt = fmaxf(takeoff_alt, _navigator->get_home_position()->alt + _param_takeoff_alt.get()); } - mavlink_log_critical(_navigator->get_mavlink_fd(), "takeoff to %.1f meters above home", (double)(takeoff_alt - _navigator->get_home_position()->alt)); + /* check if we already above takeoff altitude */ + if (_navigator->get_global_position()->alt < takeoff_alt - _navigator->get_acceptance_radius()) { + mavlink_log_critical(_navigator->get_mavlink_fd(), "takeoff to %.1f meters above home", (double)(takeoff_alt - _navigator->get_home_position()->alt)); - _mission_item.lat = _navigator->get_global_position()->lat; - _mission_item.lon = _navigator->get_global_position()->lon; - _mission_item.altitude = takeoff_alt; - _mission_item.altitude_is_relative = false; + _mission_item.nav_cmd = NAV_CMD_TAKEOFF; + _mission_item.lat = _navigator->get_global_position()->lat; + _mission_item.lon = _navigator->get_global_position()->lon; + _mission_item.altitude = takeoff_alt; + _mission_item.altitude_is_relative = false; - mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current); + mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current); - } else { - /* set current position setpoint from mission item */ - mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current); + _navigator->set_position_setpoint_triplet_updated(); + return; - /* require takeoff after landing or idle */ - if (pos_sp_triplet->current.type == SETPOINT_TYPE_LAND || pos_sp_triplet->current.type == SETPOINT_TYPE_IDLE) { - _need_takeoff = true; + } else { + /* skip takeoff */ + _takeoff = false; } + } - _navigator->set_can_loiter_at_sp(false); - reset_mission_item_reached(); + /* set current position setpoint from mission item */ + mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current); - if (_mission_type == MISSION_TYPE_OFFBOARD) { - report_current_offboard_mission_item(); - } - // TODO: report onboard mission item somehow + /* require takeoff after landing or idle */ + if (pos_sp_triplet->current.type == SETPOINT_TYPE_LAND || pos_sp_triplet->current.type == SETPOINT_TYPE_IDLE) { + _need_takeoff = true; + } - /* try to read next mission item */ - struct mission_item_s mission_item_next; + _navigator->set_can_loiter_at_sp(false); + reset_mission_item_reached(); - if (read_mission_item(_mission_type == MISSION_TYPE_ONBOARD, false, &mission_item_next)) { - /* got next mission item, update setpoint triplet */ - mission_item_to_position_setpoint(&mission_item_next, &pos_sp_triplet->next); + if (_mission_type == MISSION_TYPE_OFFBOARD) { + report_current_offboard_mission_item(); + } + // TODO: report onboard mission item somehow - } else { - /* next mission item is not available */ - pos_sp_triplet->next.valid = false; - } + /* try to read next mission item */ + struct mission_item_s mission_item_next; + + if (read_mission_item(_mission_type == MISSION_TYPE_ONBOARD, false, &mission_item_next)) { + /* got next mission item, update setpoint triplet */ + mission_item_to_position_setpoint(&mission_item_next, &pos_sp_triplet->next); + + } else { + /* next mission item is not available */ + pos_sp_triplet->next.valid = false; } _navigator->set_position_setpoint_triplet_updated(); diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h index 4da6a1155..1edaa5c8a 100644 --- a/src/modules/navigator/mission.h +++ b/src/modules/navigator/mission.h @@ -142,8 +142,8 @@ private: int _current_onboard_mission_index; int _current_offboard_mission_index; - bool _need_takeoff; - bool _takeoff; + bool _need_takeoff; /**< if true, then takeoff must be performed before going to the first waypoint (if needed) */ + bool _takeoff; /**< takeoff state flag */ orb_advert_t _mission_result_pub; struct mission_result_s _mission_result; -- cgit v1.2.3 From a972a2857197706ed414b674dcaf291d48ac0f04 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 16 Aug 2014 11:41:31 +0200 Subject: fix param name --- src/modules/commander/commander_params.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index 3d1e231c6..30159dad9 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -104,7 +104,7 @@ PARAM_DEFINE_FLOAT(BAT_CAPACITY, -1.0f); * @min 0 * @max 1 */ -PARAM_DEFINE_INT32(DL_LOSS_EN, 0); +PARAM_DEFINE_INT32(COM_DL_LOSS_EN, 0); /** Datalink loss time threshold * -- cgit v1.2.3 From 72beef90c9a9bb901355483e275a47f166f654a8 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 16 Aug 2014 13:09:10 +0200 Subject: set correct nav state for data link loss --- src/modules/commander/state_machine_helper.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 157e35ef8..f82eec695 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -510,7 +510,7 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en status->failsafe = true; if (status->condition_global_position_valid && status->condition_home_position_valid) { - status->nav_state = NAVIGATION_STATE_AUTO_RTL; + status->nav_state = NAVIGATION_STATE_AUTO_RTGS; } else if (status->condition_local_position_valid) { status->nav_state = NAVIGATION_STATE_LAND; } else if (status->condition_local_altitude_valid) { -- cgit v1.2.3 From 94765f1fe01a3ae1ba72a330f77cfb4cacbd8c0e Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 16 Aug 2014 13:17:26 +0200 Subject: datalinkloss: use vstatus from navigator For some reason the own subscription did not work (the task launch pattern used for the navigator may be the reason again) --- src/modules/navigator/datalinkloss.cpp | 14 +++++--------- src/modules/navigator/datalinkloss.h | 3 --- 2 files changed, 5 insertions(+), 12 deletions(-) diff --git a/src/modules/navigator/datalinkloss.cpp b/src/modules/navigator/datalinkloss.cpp index 7b9a7b151..ab9e67a33 100644 --- a/src/modules/navigator/datalinkloss.cpp +++ b/src/modules/navigator/datalinkloss.cpp @@ -57,7 +57,6 @@ DataLinkLoss::DataLinkLoss(Navigator *navigator, const char *name) : MissionBlock(navigator, name), - _vehicleStatus(&getSubscriptions(), ORB_ID(vehicle_status), 100), _param_commsholdwaittime(this, "CH_T"), _param_commsholdlat(this, "CH_LAT"), _param_commsholdlon(this, "CH_LON"), @@ -91,6 +90,7 @@ void DataLinkLoss::on_activation() { _dll_state = DLL_STATE_NONE; + updateParams(); advance_dll(); set_dll_item(); } @@ -99,6 +99,7 @@ void DataLinkLoss::on_active() { if (is_mission_item_reached()) { + updateParams(); advance_dll(); set_dll_item(); } @@ -109,9 +110,6 @@ DataLinkLoss::set_dll_item() { struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); - /* make sure we have the latest params */ - updateParams(); - set_previous_pos_setpoint(); _navigator->set_can_loiter_at_sp(false); @@ -167,17 +165,15 @@ DataLinkLoss::set_dll_item() void DataLinkLoss::advance_dll() { - warnx("dll_state %u", _dll_state); switch (_dll_state) { case DLL_STATE_NONE: /* Check the number of data link losses. If above home fly home directly */ - updateSubscriptions(); - if (_vehicleStatus.data_link_lost_counter > _param_numberdatalinklosses.get()) { - warnx("too many data link losses, fly to airfield home"); + if (_navigator->get_vstatus()->data_link_lost_counter > _param_numberdatalinklosses.get()) { + warnx("%d data link losses, limit is %d, fly to airfield home", _navigator->get_vstatus()->data_link_lost_counter, _param_numberdatalinklosses.get()); mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: too many DL losses, fly to home"); _dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP; } else { - warnx("fly to comms hold"); + warnx("fly to comms hold, datalink loss counter: %d", _navigator->get_vstatus()->data_link_lost_counter); mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: fly to comms hold"); _dll_state = DLL_STATE_FLYTOCOMMSHOLDWP; } diff --git a/src/modules/navigator/datalinkloss.h b/src/modules/navigator/datalinkloss.h index 5a46b5700..96b4ce010 100644 --- a/src/modules/navigator/datalinkloss.h +++ b/src/modules/navigator/datalinkloss.h @@ -64,9 +64,6 @@ public: virtual void on_active(); private: - /* Subscriptions */ - uORB::Subscription _vehicleStatus; - /* Params */ control::BlockParamFloat _param_commsholdwaittime; control::BlockParamInt _param_commsholdlat; // * 1e7 -- cgit v1.2.3 From 14189816f7e751b14725939fd5a200c39818f5df Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 16 Aug 2014 14:28:39 +0200 Subject: dlloss: better output --- src/modules/navigator/datalinkloss.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/navigator/datalinkloss.cpp b/src/modules/navigator/datalinkloss.cpp index ab9e67a33..b914fdb34 100644 --- a/src/modules/navigator/datalinkloss.cpp +++ b/src/modules/navigator/datalinkloss.cpp @@ -180,7 +180,7 @@ DataLinkLoss::advance_dll() break; case DLL_STATE_FLYTOCOMMSHOLDWP: warnx("fly to airfield home"); - mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: fly to home"); + mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: fly to airfield home"); _dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP; _navigator->get_mission_result()->stay_in_failsafe = true; _navigator->publish_mission_result(); -- cgit v1.2.3 From f480c10282efcb56b643b2c676303f1f57498fda Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 16 Aug 2014 14:37:46 +0200 Subject: state machine helper: use stay in failsafe flag --- src/modules/commander/state_machine_helper.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index f82eec695..3f4bfaa1c 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -534,11 +534,11 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en } /* don't bother if RC is lost and mission is not yet finished */ - } else if (status->rc_signal_lost) { + } else if (status->rc_signal_lost && !stay_in_failsafe) { /* this mode is ok, we don't need RC for missions */ status->nav_state = NAVIGATION_STATE_AUTO_MISSION; - } else { + } else if (!stay_in_failsafe){ /* everything is perfect */ status->nav_state = NAVIGATION_STATE_AUTO_MISSION; } -- cgit v1.2.3 From 26b2f73c6fce19982136e12a73faf96eee4895c6 Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Sat, 16 Aug 2014 14:42:18 +0200 Subject: Call the appropriate functions directly. --- src/modules/bottle_drop/bottle_drop.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/modules/bottle_drop/bottle_drop.cpp b/src/modules/bottle_drop/bottle_drop.cpp index 4367d1343..1c5c0f57f 100644 --- a/src/modules/bottle_drop/bottle_drop.cpp +++ b/src/modules/bottle_drop/bottle_drop.cpp @@ -684,7 +684,6 @@ BottleDrop::handle_command(struct vehicle_command_s *cmd) { switch (cmd->command) { case VEHICLE_CMD_CUSTOM_0: - /* * param1 and param2 set to 1: open and drop * param1 set to 1: open @@ -693,16 +692,19 @@ BottleDrop::handle_command(struct vehicle_command_s *cmd) if (cmd->param1 > 0.5f && cmd->param2 > 0.5f) { _open_door = true; _drop = true; + drop(); mavlink_log_info(_mavlink_fd, "#audio: drop bottle"); } else if (cmd->param1 > 0.5f) { _open_door = true; _drop = false; + open_bay(); mavlink_log_info(_mavlink_fd, "#audio: open doors"); } else { _open_door = false; _drop = false; + close_bay(); mavlink_log_info(_mavlink_fd, "#audio: close doors"); } -- cgit v1.2.3 From 584394d2bc1d88d291349cce2beee161c2a8448e Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Sat, 16 Aug 2014 17:53:23 +0200 Subject: Silenced commander --- src/modules/commander/commander.cpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 6c2c03070..db9ec9005 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -615,6 +615,11 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s case VEHICLE_CMD_PREFLIGHT_CALIBRATION: case VEHICLE_CMD_PREFLIGHT_SET_SENSOR_OFFSETS: case VEHICLE_CMD_PREFLIGHT_STORAGE: + case VEHICLE_CMD_CUSTOM_0: + case VEHICLE_CMD_CUSTOM_1: + case VEHICLE_CMD_CUSTOM_2: + case VEHICLE_CMD_PAYLOAD_PREPARE_DEPLOY: + case VEHICLE_CMD_PAYLOAD_CONTROL_DEPLOY: /* ignore commands that handled in low prio loop */ break; -- cgit v1.2.3 From 8a066a85faa7dd15265f705eba78a9dfe53f9f1d Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Sat, 16 Aug 2014 17:53:51 +0200 Subject: Fixed bottle drop to operate in GPS outdoor test, pending test flight --- src/modules/bottle_drop/bottle_drop.cpp | 396 +++++++++++++++++--------------- 1 file changed, 214 insertions(+), 182 deletions(-) diff --git a/src/modules/bottle_drop/bottle_drop.cpp b/src/modules/bottle_drop/bottle_drop.cpp index 1c5c0f57f..ddc41372e 100644 --- a/src/modules/bottle_drop/bottle_drop.cpp +++ b/src/modules/bottle_drop/bottle_drop.cpp @@ -103,6 +103,7 @@ public: void open_bay(); void close_bay(); void drop(); + void lock_release(); private: bool _task_should_exit; /**< if true, task should exit */ @@ -119,8 +120,6 @@ private: struct actuator_controls_s _actuators; bool _drop_approval; - bool _open_door; - bool _drop; hrt_abstime _doors_opened; hrt_abstime _drop_time; @@ -133,6 +132,14 @@ private: bool initialized; } _target_position, _drop_position; + enum DROP_STATE { + DROP_STATE_INIT = 0, + DROP_STATE_TARGET_VALID, + DROP_STATE_BAY_OPEN, + DROP_STATE_DROPPED, + DROP_STATE_BAY_CLOSED + } _drop_state; + void task_main(); void handle_command(struct vehicle_command_s *cmd); @@ -168,11 +175,12 @@ BottleDrop::BottleDrop() : _actuator_pub(-1), _actuators {}, _drop_approval(false), - _open_door(false), - _drop(false), _doors_opened(0), _drop_time(0), - _alt_clearance(0) + _alt_clearance(0), + _target_position {}, + _drop_position {}, + _drop_state(DROP_STATE_INIT) { } @@ -226,8 +234,7 @@ BottleDrop::start() void BottleDrop::status() { - warnx("Doors: %s", _open_door ? "OPEN" : "CLOSED"); - warnx("Dropping: %s", _drop ? "YES" : "NO"); + warnx("drop state: %d", _drop_state); } void @@ -242,6 +249,8 @@ BottleDrop::open_bay() warnx("open doors"); actuators_publish(); + + usleep(500 * 1000); } void @@ -254,6 +263,9 @@ BottleDrop::close_bay() _doors_opened = 0; actuators_publish(); + + // delay until the bay is closed + usleep(500 * 1000); } void @@ -269,22 +281,29 @@ BottleDrop::drop() warnx("bay not ready, forced open"); } - while (hrt_elapsed_time(&_doors_opened) < 400000 && hrt_elapsed_time(&starttime) < 2000000) { + while (hrt_elapsed_time(&_doors_opened) < 500 * 1000 && hrt_elapsed_time(&starttime) < 2000000) { usleep(50000); warnx("delayed by door!"); } - if (_drop && _doors_opened != 0 && hrt_elapsed_time(&_doors_opened) < 500000) { - _actuators.control[2] = -1.0f; - } else { - _actuators.control[2] = 1.0f; - } + _actuators.control[2] = 1.0f; _drop_time = hrt_absolute_time(); + actuators_publish(); warnx("dropping now"); + // Give it time to drop + usleep(1000 * 1000); +} + +void +BottleDrop::lock_release() +{ + _actuators.control[2] = -1.0f; actuators_publish(); + + warnx("closing release"); } int @@ -401,9 +420,6 @@ BottleDrop::task_main() memset(&onboard_mission, 0, sizeof(onboard_mission)); orb_advert_t onboard_mission_pub = -1; - double latitude; - double longitude; - struct wind_estimate_s wind; /* wakeup source(s) */ @@ -414,6 +430,7 @@ BottleDrop::task_main() fds[0].events = POLLIN; // Whatever state the bay is in, we want it closed on startup + lock_release(); close_bay(); while (!_task_should_exit) { @@ -437,9 +454,6 @@ BottleDrop::task_main() if (updated) { /* copy global position */ orb_copy(ORB_ID(vehicle_global_position), vehicle_global_position_sub, &_global_pos); - - latitude = (double)_global_pos.lat / 1e7; - longitude = (double)_global_pos.lon / 1e7; } if (_global_pos.timestamp == 0) { @@ -452,32 +466,23 @@ BottleDrop::task_main() dt_runs = 1e6f / sleeptime_us; /* switch to faster updates during the drop */ - while (_drop) { - - /* 20s after drop, reset and close everything again */ - if (_drop && _doors_opened != 0 && hrt_elapsed_time(&_doors_opened) > 20000000) { - _open_door = false; - _drop = false; - } + while (_drop_state > DROP_STATE_INIT) { + // Get wind estimate orb_check(_wind_estimate_sub, &updated); - if (updated) { orb_copy(ORB_ID(wind_estimate), _wind_estimate_sub, &wind); } + // Get vehicle position orb_check(vehicle_global_position_sub, &updated); - if (updated) { /* copy global position */ orb_copy(ORB_ID(vehicle_global_position), vehicle_global_position_sub, &_global_pos); - - latitude = (double)_global_pos.lat / 1e7; - longitude = (double)_global_pos.lon / 1e7; } + // Get parameter updates orb_check(parameter_update_sub, &updated); - if (updated) { /* copy global position */ orb_copy(ORB_ID(parameter_update), parameter_update_sub, &update); @@ -488,177 +493,201 @@ BottleDrop::task_main() param_get(param_precision, &precision); } - // Initialization float windspeed_norm = sqrtf(wind.windspeed_north * wind.windspeed_north + wind.windspeed_east * wind.windspeed_east); float groundspeed_body = sqrtf(_global_pos.vel_n * _global_pos.vel_n + _global_pos.vel_e * _global_pos.vel_e); - - distance_open_door = fabsf(t_door * groundspeed_body); - + distance_real = fabsf(get_distance_to_next_waypoint(_global_pos.lat, _global_pos.lon, _drop_position.lat, _drop_position.lon)); //warnx("absolut wind speed = %.4f", vr); //////////////////////////////////////////////////////////////////// DEBUGGING //warnx("Initialization complete\n"); //////////////////////////////////////////////////////////////////// DEBUGGING + switch (_drop_state) { + + case DROP_STATE_INIT: + // exit inner loop, wait for new drop mission + break; + + case DROP_STATE_TARGET_VALID: + { + //warnx("approval given\n"); //////////////////////////////////////////////////////////////////// DEBUGGING + // drop here + //open_now = true; + //drop = false; + //drop_start = hrt_absolute_time(); + + // Update drop point at 10 Hz + if (counter % 10 == 0) { + + az = g; // acceleration in z direction[m/s^2] + vz = 0; // velocity in z direction [m/s] + z = 0; // fallen distance [m] + h_0 = _global_pos.alt - _target_position.alt; // height over target at start[m] + h = h_0; // height over target [m] + ax = 0; // acceleration in x direction [m/s^2] + vx = groundspeed_body;// XXX project // ground speed in x direction [m/s] + x = 0; // traveled distance in x direction [m] + vw = 0; // wind speed [m/s] + vrx = 0; // relative velocity in x direction [m/s] + v = groundspeed_body; // relative speed vector [m/s] + Fd = 0; // Drag force [N] + Fdx = 0; // Drag force in x direction [N] + Fdz = 0; // Drag force in z direction [N] + + + // Compute the distance the bottle will travel after it is dropped in body frame coordinates --> x + while (h > 0.05f) { + // z-direction + vz = vz + az * dt_freefall_prediction; + z = z + vz * dt_freefall_prediction; + h = h_0 - z; + + // x-direction + vw = windspeed_norm * logf(h / z_0) / logf(ground_distance / z_0); + vx = vx + ax * dt_freefall_prediction; + x = x + vx * dt_freefall_prediction; + vrx = vx + vw; + + //Drag force + v = sqrtf(vz * vz + vrx * vrx); + Fd = 0.5f * rho * A * cd * powf(v, 2.0f); + Fdx = Fd * vrx / v; + Fdz = Fd * vz / v; + + //acceleration + az = g - Fdz / m; + ax = -Fdx / m; + } + + // Compute drop vector + x = groundspeed_body * t_signal + x; + } + + x_t = 0.0f; + y_t = 0.0f; + + float wind_direction_n, wind_direction_e; - if (_drop_approval && !state_drop) { - //warnx("approval given\n"); //////////////////////////////////////////////////////////////////// DEBUGGING - // drop here - //open_now = true; - //drop = false; - //drop_start = hrt_absolute_time(); - - // Update drop point at 10 Hz - if (counter % 10 == 0) { - - az = g; // acceleration in z direction[m/s^2] - vz = 0; // velocity in z direction [m/s] - z = 0; // fallen distance [m] - h_0 = _global_pos.alt - _target_position.alt; // height over target at start[m] - h = h_0; // height over target [m] - ax = 0; // acceleration in x direction [m/s^2] - vx = groundspeed_body;// XXX project // ground speed in x direction [m/s] - x = 0; // traveled distance in x direction [m] - vw = 0; // wind speed [m/s] - vrx = 0; // relative velocity in x direction [m/s] - v = groundspeed_body; // relative speed vector [m/s] - Fd = 0; // Drag force [N] - Fdx = 0; // Drag force in x direction [N] - Fdz = 0; // Drag force in z direction [N] - - - // Compute the distance the bottle will travel after it is dropped in body frame coordinates --> x - while (h > 0.05f) { - // z-direction - vz = vz + az * dt_freefall_prediction; - z = z + vz * dt_freefall_prediction; - h = h_0 - z; - - // x-direction - vw = windspeed_norm * logf(h / z_0) / logf(ground_distance / z_0); - vx = vx + ax * dt_freefall_prediction; - x = x + vx * dt_freefall_prediction; - vrx = vx + vw; - - //Drag force - v = sqrtf(vz * vz + vrx * vrx); - Fd = 0.5f * rho * A * cd * powf(v, 2.0f); - Fdx = Fd * vrx / v; - Fdz = Fd * vz / v; - - //acceleration - az = g - Fdz / m; - ax = -Fdx / m; + if (windspeed_norm < 0.5f) { // If there is no wind, an arbitrarily direction is chosen + wind_direction_n = 1.0f; + wind_direction_e = 0.0f; + + } else { + wind_direction_n = wind.windspeed_north / windspeed_norm; + wind_direction_e = wind.windspeed_east / windspeed_norm; } - // Compute drop vector - x = groundspeed_body * t_signal + x; - } + x_drop = x_t + x * wind_direction_n; + y_drop = y_t + x * wind_direction_e; + map_projection_reproject(&ref, x_drop, y_drop, &_drop_position.lat, &_drop_position.lon); + //warnx("drop point: lat = %.7f , lon = %.7f", drop_position.lat, drop_position.lon); //////////////////////////////////////////////////////////////////// DEBUGGING - x_t = 0.0f; - y_t = 0.0f; - float wind_direction_n, wind_direction_e; - if (windspeed_norm < 0.5f) { // If there is no wind, an arbitrarily direction is chosen - wind_direction_n = 1.0f; - wind_direction_e = 0.0f; + // Compute flight vector + map_projection_reproject(&ref, x_drop + 2 * turn_radius * wind_direction_n, y_drop + 2 * turn_radius * wind_direction_n, + &(flight_vector_s.lat), &(flight_vector_s.lon)); + flight_vector_s.altitude = _drop_position.alt + _alt_clearance; + map_projection_reproject(&ref, x_drop - turn_radius * wind_direction_e, y_drop - turn_radius * wind_direction_e, + &flight_vector_e.lat, &flight_vector_e.lon); + flight_vector_e.altitude = _drop_position.alt + _alt_clearance; + //warnx("Flight vector: starting point = %.7f %.7f , end point = %.7f %.7f", flight_vector_s.lat,flight_vector_s.lon,flight_vector_e.lat,flight_vector_e.lon); //////////////////////////////////////////////////////////////////// DEBUGGING - } else { - wind_direction_n = wind.windspeed_north / windspeed_norm; - wind_direction_e = wind.windspeed_east / windspeed_norm; - } + // Drop Cancellation if terms are not met - x_drop = x_t + x * wind_direction_n; - y_drop = y_t + x * wind_direction_e; - map_projection_reproject(&ref, x_drop, y_drop, &_drop_position.lat, &_drop_position.lon); - //warnx("drop point: lat = %.7f , lon = %.7f", drop_position.lat, drop_position.lon); //////////////////////////////////////////////////////////////////// DEBUGGING - - - - // Compute flight vector - map_projection_reproject(&ref, x_drop + 2 * turn_radius * wind_direction_n, y_drop + 2 * turn_radius * wind_direction_n, - &(flight_vector_s.lat), &(flight_vector_s.lon)); - flight_vector_s.altitude = _drop_position.alt + _alt_clearance; - map_projection_reproject(&ref, x_drop - turn_radius * wind_direction_e, y_drop - turn_radius * wind_direction_e, - &flight_vector_e.lat, &flight_vector_e.lon); - flight_vector_e.altitude = _drop_position.alt + _alt_clearance; - //warnx("Flight vector: starting point = %.7f %.7f , end point = %.7f %.7f", flight_vector_s.lat,flight_vector_s.lon,flight_vector_e.lat,flight_vector_e.lon); //////////////////////////////////////////////////////////////////// DEBUGGING - - // Drop Cancellation if terms are not met - - // warnx("latitude:%.2f", latitude); - // warnx("longitude:%.2f", longitude); - // warnx("drop_position.lat:%.2f", drop_position.lat); - // warnx("drop_position.lon:%.2f", drop_position.lon); - - distance_real = fabsf(get_distance_to_next_waypoint(latitude, longitude, _drop_position.lat, _drop_position.lon)); - map_projection_project(&ref, latitude, longitude, &x_l, &y_l); - x_f = x_l + _global_pos.vel_n * dt_runs; - y_f = y_l + _global_pos.vel_e * dt_runs; - map_projection_reproject(&ref, x_f, y_f, &x_f_NED, &y_f_NED); - future_distance = fabsf(get_distance_to_next_waypoint(x_f_NED, y_f_NED, _drop_position.lat, _drop_position.lon)); - //warnx("position to-be: = %.7f %.7f" ,x_f_NED, y_f_NED ); //////////////////////////////////////////////////////////////////// DEBUGGING - - // if (counter % 10 ==0) { - // warnx("x: %.4f", x); - // warnx("drop_position.lat: %.4f, drop_position.lon: %.4f", drop_position.lat, drop_position.lon); - // warnx("latitude %.4f, longitude: %.4f", latitude, longitude); - // warnx("future_distance: %.2f, precision: %.2f", future_distance, precision); - // } - - /* Save WPs in datamanager */ - const ssize_t len = sizeof(struct mission_item_s); - - if (dm_write(DM_KEY_WAYPOINTS_ONBOARD, 0, DM_PERSIST_IN_FLIGHT_RESET, &flight_vector_s, len) != len) { - warnx("ERROR: could not save onboard WP"); - } + // warnx("latitude:%.2f", _global_pos.lat); + // warnx("longitude:%.2f", _global_pos.lon); + // warnx("drop_position.lat:%.2f", drop_position.lat); + // warnx("drop_position.lon:%.2f", drop_position.lon); - if (dm_write(DM_KEY_WAYPOINTS_ONBOARD, 1, DM_PERSIST_IN_FLIGHT_RESET, &flight_vector_e, len) != len) { - warnx("ERROR: could not save onboard WP"); - } + //warnx("position to-be: = %.7f %.7f" ,x_f_NED, y_f_NED ); //////////////////////////////////////////////////////////////////// DEBUGGING - onboard_mission.count = 2; + // if (counter % 10 ==0) { + // warnx("x: %.4f", x); + // warnx("drop_position.lat: %.4f, drop_position.lon: %.4f", drop_position.lat, drop_position.lon); + // warnx("latitude %.4f, longitude: %.4f", _global_pos.lat, _global_pos.lon); + // warnx("future_distance: %.2f, precision: %.2f", future_distance, precision); + // } - if (state_run && !state_drop) { - onboard_mission.current_seq = 0; + /* Save WPs in datamanager */ + const ssize_t len = sizeof(struct mission_item_s); - } else { - onboard_mission.current_seq = -1; - } + if (dm_write(DM_KEY_WAYPOINTS_ONBOARD, 0, DM_PERSIST_IN_FLIGHT_RESET, &flight_vector_s, len) != len) { + warnx("ERROR: could not save onboard WP"); + } - if (counter % 10 == 0) - warnx("Distance real: %.2f, distance_open_door: %.2f, angle to wind: %.2f", (double)distance_real, - (double)distance_open_door, - (double)(_wrap_pi(_global_pos.yaw - atan2f(wind.windspeed_north, wind.windspeed_east)))); + if (dm_write(DM_KEY_WAYPOINTS_ONBOARD, 1, DM_PERSIST_IN_FLIGHT_RESET, &flight_vector_e, len) != len) { + warnx("ERROR: could not save onboard WP"); + } - if (onboard_mission_pub > 0) { - orb_publish(ORB_ID(onboard_mission), onboard_mission_pub, &onboard_mission); + onboard_mission.count = 2; - } else { - onboard_mission_pub = orb_advertise(ORB_ID(onboard_mission), &onboard_mission); - } + if (state_run && !state_drop) { + onboard_mission.current_seq = 0; - } + } else { + onboard_mission.current_seq = -1; + } - if (isfinite(distance_real) && distance_real < distance_open_door && _drop_approval) { - open_bay(); - } else { - close_bay(); - } + if (onboard_mission_pub > 0) { + orb_publish(ORB_ID(onboard_mission), onboard_mission_pub, &onboard_mission); - if (isfinite(distance_real) && distance_real < precision && distance_real < future_distance) { // Drop only if the distance between drop point and actual position is getting larger again - // XXX this check needs to be carefully validated - right now we prefer to drop if we're close to the goal - - // if(fabsf(_wrap_pi(_global_pos.yaw-atan2f(wind.windspeed_north,wind_speed.windspeed_east)+M_PI_F)) < 80.0f/180.0f*M_PI_F) // if flight trajectory deviates more than 80 degrees from calculated path, it will no drop - // { - drop(); - // } - // else - // { - // state_run = true; - // } + } else { + onboard_mission_pub = orb_advertise(ORB_ID(onboard_mission), &onboard_mission); + } + + // We're close enough - open the bay + distance_open_door = math::max(3.0f, fabsf(t_door * groundspeed_body)); + + if (counter % 10 == 0) + warnx("dist real: %.2f, distance_open_door: %.2f, angle to wind: %.2f", + (double)distance_real, + (double)distance_open_door, + (double)(_wrap_pi(_global_pos.yaw - atan2f(wind.windspeed_north, wind.windspeed_east)))); + + if (isfinite(distance_real) && distance_real < distance_open_door && _drop_approval) { + open_bay(); + _drop_state = DROP_STATE_BAY_OPEN; + } + } + break; + + case DROP_STATE_BAY_OPEN: + { + map_projection_project(&ref, _global_pos.lat, _global_pos.lon, &x_l, &y_l); + x_f = x_l + _global_pos.vel_n * dt_runs; + y_f = y_l + _global_pos.vel_e * dt_runs; + map_projection_reproject(&ref, x_f, y_f, &x_f_NED, &y_f_NED); + future_distance = fabsf(get_distance_to_next_waypoint(x_f_NED, y_f_NED, _drop_position.lat, _drop_position.lon)); + + warnx("Distance real: %.2f", (double)distance_real); + + if (isfinite(distance_real) && (((distance_real < precision) && (distance_real < future_distance)) || + /* we drop as well if we're really close */ (distance_real < precision / 10.0f))) { // Drop only if the distance between drop point and actual position is getting larger again + // XXX this check needs to be carefully validated - right now we prefer to drop if we're close to the goal + + // if(fabsf(_wrap_pi(_global_pos.yaw-atan2f(wind.windspeed_north,wind_speed.windspeed_east)+M_PI_F)) < 80.0f/180.0f*M_PI_F) // if flight trajectory deviates more than 80 degrees from calculated path, it will no drop + // { + drop(); + _drop_state = DROP_STATE_DROPPED; + // } + // else + // { + // state_run = true; + // } + } + } + break; + + case DROP_STATE_DROPPED: + /* 2s after drop, reset and close everything again */ + if ((hrt_elapsed_time(&_doors_opened) > 2 * 1000 * 1000)) { + _drop_state = DROP_STATE_INIT; + lock_release(); + close_bay(); + } + break; } counter++; @@ -690,20 +719,15 @@ BottleDrop::handle_command(struct vehicle_command_s *cmd) * else: close (and don't drop) */ if (cmd->param1 > 0.5f && cmd->param2 > 0.5f) { - _open_door = true; - _drop = true; + open_bay(); drop(); mavlink_log_info(_mavlink_fd, "#audio: drop bottle"); } else if (cmd->param1 > 0.5f) { - _open_door = true; - _drop = false; open_bay(); mavlink_log_info(_mavlink_fd, "#audio: open doors"); } else { - _open_door = false; - _drop = false; close_bay(); mavlink_log_info(_mavlink_fd, "#audio: close doors"); } @@ -720,6 +744,7 @@ BottleDrop::handle_command(struct vehicle_command_s *cmd) case 1: _drop_approval = true; + mavlink_log_info(_mavlink_fd, "#audio: prepare deploy approval"); break; default: @@ -734,6 +759,9 @@ BottleDrop::handle_command(struct vehicle_command_s *cmd) _target_position.lon = cmd->param6; _target_position.alt = cmd->param7; _target_position.initialized; + _drop_state = DROP_STATE_TARGET_VALID; + warnx("got target: %8.4f, %8.4f, %8.4f", (double)_target_position.lat, + (double)_target_position.lon, (double)_target_position.alt); map_projection_init(&ref, _target_position.lat, _target_position.lon); answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); break; @@ -746,6 +774,7 @@ BottleDrop::handle_command(struct vehicle_command_s *cmd) case 1: _drop_approval = true; + mavlink_log_info(_mavlink_fd, "#audio: control deploy approval"); break; default: @@ -848,6 +877,9 @@ int bottle_drop_main(int argc, char *argv[]) } else if (!strcmp(argv[1], "close")) { bottle_drop::g_bottle_drop->close_bay(); + } else if (!strcmp(argv[1], "lock")) { + bottle_drop::g_bottle_drop->lock_release(); + } else { usage(); } -- cgit v1.2.3 From ab9b234893448237d84b4e1f95b807be3a68e98f Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 16 Aug 2014 18:07:21 +0200 Subject: commander: mavlink output on flight termination --- src/modules/commander/commander.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 8a12e16ca..2ab40a5eb 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1313,9 +1313,10 @@ int commander_thread_main(int argc, char *argv[]) /* Check for geofence violation */ if (pos_sp_triplet.geofence_violated) { //XXX: make this configurable to select different actions (e.g. navigation modes) - /* this will only trigger if geofence is activated via param and a geofence file is present */ + /* this will only trigger if geofence is activated via param and a geofence file is present, also there is a circuit breaker to disable the actual flight termination in the px4io driver */ armed.force_failsafe = true; status_changed = true; + mavlink_log_emergency(mavlink_fd, "Geofence violated: terminating"); } // no reset is done here on purpose, on geofence violation we want to stay in flighttermination } -- cgit v1.2.3 From fc0bdfb6f5021f5cc2d155ea3cee9f5d5102cdef Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 16 Aug 2014 18:42:12 +0200 Subject: mc_pos_control: trajectory following, using previous and next waypoints --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 184 +++++++++++++++++---- 1 file changed, 156 insertions(+), 28 deletions(-) diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index ecc40428d..488ca924a 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -76,6 +76,7 @@ #define TILT_COS_MAX 0.7f #define SIGMA 0.000001f +#define MIN_DIST 0.01f /** * Multicopter position control app start / stop handling function @@ -229,6 +230,14 @@ private: */ void control_offboard(float dt); + bool cross_sphere_line(const math::Vector<3>& sphere_c, float sphere_r, + const math::Vector<3> line_a, const math::Vector<3> line_b, math::Vector<3>& res); + + /** + * Set position setpoint for AUTO + */ + void control_auto(); + /** * Select between barometric and global (AMSL) altitudes */ @@ -647,6 +656,152 @@ MulticopterPositionControl::control_offboard(float dt) } } +bool +MulticopterPositionControl::cross_sphere_line(const math::Vector<3>& sphere_c, float sphere_r, + const math::Vector<3> line_a, const math::Vector<3> line_b, math::Vector<3>& res) +{ + /* project center of sphere on line */ + /* normalized AB */ + math::Vector<3> ab_norm = line_b - line_a; + ab_norm.normalize(); + math::Vector<3> d = line_a + ab_norm * ((sphere_c - line_a) * ab_norm); + float cd_len = (sphere_c - d).length(); + + /* we have triangle CDX with known CD and CX = R, find DX */ + if (sphere_r > cd_len) { + /* have two roots, select one in A->B direction from D */ + float dx_len = sqrtf(sphere_r * sphere_r - cd_len * cd_len); + res = d + ab_norm * dx_len; + return true; + + } else { + /* have no roots, return D */ + res = d; + return false; + } +} + +void +MulticopterPositionControl::control_auto() +{ + bool updated; + orb_check(_pos_sp_triplet_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_sub, &_pos_sp_triplet); + } + + if (_pos_sp_triplet.current.valid) { + /* in case of interrupted mission don't go to waypoint but stay at current position */ + _reset_pos_sp = true; + _reset_alt_sp = true; + + /* project setpoint to local frame */ + math::Vector<3> curr_sp; + map_projection_project(&_ref_pos, + _pos_sp_triplet.current.lat, _pos_sp_triplet.current.lon, + &curr_sp.data[0], &curr_sp.data[1]); + curr_sp(2) = -(_pos_sp_triplet.current.alt - _ref_alt); + + /* by default use current setpoint as is */ + _pos_sp = curr_sp; + + if (_pos_sp_triplet.current.type == SETPOINT_TYPE_POSITION && _pos_sp_triplet.previous.valid) { + /* follow "previous - current" line */ + math::Vector<3> prev_sp; + map_projection_project(&_ref_pos, + _pos_sp_triplet.previous.lat, _pos_sp_triplet.previous.lon, + &prev_sp.data[0], &prev_sp.data[1]); + prev_sp(2) = -(_pos_sp_triplet.previous.alt - _ref_alt); + + if ((curr_sp - prev_sp).length() > MIN_DIST) { + /* scaled space: 1 == position error resulting max allowed speed, L1 = 1 in this space */ + math::Vector<3> scale = _params.pos_p.edivide(_params.vel_max); + + /* find X - cross point of L1 sphere and trajectory */ + math::Vector<3> pos_s = _pos.emult(scale); + math::Vector<3> prev_sp_s = prev_sp.emult(scale); + math::Vector<3> curr_sp_s = curr_sp.emult(scale); + math::Vector<3> curr_pos_s = pos_s - curr_sp_s; + float curr_pos_s_len = curr_pos_s.length(); + math::Vector<3> x; + if (curr_pos_s_len < 1.0f) { + /* copter is closer to waypoint than L1 radius */ + /* check next waypoint and use it to avoid slowing down when passing via waypoint */ + if (_pos_sp_triplet.next.valid) { + math::Vector<3> next_sp; + map_projection_project(&_ref_pos, + _pos_sp_triplet.next.lat, _pos_sp_triplet.next.lon, + &next_sp.data[0], &next_sp.data[1]); + next_sp(2) = -(_pos_sp_triplet.next.alt - _ref_alt); + + if ((next_sp - curr_sp).length() > MIN_DIST) { + math::Vector<3> next_sp_s = next_sp.emult(scale); + + /* calculate angle prev - curr - next */ + math::Vector<3> curr_next_s = next_sp_s - curr_sp_s; + math::Vector<3> prev_curr_s_norm = (curr_sp_s - prev_sp_s).normalized(); + + /* cos(a) = angle between current and next trajectory segments */ + float cos_a = prev_curr_s_norm * curr_next_s; + + /* cos(b) = angle pos - curr_sp - prev_sp */ + float cos_b = -curr_pos_s * prev_curr_s_norm / curr_pos_s_len; + + if (cos_a > 0.0f && cos_b > 0.0f) { + float curr_next_s_len = curr_next_s.length(); + /* if curr - next distance is larger than L1 radius, limit it */ + if (curr_next_s_len > 1.0f) { + cos_a /= curr_next_s_len; + } + + /* feed forward position setpoint offset */ + math::Vector<3> pos_ff = prev_curr_s_norm * + cos_a * cos_b * cos_b * (1.0f - curr_pos_s_len) * + (1.0f - expf(-curr_pos_s_len * curr_pos_s_len * 20.0f)); + _pos_sp += pos_ff.edivide(scale); + } + } + } + + } else { + bool near = cross_sphere_line(pos_s, 1.0f, prev_sp_s, curr_sp_s, x); + if (near) { + /* L1 sphere crosses trajectory */ + + } else { + /* copter is too far from trajectory */ + /* if copter is behind prev waypoint, go directly to prev waypoint */ + if ((x - prev_sp_s) * (curr_sp_s - prev_sp) < 0.0f) { + x = prev_sp_s; + } + + /* if copter is in front of curr waypoint, go directly to curr waypoint */ + if ((x - curr_sp_s) * (curr_sp_s - prev_sp) > 0.0f) { + x = curr_sp_s; + } + + x = pos_s + (x - pos_s).normalized(); + } + + /* scale result back to normal space */ + _pos_sp = x.edivide(scale); + } + } + } + + /* update yaw setpoint if needed */ + if (isfinite(_pos_sp_triplet.current.yaw)) { + _att_sp.yaw_body = _pos_sp_triplet.current.yaw; + } + + } else { + /* no waypoint, loiter, reset position setpoint if needed */ + reset_pos_sp(); + reset_alt_sp(); + } +} + void MulticopterPositionControl::task_main() { @@ -757,34 +912,7 @@ MulticopterPositionControl::task_main() } else { /* AUTO */ - bool updated; - orb_check(_pos_sp_triplet_sub, &updated); - - if (updated) { - orb_copy(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_sub, &_pos_sp_triplet); - } - - if (_pos_sp_triplet.current.valid) { - /* in case of interrupted mission don't go to waypoint but stay at current position */ - _reset_pos_sp = true; - _reset_alt_sp = true; - - /* project setpoint to local frame */ - map_projection_project(&_ref_pos, - _pos_sp_triplet.current.lat, _pos_sp_triplet.current.lon, - &_pos_sp.data[0], &_pos_sp.data[1]); - _pos_sp(2) = -(_pos_sp_triplet.current.alt - _ref_alt); - - /* update yaw setpoint if needed */ - if (isfinite(_pos_sp_triplet.current.yaw)) { - _att_sp.yaw_body = _pos_sp_triplet.current.yaw; - } - - } else { - /* no waypoint, loiter, reset position setpoint if needed */ - reset_pos_sp(); - reset_alt_sp(); - } + control_auto(); } /* fill local position setpoint */ -- cgit v1.2.3 From 7b4448510436ddb3d5c548a4039bfa53c3aabfc4 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 16 Aug 2014 18:46:42 +0200 Subject: mission: takeoff and next waypoint related fixes --- src/modules/navigator/mission.cpp | 35 ++++++++++++++++++++++++----------- 1 file changed, 24 insertions(+), 11 deletions(-) diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 98a4340e1..128dab7d7 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -344,6 +344,10 @@ Mission::set_mission_items() /* no mission available or mission finished, switch to loiter */ if (_mission_type != MISSION_TYPE_NONE) { mavlink_log_critical(_navigator->get_mavlink_fd(), "mission finished, loitering"); + + /* use last setpoint for loiter */ + _navigator->set_can_loiter_at_sp(true); + } else if (!user_feedback_done) { /* only tell users that we got no mission if there has not been any * better, more specific feedback yet @@ -360,6 +364,7 @@ Mission::set_mission_items() mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current); pos_sp_triplet->next.valid = false; + /* reuse setpoint for LOITER only if it's not IDLE */ _navigator->set_can_loiter_at_sp(pos_sp_triplet->current.type == SETPOINT_TYPE_LOITER); reset_mission_item_reached(); @@ -412,11 +417,13 @@ Mission::set_mission_items() if (_navigator->get_global_position()->alt < takeoff_alt - _navigator->get_acceptance_radius()) { mavlink_log_critical(_navigator->get_mavlink_fd(), "takeoff to %.1f meters above home", (double)(takeoff_alt - _navigator->get_home_position()->alt)); - _mission_item.nav_cmd = NAV_CMD_TAKEOFF; - _mission_item.lat = _navigator->get_global_position()->lat; - _mission_item.lon = _navigator->get_global_position()->lon; - _mission_item.altitude = takeoff_alt; - _mission_item.altitude_is_relative = false; + _mission_item.nav_cmd = NAV_CMD_TAKEOFF; + _mission_item.lat = _navigator->get_global_position()->lat; + _mission_item.lon = _navigator->get_global_position()->lon; + _mission_item.altitude = takeoff_alt; + _mission_item.altitude_is_relative = false; + _mission_item.autocontinue = true; + _mission_item.time_inside = 0; mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current); @@ -445,15 +452,21 @@ Mission::set_mission_items() } // TODO: report onboard mission item somehow - /* try to read next mission item */ - struct mission_item_s mission_item_next; + if (_mission_item.autocontinue && _mission_item.time_inside <= 0.001f) { + /* try to read next mission item */ + struct mission_item_s mission_item_next; - if (read_mission_item(_mission_type == MISSION_TYPE_ONBOARD, false, &mission_item_next)) { - /* got next mission item, update setpoint triplet */ - mission_item_to_position_setpoint(&mission_item_next, &pos_sp_triplet->next); + if (read_mission_item(_mission_type == MISSION_TYPE_ONBOARD, false, &mission_item_next)) { + /* got next mission item, update setpoint triplet */ + mission_item_to_position_setpoint(&mission_item_next, &pos_sp_triplet->next); + + } else { + /* next mission item is not available */ + pos_sp_triplet->next.valid = false; + } } else { - /* next mission item is not available */ + /* vehicle will be paused on current waypoint, don't set next item */ pos_sp_triplet->next.valid = false; } -- cgit v1.2.3 From 789f949e746cdd79e1214830a4ef822ef5634bbe Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Sun, 17 Aug 2014 10:19:48 +0200 Subject: Added debugging and small locking fix. --- src/modules/bottle_drop/bottle_drop.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/modules/bottle_drop/bottle_drop.cpp b/src/modules/bottle_drop/bottle_drop.cpp index ddc41372e..14b9a8379 100644 --- a/src/modules/bottle_drop/bottle_drop.cpp +++ b/src/modules/bottle_drop/bottle_drop.cpp @@ -728,6 +728,7 @@ BottleDrop::handle_command(struct vehicle_command_s *cmd) mavlink_log_info(_mavlink_fd, "#audio: open doors"); } else { + lock_release(); close_bay(); mavlink_log_info(_mavlink_fd, "#audio: close doors"); } @@ -760,7 +761,7 @@ BottleDrop::handle_command(struct vehicle_command_s *cmd) _target_position.alt = cmd->param7; _target_position.initialized; _drop_state = DROP_STATE_TARGET_VALID; - warnx("got target: %8.4f, %8.4f, %8.4f", (double)_target_position.lat, + mavlink_log_info(_mavlink_fd, "got target: %8.4f, %8.4f, %8.4f", (double)_target_position.lat, (double)_target_position.lon, (double)_target_position.alt); map_projection_init(&ref, _target_position.lat, _target_position.lon); answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); -- cgit v1.2.3 From b1008842204418f5d8cd0475547ecfb8f378b4c7 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 17 Aug 2014 12:07:02 +0200 Subject: geofence: support AMSL mode --- src/modules/navigator/geofence.cpp | 22 ++++++++++++------ src/modules/navigator/geofence.h | 40 +++++++++++++++++++++----------- src/modules/navigator/geofence_params.c | 12 ++++++++++ src/modules/navigator/navigator.h | 8 +++++++ src/modules/navigator/navigator_main.cpp | 27 +++++++++++++++++++-- 5 files changed, 86 insertions(+), 23 deletions(-) diff --git a/src/modules/navigator/geofence.cpp b/src/modules/navigator/geofence.cpp index 266215308..2b9ce752b 100644 --- a/src/modules/navigator/geofence.cpp +++ b/src/modules/navigator/geofence.cpp @@ -62,7 +62,8 @@ Geofence::Geofence() : _altitude_min(0), _altitude_max(0), _verticesCount(0), - param_geofence_on(this, "ON") + _param_geofence_on(this, "ON"), + _param_altitude_mode(this, "ALTMODE") { /* Load initial params */ updateParams(); @@ -74,19 +75,26 @@ Geofence::~Geofence() } -bool Geofence::inside(const struct vehicle_global_position_s *vehicle) +bool Geofence::inside(const struct vehicle_global_position_s &global_position) { - double lat = vehicle->lat / 1e7d; - double lon = vehicle->lon / 1e7d; - //float alt = vehicle->alt; + double lat = global_position.lat / 1e7d; + double lon = global_position.lon / 1e7d; - return inside(lat, lon, vehicle->alt); + return inside(lat, lon, global_position.alt); +} + +bool Geofence::inside(const struct vehicle_global_position_s &global_position, float baro_altitude_amsl) { + + double lat = global_position.lat / 1e7d; + double lon = global_position.lon / 1e7d; + + return inside(lat, lon, baro_altitude_amsl); } bool Geofence::inside(double lat, double lon, float altitude) { /* Return true if geofence is disabled */ - if (param_geofence_on.get() != 1) + if (_param_geofence_on.get() != 1) return true; if (valid()) { diff --git a/src/modules/navigator/geofence.h b/src/modules/navigator/geofence.h index 2eb126ab5..e2c0f08d8 100644 --- a/src/modules/navigator/geofence.h +++ b/src/modules/navigator/geofence.h @@ -42,6 +42,8 @@ #define GEOFENCE_H_ #include +#include +#include #include #include @@ -49,29 +51,25 @@ class Geofence : public control::SuperBlock { -private: - orb_advert_t _fence_pub; /**< publish fence topic */ - - float _altitude_min; - float _altitude_max; - - unsigned _verticesCount; - - /* Params */ - control::BlockParamInt param_geofence_on; public: Geofence(); ~Geofence(); + /* Altitude mode, corresponding to the param GF_ALTMODE */ + enum { + GF_ALT_MODE_GPS = 0, + GF_ALT_MODE_AMSL = 1 + }; + /** - * Return whether craft is inside geofence. + * Return whether system is inside geofence. * * Calculate whether point is inside arbitrary polygon * @param craft pointer craft coordinates - * @param fence pointer to array of coordinates, one per vertex. First and last vertex are assumed connected - * @return true: craft is inside fence, false:craft is outside fence + * @return true: system is inside fence, false: system is outside fence */ - bool inside(const struct vehicle_global_position_s *craft); + bool inside(const struct vehicle_global_position_s &global_position); + bool inside(const struct vehicle_global_position_s &global_position, float baro_altitude_amsl); bool inside(double lat, double lon, float altitude); int clearDm(); @@ -88,6 +86,20 @@ public: int loadFromFile(const char *filename); bool isEmpty() {return _verticesCount == 0;} + + int getAltitudeMode() { return _param_altitude_mode.get(); } + +private: + orb_advert_t _fence_pub; /**< publish fence topic */ + + float _altitude_min; + float _altitude_max; + + unsigned _verticesCount; + + /* Params */ + control::BlockParamInt _param_geofence_on; + control::BlockParamInt _param_altitude_mode; }; diff --git a/src/modules/navigator/geofence_params.c b/src/modules/navigator/geofence_params.c index 653b1ad84..29b42cd54 100644 --- a/src/modules/navigator/geofence_params.c +++ b/src/modules/navigator/geofence_params.c @@ -58,3 +58,15 @@ * @group Geofence */ PARAM_DEFINE_INT32(GF_ON, 1); + +/** + * Geofence altitude mode + * + * Select which altitude reference should be used + * 0 = GPS, 1 = AMSL + * + * @min 0 + * @max 1 + * @group Geofence + */ +PARAM_DEFINE_INT32(GF_ALTMODE, 1); diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index 536977972..59a6752a9 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -120,6 +120,7 @@ public: struct vehicle_status_s* get_vstatus() { return &_vstatus; } struct vehicle_control_mode_s* get_control_mode() { return &_control_mode; } struct vehicle_global_position_s* get_global_position() { return &_global_pos; } + struct sensor_combined_s* get_sensor_combined() { return &_sensor_combined; } struct home_position_s* get_home_position() { return &_home_pos; } struct position_setpoint_triplet_s* get_position_setpoint_triplet() { return &_pos_sp_triplet; } struct mission_result_s* get_mission_result() { return &_mission_result; } @@ -141,6 +142,7 @@ private: int _mavlink_fd; /**< the file descriptor to send messages over mavlink */ int _global_pos_sub; /**< global position subscription */ + int _sensor_combined_sub; /**< sensor combined subscription */ int _home_pos_sub; /**< home position subscription */ int _vstatus_sub; /**< vehicle status subscription */ int _capabilities_sub; /**< notification of vehicle capabilities updates */ @@ -156,6 +158,7 @@ private: vehicle_status_s _vstatus; /**< vehicle status */ vehicle_control_mode_s _control_mode; /**< vehicle control mode */ vehicle_global_position_s _global_pos; /**< global vehicle position */ + sensor_combined_s _sensor_combined; /**< sensor values */ home_position_s _home_pos; /**< home position for RTL */ mission_item_s _mission_item; /**< current mission item */ navigation_capabilities_s _nav_caps; /**< navigation capabilities */ @@ -195,6 +198,11 @@ private: */ void global_position_update(); + /** + * Retrieve sensor values + */ + void sensor_combined_update(); + /** * Retrieve home position */ diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 18bfc2cec..d77acf74e 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -69,6 +69,7 @@ #include #include #include +#include #include #include @@ -112,6 +113,7 @@ Navigator::Navigator() : _vstatus{}, _control_mode{}, _global_pos{}, + _sensor_combined{}, _home_pos{}, _mission_item{}, _nav_caps{}, @@ -178,6 +180,12 @@ Navigator::global_position_update() orb_copy(ORB_ID(vehicle_global_position), _global_pos_sub, &_global_pos); } +void +Navigator::sensor_combined_update() +{ + orb_copy(ORB_ID(sensor_combined), _sensor_combined_sub, &_sensor_combined); +} + void Navigator::home_position_update() { @@ -248,6 +256,7 @@ Navigator::task_main() /* do subscriptions */ _global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); + _sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined)); _capabilities_sub = orb_subscribe(ORB_ID(navigation_capabilities)); _vstatus_sub = orb_subscribe(ORB_ID(vehicle_status)); _control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); @@ -261,6 +270,7 @@ Navigator::task_main() vehicle_status_update(); vehicle_control_mode_update(); global_position_update(); + sensor_combined_update(); home_position_update(); navigation_capabilities_update(); params_update(); @@ -272,7 +282,7 @@ Navigator::task_main() const hrt_abstime mavlink_open_interval = 500000; /* wakeup source(s) */ - struct pollfd fds[6]; + struct pollfd fds[7]; /* Setup of loop */ fds[0].fd = _global_pos_sub; @@ -287,6 +297,8 @@ Navigator::task_main() fds[4].events = POLLIN; fds[5].fd = _param_update_sub; fds[5].events = POLLIN; + fds[6].fd = _sensor_combined_sub; + fds[6].events = POLLIN; while (!_task_should_exit) { @@ -311,6 +323,11 @@ Navigator::task_main() _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); } + /* sensors combined updated */ + if (fds[6].revents & POLLIN) { + sensor_combined_update(); + } + /* parameters updated */ if (fds[5].revents & POLLIN) { params_update(); @@ -342,7 +359,13 @@ Navigator::task_main() global_position_update(); /* Check geofence violation */ - if (!_geofence.inside(&_global_pos)) { + bool inside = false; + if (_geofence.getAltitudeMode() == Geofence::GF_ALT_MODE_GPS) { + inside = _geofence.inside(_global_pos); + } else { + inside = _geofence.inside(_global_pos, _sensor_combined.baro_alt_meter); + } + if (!inside) { /* inform other apps via the sp triplet */ _pos_sp_triplet.geofence_violated = true; if (_pos_sp_triplet.geofence_violated != true) { -- cgit v1.2.3 From 5832948371866aec8f0c7f16b13869f270d36aad Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 17 Aug 2014 12:37:14 +0200 Subject: geofence: lat/lon is double types changed but the geofence implentation was not updated, this was forgotten in 58792c5ca6e42bc251dd3c92b0e79217ff5d5403 --- src/modules/navigator/geofence.cpp | 17 ++++++----------- 1 file changed, 6 insertions(+), 11 deletions(-) diff --git a/src/modules/navigator/geofence.cpp b/src/modules/navigator/geofence.cpp index 2b9ce752b..7897c7ba0 100644 --- a/src/modules/navigator/geofence.cpp +++ b/src/modules/navigator/geofence.cpp @@ -77,18 +77,12 @@ Geofence::~Geofence() bool Geofence::inside(const struct vehicle_global_position_s &global_position) { - double lat = global_position.lat / 1e7d; - double lon = global_position.lon / 1e7d; - - return inside(lat, lon, global_position.alt); + return inside(global_position.lat, global_position.lon, global_position.alt); } -bool Geofence::inside(const struct vehicle_global_position_s &global_position, float baro_altitude_amsl) { - - double lat = global_position.lat / 1e7d; - double lon = global_position.lon / 1e7d; - - return inside(lat, lon, baro_altitude_amsl); +bool Geofence::inside(const struct vehicle_global_position_s &global_position, float baro_altitude_amsl) +{ + return inside(global_position.lat, global_position.lon, baro_altitude_amsl); } bool Geofence::inside(double lat, double lon, float altitude) @@ -101,8 +95,9 @@ bool Geofence::inside(double lat, double lon, float altitude) if (!isEmpty()) { /* Vertical check */ - if (altitude > _altitude_max || altitude < _altitude_min) + if (altitude > _altitude_max || altitude < _altitude_min) { return false; + } /*Horizontal check */ /* Adaptation of algorithm originally presented as -- cgit v1.2.3 From 99860da9b70bfa87ef2834efa5e7b9ba96ee4e9b Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 17 Aug 2014 16:36:35 +0200 Subject: commander: remove unnecessary output (navigator already does the output) --- src/modules/commander/commander.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 2ab40a5eb..84a4be948 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1316,7 +1316,6 @@ int commander_thread_main(int argc, char *argv[]) /* this will only trigger if geofence is activated via param and a geofence file is present, also there is a circuit breaker to disable the actual flight termination in the px4io driver */ armed.force_failsafe = true; status_changed = true; - mavlink_log_emergency(mavlink_fd, "Geofence violated: terminating"); } // no reset is done here on purpose, on geofence violation we want to stay in flighttermination } -- cgit v1.2.3 From 60706500df4e5264f77d0a7c36edb7b6b60e21ac Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 17 Aug 2014 20:48:38 +0200 Subject: Make some space on FMUv1 --- makefiles/config_px4fmu-v1_default.mk | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk index a7c10f52f..97eddfdd2 100644 --- a/makefiles/config_px4fmu-v1_default.mk +++ b/makefiles/config_px4fmu-v1_default.mk @@ -25,7 +25,6 @@ MODULES += drivers/mpu6000 MODULES += drivers/hmc5883 MODULES += drivers/ms5611 MODULES += drivers/mb12xx -MODULES += drivers/ll40ls MODULES += drivers/gps MODULES += drivers/hil MODULES += drivers/hott/hott_telemetry @@ -44,7 +43,6 @@ MODULES += modules/sensors # MODULES += systemcmds/mtd MODULES += systemcmds/bl_update -MODULES += systemcmds/i2c MODULES += systemcmds/mixer MODULES += systemcmds/param MODULES += systemcmds/perf @@ -152,5 +150,4 @@ endef # command priority stack entrypoint BUILTIN_COMMANDS := \ $(call _B, sercon, , 2048, sercon_main ) \ - $(call _B, serdis, , 2048, serdis_main ) \ - $(call _B, sysinfo, , 2048, sysinfo_main ) + $(call _B, serdis, , 2048, serdis_main ) -- cgit v1.2.3 From 171857811f76fb4d8fb3b65329345dd5af0ec9db Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 17 Aug 2014 20:49:46 +0200 Subject: EKF filter update. Now correctly scaling variance (well, if you could call this ever "correct") for repeated fusion of the same data quantity. --- .../ekf_att_pos_estimator_main.cpp | 33 ++++++++-- .../ekf_att_pos_estimator/estimator_23states.cpp | 72 ++++++++++++++++++---- .../ekf_att_pos_estimator/estimator_23states.h | 17 ++++- 3 files changed, 104 insertions(+), 18 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index 91d17e787..6cb68fe77 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 @@ -96,7 +96,10 @@ extern "C" __EXPORT int ekf_att_pos_estimator_main(int argc, char *argv[]); __EXPORT uint32_t millis(); +__EXPORT uint64_t getMicros(); + static uint64_t IMUmsec = 0; +static uint64_t IMUusec = 0; static const uint64_t FILTER_INIT_DELAY = 1 * 1000 * 1000; uint32_t millis() @@ -104,6 +107,11 @@ uint32_t millis() return IMUmsec; } +uint64_t getMicros() +{ + return IMUusec; +} + class FixedwingEstimator { public: @@ -850,7 +858,8 @@ FixedwingEstimator::task_main() } _last_sensor_timestamp = _gyro.timestamp; - IMUmsec = _gyro.timestamp / 1e3f; + IMUmsec = _gyro.timestamp / 1e3; + IMUusec = _gyro.timestamp; float deltaT = (_gyro.timestamp - _last_run) / 1e6f; _last_run = _gyro.timestamp; @@ -914,7 +923,8 @@ FixedwingEstimator::task_main() // Copy gyro and accel _last_sensor_timestamp = _sensor_combined.timestamp; - IMUmsec = _sensor_combined.timestamp / 1e3f; + IMUmsec = _sensor_combined.timestamp / 1e3; + IMUusec = _sensor_combined.timestamp; float deltaT = (_sensor_combined.timestamp - _last_run) / 1e6f; @@ -994,8 +1004,6 @@ FixedwingEstimator::task_main() if (gps_updated) { - last_gps = _gps.timestamp_position; - orb_copy(ORB_ID(vehicle_gps_position), _gps_sub, &_gps); perf_count(_perf_gps); @@ -1008,11 +1016,17 @@ FixedwingEstimator::task_main() _gps_start_time = hrt_absolute_time(); /* check if we had a GPS outage for a long time */ - if (hrt_elapsed_time(&last_gps) > 5 * 1000 * 1000) { + float gps_elapsed = hrt_elapsed_time(&last_gps) / 1e6f; + + const float pos_reset_threshold = 5.0f; // seconds + + /* timeout of 5 seconds */ + if (gps_elapsed > pos_reset_threshold) { _ekf->ResetPosition(); _ekf->ResetVelocity(); _ekf->ResetStoredStates(); } + _ekf->updateDtGpsFilt(math::constrain((_gps.timestamp_position - last_gps) / 1e6f, 0.01f, pos_reset_threshold)); /* fuse GPS updates */ @@ -1044,6 +1058,8 @@ FixedwingEstimator::task_main() newDataGps = true; + last_gps = _gps.timestamp_position; + } } @@ -1052,8 +1068,15 @@ FixedwingEstimator::task_main() orb_check(_baro_sub, &baro_updated); if (baro_updated) { + + hrt_abstime baro_last = _baro.timestamp; + orb_copy(ORB_ID(sensor_baro0), _baro_sub, &_baro); + float baro_elapsed = (_baro.timestamp - baro_last) / 1e6f; + + _ekf->updateDtHgtFilt(math::constrain(baro_elapsed, 0.001f, 0.1)); + _ekf->baroHgt = _baro.altitude; if (!_baro_init) { diff --git a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp index c7c7305b2..d1349535d 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp @@ -2,6 +2,11 @@ #include #include #include +#include + +#ifndef M_PI_F +#define M_PI_F ((float)M_PI) +#endif #define EKF_COVARIANCE_DIVERGED 1.0e8f @@ -38,6 +43,7 @@ AttPosEKF::AttPosEKF() : resetStates{}, storedStates{}, statetimeStamp{}, + lastVelPosFusion(millis()), statesAtVelTime{}, statesAtPosTime{}, statesAtHgtTime{}, @@ -59,7 +65,12 @@ AttPosEKF::AttPosEKF() : accel(), dVelIMU(), dAngIMU(), - dtIMU(0), + dtIMU(0.005f), + dtIMUfilt(0.005f), + dtVelPos(0.01f), + dtVelPosFilt(0.01f), + dtHgtFilt(0.01f), + dtGpsFilt(0.1f), fusionModeGPS(0), innovVelPos{}, varInnovVelPos{}, @@ -260,6 +271,9 @@ void AttPosEKF::UpdateStrapdownEquationsNED() // Constrain states (to protect against filter divergence) ConstrainStates(); + + // update filtered IMU time step length + dtIMUfilt = 0.99f * dtIMUfilt + 0.01f * dtIMU; } void AttPosEKF::CovariancePrediction(float dt) @@ -962,6 +976,21 @@ void AttPosEKF::CovariancePrediction(float dt) ConstrainVariances(); } +void AttPosEKF::updateDtGpsFilt(float dt) +{ + dtGpsFilt = ConstrainFloat(dt, 0.001f, 2.0f) * 0.05f + dtGpsFilt * 0.95f; +} + +void AttPosEKF::updateDtHgtFilt(float dt) +{ + dtHgtFilt = ConstrainFloat(dt, 0.001f, 2.0f) * 0.05f + dtHgtFilt * 0.95f; +} + +void AttPosEKF::updateDtVelPosFilt(float dt) +{ + dtVelPosFilt = ConstrainFloat(dt, 0.0005f, 2.0f) * 0.05f + dtVelPosFilt * 0.95f; +} + void AttPosEKF::FuseVelposNED() { @@ -998,6 +1027,18 @@ void AttPosEKF::FuseVelposNED() // associated with sequential fusion if (fuseVelData || fusePosData || fuseHgtData) { + uint64_t tNow = getMicros(); + updateDtVelPosFilt((tNow - lastVelPosFusion) / 1e6f); + lastVelPosFusion = tNow; + + // scaler according to the number of repetitions of the + // same measurement in one fusion step + float gpsVarianceScaler = dtGpsFilt / dtVelPosFilt; + + // scaler according to the number of repetitions of the + // same measurement in one fusion step + float hgtVarianceScaler = dtHgtFilt / dtVelPosFilt; + // set the GPS data timeout depending on whether airspeed data is present if (useAirspeed) horizRetryTime = gpsRetryTime; else horizRetryTime = gpsRetryTimeNoTAS; @@ -1010,12 +1051,12 @@ void AttPosEKF::FuseVelposNED() // Estimate the GPS Velocity, GPS horiz position and height measurement variances. velErr = 0.2f*accNavMag; // additional error in GPS velocities caused by manoeuvring posErr = 0.2f*accNavMag; // additional error in GPS position caused by manoeuvring - R_OBS[0] = sq(vneSigma) + sq(velErr); + R_OBS[0] = gpsVarianceScaler * sq(vneSigma) + sq(velErr); R_OBS[1] = R_OBS[0]; - R_OBS[2] = sq(vdSigma) + sq(velErr); - R_OBS[3] = sq(posNeSigma) + sq(posErr); + R_OBS[2] = gpsVarianceScaler * sq(vdSigma) + sq(velErr); + R_OBS[3] = gpsVarianceScaler * sq(posNeSigma) + sq(posErr); R_OBS[4] = R_OBS[3]; - R_OBS[5] = sq(posDSigma) + sq(posErr); + R_OBS[5] = hgtVarianceScaler * sq(posDSigma) + sq(posErr); // calculate innovations and check GPS data validity using an innovation consistency check if (fuseVelData) @@ -1995,9 +2036,8 @@ void AttPosEKF::FuseOptFlow() varInnovOptFlow[0] = 1.0f/SK_LOS[0]; innovOptFlow[0] = losPred[0] - losData[0]; - // reset the observation index to 0 (we start by fusing the X - // measurement) - obsIndex = 0; + // set the observation index to 1 to fuse the y component next time round and reset the commence fusion flag + obsIndex = 1; fuseOptFlowData = false; } else if (obsIndex == 1) // we are now fusing the Y measurement @@ -2041,6 +2081,10 @@ void AttPosEKF::FuseOptFlow() Kfusion[22] = SK_LOS[1]*(P[22][0]*SKK_LOS[10] - P[22][3]*SKK_LOS[13] + P[22][1]*SKK_LOS[11] + P[22][2]*SKK_LOS[12] - P[22][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[22][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[22][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); varInnovOptFlow[1] = 1.0f/SK_LOS[1]; innovOptFlow[1] = losPred[1] - losData[1]; + + // reset the observation index + obsIndex = 0; + fuseOptFlowData = false; } // Check the innovation for consistency and don't fuse if > 3Sigma @@ -2102,10 +2146,9 @@ void AttPosEKF::FuseOptFlow() P[i][j] = P[i][j] - KHP[i][j]; } } + ForceSymmetry(); + ConstrainVariances(); } - obsIndex = obsIndex + 1; - ForceSymmetry(); - ConstrainVariances(); } void AttPosEKF::zeroCols(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last) @@ -3006,9 +3049,14 @@ void AttPosEKF::ZeroVariables() { // Initialize on-init initialized variables - + dtIMUfilt = ConstrainFloat(dtIMU, 0.001f, 0.02f); + dtVelPosFilt = ConstrainFloat(dtVelPos, 0.04f, 0.5f); + dtGpsFilt = 1.0f / 5.0f; + dtHgtFilt = 1.0f / 100.0f; storeIndex = 0; + lastVelPosFusion = millis(); + // Do the data structure init for (unsigned i = 0; i < n_states; i++) { for (unsigned j = 0; j < n_states; j++) { diff --git a/src/modules/ekf_att_pos_estimator/estimator_23states.h b/src/modules/ekf_att_pos_estimator/estimator_23states.h index faa6735ca..12cbc53b8 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_23states.h +++ b/src/modules/ekf_att_pos_estimator/estimator_23states.h @@ -116,6 +116,9 @@ public: float storedStates[n_states][data_buffer_size]; // state vectors stored for the last 50 time steps uint32_t statetimeStamp[data_buffer_size]; // time stamp for each state vector stored + // Times + uint64_t lastVelPosFusion; // the time of the last velocity fusion, in the standard time unit of the filter + float statesAtVelTime[n_states]; // States at the effective measurement time for posNE and velNED measurements float statesAtPosTime[n_states]; // States at the effective measurement time for posNE and velNED measurements float statesAtHgtTime[n_states]; // States at the effective measurement time for the hgtMea measurement @@ -140,7 +143,12 @@ public: Vector3f accel; // acceleration vector in XYZ body axes measured by the IMU (m/s^2) Vector3f dVelIMU; Vector3f dAngIMU; - float dtIMU; // time lapsed since the last IMU measurement or covariance update (sec) + float dtIMU; // time lapsed since the last IMU measurement or covariance update (sec), this may have significant jitter + float dtIMUfilt; // average time between IMU measurements (sec) + float dtVelPos; // time lapsed since the last position / velocity fusion (seconds), this may have significant jitter + float dtVelPosFilt; // average time between position / velocity fusion steps + float dtHgtFilt; // average time between height measurement updates + float dtGpsFilt; // average time between gps measurement updates uint8_t fusionModeGPS; // 0 = GPS outputs 3D velocity, 1 = GPS outputs 2D velocity, 2 = GPS outputs no velocity float innovVelPos[6]; // innovation output float varInnovVelPos[6]; // innovation variance output @@ -211,6 +219,9 @@ public: unsigned storeIndex; +void updateDtGpsFilt(float dt); + +void updateDtHgtFilt(float dt); void UpdateStrapdownEquationsNED(); @@ -300,6 +311,8 @@ void InitializeDynamic(float (&initvelNED)[3], float declination); protected: +void updateDtVelPosFilt(float dt); + bool FilterHealthy(); bool GyroOffsetsDiverged(); @@ -314,3 +327,5 @@ void AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, fl uint32_t millis(); +uint64_t getMicros(); + -- cgit v1.2.3 From f31c3243b05fae6ffa4ab8cccb8e009470ca1294 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 17 Aug 2014 23:28:48 +0200 Subject: mc_pos_control: navigation fixes, smooth position setpoint movements --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 77 ++++++++++++---------- 1 file changed, 44 insertions(+), 33 deletions(-) diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 488ca924a..847a92151 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -236,7 +236,7 @@ private: /** * Set position setpoint for AUTO */ - void control_auto(); + void control_auto(float dt); /** * Select between barometric and global (AMSL) altitudes @@ -682,7 +682,7 @@ MulticopterPositionControl::cross_sphere_line(const math::Vector<3>& sphere_c, f } void -MulticopterPositionControl::control_auto() +MulticopterPositionControl::control_auto(float dt) { bool updated; orb_check(_pos_sp_triplet_sub, &updated); @@ -703,8 +703,14 @@ MulticopterPositionControl::control_auto() &curr_sp.data[0], &curr_sp.data[1]); curr_sp(2) = -(_pos_sp_triplet.current.alt - _ref_alt); + /* scaled space: 1 == position error resulting max allowed speed, L1 = 1 in this space */ + math::Vector<3> scale = _params.pos_p.edivide(_params.vel_max); // TODO add mult param here + + /* convert current setpoint to scaled space */ + math::Vector<3> curr_sp_s = curr_sp.emult(scale); + /* by default use current setpoint as is */ - _pos_sp = curr_sp; + math::Vector<3> pos_sp_s = curr_sp_s; if (_pos_sp_triplet.current.type == SETPOINT_TYPE_POSITION && _pos_sp_triplet.previous.valid) { /* follow "previous - current" line */ @@ -715,16 +721,13 @@ MulticopterPositionControl::control_auto() prev_sp(2) = -(_pos_sp_triplet.previous.alt - _ref_alt); if ((curr_sp - prev_sp).length() > MIN_DIST) { - /* scaled space: 1 == position error resulting max allowed speed, L1 = 1 in this space */ - math::Vector<3> scale = _params.pos_p.edivide(_params.vel_max); /* find X - cross point of L1 sphere and trajectory */ math::Vector<3> pos_s = _pos.emult(scale); math::Vector<3> prev_sp_s = prev_sp.emult(scale); - math::Vector<3> curr_sp_s = curr_sp.emult(scale); + math::Vector<3> prev_curr_s = curr_sp_s - prev_sp_s; math::Vector<3> curr_pos_s = pos_s - curr_sp_s; float curr_pos_s_len = curr_pos_s.length(); - math::Vector<3> x; if (curr_pos_s_len < 1.0f) { /* copter is closer to waypoint than L1 radius */ /* check next waypoint and use it to avoid slowing down when passing via waypoint */ @@ -740,56 +743,64 @@ MulticopterPositionControl::control_auto() /* calculate angle prev - curr - next */ math::Vector<3> curr_next_s = next_sp_s - curr_sp_s; - math::Vector<3> prev_curr_s_norm = (curr_sp_s - prev_sp_s).normalized(); + math::Vector<3> prev_curr_s_norm = prev_curr_s.normalized(); - /* cos(a) = angle between current and next trajectory segments */ - float cos_a = prev_curr_s_norm * curr_next_s; + /* cos(a) * curr_next, a = angle between current and next trajectory segments */ + float cos_a_curr_next = prev_curr_s_norm * curr_next_s; - /* cos(b) = angle pos - curr_sp - prev_sp */ + /* cos(b), b = angle pos - curr_sp - prev_sp */ float cos_b = -curr_pos_s * prev_curr_s_norm / curr_pos_s_len; - if (cos_a > 0.0f && cos_b > 0.0f) { + if (cos_a_curr_next > 0.0f && cos_b > 0.0f) { float curr_next_s_len = curr_next_s.length(); /* if curr - next distance is larger than L1 radius, limit it */ if (curr_next_s_len > 1.0f) { - cos_a /= curr_next_s_len; + cos_a_curr_next /= curr_next_s_len; } /* feed forward position setpoint offset */ math::Vector<3> pos_ff = prev_curr_s_norm * - cos_a * cos_b * cos_b * (1.0f - curr_pos_s_len) * + cos_a_curr_next * cos_b * cos_b * (1.0f - curr_pos_s_len) * (1.0f - expf(-curr_pos_s_len * curr_pos_s_len * 20.0f)); - _pos_sp += pos_ff.edivide(scale); + pos_sp_s += pos_ff; } } } } else { - bool near = cross_sphere_line(pos_s, 1.0f, prev_sp_s, curr_sp_s, x); + bool near = cross_sphere_line(pos_s, 1.0f, prev_sp_s, curr_sp_s, pos_sp_s); if (near) { /* L1 sphere crosses trajectory */ } else { /* copter is too far from trajectory */ /* if copter is behind prev waypoint, go directly to prev waypoint */ - if ((x - prev_sp_s) * (curr_sp_s - prev_sp) < 0.0f) { - x = prev_sp_s; + if ((pos_sp_s - prev_sp_s) * prev_curr_s < 0.0f) { + pos_sp_s = prev_sp_s; } /* if copter is in front of curr waypoint, go directly to curr waypoint */ - if ((x - curr_sp_s) * (curr_sp_s - prev_sp) > 0.0f) { - x = curr_sp_s; + if ((pos_sp_s - curr_sp_s) * prev_curr_s > 0.0f) { + pos_sp_s = curr_sp_s; } - x = pos_s + (x - pos_s).normalized(); + pos_sp_s = pos_s + (pos_sp_s - pos_s).normalized(); } - - /* scale result back to normal space */ - _pos_sp = x.edivide(scale); } } } + /* move setpoint not faster than max allowed speed */ + math::Vector<3> pos_sp_old_s = _pos_sp.emult(scale); + math::Vector<3> d_pos_s = pos_sp_s - pos_sp_old_s; + float d_pos_s_len = d_pos_s.length(); + if (d_pos_s_len > dt) { + pos_sp_s = pos_sp_old_s + d_pos_s / d_pos_s_len * dt; + } + + /* scale result back to normal space */ + _pos_sp = pos_sp_s.edivide(scale); + /* update yaw setpoint if needed */ if (isfinite(_pos_sp_triplet.current.yaw)) { _att_sp.yaw_body = _pos_sp_triplet.current.yaw; @@ -912,7 +923,7 @@ MulticopterPositionControl::task_main() } else { /* AUTO */ - control_auto(); + control_auto(dt); } /* fill local position setpoint */ @@ -975,14 +986,14 @@ MulticopterPositionControl::task_main() } - if (!_control_mode.flag_control_manual_enabled) { - /* limit 3D speed only in non-manual modes */ - float vel_sp_norm = _vel_sp.edivide(_params.vel_max).length(); - - if (vel_sp_norm > 1.0f) { - _vel_sp /= vel_sp_norm; - } - } +// if (!_control_mode.flag_control_manual_enabled) { +// /* limit 3D speed only in non-manual modes */ +// float vel_sp_norm = _vel_sp.edivide(_params.vel_max).length(); +// +// if (vel_sp_norm > 1.0f) { +// _vel_sp /= vel_sp_norm; +// } +// } _global_vel_sp.vx = _vel_sp(0); _global_vel_sp.vy = _vel_sp(1); -- cgit v1.2.3 From 47b3f1253eb2a46c216aee1ca9f08d348266ea41 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 18 Aug 2014 16:47:30 +0200 Subject: Fix bottle drop logic --- src/modules/bottle_drop/bottle_drop.cpp | 47 ++++++++++++++------------------- 1 file changed, 20 insertions(+), 27 deletions(-) diff --git a/src/modules/bottle_drop/bottle_drop.cpp b/src/modules/bottle_drop/bottle_drop.cpp index 14b9a8379..77f691f6c 100644 --- a/src/modules/bottle_drop/bottle_drop.cpp +++ b/src/modules/bottle_drop/bottle_drop.cpp @@ -126,10 +126,9 @@ private: float _alt_clearance; struct position_s { - double lat; //degrees - double lon; //degrees - float alt; //m - bool initialized; + double lat; ///< degrees + double lon; ///< degrees + float alt; ///< m } _target_position, _drop_position; enum DROP_STATE { @@ -646,7 +645,7 @@ BottleDrop::task_main() (double)distance_open_door, (double)(_wrap_pi(_global_pos.yaw - atan2f(wind.windspeed_north, wind.windspeed_east)))); - if (isfinite(distance_real) && distance_real < distance_open_door && _drop_approval) { + if (isfinite(distance_real) && distance_real < distance_open_door) { open_bay(); _drop_state = DROP_STATE_BAY_OPEN; } @@ -655,27 +654,21 @@ BottleDrop::task_main() case DROP_STATE_BAY_OPEN: { - map_projection_project(&ref, _global_pos.lat, _global_pos.lon, &x_l, &y_l); - x_f = x_l + _global_pos.vel_n * dt_runs; - y_f = y_l + _global_pos.vel_e * dt_runs; - map_projection_reproject(&ref, x_f, y_f, &x_f_NED, &y_f_NED); - future_distance = fabsf(get_distance_to_next_waypoint(x_f_NED, y_f_NED, _drop_position.lat, _drop_position.lon)); - - warnx("Distance real: %.2f", (double)distance_real); - - if (isfinite(distance_real) && (((distance_real < precision) && (distance_real < future_distance)) || - /* we drop as well if we're really close */ (distance_real < precision / 10.0f))) { // Drop only if the distance between drop point and actual position is getting larger again - // XXX this check needs to be carefully validated - right now we prefer to drop if we're close to the goal - - // if(fabsf(_wrap_pi(_global_pos.yaw-atan2f(wind.windspeed_north,wind_speed.windspeed_east)+M_PI_F)) < 80.0f/180.0f*M_PI_F) // if flight trajectory deviates more than 80 degrees from calculated path, it will no drop - // { - drop(); - _drop_state = DROP_STATE_DROPPED; - // } - // else - // { - // state_run = true; - // } + if (_drop_approval) { + map_projection_project(&ref, _global_pos.lat, _global_pos.lon, &x_l, &y_l); + x_f = x_l + _global_pos.vel_n * dt_runs; + y_f = y_l + _global_pos.vel_e * dt_runs; + map_projection_reproject(&ref, x_f, y_f, &x_f_NED, &y_f_NED); + future_distance = fabsf(get_distance_to_next_waypoint(x_f_NED, y_f_NED, _drop_position.lat, _drop_position.lon)); + + warnx("Distance real: %.2f", (double)distance_real); + + if (isfinite(distance_real) && + (((distance_real < precision) && (distance_real < future_distance)) || + (distance_real < precision / 10.0f))) { + drop(); + _drop_state = DROP_STATE_DROPPED; + } } } break; @@ -684,6 +677,7 @@ BottleDrop::task_main() /* 2s after drop, reset and close everything again */ if ((hrt_elapsed_time(&_doors_opened) > 2 * 1000 * 1000)) { _drop_state = DROP_STATE_INIT; + _drop_approval = false; lock_release(); close_bay(); } @@ -759,7 +753,6 @@ BottleDrop::handle_command(struct vehicle_command_s *cmd) _target_position.lat = cmd->param5; _target_position.lon = cmd->param6; _target_position.alt = cmd->param7; - _target_position.initialized; _drop_state = DROP_STATE_TARGET_VALID; mavlink_log_info(_mavlink_fd, "got target: %8.4f, %8.4f, %8.4f", (double)_target_position.lat, (double)_target_position.lon, (double)_target_position.alt); -- cgit v1.2.3 From cfcb76f627d164b3244e9b81b2fe2fe2e54ee045 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 18 Aug 2014 21:23:59 +0200 Subject: Add long-term stable wind estimate --- .../ekf_att_pos_estimator/estimator_23states.cpp | 21 +++++++++++++++++++++ .../ekf_att_pos_estimator/estimator_23states.h | 3 +++ 2 files changed, 24 insertions(+) diff --git a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp index d1349535d..d176ccee2 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp @@ -71,6 +71,9 @@ AttPosEKF::AttPosEKF() : dtVelPosFilt(0.01f), dtHgtFilt(0.01f), dtGpsFilt(0.1f), + windSpdFiltNorth(0.0f), + windSpdFiltEast(0.0f), + windSpdFiltAltitude(0.0f), fusionModeGPS(0), innovVelPos{}, varInnovVelPos{}, @@ -1657,6 +1660,24 @@ void AttPosEKF::FuseAirspeed() // Perform fusion of True Airspeed measurement if (useAirspeed && fuseVtasData && (VtasPred > 1.0f) && (VtasMeas > 8.0f)) { + // Lowpass the output of the wind estimate - we want a long-term + // stable estimate, but not start to load into the overall dynamics + // of the system (which adjusting covariances would do) + + // Nominally damp to 0.02% of the noise, but reduce the damping for strong altitude variations + // assuming equal wind speeds on the same altitude and varying wind speeds on + // different altitudes + float windFiltCoeff = 0.0002f; + + float altDiff = fabsf(windSpdFiltAltitude - hgtMea); + + // Change filter coefficient based on altitude + windFiltCoeff += ConstrainFloat(0.00001f * altDiff, windFiltCoeff, 0.1f); + + windSpdFiltNorth = ((1.0f - windFiltCoeff) * windSpdFiltNorth) + (windFiltCoeff * vwn); + windSpdFiltEast = ((1.0f - windFiltCoeff) * windSpdFiltEast) + (windFiltCoeff * vwe); + windSpdFiltAltitude = hgtMea; + // Calculate observation jacobians SH_TAS[0] = 1/(sqrt(sq(ve - vwe) + sq(vn - vwn) + sq(vd))); SH_TAS[1] = (SH_TAS[0]*(2.0f*ve - 2*vwe))/2.0f; diff --git a/src/modules/ekf_att_pos_estimator/estimator_23states.h b/src/modules/ekf_att_pos_estimator/estimator_23states.h index 12cbc53b8..6349b03f0 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_23states.h +++ b/src/modules/ekf_att_pos_estimator/estimator_23states.h @@ -149,6 +149,9 @@ public: float dtVelPosFilt; // average time between position / velocity fusion steps float dtHgtFilt; // average time between height measurement updates float dtGpsFilt; // average time between gps measurement updates + float windSpdFiltNorth; // average wind speed north component + float windSpdFiltEast; // average wind speed east component + float windSpdFiltAltitude; // the last altitude used to filter wind speed uint8_t fusionModeGPS; // 0 = GPS outputs 3D velocity, 1 = GPS outputs 2D velocity, 2 = GPS outputs no velocity float innovVelPos[6]; // innovation output float varInnovVelPos[6]; // innovation variance output -- cgit v1.2.3 From 2c0d192944744086905e622f445a523d6650cdc5 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 18 Aug 2014 21:26:18 +0200 Subject: Use the wind speed estimate filtered values --- src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index 6cb68fe77..c384b2566 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 @@ -1490,8 +1490,10 @@ FixedwingEstimator::task_main() if (hrt_elapsed_time(&_wind.timestamp) > 99000) { _wind.timestamp = _global_pos.timestamp; - _wind.windspeed_north = _ekf->states[14]; - _wind.windspeed_east = _ekf->states[15]; + _wind.windspeed_north = _ekf->windSpdFiltNorth; + _wind.windspeed_east = _ekf->windSpdFiltEast; + // XXX we need to do something smart about the covariance here + // but we default to the estimate covariance for now _wind.covariance_north = _ekf->P[14][14]; _wind.covariance_east = _ekf->P[15][15]; -- cgit v1.2.3 From 7f9c996555975301288da58745f69b39f05facbe Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 19 Aug 2014 07:30:19 +0200 Subject: engine fail: small state machine fix --- src/modules/navigator/enginefailure.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/modules/navigator/enginefailure.cpp b/src/modules/navigator/enginefailure.cpp index de567f0dc..e007b208b 100644 --- a/src/modules/navigator/enginefailure.cpp +++ b/src/modules/navigator/enginefailure.cpp @@ -76,7 +76,8 @@ EngineFailure::on_inactive() void EngineFailure::on_activation() { - _ef_state = EF_STATE_LOITERDOWN; + _ef_state = EF_STATE_NONE; + advance_ef(); set_ef_item(); } @@ -139,6 +140,7 @@ EngineFailure::advance_ef() { switch (_ef_state) { case EF_STATE_NONE: + mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: Engine failure. Loitering down"); _ef_state = EF_STATE_LOITERDOWN; break; default: -- cgit v1.2.3 From 64ca94412e710164fb2ae69f6dfc3edbeff9c12b Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 19 Aug 2014 07:31:55 +0200 Subject: engine fail: fw pos control limits pitch and sets 0 throttle --- .../fw_pos_control_l1/fw_pos_control_l1_main.cpp | 65 +++++++++++++++++----- 1 file changed, 52 insertions(+), 13 deletions(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 350ce6dec..0c35ccb2c 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -139,7 +139,8 @@ private: int _pos_sp_triplet_sub; int _att_sub; /**< vehicle attitude subscription */ int _airspeed_sub; /**< airspeed subscription */ - int _control_mode_sub; /**< vehicle status subscription */ + int _control_mode_sub; /**< control mode subscription */ + int _vehicle_status_sub; /**< vehicle status subscription */ int _params_sub; /**< notification of parameter updates */ int _manual_control_sub; /**< notification of manual control updates */ int _sensor_combined_sub; /**< for body frame accelerations */ @@ -154,7 +155,8 @@ private: struct navigation_capabilities_s _nav_capabilities; /**< navigation capabilities */ struct manual_control_setpoint_s _manual; /**< r/c channel data */ struct airspeed_s _airspeed; /**< airspeed */ - struct vehicle_control_mode_s _control_mode; /**< vehicle status */ + struct vehicle_control_mode_s _control_mode; /**< control mode */ + struct vehicle_status_s _vehicle_status; /**< vehicle status */ struct vehicle_global_position_s _global_pos; /**< global vehicle position */ struct position_setpoint_triplet_s _pos_sp_triplet; /**< triplet of mission items */ struct sensor_combined_s _sensor_combined; /**< for body frame accelerations */ @@ -301,10 +303,15 @@ private: void control_update(); /** - * Check for changes in vehicle status. + * Check for changes in control mode */ void vehicle_control_mode_poll(); + /** + * Check for changes in vehicle status. + */ + void vehicle_status_poll(); + /** * Check for airspeed updates. */ @@ -408,6 +415,7 @@ FixedwingPositionControl::FixedwingPositionControl() : _att_sub(-1), _airspeed_sub(-1), _control_mode_sub(-1), + _vehicle_status_sub(-1), _params_sub(-1), _manual_control_sub(-1), _sensor_combined_sub(-1), @@ -425,6 +433,7 @@ FixedwingPositionControl::FixedwingPositionControl() : _manual(), _airspeed(), _control_mode(), + _vehicle_status(), _global_pos(), _pos_sp_triplet(), _sensor_combined(), @@ -627,16 +636,27 @@ FixedwingPositionControl::parameters_update() void FixedwingPositionControl::vehicle_control_mode_poll() { - bool vstatus_updated; + bool updated; - /* Check HIL state if vehicle status has changed */ - orb_check(_control_mode_sub, &vstatus_updated); + orb_check(_control_mode_sub, &updated); - if (vstatus_updated) { + if (updated) { orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode); } } +void +FixedwingPositionControl::vehicle_status_poll() +{ + bool updated; + + orb_check(_vehicle_status_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &_vehicle_status); + } +} + bool FixedwingPositionControl::vehicle_airspeed_poll() { @@ -1182,10 +1202,13 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } } - if (usePreTakeoffThrust) { - _att_sp.thrust = launchDetector.getThrottlePreTakeoff(); + if (_vehicle_status.engine_failure || _vehicle_status.engine_failure_cmd) { + /* Set thrrust to 0 to minimize damage */ + _att_sp.thrust = 0.0f; } - else { + else if (usePreTakeoffThrust) { + _att_sp.thrust = launchDetector.getThrottlePreTakeoff(); + } else { _att_sp.thrust = math::min(_mTecs.getEnabled() ? _mTecs.getThrottleSetpoint() : _tecs.get_throttle_demand(), throttle_max); } _att_sp.pitch_body = _mTecs.getEnabled() ? _mTecs.getPitchSetpoint() : _tecs.get_pitch_demand(); @@ -1212,13 +1235,16 @@ FixedwingPositionControl::task_main() _att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); _sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined)); _control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); + _vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status)); _airspeed_sub = orb_subscribe(ORB_ID(airspeed)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); _manual_control_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); _range_finder_sub = orb_subscribe(ORB_ID(sensor_range_finder)); - /* rate limit vehicle status updates to 5Hz */ + /* rate limit control mode updates to 5Hz */ orb_set_interval(_control_mode_sub, 200); + /* rate limit vehicle status updates to 5Hz */ + orb_set_interval(_vehicle_status_sub, 200); /* rate limit position updates to 50 Hz */ orb_set_interval(_global_pos_sub, 20); @@ -1257,9 +1283,12 @@ FixedwingPositionControl::task_main() perf_begin(_loop_perf); - /* check vehicle status for changes to publication state */ + /* check vehicle control mode for changes to publication state */ vehicle_control_mode_poll(); + /* check vehicle status for changes to publication state */ + vehicle_status_poll(); + /* only update parameters if they changed */ if (fds[0].revents & POLLIN) { /* read from param to clear updated flag */ @@ -1372,7 +1401,12 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_ flightPathAngle = -asinf(ground_speed(2)/ground_speed_length); } fwPosctrl::LimitOverride limitOverride; - if (climbout_mode) { + if (_vehicle_status.engine_failure || _vehicle_status.engine_failure_cmd) { + /* Force the slow downwards spiral */ + limitOverride.enablePitchMinOverride(-1.0f); + limitOverride.enablePitchMaxOverride(5.0f); + + } else if (climbout_mode) { limitOverride.enablePitchMinOverride(M_RAD_TO_DEG_F * climbout_pitch_min_rad); } else { limitOverride.disablePitchMinOverride(); @@ -1380,6 +1414,11 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_ _mTecs.updateAltitudeSpeed(flightPathAngle, altitude, alt_sp, _airspeed.true_airspeed_m_s, v_sp, mode, limitOverride); } else { + if (_vehicle_status.engine_failure || _vehicle_status.engine_failure_cmd) { + /* Force the slow downwards spiral */ + pitch_min_rad = M_DEG_TO_RAD_F * -1.0f; + pitch_max_rad = M_DEG_TO_RAD_F * 5.0f; + } /* Using tecs library */ _tecs.update_pitch_throttle(_R_nb, _att.pitch, altitude, alt_sp, v_sp, _airspeed.indicated_airspeed_m_s, eas2tas, -- 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 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 09a9ea87e77d2627b20bd6893a39627f57421f4d Mon Sep 17 00:00:00 2001 From: Holger Steinhaus Date: Fri, 15 Aug 2014 14:22:41 +0200 Subject: uavcan: increased thread prio, reduces roundtrip latency by a factor of 5..7 --- src/modules/uavcan/uavcan_main.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index 4535b6d6a..f4d3cc77b 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -40,6 +40,7 @@ #include #include #include +#include #include #include #include @@ -164,7 +165,7 @@ int UavcanNode::start(uavcan::NodeID node_id, uint32_t bitrate) * Start the task. Normally it should never exit. */ static auto run_trampoline = [](int, char *[]) {return UavcanNode::_instance->run();}; - _instance->_task = task_spawn_cmd("uavcan", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, StackSize, + _instance->_task = task_spawn_cmd("uavcan", SCHED_DEFAULT, SCHED_PRIORITY_ACTUATOR_OUTPUTS, StackSize, static_cast(run_trampoline), nullptr); if (_instance->_task < 0) { -- cgit v1.2.3 From 760a7ff548bcef6910f84beaa981600f3609358b Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 20 Aug 2014 07:45:01 +0200 Subject: gpsfailure: add skeleton class, activate in commander --- src/modules/commander/state_machine_helper.cpp | 7 +- src/modules/navigator/datalinkloss.cpp | 2 +- src/modules/navigator/gpsfailure.cpp | 170 +++++++++++++++++++++++++ src/modules/navigator/gpsfailure.h | 86 +++++++++++++ src/modules/navigator/module.mk | 3 +- src/modules/navigator/navigator.h | 4 +- src/modules/navigator/navigator_main.cpp | 5 + 7 files changed, 272 insertions(+), 5 deletions(-) create mode 100644 src/modules/navigator/gpsfailure.cpp create mode 100644 src/modules/navigator/gpsfailure.h diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 3f4bfaa1c..404bafb04 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -499,12 +499,15 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en status->nav_state = NAVIGATION_STATE_AUTO_LANDENGFAIL; } else if (status->data_link_lost_cmd) { status->nav_state = NAVIGATION_STATE_AUTO_RTGS; - //} else if (status->gps_failure_cmd) { - //status->nav_state = NAVIGATION_STATE_AUTO_***; + } else if (status->gps_failure_cmd) { + status->nav_state = NAVIGATION_STATE_AUTO_LANDGPSFAIL; } else if (status->rc_signal_lost_cmd) { status->nav_state = NAVIGATION_STATE_AUTO_RTGS; //XXX + /* Finished handling commands which have priority , now handle failures */ } else if (status->engine_failure) { status->nav_state = NAVIGATION_STATE_AUTO_LANDENGFAIL; + } else if (status->gps_failure) { + status->nav_state = NAVIGATION_STATE_AUTO_LANDGPSFAIL; } else if (((status->data_link_lost && data_link_loss_enabled) && status->rc_signal_lost) || (!data_link_loss_enabled && status->rc_signal_lost && mission_finished)) { status->failsafe = true; diff --git a/src/modules/navigator/datalinkloss.cpp b/src/modules/navigator/datalinkloss.cpp index b914fdb34..d8a1de229 100644 --- a/src/modules/navigator/datalinkloss.cpp +++ b/src/modules/navigator/datalinkloss.cpp @@ -32,7 +32,7 @@ ****************************************************************************/ /** * @file datalinkloss.cpp - * Helper class for Data Link Loss Mode acording to the OBC rules + * Helper class for Data Link Loss Mode according to the OBC rules * * @author Thomas Gubler */ diff --git a/src/modules/navigator/gpsfailure.cpp b/src/modules/navigator/gpsfailure.cpp new file mode 100644 index 000000000..4c526b76e --- /dev/null +++ b/src/modules/navigator/gpsfailure.cpp @@ -0,0 +1,170 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ +/** + * @file gpsfailure.cpp + * Helper class for gpsfailure mode according to the OBC rules + * + * @author Thomas Gubler + */ + +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include + +#include "navigator.h" +#include "gpsfailure.h" + +#define DELAY_SIGMA 0.01f + +GpsFailure::GpsFailure(Navigator *navigator, const char *name) : + MissionBlock(navigator, name), + _gpsf_state(GPSF_STATE_NONE) +{ + /* load initial params */ + updateParams(); + /* initial reset */ + on_inactive(); +} + +GpsFailure::~GpsFailure() +{ +} + +void +GpsFailure::on_inactive() +{ + /* reset GPSF state only if setpoint moved */ + if (!_navigator->get_can_loiter_at_sp()) { + _gpsf_state = GPSF_STATE_NONE; + } +} + +void +GpsFailure::on_activation() +{ + _gpsf_state = GPSF_STATE_NONE; + updateParams(); + advance_gpsf(); + set_gpsf_item(); +} + +void +GpsFailure::on_active() +{ + if (is_mission_item_reached()) { + updateParams(); + advance_gpsf(); + set_gpsf_item(); + } +} + +void +GpsFailure::set_gpsf_item() +{ + struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); + + set_previous_pos_setpoint(); + _navigator->set_can_loiter_at_sp(false); + + switch (_gpsf_state) { + case GPSF_STATE_LOITER: { + //_mission_item.lat = (double)(_param_commsholdlat.get()) * 1.0e-7; + //_mission_item.lon = (double)(_param_commsholdlon.get()) * 1.0e-7; + //_mission_item.altitude_is_relative = false; + //_mission_item.altitude = _param_commsholdalt.get(); + //_mission_item.yaw = NAN; + //_mission_item.loiter_radius = _navigator->get_loiter_radius(); + //_mission_item.loiter_direction = 1; + //_mission_item.nav_cmd = NAV_CMD_LOITER_TIME_LIMIT; + //_mission_item.acceptance_radius = _navigator->get_acceptance_radius(); + //_mission_item.time_inside = _param_commsholdwaittime.get() < 0.0f ? 0.0f : _param_commsholdwaittime.get(); + //_mission_item.pitch_min = 0.0f; + //_mission_item.autocontinue = true; + //_mission_item.origin = ORIGIN_ONBOARD; + + //_navigator->set_can_loiter_at_sp(true); + break; + } + case GPSF_STATE_TERMINATE: { + //_mission_item.lat = (double)(_param_airfieldhomelat.get()) * 1.0e-7; + //_mission_item.lon = (double)(_param_airfieldhomelon.get()) * 1.0e-7; + //_mission_item.altitude_is_relative = false; + //_mission_item.altitude = _param_airfieldhomealt.get(); + //_mission_item.yaw = NAN; + //_mission_item.loiter_radius = _navigator->get_loiter_radius(); + //_mission_item.loiter_direction = 1; + //_mission_item.nav_cmd = NAV_CMD_LOITER_UNLIMITED; + //_mission_item.acceptance_radius = _navigator->get_acceptance_radius(); + //_mission_item.pitch_min = 0.0f; + //_mission_item.autocontinue = true; + //_mission_item.origin = ORIGIN_ONBOARD; + + //_navigator->set_can_loiter_at_sp(true); + break; + } + default: + break; + } + + reset_mission_item_reached(); + + /* convert mission item to current position setpoint and make it valid */ + mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current); + pos_sp_triplet->next.valid = false; + + _navigator->set_position_setpoint_triplet_updated(); +} + +void +GpsFailure::advance_gpsf() +{ + switch (_gpsf_state) { + case GPSF_STATE_NONE: + _gpsf_state = GPSF_STATE_LOITER; + break; + case GPSF_STATE_LOITER: + _gpsf_state = GPSF_STATE_TERMINATE; + break; + default: + break; + } +} diff --git a/src/modules/navigator/gpsfailure.h b/src/modules/navigator/gpsfailure.h new file mode 100644 index 000000000..88d386ec4 --- /dev/null +++ b/src/modules/navigator/gpsfailure.h @@ -0,0 +1,86 @@ +/*************************************************************************** + * + * Copyright (c) 2013-2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ +/** + * @file gpsfailure.h + * Helper class for Data Link Loss Mode according to the OBC rules + * + * @author Thomas Gubler + */ + +#ifndef NAVIGATOR_GPSFAILURE_H +#define NAVIGATOR_GPSFAILURE_H + +#include +#include + +#include + +#include "navigator_mode.h" +#include "mission_block.h" + +class Navigator; + +class GpsFailure : public MissionBlock +{ +public: + GpsFailure(Navigator *navigator, const char *name); + + ~GpsFailure(); + + virtual void on_inactive(); + + virtual void on_activation(); + + virtual void on_active(); + +private: + /* Params */ + + enum GPSFState { + GPSF_STATE_NONE = 0, + GPSF_STATE_LOITER = 1, + GPSF_STATE_TERMINATE = 2, + } _gpsf_state; + + /** + * Set the GPSF item + */ + void set_gpsf_item(); + + /** + * Move to next GPSF item + */ + void advance_gpsf(); + +}; +#endif diff --git a/src/modules/navigator/module.mk b/src/modules/navigator/module.mk index 0539087df..f6590605b 100644 --- a/src/modules/navigator/module.mk +++ b/src/modules/navigator/module.mk @@ -52,7 +52,8 @@ SRCS = navigator_main.cpp \ geofence_params.c \ datalinkloss.cpp \ enginefailure.cpp \ - datalinkloss_params.c + datalinkloss_params.c \ + gpsfailure.cpp INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index 59a6752a9..b161c2984 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -61,12 +61,13 @@ #include "offboard.h" #include "datalinkloss.h" #include "enginefailure.h" +#include "gpsfailure.h" #include "geofence.h" /** * Number of navigation modes that need on_active/on_inactive calls */ -#define NAVIGATOR_MODE_ARRAY_SIZE 6 +#define NAVIGATOR_MODE_ARRAY_SIZE 7 class Navigator : public control::SuperBlock { @@ -184,6 +185,7 @@ private: DataLinkLoss _dataLinkLoss; /**< class that handles the OBC datalink loss mode */ EngineFailure _engineFailure; /**< class that handles the engine failure mode (FW only!) */ + GpsFailure _gpsFailure; /**< class that handles the OBC gpsfailure loss mode */ NavigatorMode *_navigation_mode_array[NAVIGATOR_MODE_ARRAY_SIZE]; /**< array of navigation modes */ diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index d77acf74e..d77825715 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -132,6 +132,7 @@ Navigator::Navigator() : _offboard(this, "OFF"), _dataLinkLoss(this, "DLL"), _engineFailure(this, "EF"), + _gpsFailure(this, "GPSF"), _can_loiter_at_sp(false), _pos_sp_triplet_updated(false), _param_loiter_radius(this, "LOITER_RAD"), @@ -145,6 +146,7 @@ Navigator::Navigator() : _navigation_mode_array[3] = &_offboard; _navigation_mode_array[4] = &_dataLinkLoss; _navigation_mode_array[5] = &_engineFailure; + _navigation_mode_array[6] = &_gpsFailure; updateParams(); } @@ -421,6 +423,9 @@ Navigator::task_main() case NAVIGATION_STATE_AUTO_LANDENGFAIL: _navigation_mode = &_engineFailure; break; + case NAVIGATION_STATE_AUTO_LANDGPSFAIL: + _navigation_mode = &_gpsFailure; + break; case NAVIGATION_STATE_LAND: case NAVIGATION_STATE_TERMINATION: case NAVIGATION_STATE_OFFBOARD: -- cgit v1.2.3 From 3cbfe989c793377cc5eac627c5f12573655b3ad2 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 20 Aug 2014 21:54:38 +0200 Subject: mavlink: missions manager compID cleanup, use the common compID, stict compID checks for incoming messages --- src/modules/mavlink/mavlink_mission.cpp | 18 +++++++++--------- src/modules/mavlink/mavlink_mission.h | 2 -- 2 files changed, 9 insertions(+), 11 deletions(-) diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp index 7a761588a..90a841787 100644 --- a/src/modules/mavlink/mavlink_mission.cpp +++ b/src/modules/mavlink/mavlink_mission.cpp @@ -58,6 +58,7 @@ #endif static const int ERROR = -1; +#define CHECK_SYSID_COMPID(_msg) (_msg.target_system == mavlink_system.sysid && _msg.target_component == mavlink_system.compid) MavlinkMissionManager::MavlinkMissionManager(Mavlink *mavlink) : MavlinkStream(mavlink), _state(MAVLINK_WPM_STATE_IDLE), @@ -79,8 +80,7 @@ MavlinkMissionManager::MavlinkMissionManager(Mavlink *mavlink) : MavlinkStream(m _mission_result_sub(-1), _offboard_mission_pub(-1), _slow_rate_limiter(_interval / 10.0f), - _verbose(false), - _comp_id(MAV_COMP_ID_MISSIONPLANNER) + _verbose(false) { _offboard_mission_sub = orb_subscribe(ORB_ID(offboard_mission)); _mission_result_sub = orb_subscribe(ORB_ID(mission_result)); @@ -384,7 +384,7 @@ MavlinkMissionManager::handle_mission_ack(const mavlink_message_t *msg) mavlink_mission_ack_t wpa; mavlink_msg_mission_ack_decode(msg, &wpa); - if (wpa.target_system == mavlink_system.sysid /*&& wpa.target_component == mavlink_wpm_comp_id*/) { + if (CHECK_SYSID_COMPID(wpa)) { if ((msg->sysid == _transfer_partner_sysid && msg->compid == _transfer_partner_compid)) { if (_state == MAVLINK_WPM_STATE_SENDLIST) { _time_last_recv = hrt_absolute_time(); @@ -416,7 +416,7 @@ MavlinkMissionManager::handle_mission_set_current(const mavlink_message_t *msg) mavlink_mission_set_current_t wpc; mavlink_msg_mission_set_current_decode(msg, &wpc); - if (wpc.target_system == mavlink_system.sysid /*&& wpc.target_component == mavlink_wpm_comp_id*/) { + if (CHECK_SYSID_COMPID(wpc)) { if (_state == MAVLINK_WPM_STATE_IDLE) { _time_last_recv = hrt_absolute_time(); @@ -451,7 +451,7 @@ MavlinkMissionManager::handle_mission_request_list(const mavlink_message_t *msg) mavlink_mission_request_list_t wprl; mavlink_msg_mission_request_list_decode(msg, &wprl); - if (wprl.target_system == mavlink_system.sysid /*&& wprl.target_component == mavlink_wpm_comp_id*/) { + if (CHECK_SYSID_COMPID(wprl)) { if (_state == MAVLINK_WPM_STATE_IDLE || _state == MAVLINK_WPM_STATE_SENDLIST) { _time_last_recv = hrt_absolute_time(); @@ -487,7 +487,7 @@ MavlinkMissionManager::handle_mission_request(const mavlink_message_t *msg) mavlink_mission_request_t wpr; mavlink_msg_mission_request_decode(msg, &wpr); - if (wpr.target_system == mavlink_system.sysid /*&& wpr.target_component == mavlink_wpm_comp_id*/) { + if (CHECK_SYSID_COMPID(wpr)) { if (msg->sysid == _transfer_partner_sysid && msg->compid == _transfer_partner_compid) { if (_state == MAVLINK_WPM_STATE_SENDLIST) { _time_last_recv = hrt_absolute_time(); @@ -558,7 +558,7 @@ MavlinkMissionManager::handle_mission_count(const mavlink_message_t *msg) mavlink_mission_count_t wpc; mavlink_msg_mission_count_decode(msg, &wpc); - if (wpc.target_system == mavlink_system.sysid/* && wpc.target_component == mavlink_wpm_comp_id*/) { + if (CHECK_SYSID_COMPID(wpc)) { if (_state == MAVLINK_WPM_STATE_IDLE) { _time_last_recv = hrt_absolute_time(); @@ -624,7 +624,7 @@ MavlinkMissionManager::handle_mission_item(const mavlink_message_t *msg) mavlink_mission_item_t wp; mavlink_msg_mission_item_decode(msg, &wp); - if (wp.target_system == mavlink_system.sysid && wp.target_component == _comp_id) { + if (CHECK_SYSID_COMPID(wp)) { if (_state == MAVLINK_WPM_STATE_GETLIST) { _time_last_recv = hrt_absolute_time(); @@ -710,7 +710,7 @@ MavlinkMissionManager::handle_mission_clear_all(const mavlink_message_t *msg) mavlink_mission_clear_all_t wpca; mavlink_msg_mission_clear_all_decode(msg, &wpca); - if (wpca.target_system == mavlink_system.sysid /*&& wpca.target_component == mavlink_wpm_comp_id */) { + if (CHECK_SYSID_COMPID(wpca)) { if (_state == MAVLINK_WPM_STATE_IDLE) { /* don't touch mission items storage itself, but only items count in mission state */ diff --git a/src/modules/mavlink/mavlink_mission.h b/src/modules/mavlink/mavlink_mission.h index 8ff27f87d..a7bb94c22 100644 --- a/src/modules/mavlink/mavlink_mission.h +++ b/src/modules/mavlink/mavlink_mission.h @@ -126,8 +126,6 @@ private: bool _verbose; - uint8_t _comp_id; - /* do not allow top copying this class */ MavlinkMissionManager(MavlinkMissionManager &); MavlinkMissionManager& operator = (const MavlinkMissionManager &); -- cgit v1.2.3 From f7dbc340275a79aafbdd14d10c1fb721a4c1e276 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 21 Aug 2014 12:43:34 +0200 Subject: sensors: remove warnx --- src/modules/sensors/sensors.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index aac297ef8..f40034d79 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -857,7 +857,6 @@ Sensors::parameters_update() errx(1, "FATAL: no barometer found"); } else { - warnx("qnh ioctl, %lu", (unsigned long)(_parameters.baro_qnh * 100)); int ret = ioctl(fd, BAROIOCSMSLPRESSURE, (unsigned long)(_parameters.baro_qnh * 100)); if (ret) { warnx("qnh could not be set"); -- cgit v1.2.3 From 9c75b9562e904210389a497c8baa46374cbc9927 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 21 Aug 2014 14:39:17 +0200 Subject: Be more permissive with mission component IDs, renamed camera to onboard link but still accepting same commandline syntax --- src/modules/mavlink/mavlink_main.cpp | 12 ++++++++---- src/modules/mavlink/mavlink_main.h | 2 +- src/modules/mavlink/mavlink_mission.cpp | 19 +++++++++++-------- 3 files changed, 20 insertions(+), 13 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index c27716f74..d702179f0 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1229,7 +1229,10 @@ Mavlink::task_main(int argc, char *argv[]) _mode = MAVLINK_MODE_CUSTOM; } else if (strcmp(optarg, "camera") == 0) { - _mode = MAVLINK_MODE_CAMERA; + // left in here for compatibility + _mode = MAVLINK_MODE_ONBOARD; + } else if (strcmp(optarg, "onboard") == 0) { + _mode = MAVLINK_MODE_ONBOARD; } break; @@ -1289,8 +1292,8 @@ Mavlink::task_main(int argc, char *argv[]) warnx("mode: CUSTOM"); break; - case MAVLINK_MODE_CAMERA: - warnx("mode: CAMERA"); + case MAVLINK_MODE_ONBOARD: + warnx("mode: ONBOARD"); break; default: @@ -1393,9 +1396,10 @@ Mavlink::task_main(int argc, char *argv[]) configure_stream("POSITION_TARGET_GLOBAL_INT", 3.0f); configure_stream("ATTITUDE_TARGET", 3.0f); configure_stream("DISTANCE_SENSOR", 0.5f); + configure_stream("OPTICAL_FLOW", 0.5f); break; - case MAVLINK_MODE_CAMERA: + case MAVLINK_MODE_ONBOARD: configure_stream("SYS_STATUS", 1.0f); configure_stream("ATTITUDE", 15.0f); configure_stream("GLOBAL_POSITION_INT", 15.0f); diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 1e2e94cef..7f9d225bb 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -127,7 +127,7 @@ public: enum MAVLINK_MODE { MAVLINK_MODE_NORMAL = 0, MAVLINK_MODE_CUSTOM, - MAVLINK_MODE_CAMERA + MAVLINK_MODE_ONBOARD }; void set_mode(enum MAVLINK_MODE); diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp index 90a841787..d436c95e9 100644 --- a/src/modules/mavlink/mavlink_mission.cpp +++ b/src/modules/mavlink/mavlink_mission.cpp @@ -58,7 +58,10 @@ #endif static const int ERROR = -1; -#define CHECK_SYSID_COMPID(_msg) (_msg.target_system == mavlink_system.sysid && _msg.target_component == mavlink_system.compid) +#define CHECK_SYSID_COMPID_MISSION(_msg) (_msg.target_system == mavlink_system.sysid && \ + ((_msg.target_component == mavlink_system.compid) || \ + (_msg.target_component == MAV_COMP_ID_MISSIONPLANNER) || \ + (_msg.target_component == MAV_COMP_ID_ALL))) MavlinkMissionManager::MavlinkMissionManager(Mavlink *mavlink) : MavlinkStream(mavlink), _state(MAVLINK_WPM_STATE_IDLE), @@ -384,7 +387,7 @@ MavlinkMissionManager::handle_mission_ack(const mavlink_message_t *msg) mavlink_mission_ack_t wpa; mavlink_msg_mission_ack_decode(msg, &wpa); - if (CHECK_SYSID_COMPID(wpa)) { + if (CHECK_SYSID_COMPID_MISSION(wpa)) { if ((msg->sysid == _transfer_partner_sysid && msg->compid == _transfer_partner_compid)) { if (_state == MAVLINK_WPM_STATE_SENDLIST) { _time_last_recv = hrt_absolute_time(); @@ -416,7 +419,7 @@ MavlinkMissionManager::handle_mission_set_current(const mavlink_message_t *msg) mavlink_mission_set_current_t wpc; mavlink_msg_mission_set_current_decode(msg, &wpc); - if (CHECK_SYSID_COMPID(wpc)) { + if (CHECK_SYSID_COMPID_MISSION(wpc)) { if (_state == MAVLINK_WPM_STATE_IDLE) { _time_last_recv = hrt_absolute_time(); @@ -451,7 +454,7 @@ MavlinkMissionManager::handle_mission_request_list(const mavlink_message_t *msg) mavlink_mission_request_list_t wprl; mavlink_msg_mission_request_list_decode(msg, &wprl); - if (CHECK_SYSID_COMPID(wprl)) { + if (CHECK_SYSID_COMPID_MISSION(wprl)) { if (_state == MAVLINK_WPM_STATE_IDLE || _state == MAVLINK_WPM_STATE_SENDLIST) { _time_last_recv = hrt_absolute_time(); @@ -487,7 +490,7 @@ MavlinkMissionManager::handle_mission_request(const mavlink_message_t *msg) mavlink_mission_request_t wpr; mavlink_msg_mission_request_decode(msg, &wpr); - if (CHECK_SYSID_COMPID(wpr)) { + if (CHECK_SYSID_COMPID_MISSION(wpr)) { if (msg->sysid == _transfer_partner_sysid && msg->compid == _transfer_partner_compid) { if (_state == MAVLINK_WPM_STATE_SENDLIST) { _time_last_recv = hrt_absolute_time(); @@ -558,7 +561,7 @@ MavlinkMissionManager::handle_mission_count(const mavlink_message_t *msg) mavlink_mission_count_t wpc; mavlink_msg_mission_count_decode(msg, &wpc); - if (CHECK_SYSID_COMPID(wpc)) { + if (CHECK_SYSID_COMPID_MISSION(wpc)) { if (_state == MAVLINK_WPM_STATE_IDLE) { _time_last_recv = hrt_absolute_time(); @@ -624,7 +627,7 @@ MavlinkMissionManager::handle_mission_item(const mavlink_message_t *msg) mavlink_mission_item_t wp; mavlink_msg_mission_item_decode(msg, &wp); - if (CHECK_SYSID_COMPID(wp)) { + if (CHECK_SYSID_COMPID_MISSION(wp)) { if (_state == MAVLINK_WPM_STATE_GETLIST) { _time_last_recv = hrt_absolute_time(); @@ -710,7 +713,7 @@ MavlinkMissionManager::handle_mission_clear_all(const mavlink_message_t *msg) mavlink_mission_clear_all_t wpca; mavlink_msg_mission_clear_all_decode(msg, &wpca); - if (CHECK_SYSID_COMPID(wpca)) { + if (CHECK_SYSID_COMPID_MISSION(wpca)) { if (_state == MAVLINK_WPM_STATE_IDLE) { /* don't touch mission items storage itself, but only items count in mission state */ -- cgit v1.2.3 From 2a99ff39dcdb78d97fd645f2c47bf6d36798a1dc Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 21 Aug 2014 15:14:55 +0200 Subject: Robustify bottle drop --- src/modules/bottle_drop/bottle_drop.cpp | 18 +++++++++++------- 1 file changed, 11 insertions(+), 7 deletions(-) diff --git a/src/modules/bottle_drop/bottle_drop.cpp b/src/modules/bottle_drop/bottle_drop.cpp index 77f691f6c..35b2266e4 100644 --- a/src/modules/bottle_drop/bottle_drop.cpp +++ b/src/modules/bottle_drop/bottle_drop.cpp @@ -459,7 +459,7 @@ BottleDrop::task_main() continue; } - const unsigned sleeptime_us = 10000; + const unsigned sleeptime_us = 50000; hrt_abstime last_run = hrt_absolute_time(); dt_runs = 1e6f / sleeptime_us; @@ -648,6 +648,7 @@ BottleDrop::task_main() if (isfinite(distance_real) && distance_real < distance_open_door) { open_bay(); _drop_state = DROP_STATE_BAY_OPEN; + mavlink_log_info(_mavlink_fd, "#audio: opening bay"); } } break; @@ -664,10 +665,11 @@ BottleDrop::task_main() warnx("Distance real: %.2f", (double)distance_real); if (isfinite(distance_real) && - (((distance_real < precision) && (distance_real < future_distance)) || + (distance_real < precision) && ((distance_real < future_distance) || (distance_real < precision / 10.0f))) { drop(); _drop_state = DROP_STATE_DROPPED; + mavlink_log_info(_mavlink_fd, "#audio: payload dropped"); } } } @@ -680,6 +682,7 @@ BottleDrop::task_main() _drop_approval = false; lock_release(); close_bay(); + mavlink_log_info(_mavlink_fd, "#audio: closing bay"); } break; } @@ -688,7 +691,7 @@ BottleDrop::task_main() // update_actuators(); - // run at roughly 100 Hz + // run at roughly 20 Hz usleep(sleeptime_us); dt_runs = 1e6f / hrt_elapsed_time(&last_run); @@ -719,12 +722,12 @@ BottleDrop::handle_command(struct vehicle_command_s *cmd) } else if (cmd->param1 > 0.5f) { open_bay(); - mavlink_log_info(_mavlink_fd, "#audio: open doors"); + mavlink_log_info(_mavlink_fd, "#audio: opening bay"); } else { lock_release(); close_bay(); - mavlink_log_info(_mavlink_fd, "#audio: close doors"); + mavlink_log_info(_mavlink_fd, "#audio: closing bay"); } answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); @@ -735,11 +738,12 @@ BottleDrop::handle_command(struct vehicle_command_s *cmd) switch ((int)(cmd->param1 + 0.5f)) { case 0: _drop_approval = false; + mavlink_log_info(_mavlink_fd, "#audio: got drop position, no approval"); break; case 1: _drop_approval = true; - mavlink_log_info(_mavlink_fd, "#audio: prepare deploy approval"); + mavlink_log_info(_mavlink_fd, "#audio: got drop position and approval"); break; default: @@ -768,7 +772,7 @@ BottleDrop::handle_command(struct vehicle_command_s *cmd) case 1: _drop_approval = true; - mavlink_log_info(_mavlink_fd, "#audio: control deploy approval"); + mavlink_log_info(_mavlink_fd, "#audio: got drop approval"); break; default: -- cgit v1.2.3 From 0bdf28f9ba6f0875bfb70ff961fc61e73b813edc Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 21 Aug 2014 15:26:04 +0200 Subject: audio feedback on distance --- src/modules/bottle_drop/bottle_drop.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/modules/bottle_drop/bottle_drop.cpp b/src/modules/bottle_drop/bottle_drop.cpp index 35b2266e4..a899cbe60 100644 --- a/src/modules/bottle_drop/bottle_drop.cpp +++ b/src/modules/bottle_drop/bottle_drop.cpp @@ -636,6 +636,10 @@ BottleDrop::task_main() onboard_mission_pub = orb_advertise(ORB_ID(onboard_mission), &onboard_mission); } + if (counter % 90 == 0) { + mavlink_log_info(_mavlink_fd, "#audio: drop distance %.2f", (double)distance_real); + } + // We're close enough - open the bay distance_open_door = math::max(3.0f, fabsf(t_door * groundspeed_body)); -- cgit v1.2.3 From 6051b2112efb584e5f84fb0648d4dddf5ac6c918 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 21 Aug 2014 15:31:06 +0200 Subject: Moved pos output --- src/modules/bottle_drop/bottle_drop.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/modules/bottle_drop/bottle_drop.cpp b/src/modules/bottle_drop/bottle_drop.cpp index a899cbe60..6a501d97c 100644 --- a/src/modules/bottle_drop/bottle_drop.cpp +++ b/src/modules/bottle_drop/bottle_drop.cpp @@ -497,6 +497,10 @@ BottleDrop::task_main() float groundspeed_body = sqrtf(_global_pos.vel_n * _global_pos.vel_n + _global_pos.vel_e * _global_pos.vel_e); distance_real = fabsf(get_distance_to_next_waypoint(_global_pos.lat, _global_pos.lon, _drop_position.lat, _drop_position.lon)); + if (counter % 90 == 0) { + mavlink_log_info(_mavlink_fd, "#audio: drop distance %.2f", (double)distance_real); + } + //warnx("absolut wind speed = %.4f", vr); //////////////////////////////////////////////////////////////////// DEBUGGING @@ -636,10 +640,6 @@ BottleDrop::task_main() onboard_mission_pub = orb_advertise(ORB_ID(onboard_mission), &onboard_mission); } - if (counter % 90 == 0) { - mavlink_log_info(_mavlink_fd, "#audio: drop distance %.2f", (double)distance_real); - } - // We're close enough - open the bay distance_open_door = math::max(3.0f, fabsf(t_door * groundspeed_body)); -- cgit v1.2.3 From 1026805422495e9b6d5351a38f2a2ca3ce70a352 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 21 Aug 2014 15:41:03 +0200 Subject: removed unnecessary switch statement --- src/modules/bottle_drop/bottle_drop.cpp | 4 ---- 1 file changed, 4 deletions(-) diff --git a/src/modules/bottle_drop/bottle_drop.cpp b/src/modules/bottle_drop/bottle_drop.cpp index 6a501d97c..dbf94cb3d 100644 --- a/src/modules/bottle_drop/bottle_drop.cpp +++ b/src/modules/bottle_drop/bottle_drop.cpp @@ -508,10 +508,6 @@ BottleDrop::task_main() switch (_drop_state) { - case DROP_STATE_INIT: - // exit inner loop, wait for new drop mission - break; - case DROP_STATE_TARGET_VALID: { //warnx("approval given\n"); //////////////////////////////////////////////////////////////////// DEBUGGING -- cgit v1.2.3 From 7c7dce3b440a7b6deb8f0f9340c4ba6b21daa800 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 21 Aug 2014 15:46:58 +0200 Subject: Open doors sooner for drop --- src/modules/bottle_drop/bottle_drop.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/bottle_drop/bottle_drop.cpp b/src/modules/bottle_drop/bottle_drop.cpp index dbf94cb3d..bc182b501 100644 --- a/src/modules/bottle_drop/bottle_drop.cpp +++ b/src/modules/bottle_drop/bottle_drop.cpp @@ -637,7 +637,7 @@ BottleDrop::task_main() } // We're close enough - open the bay - distance_open_door = math::max(3.0f, fabsf(t_door * groundspeed_body)); + distance_open_door = math::max(3.0f, 3.0f * fabsf(t_door * groundspeed_body)); if (counter % 10 == 0) warnx("dist real: %.2f, distance_open_door: %.2f, angle to wind: %.2f", -- cgit v1.2.3 From 8e7722f8948d3f8f21a5a661e9cdcc6097472fd9 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 21 Aug 2014 15:47:37 +0200 Subject: Open bay at a minimum distance of 5 meters, irrelevant of the velocity --- src/modules/bottle_drop/bottle_drop.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/bottle_drop/bottle_drop.cpp b/src/modules/bottle_drop/bottle_drop.cpp index bc182b501..5d2343cfb 100644 --- a/src/modules/bottle_drop/bottle_drop.cpp +++ b/src/modules/bottle_drop/bottle_drop.cpp @@ -637,7 +637,7 @@ BottleDrop::task_main() } // We're close enough - open the bay - distance_open_door = math::max(3.0f, 3.0f * fabsf(t_door * groundspeed_body)); + distance_open_door = math::max(5.0f, 3.0f * fabsf(t_door * groundspeed_body)); if (counter % 10 == 0) warnx("dist real: %.2f, distance_open_door: %.2f, angle to wind: %.2f", -- cgit v1.2.3 From 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(-) 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(-) 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 f87860bc96a61c538de50b5b74aba0452f9b5784 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Thu, 21 Aug 2014 23:23:35 +0400 Subject: UAVCAN update --- uavcan | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/uavcan b/uavcan index 6980ee824..446577e4f 160000 --- a/uavcan +++ b/uavcan @@ -1 +1 @@ -Subproject commit 6980ee824074aa2f7a62221bf6040ee411119520 +Subproject commit 446577e4fb979ee4c629081368233eaa5683d086 -- cgit v1.2.3 From e6a8eebd0a86c0dd7b8b23882772ccd3ba962ddc Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 21 Aug 2014 21:43:53 +0200 Subject: Fixed approach / navigation logic for bottle drop --- src/modules/bottle_drop/bottle_drop.cpp | 93 +++++++++++++++++++++------------ 1 file changed, 60 insertions(+), 33 deletions(-) diff --git a/src/modules/bottle_drop/bottle_drop.cpp b/src/modules/bottle_drop/bottle_drop.cpp index 30c9ce887..ae97cc570 100644 --- a/src/modules/bottle_drop/bottle_drop.cpp +++ b/src/modules/bottle_drop/bottle_drop.cpp @@ -134,6 +134,7 @@ private: enum DROP_STATE { DROP_STATE_INIT = 0, DROP_STATE_TARGET_VALID, + DROP_STATE_TARGET_SET, DROP_STATE_BAY_OPEN, DROP_STATE_DROPPED, DROP_STATE_BAY_CLOSED @@ -381,10 +382,6 @@ BottleDrop::task_main() float distance_real = 0; // The distance between the UAVs position and the drop point [m] float future_distance = 0; // The distance between the UAVs to-be position and the drop point [m] - // states - bool state_drop = false; // Drop occurred = true, Drop din't occur = false - bool state_run = false; // A drop was attempted = true, the drop is still in progress = false - unsigned counter = 0; param_t param_gproperties = param_find("BD_GPROPERTIES"); @@ -621,13 +618,7 @@ BottleDrop::task_main() } onboard_mission.count = 2; - - if (state_run && !state_drop) { - onboard_mission.current_seq = 0; - - } else { - onboard_mission.current_seq = -1; - } + onboard_mission.current_seq = 0; if (onboard_mission_pub > 0) { orb_publish(ORB_ID(onboard_mission), onboard_mission_pub, &onboard_mission); @@ -636,19 +627,27 @@ BottleDrop::task_main() onboard_mission_pub = orb_advertise(ORB_ID(onboard_mission), &onboard_mission); } - // We're close enough - open the bay - distance_open_door = math::max(5.0f, 3.0f * fabsf(t_door * groundspeed_body)); + _drop_state = DROP_STATE_TARGET_SET; + } + break; + + case DROP_STATE_TARGET_SET - if (counter % 10 == 0) - warnx("dist real: %.2f, distance_open_door: %.2f, angle to wind: %.2f", - (double)distance_real, - (double)distance_open_door, - (double)(_wrap_pi(_global_pos.yaw - atan2f(wind.windspeed_north, wind.windspeed_east)))); + float distance_wp2 = get_distance_to_next_waypoint(flight_vector_e.lat, flight_vector_e.lon, _drop_position.lat, _drop_position.lon); - if (isfinite(distance_real) && distance_real < distance_open_door) { - open_bay(); - _drop_state = DROP_STATE_BAY_OPEN; - mavlink_log_info(_mavlink_fd, "#audio: opening bay"); + if (distance_wp2 < distance_real) { + onboard_mission.current_seq = 0; + orb_publish(ORB_ID(onboard_mission), onboard_mission_pub, &onboard_mission); + } else { + + // We're close enough - open the bay + distance_open_door = math::max(5.0f, 3.0f * fabsf(t_door * groundspeed_body)); + + if (isfinite(distance_real) && distance_real < distance_open_door) { + open_bay(); + _drop_state = DROP_STATE_BAY_OPEN; + mavlink_log_info(_mavlink_fd, "#audio: opening bay"); + } } } break; @@ -670,6 +669,14 @@ BottleDrop::task_main() drop(); _drop_state = DROP_STATE_DROPPED; mavlink_log_info(_mavlink_fd, "#audio: payload dropped"); + } else { + + float distance_wp2 = get_distance_to_next_waypoint(flight_vector_e.lat, flight_vector_e.lon, _drop_position.lat, _drop_position.lon); + + if (distance_wp2 < distance_real) { + onboard_mission.current_seq = 0; + orb_publish(ORB_ID(onboard_mission), onboard_mission_pub, &onboard_mission); + } } } } @@ -683,6 +690,10 @@ BottleDrop::task_main() lock_release(); close_bay(); mavlink_log_info(_mavlink_fd, "#audio: closing bay"); + + // remove onboard mission + onboard_mission.current_seq = -1; + orb_publish(ORB_ID(onboard_mission), onboard_mission_pub, &onboard_mission); } break; } @@ -765,20 +776,36 @@ BottleDrop::handle_command(struct vehicle_command_s *cmd) break; case VEHICLE_CMD_PAYLOAD_CONTROL_DEPLOY: - switch ((int)(cmd->param1 + 0.5f)) { - case 0: - _drop_approval = false; - break; - case 1: - _drop_approval = true; - mavlink_log_info(_mavlink_fd, "#audio: got drop approval"); - break; + if (cmd->param1 < 0) { - default: + // Clear internal states _drop_approval = false; - break; - // XXX handle other values + _drop_state = DROP_STATE_INIT; + + // Abort if mission is present + onboard_mission.current_seq = -1; + + if (onboard_mission_pub > 0) { + orb_publish(ORB_ID(onboard_mission), onboard_mission_pub, &onboard_mission); + } + + } else { + switch ((int)(cmd->param1 + 0.5f)) { + case 0: + _drop_approval = false; + break; + + case 1: + _drop_approval = true; + mavlink_log_info(_mavlink_fd, "#audio: got drop approval"); + break; + + default: + _drop_approval = false; + break; + // XXX handle other values + } } answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); -- cgit v1.2.3 From 0d9f6b6e2ec86f7fa95bc2e7650256e1392544b2 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Fri, 22 Aug 2014 01:52:23 +0400 Subject: UAVCAN: Refactored and generalized sensor bridge support --- src/modules/uavcan/actuators/esc.cpp | 138 ++++++++++++++++++++++++ src/modules/uavcan/actuators/esc.hpp | 107 ++++++++++++++++++ src/modules/uavcan/esc_controller.cpp | 138 ------------------------ src/modules/uavcan/esc_controller.hpp | 107 ------------------ src/modules/uavcan/gnss_receiver.cpp | 153 -------------------------- src/modules/uavcan/gnss_receiver.hpp | 84 --------------- src/modules/uavcan/module.mk | 14 ++- src/modules/uavcan/sensors/gnss.cpp | 156 +++++++++++++++++++++++++++ src/modules/uavcan/sensors/gnss.hpp | 88 +++++++++++++++ src/modules/uavcan/sensors/sensor_bridge.cpp | 48 +++++++++ src/modules/uavcan/sensors/sensor_bridge.hpp | 70 ++++++++++++ src/modules/uavcan/uavcan_main.cpp | 121 +++++++++++++++++---- src/modules/uavcan/uavcan_main.hpp | 21 ++-- 13 files changed, 731 insertions(+), 514 deletions(-) create mode 100644 src/modules/uavcan/actuators/esc.cpp create mode 100644 src/modules/uavcan/actuators/esc.hpp delete mode 100644 src/modules/uavcan/esc_controller.cpp delete mode 100644 src/modules/uavcan/esc_controller.hpp delete mode 100644 src/modules/uavcan/gnss_receiver.cpp delete mode 100644 src/modules/uavcan/gnss_receiver.hpp create mode 100644 src/modules/uavcan/sensors/gnss.cpp create mode 100644 src/modules/uavcan/sensors/gnss.hpp create mode 100644 src/modules/uavcan/sensors/sensor_bridge.cpp create mode 100644 src/modules/uavcan/sensors/sensor_bridge.hpp diff --git a/src/modules/uavcan/actuators/esc.cpp b/src/modules/uavcan/actuators/esc.cpp new file mode 100644 index 000000000..223d94731 --- /dev/null +++ b/src/modules/uavcan/actuators/esc.cpp @@ -0,0 +1,138 @@ +/**************************************************************************** + * + * 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 esc.cpp + * + * @author Pavel Kirienko + */ + +#include "esc.hpp" +#include + +UavcanEscController::UavcanEscController(uavcan::INode &node) : + _node(node), + _uavcan_pub_raw_cmd(node), + _uavcan_sub_status(node), + _orb_timer(node) +{ +} + +UavcanEscController::~UavcanEscController() +{ + perf_free(_perfcnt_invalid_input); + perf_free(_perfcnt_scaling_error); +} + +int UavcanEscController::init() +{ + // ESC status subscription + int res = _uavcan_sub_status.start(StatusCbBinder(this, &UavcanEscController::esc_status_sub_cb)); + if (res < 0) + { + warnx("ESC status sub failed %i", res); + return res; + } + + // ESC status will be relayed from UAVCAN bus into ORB at this rate + _orb_timer.setCallback(TimerCbBinder(this, &UavcanEscController::orb_pub_timer_cb)); + _orb_timer.startPeriodic(uavcan::MonotonicDuration::fromMSec(1000 / ESC_STATUS_UPDATE_RATE_HZ)); + + return res; +} + +void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs) +{ + if ((outputs == nullptr) || (num_outputs > MAX_ESCS)) { + perf_count(_perfcnt_invalid_input); + return; + } + + /* + * Rate limiting - we don't want to congest the bus + */ + const auto timestamp = _node.getMonotonicTime(); + if ((timestamp - _prev_cmd_pub).toUSec() < (1000000 / MAX_RATE_HZ)) { + return; + } + _prev_cmd_pub = timestamp; + + /* + * Fill the command message + * If unarmed, we publish an empty message anyway + */ + uavcan::equipment::esc::RawCommand msg; + + if (_armed) { + for (unsigned i = 0; i < num_outputs; i++) { + + float scaled = (outputs[i] + 1.0F) * 0.5F * uavcan::equipment::esc::RawCommand::CMD_MAX; + if (scaled < 1.0F) { + scaled = 1.0F; // Since we're armed, we don't want to stop it completely + } + + if (scaled < uavcan::equipment::esc::RawCommand::CMD_MIN) { + scaled = uavcan::equipment::esc::RawCommand::CMD_MIN; + perf_count(_perfcnt_scaling_error); + } else if (scaled > uavcan::equipment::esc::RawCommand::CMD_MAX) { + scaled = uavcan::equipment::esc::RawCommand::CMD_MAX; + perf_count(_perfcnt_scaling_error); + } else { + ; // Correct value + } + + msg.cmd.push_back(static_cast(scaled)); + } + } + + /* + * Publish the command message to the bus + * Note that for a quadrotor it takes one CAN frame + */ + (void)_uavcan_pub_raw_cmd.broadcast(msg); +} + +void UavcanEscController::arm_esc(bool arm) +{ + _armed = arm; +} + +void UavcanEscController::esc_status_sub_cb(const uavcan::ReceivedDataStructure &msg) +{ + // TODO save status into a local storage; publish to ORB later from orb_pub_timer_cb() +} + +void UavcanEscController::orb_pub_timer_cb(const uavcan::TimerEvent&) +{ + // TODO publish to ORB +} diff --git a/src/modules/uavcan/actuators/esc.hpp b/src/modules/uavcan/actuators/esc.hpp new file mode 100644 index 000000000..cf0988210 --- /dev/null +++ b/src/modules/uavcan/actuators/esc.hpp @@ -0,0 +1,107 @@ +/**************************************************************************** + * + * 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 esc.hpp + * + * UAVCAN <--> ORB bridge for ESC messages: + * uavcan.equipment.esc.RawCommand + * uavcan.equipment.esc.RPMCommand + * uavcan.equipment.esc.Status + * + * @author Pavel Kirienko + */ + +#pragma once + +#include +#include +#include +#include + +class UavcanEscController +{ +public: + UavcanEscController(uavcan::INode& node); + ~UavcanEscController(); + + int init(); + + void update_outputs(float *outputs, unsigned num_outputs); + + void arm_esc(bool arm); + +private: + /** + * ESC status message reception will be reported via this callback. + */ + void esc_status_sub_cb(const uavcan::ReceivedDataStructure &msg); + + /** + * ESC status will be published to ORB from this callback (fixed rate). + */ + void orb_pub_timer_cb(const uavcan::TimerEvent &event); + + + static constexpr unsigned MAX_RATE_HZ = 100; ///< XXX make this configurable + static constexpr unsigned ESC_STATUS_UPDATE_RATE_HZ = 5; + static constexpr unsigned MAX_ESCS = uavcan::equipment::esc::RawCommand::FieldTypes::cmd::MaxSize; + + typedef uavcan::MethodBinder&)> + StatusCbBinder; + + typedef uavcan::MethodBinder + TimerCbBinder; + + /* + * libuavcan related things + */ + uavcan::MonotonicTime _prev_cmd_pub; ///< rate limiting + uavcan::INode &_node; + uavcan::Publisher _uavcan_pub_raw_cmd; + uavcan::Subscriber _uavcan_sub_status; + uavcan::TimerEventForwarder _orb_timer; + + /* + * ESC states + */ + bool _armed = false; + uavcan::equipment::esc::Status _states[MAX_ESCS]; + + /* + * Perf counters + */ + perf_counter_t _perfcnt_invalid_input = perf_alloc(PC_COUNT, "uavcan_esc_invalid_input"); + perf_counter_t _perfcnt_scaling_error = perf_alloc(PC_COUNT, "uavcan_esc_scaling_error"); +}; diff --git a/src/modules/uavcan/esc_controller.cpp b/src/modules/uavcan/esc_controller.cpp deleted file mode 100644 index 406eba88c..000000000 --- a/src/modules/uavcan/esc_controller.cpp +++ /dev/null @@ -1,138 +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 esc_controller.cpp - * - * @author Pavel Kirienko - */ - -#include "esc_controller.hpp" -#include - -UavcanEscController::UavcanEscController(uavcan::INode &node) : - _node(node), - _uavcan_pub_raw_cmd(node), - _uavcan_sub_status(node), - _orb_timer(node) -{ -} - -UavcanEscController::~UavcanEscController() -{ - perf_free(_perfcnt_invalid_input); - perf_free(_perfcnt_scaling_error); -} - -int UavcanEscController::init() -{ - // ESC status subscription - int res = _uavcan_sub_status.start(StatusCbBinder(this, &UavcanEscController::esc_status_sub_cb)); - if (res < 0) - { - warnx("ESC status sub failed %i", res); - return res; - } - - // ESC status will be relayed from UAVCAN bus into ORB at this rate - _orb_timer.setCallback(TimerCbBinder(this, &UavcanEscController::orb_pub_timer_cb)); - _orb_timer.startPeriodic(uavcan::MonotonicDuration::fromMSec(1000 / ESC_STATUS_UPDATE_RATE_HZ)); - - return res; -} - -void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs) -{ - if ((outputs == nullptr) || (num_outputs > MAX_ESCS)) { - perf_count(_perfcnt_invalid_input); - return; - } - - /* - * Rate limiting - we don't want to congest the bus - */ - const auto timestamp = _node.getMonotonicTime(); - if ((timestamp - _prev_cmd_pub).toUSec() < (1000000 / MAX_RATE_HZ)) { - return; - } - _prev_cmd_pub = timestamp; - - /* - * Fill the command message - * If unarmed, we publish an empty message anyway - */ - uavcan::equipment::esc::RawCommand msg; - - if (_armed) { - for (unsigned i = 0; i < num_outputs; i++) { - - float scaled = (outputs[i] + 1.0F) * 0.5F * uavcan::equipment::esc::RawCommand::CMD_MAX; - if (scaled < 1.0F) { - scaled = 1.0F; // Since we're armed, we don't want to stop it completely - } - - if (scaled < uavcan::equipment::esc::RawCommand::CMD_MIN) { - scaled = uavcan::equipment::esc::RawCommand::CMD_MIN; - perf_count(_perfcnt_scaling_error); - } else if (scaled > uavcan::equipment::esc::RawCommand::CMD_MAX) { - scaled = uavcan::equipment::esc::RawCommand::CMD_MAX; - perf_count(_perfcnt_scaling_error); - } else { - ; // Correct value - } - - msg.cmd.push_back(static_cast(scaled)); - } - } - - /* - * Publish the command message to the bus - * Note that for a quadrotor it takes one CAN frame - */ - (void)_uavcan_pub_raw_cmd.broadcast(msg); -} - -void UavcanEscController::arm_esc(bool arm) -{ - _armed = arm; -} - -void UavcanEscController::esc_status_sub_cb(const uavcan::ReceivedDataStructure &msg) -{ - // TODO save status into a local storage; publish to ORB later from orb_pub_timer_cb() -} - -void UavcanEscController::orb_pub_timer_cb(const uavcan::TimerEvent&) -{ - // TODO publish to ORB -} diff --git a/src/modules/uavcan/esc_controller.hpp b/src/modules/uavcan/esc_controller.hpp deleted file mode 100644 index 559ede561..000000000 --- a/src/modules/uavcan/esc_controller.hpp +++ /dev/null @@ -1,107 +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 esc_controller.hpp - * - * UAVCAN <--> ORB bridge for ESC messages: - * uavcan.equipment.esc.RawCommand - * uavcan.equipment.esc.RPMCommand - * uavcan.equipment.esc.Status - * - * @author Pavel Kirienko - */ - -#pragma once - -#include -#include -#include -#include - -class UavcanEscController -{ -public: - UavcanEscController(uavcan::INode& node); - ~UavcanEscController(); - - int init(); - - void update_outputs(float *outputs, unsigned num_outputs); - - void arm_esc(bool arm); - -private: - /** - * ESC status message reception will be reported via this callback. - */ - void esc_status_sub_cb(const uavcan::ReceivedDataStructure &msg); - - /** - * ESC status will be published to ORB from this callback (fixed rate). - */ - void orb_pub_timer_cb(const uavcan::TimerEvent &event); - - - static constexpr unsigned MAX_RATE_HZ = 100; ///< XXX make this configurable - static constexpr unsigned ESC_STATUS_UPDATE_RATE_HZ = 5; - static constexpr unsigned MAX_ESCS = uavcan::equipment::esc::RawCommand::FieldTypes::cmd::MaxSize; - - typedef uavcan::MethodBinder&)> - StatusCbBinder; - - typedef uavcan::MethodBinder - TimerCbBinder; - - /* - * libuavcan related things - */ - uavcan::MonotonicTime _prev_cmd_pub; ///< rate limiting - uavcan::INode &_node; - uavcan::Publisher _uavcan_pub_raw_cmd; - uavcan::Subscriber _uavcan_sub_status; - uavcan::TimerEventForwarder _orb_timer; - - /* - * ESC states - */ - bool _armed = false; - uavcan::equipment::esc::Status _states[MAX_ESCS]; - - /* - * Perf counters - */ - perf_counter_t _perfcnt_invalid_input = perf_alloc(PC_COUNT, "uavcan_esc_invalid_input"); - perf_counter_t _perfcnt_scaling_error = perf_alloc(PC_COUNT, "uavcan_esc_scaling_error"); -}; diff --git a/src/modules/uavcan/gnss_receiver.cpp b/src/modules/uavcan/gnss_receiver.cpp deleted file mode 100644 index ba1fe5e49..000000000 --- a/src/modules/uavcan/gnss_receiver.cpp +++ /dev/null @@ -1,153 +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 gnss_receiver.cpp - * - * @author Pavel Kirienko - * @author Andrew Chambers - * - */ - -#include "gnss_receiver.hpp" -#include -#include - -#define MM_PER_CM 10 // Millimeters per centimeter - -UavcanGnssReceiver::UavcanGnssReceiver(uavcan::INode &node) : -_node(node), -_uavcan_sub_status(node), -_report_pub(-1) -{ -} - -int UavcanGnssReceiver::init() -{ - int res = -1; - - // GNSS fix subscription - res = _uavcan_sub_status.start(FixCbBinder(this, &UavcanGnssReceiver::gnss_fix_sub_cb)); - if (res < 0) - { - warnx("GNSS fix sub failed %i", res); - return res; - } - - // Clear the uORB GPS report - memset(&_report, 0, sizeof(_report)); - - return res; -} - -void UavcanGnssReceiver::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure &msg) -{ - _report.timestamp_position = hrt_absolute_time(); - _report.lat = msg.lat_1e7; - _report.lon = msg.lon_1e7; - _report.alt = msg.alt_1e2 * MM_PER_CM; // Convert from centimeter (1e2) to millimeters (1e3) - - _report.timestamp_variance = _report.timestamp_position; - - - // Check if the msg contains valid covariance information - const bool valid_position_covariance = !msg.position_covariance.empty(); - const bool valid_velocity_covariance = !msg.velocity_covariance.empty(); - - if (valid_position_covariance) { - float pos_cov[9]; - msg.position_covariance.unpackSquareMatrix(pos_cov); - - // Horizontal position uncertainty - const float horizontal_pos_variance = math::max(pos_cov[0], pos_cov[4]); - _report.eph = (horizontal_pos_variance > 0) ? sqrtf(horizontal_pos_variance) : -1.0F; - - // Vertical position uncertainty - _report.epv = (pos_cov[8] > 0) ? sqrtf(pos_cov[8]) : -1.0F; - } else { - _report.eph = -1.0F; - _report.epv = -1.0F; - } - - if (valid_velocity_covariance) { - float vel_cov[9]; - msg.velocity_covariance.unpackSquareMatrix(vel_cov); - _report.s_variance_m_s = math::max(math::max(vel_cov[0], vel_cov[4]), vel_cov[8]); - - /* There is a nonlinear relationship between the velocity vector and the heading. - * Use Jacobian to transform velocity covariance to heading covariance - * - * Nonlinear equation: - * heading = atan2(vel_e_m_s, vel_n_m_s) - * For math, see http://en.wikipedia.org/wiki/Atan2#Derivative - * - * To calculate the variance of heading from the variance of velocity, - * cov(heading) = J(velocity)*cov(velocity)*J(velocity)^T - */ - float vel_n = msg.ned_velocity[0]; - float vel_e = msg.ned_velocity[1]; - float vel_n_sq = vel_n * vel_n; - float vel_e_sq = vel_e * vel_e; - _report.c_variance_rad = - (vel_e_sq * vel_cov[0] + - -2 * vel_n * vel_e * vel_cov[1] + // Covariance matrix is symmetric - vel_n_sq* vel_cov[4]) / ((vel_n_sq + vel_e_sq) * (vel_n_sq + vel_e_sq)); - - } else { - _report.s_variance_m_s = -1.0F; - _report.c_variance_rad = -1.0F; - } - - _report.fix_type = msg.status; - - _report.timestamp_velocity = _report.timestamp_position; - _report.vel_n_m_s = msg.ned_velocity[0]; - _report.vel_e_m_s = msg.ned_velocity[1]; - _report.vel_d_m_s = msg.ned_velocity[2]; - _report.vel_m_s = sqrtf(_report.vel_n_m_s * _report.vel_n_m_s + _report.vel_e_m_s * _report.vel_e_m_s + _report.vel_d_m_s * _report.vel_d_m_s); - _report.cog_rad = atan2f(_report.vel_e_m_s, _report.vel_n_m_s); - _report.vel_ned_valid = true; - - _report.timestamp_time = _report.timestamp_position; - _report.time_gps_usec = uavcan::UtcTime(msg.gnss_timestamp).toUSec(); // Convert to microseconds - - _report.satellites_used = msg.sats_used; - - if (_report_pub > 0) { - orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &_report); - - } else { - _report_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report); - } - -} diff --git a/src/modules/uavcan/gnss_receiver.hpp b/src/modules/uavcan/gnss_receiver.hpp deleted file mode 100644 index 18df8da2f..000000000 --- a/src/modules/uavcan/gnss_receiver.hpp +++ /dev/null @@ -1,84 +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 gnss_receiver.hpp - * - * UAVCAN --> ORB bridge for GNSS messages: - * uavcan.equipment.gnss.Fix - * - * @author Pavel Kirienko - * @author Andrew Chambers - */ - -#pragma once - -#include - -#include -#include - -#include -#include - -class UavcanGnssReceiver -{ -public: - UavcanGnssReceiver(uavcan::INode& node); - - int init(); - -private: - /** - * GNSS fix message will be reported via this callback. - */ - void gnss_fix_sub_cb(const uavcan::ReceivedDataStructure &msg); - - - typedef uavcan::MethodBinder&)> - FixCbBinder; - - /* - * libuavcan related things - */ - uavcan::INode &_node; - uavcan::Subscriber _uavcan_sub_status; - - /* - * uORB - */ - struct vehicle_gps_position_s _report; ///< uORB topic for gnss position - orb_advert_t _report_pub; ///< uORB pub for gnss position - -}; diff --git a/src/modules/uavcan/module.mk b/src/modules/uavcan/module.mk index 3865f2468..2b9729045 100644 --- a/src/modules/uavcan/module.mk +++ b/src/modules/uavcan/module.mk @@ -40,10 +40,16 @@ MODULE_COMMAND = uavcan MAXOPTIMIZATION = -Os -SRCS += uavcan_main.cpp \ - uavcan_clock.cpp \ - esc_controller.cpp \ - gnss_receiver.cpp +# Main +SRCS += uavcan_main.cpp \ + uavcan_clock.cpp + +# Actuators +SRCS += actuators/esc.cpp + +# Sensors +SRCS += sensors/sensor_bridge.cpp \ + sensors/gnss.cpp # # libuavcan diff --git a/src/modules/uavcan/sensors/gnss.cpp b/src/modules/uavcan/sensors/gnss.cpp new file mode 100644 index 000000000..4fc0743ff --- /dev/null +++ b/src/modules/uavcan/sensors/gnss.cpp @@ -0,0 +1,156 @@ +/**************************************************************************** + * + * 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 gnss.cpp + * + * @author Pavel Kirienko + * @author Andrew Chambers + * + */ + +#include "gnss.hpp" +#include +#include + +#define MM_PER_CM 10 // Millimeters per centimeter + +UavcanGnssBridge::UavcanGnssBridge(uavcan::INode &node) : +_node(node), +_uavcan_sub_status(node), +_report_pub(-1) +{ +} + +const char *UavcanGnssBridge::get_name() const { return "gnss"; } + +int UavcanGnssBridge::init() +{ + int res = -1; + + // GNSS fix subscription + res = _uavcan_sub_status.start(FixCbBinder(this, &UavcanGnssBridge::gnss_fix_sub_cb)); + if (res < 0) + { + warnx("GNSS fix sub failed %i", res); + return res; + } + + // Clear the uORB GPS report + memset(&_report, 0, sizeof(_report)); + + warnx("gnss sensor bridge init ok"); + return res; +} + +void UavcanGnssBridge::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure &msg) +{ + _report.timestamp_position = hrt_absolute_time(); + _report.lat = msg.lat_1e7; + _report.lon = msg.lon_1e7; + _report.alt = msg.alt_1e2 * MM_PER_CM; // Convert from centimeter (1e2) to millimeters (1e3) + + _report.timestamp_variance = _report.timestamp_position; + + + // Check if the msg contains valid covariance information + const bool valid_position_covariance = !msg.position_covariance.empty(); + const bool valid_velocity_covariance = !msg.velocity_covariance.empty(); + + if (valid_position_covariance) { + float pos_cov[9]; + msg.position_covariance.unpackSquareMatrix(pos_cov); + + // Horizontal position uncertainty + const float horizontal_pos_variance = math::max(pos_cov[0], pos_cov[4]); + _report.eph = (horizontal_pos_variance > 0) ? sqrtf(horizontal_pos_variance) : -1.0F; + + // Vertical position uncertainty + _report.epv = (pos_cov[8] > 0) ? sqrtf(pos_cov[8]) : -1.0F; + } else { + _report.eph = -1.0F; + _report.epv = -1.0F; + } + + if (valid_velocity_covariance) { + float vel_cov[9]; + msg.velocity_covariance.unpackSquareMatrix(vel_cov); + _report.s_variance_m_s = math::max(math::max(vel_cov[0], vel_cov[4]), vel_cov[8]); + + /* There is a nonlinear relationship between the velocity vector and the heading. + * Use Jacobian to transform velocity covariance to heading covariance + * + * Nonlinear equation: + * heading = atan2(vel_e_m_s, vel_n_m_s) + * For math, see http://en.wikipedia.org/wiki/Atan2#Derivative + * + * To calculate the variance of heading from the variance of velocity, + * cov(heading) = J(velocity)*cov(velocity)*J(velocity)^T + */ + float vel_n = msg.ned_velocity[0]; + float vel_e = msg.ned_velocity[1]; + float vel_n_sq = vel_n * vel_n; + float vel_e_sq = vel_e * vel_e; + _report.c_variance_rad = + (vel_e_sq * vel_cov[0] + + -2 * vel_n * vel_e * vel_cov[1] + // Covariance matrix is symmetric + vel_n_sq* vel_cov[4]) / ((vel_n_sq + vel_e_sq) * (vel_n_sq + vel_e_sq)); + + } else { + _report.s_variance_m_s = -1.0F; + _report.c_variance_rad = -1.0F; + } + + _report.fix_type = msg.status; + + _report.timestamp_velocity = _report.timestamp_position; + _report.vel_n_m_s = msg.ned_velocity[0]; + _report.vel_e_m_s = msg.ned_velocity[1]; + _report.vel_d_m_s = msg.ned_velocity[2]; + _report.vel_m_s = sqrtf(_report.vel_n_m_s * _report.vel_n_m_s + _report.vel_e_m_s * _report.vel_e_m_s + _report.vel_d_m_s * _report.vel_d_m_s); + _report.cog_rad = atan2f(_report.vel_e_m_s, _report.vel_n_m_s); + _report.vel_ned_valid = true; + + _report.timestamp_time = _report.timestamp_position; + _report.time_gps_usec = uavcan::UtcTime(msg.gnss_timestamp).toUSec(); // Convert to microseconds + + _report.satellites_used = msg.sats_used; + + if (_report_pub > 0) { + orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &_report); + + } else { + _report_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report); + } + +} diff --git a/src/modules/uavcan/sensors/gnss.hpp b/src/modules/uavcan/sensors/gnss.hpp new file mode 100644 index 000000000..eec595ad1 --- /dev/null +++ b/src/modules/uavcan/sensors/gnss.hpp @@ -0,0 +1,88 @@ +/**************************************************************************** + * + * 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 gnss.hpp + * + * UAVCAN --> ORB bridge for GNSS messages: + * uavcan.equipment.gnss.Fix + * + * @author Pavel Kirienko + * @author Andrew Chambers + */ + +#pragma once + +#include + +#include +#include + +#include +#include + +#include "sensor_bridge.hpp" + +class UavcanGnssBridge : public IUavcanSensorBridge +{ +public: + UavcanGnssBridge(uavcan::INode& node); + + const char *get_name() const override; + + int init() override; + +private: + /** + * GNSS fix message will be reported via this callback. + */ + void gnss_fix_sub_cb(const uavcan::ReceivedDataStructure &msg); + + + typedef uavcan::MethodBinder&)> + FixCbBinder; + + /* + * libuavcan related things + */ + uavcan::INode &_node; + uavcan::Subscriber _uavcan_sub_status; + + /* + * uORB + */ + struct vehicle_gps_position_s _report; ///< uORB topic for gnss position + orb_advert_t _report_pub; ///< uORB pub for gnss position + +}; diff --git a/src/modules/uavcan/sensors/sensor_bridge.cpp b/src/modules/uavcan/sensors/sensor_bridge.cpp new file mode 100644 index 000000000..ba27d6c71 --- /dev/null +++ b/src/modules/uavcan/sensors/sensor_bridge.cpp @@ -0,0 +1,48 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +/** + * @author Pavel Kirienko + */ + +#include "sensor_bridge.hpp" +#include "gnss.hpp" + +IUavcanSensorBridge* IUavcanSensorBridge::make(uavcan::INode &node, const char *bridge_name) +{ + if (!std::strncmp("gnss", bridge_name, MaxNameLen)) { + return new UavcanGnssBridge(node); + } else { + return nullptr; + } +} diff --git a/src/modules/uavcan/sensors/sensor_bridge.hpp b/src/modules/uavcan/sensors/sensor_bridge.hpp new file mode 100644 index 000000000..1667c2b57 --- /dev/null +++ b/src/modules/uavcan/sensors/sensor_bridge.hpp @@ -0,0 +1,70 @@ +/**************************************************************************** + * + * Copyright (C) 2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @author Pavel Kirienko + */ + +#pragma once + +#include +#include + +/** + * A sensor bridge class must implement this interface. + */ +class IUavcanSensorBridge : uavcan::Noncopyable, public ListNode +{ +public: + static constexpr unsigned MaxNameLen = 20; + + virtual ~IUavcanSensorBridge() { } + + /** + * Returns ASCII name of the bridge. + */ + virtual const char *get_name() const = 0; + + /** + * Starts the bridge. + * @return Non-negative value on success, negative on error. + */ + virtual int init() = 0; + + /** + * Sensor bridge factory. + * Creates a bridge object by its ASCII name, e.g. "gnss", "mag". + * @return nullptr if such bridge can't be created. + */ + static IUavcanSensorBridge* make(uavcan::INode &node, const char *bridge_name); +}; diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index 4535b6d6a..fc5521aa7 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -65,16 +65,18 @@ UavcanNode *UavcanNode::_instance; UavcanNode::UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &system_clock) : CDev("uavcan", UAVCAN_DEVICE_PATH), _node(can_driver, system_clock), - _esc_controller(_node), - _gnss_receiver(_node) + _node_mutex(), + _esc_controller(_node) { _control_topics[0] = ORB_ID(actuator_controls_0); _control_topics[1] = ORB_ID(actuator_controls_1); _control_topics[2] = ORB_ID(actuator_controls_2); _control_topics[3] = ORB_ID(actuator_controls_3); - // memset(_controls, 0, sizeof(_controls)); - // memset(_poll_fds, 0, sizeof(_poll_fds)); + const int res = pthread_mutex_init(&_node_mutex, nullptr); + if (res < 0) { + std::abort(); + } } UavcanNode::~UavcanNode() @@ -99,7 +101,7 @@ UavcanNode::~UavcanNode() } /* clean up the alternate device node */ - // unregister_driver(PWM_OUTPUT_DEVICE_PATH); + // unregister_driver(PWM_OUTPUT_DEVICE_PATH); ::close(_armed_sub); @@ -231,10 +233,6 @@ int UavcanNode::init(uavcan::NodeID node_id) if (ret < 0) return ret; - ret = _gnss_receiver.init(); - if (ret < 0) - return ret; - return _node.start(); } @@ -248,6 +246,8 @@ void UavcanNode::node_spin_once() int UavcanNode::run() { + (void)pthread_mutex_lock(&_node_mutex); + const unsigned PollTimeoutMs = 50; // XXX figure out the output count @@ -291,8 +291,13 @@ int UavcanNode::run() _groups_subscribed = _groups_required; } + // Mutex is unlocked while the thread is blocked on IO multiplexing + (void)pthread_mutex_unlock(&_node_mutex); + const int poll_ret = ::poll(_poll_fds, _poll_fds_num, PollTimeoutMs); + (void)pthread_mutex_lock(&_node_mutex); + node_spin_once(); // Non-blocking // this would be bad... @@ -352,7 +357,6 @@ int UavcanNode::run() // Output to the bus _esc_controller.update_outputs(outputs.output, outputs.noutputs); } - } // Check arming state @@ -376,10 +380,7 @@ int UavcanNode::run() } int -UavcanNode::control_callback(uintptr_t handle, - uint8_t control_group, - uint8_t control_index, - float &input) +UavcanNode::control_callback(uintptr_t handle, uint8_t control_group, uint8_t control_index, float &input) { const actuator_controls_s *controls = (actuator_controls_s *)handle; @@ -524,12 +525,69 @@ UavcanNode::print_info() warnx("mixer: %s", (_mixers == nullptr) ? "NONE" : "OK"); } +int UavcanNode::sensor_enable(const char *bridge_name) +{ + int retval = -1; + + (void)pthread_mutex_lock(&_node_mutex); + + // Checking if such bridge already exists + { + auto pos = _sensor_bridges.getHead(); + while (pos != nullptr) { + if (!std::strncmp(pos->get_name(), bridge_name, IUavcanSensorBridge::MaxNameLen)) { + warnx("sensor bridge '%s' already exists", bridge_name); + retval = -1; + goto leave; + } + pos = pos->getSibling(); + } + } + + // Creating and initing + { + auto bridge = IUavcanSensorBridge::make(get_node(), bridge_name); + if (bridge == nullptr) { + warnx("cannot create sensor bridge '%s'", bridge_name); + retval = -1; + goto leave; + } + + assert(!std::strncmp(bridge->get_name(), bridge_name, IUavcanSensorBridge::MaxNameLen)); + + retval = bridge->init(); + if (retval >= 0) { + _sensor_bridges.add(bridge); + } + } + +leave: + (void)pthread_mutex_unlock(&_node_mutex); + return retval; +} + +void UavcanNode::sensor_print_enabled() +{ + (void)pthread_mutex_lock(&_node_mutex); + + auto pos = _sensor_bridges.getHead(); + while (pos != nullptr) { + warnx("%s", pos->get_name()); + pos = pos->getSibling(); + } + + (void)pthread_mutex_unlock(&_node_mutex); +} + /* * App entry point */ static void print_usage() { - warnx("usage: uavcan start [can_bitrate]"); + warnx("usage: \n" + "\tuavcan start [can_bitrate]\n" + "\tuavcan sensor enable \n" + "\tuavcan sensor list"); } extern "C" __EXPORT int uavcan_main(int argc, char *argv[]); @@ -585,7 +643,7 @@ int uavcan_main(int argc, char *argv[]) } /* commands below require the app to be started */ - UavcanNode *inst = UavcanNode::instance(); + UavcanNode *const inst = UavcanNode::instance(); if (!inst) { errx(1, "application not running"); @@ -594,14 +652,37 @@ int uavcan_main(int argc, char *argv[]) if (!std::strcmp(argv[1], "status") || !std::strcmp(argv[1], "info")) { inst->print_info(); - return OK; + ::exit(0); } if (!std::strcmp(argv[1], "stop")) { - delete inst; - inst = nullptr; - return OK; + ::exit(0); + } + + if (!std::strcmp(argv[1], "sensor")) { + if (argc < 3) { + print_usage(); + ::exit(1); + } + + if (!std::strcmp(argv[2], "list")) { + inst->sensor_print_enabled(); + ::exit(0); + } + + if (argc < 4) { + print_usage(); + ::exit(1); + } + + if (!std::strcmp(argv[2], "enable")) { + const int res = inst->sensor_enable(argv[3]); + if (res < 0) { + warnx("failed to enable sensor '%s': error %d", argv[3], res); + } + ::exit(0); + } } print_usage(); diff --git a/src/modules/uavcan/uavcan_main.hpp b/src/modules/uavcan/uavcan_main.hpp index 05b66fd7b..bca1aa530 100644 --- a/src/modules/uavcan/uavcan_main.hpp +++ b/src/modules/uavcan/uavcan_main.hpp @@ -42,8 +42,8 @@ #include #include -#include "esc_controller.hpp" -#include "gnss_receiver.hpp" +#include "actuators/esc.hpp" +#include "sensors/sensor_bridge.hpp" /** * @file uavcan_main.hpp @@ -77,12 +77,10 @@ public: static int start(uavcan::NodeID node_id, uint32_t bitrate); - Node& getNode() { return _node; } + Node& get_node() { return _node; } - static int control_callback(uintptr_t handle, - uint8_t control_group, - uint8_t control_index, - float &input); + // TODO: move the actuator mixing stuff into the ESC controller class + static int control_callback(uintptr_t handle, uint8_t control_group, uint8_t control_index, float &input); void subscribe(); @@ -91,6 +89,10 @@ public: void print_info(); + int sensor_enable(const char *bridge_name); + + void sensor_print_enabled(); + static UavcanNode* instance() { return _instance; } private: @@ -109,8 +111,11 @@ private: static UavcanNode *_instance; ///< singleton pointer Node _node; ///< library instance + pthread_mutex_t _node_mutex; + UavcanEscController _esc_controller; - UavcanGnssReceiver _gnss_receiver; + + List _sensor_bridges; ///< Append-only list of active sensor bridges MixerGroup *_mixers = nullptr; -- cgit v1.2.3 From 1a14ff250e5a2ead69576762fd5b7f176c4b6fac Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 22 Aug 2014 00:39:44 +0200 Subject: fw att control: use RC only if in manual --- src/modules/fw_att_control/fw_att_control_main.cpp | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index 0cea13cc4..ee112a40e 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -717,7 +717,14 @@ FixedwingAttitudeControl::task_main() float pitch_sp = _parameters.pitchsp_offset_rad; float throttle_sp = 0.0f; - if (_vcontrol_mode.flag_control_velocity_enabled || _vcontrol_mode.flag_control_position_enabled) { + /* Read attitude setpoint from uorb if + * - velocity control or position control is enabled (pos controller is running) + * - manual control is disabled (another app may send the setpoint, but it should + * for sure not be set from the remote control values) + */ + if (_vcontrol_mode.flag_control_velocity_enabled || + _vcontrol_mode.flag_control_position_enabled || + !_vcontrol_mode.flag_control_manual_enabled) { /* read in attitude setpoint from attitude setpoint uorb topic */ roll_sp = _att_sp.roll_body + _parameters.rollsp_offset_rad; pitch_sp = _att_sp.pitch_body + _parameters.pitchsp_offset_rad; -- cgit v1.2.3 From 752a0a562564ccc6f7d49ceebe810de7e6a6d358 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 22 Aug 2014 00:40:45 +0200 Subject: add obc gps failure mode --- src/modules/commander/commander.cpp | 18 +++- src/modules/navigator/gpsfailure.cpp | 95 +++++++++++---------- src/modules/navigator/gpsfailure.h | 13 ++- src/modules/navigator/gpsfailure_params.c | 97 ++++++++++++++++++++++ src/modules/navigator/module.mk | 3 +- src/modules/navigator/navigator.h | 12 +++ src/modules/navigator/navigator_main.cpp | 16 ++++ .../uORB/topics/position_setpoint_triplet.h | 1 + 8 files changed, 207 insertions(+), 48 deletions(-) create mode 100644 src/modules/navigator/gpsfailure_params.c diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 84a4be948..109ab403a 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1311,11 +1311,12 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(position_setpoint_triplet), pos_sp_triplet_sub, &pos_sp_triplet); /* Check for geofence violation */ - if (pos_sp_triplet.geofence_violated) { + if (pos_sp_triplet.geofence_violated || pos_sp_triplet.flight_termination) { //XXX: make this configurable to select different actions (e.g. navigation modes) /* this will only trigger if geofence is activated via param and a geofence file is present, also there is a circuit breaker to disable the actual flight termination in the px4io driver */ - armed.force_failsafe = true; + armed.fosrce_failsafe = true; status_changed = true; + warnx("Flight termination"); } // no reset is done here on purpose, on geofence violation we want to stay in flighttermination } @@ -2060,6 +2061,7 @@ set_control_mode() case NAVIGATION_STATE_AUTO_LOITER: case NAVIGATION_STATE_AUTO_RTL: case NAVIGATION_STATE_AUTO_RTGS: + case NAVIGATION_STATE_AUTO_LANDENGFAIL: control_mode.flag_control_manual_enabled = false; control_mode.flag_control_auto_enabled = true; control_mode.flag_control_rates_enabled = true; @@ -2071,6 +2073,18 @@ set_control_mode() control_mode.flag_control_termination_enabled = false; break; + case NAVIGATION_STATE_AUTO_LANDGPSFAIL: + control_mode.flag_control_manual_enabled = false; + control_mode.flag_control_auto_enabled = false; + control_mode.flag_control_rates_enabled = true; + control_mode.flag_control_attitude_enabled = true; + control_mode.flag_control_altitude_enabled = false; + control_mode.flag_control_climb_rate_enabled = true; + control_mode.flag_control_position_enabled = false; + control_mode.flag_control_velocity_enabled = false; + control_mode.flag_control_termination_enabled = false; + break; + case NAVIGATION_STATE_LAND: control_mode.flag_control_manual_enabled = false; control_mode.flag_control_auto_enabled = true; diff --git a/src/modules/navigator/gpsfailure.cpp b/src/modules/navigator/gpsfailure.cpp index 4c526b76e..02e766ffb 100644 --- a/src/modules/navigator/gpsfailure.cpp +++ b/src/modules/navigator/gpsfailure.cpp @@ -57,7 +57,12 @@ GpsFailure::GpsFailure(Navigator *navigator, const char *name) : MissionBlock(navigator, name), - _gpsf_state(GPSF_STATE_NONE) + _param_loitertime(this, "LT"), + _param_openlooploiter_roll(this, "R"), + _param_openlooploiter_pitch(this, "P"), + _param_openlooploiter_thrust(this, "TR"), + _gpsf_state(GPSF_STATE_NONE), + _timestamp_activation(0) { /* load initial params */ updateParams(); @@ -82,6 +87,7 @@ void GpsFailure::on_activation() { _gpsf_state = GPSF_STATE_NONE; + _timestamp_activation = hrt_absolute_time(); updateParams(); advance_gpsf(); set_gpsf_item(); @@ -90,10 +96,34 @@ GpsFailure::on_activation() void GpsFailure::on_active() { - if (is_mission_item_reached()) { - updateParams(); - advance_gpsf(); + + switch (_gpsf_state) { + case GPSF_STATE_LOITER: { + /* Position controller does not run in this mode: + * navigator has to publish an attitude setpoint */ + _navigator->get_att_sp()->roll_body = M_DEG_TO_RAD_F * _param_openlooploiter_roll.get(); + _navigator->get_att_sp()->pitch_body = M_DEG_TO_RAD_F * _param_openlooploiter_pitch.get(); + _navigator->get_att_sp()->thrust = _param_openlooploiter_thrust.get(); + _navigator->publish_att_sp(); + + /* Measure time */ + hrt_abstime elapsed = hrt_elapsed_time(&_timestamp_activation); + + //warnx("open loop loiter, posctl enabled %u, elapsed %.1fs, thrust %.2f", + //_navigator->get_control_mode()->flag_control_position_enabled, elapsed * 1e-6, (double)_param_openlooploiter_thrust.get()); + if (elapsed > _param_loitertime.get() * 1e6f) { + /* no recovery, adavance the state machine */ + warnx("gps not recovered, switch to next state"); + advance_gpsf(); + } + break; + } + case GPSF_STATE_TERMINATE: set_gpsf_item(); + advance_gpsf(); + break; + default: + break; } } @@ -102,68 +132,45 @@ GpsFailure::set_gpsf_item() { struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); - set_previous_pos_setpoint(); - _navigator->set_can_loiter_at_sp(false); + /* Set pos sp triplet to invalid to stop pos controller */ + pos_sp_triplet->previous.valid = false; + pos_sp_triplet->current.valid = false; + pos_sp_triplet->next.valid = false; switch (_gpsf_state) { - case GPSF_STATE_LOITER: { - //_mission_item.lat = (double)(_param_commsholdlat.get()) * 1.0e-7; - //_mission_item.lon = (double)(_param_commsholdlon.get()) * 1.0e-7; - //_mission_item.altitude_is_relative = false; - //_mission_item.altitude = _param_commsholdalt.get(); - //_mission_item.yaw = NAN; - //_mission_item.loiter_radius = _navigator->get_loiter_radius(); - //_mission_item.loiter_direction = 1; - //_mission_item.nav_cmd = NAV_CMD_LOITER_TIME_LIMIT; - //_mission_item.acceptance_radius = _navigator->get_acceptance_radius(); - //_mission_item.time_inside = _param_commsholdwaittime.get() < 0.0f ? 0.0f : _param_commsholdwaittime.get(); - //_mission_item.pitch_min = 0.0f; - //_mission_item.autocontinue = true; - //_mission_item.origin = ORIGIN_ONBOARD; - - //_navigator->set_can_loiter_at_sp(true); - break; - } case GPSF_STATE_TERMINATE: { - //_mission_item.lat = (double)(_param_airfieldhomelat.get()) * 1.0e-7; - //_mission_item.lon = (double)(_param_airfieldhomelon.get()) * 1.0e-7; - //_mission_item.altitude_is_relative = false; - //_mission_item.altitude = _param_airfieldhomealt.get(); - //_mission_item.yaw = NAN; - //_mission_item.loiter_radius = _navigator->get_loiter_radius(); - //_mission_item.loiter_direction = 1; - //_mission_item.nav_cmd = NAV_CMD_LOITER_UNLIMITED; - //_mission_item.acceptance_radius = _navigator->get_acceptance_radius(); - //_mission_item.pitch_min = 0.0f; - //_mission_item.autocontinue = true; - //_mission_item.origin = ORIGIN_ONBOARD; - - //_navigator->set_can_loiter_at_sp(true); - break; + /* Request flight termination from the commander */ + pos_sp_triplet->flight_termination = true; + warnx("request flight termination"); } default: break; } reset_mission_item_reached(); - - /* convert mission item to current position setpoint and make it valid */ - mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current); - pos_sp_triplet->next.valid = false; - _navigator->set_position_setpoint_triplet_updated(); } void GpsFailure::advance_gpsf() { + updateParams(); + switch (_gpsf_state) { case GPSF_STATE_NONE: _gpsf_state = GPSF_STATE_LOITER; + warnx("gpsf loiter"); + mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: open loop loiter"); break; case GPSF_STATE_LOITER: _gpsf_state = GPSF_STATE_TERMINATE; + warnx("gpsf terminate"); + mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: no gps recovery, termination"); + warnx("mavlink sent"); break; + case GPSF_STATE_TERMINATE: + warnx("gpsf end"); + _gpsf_state = GPSF_STATE_END; default: break; } diff --git a/src/modules/navigator/gpsfailure.h b/src/modules/navigator/gpsfailure.h index 88d386ec4..e9e72babf 100644 --- a/src/modules/navigator/gpsfailure.h +++ b/src/modules/navigator/gpsfailure.h @@ -43,7 +43,11 @@ #include #include -#include +#include +#include +#include + +#include #include "navigator_mode.h" #include "mission_block.h" @@ -65,13 +69,20 @@ public: private: /* Params */ + control::BlockParamFloat _param_loitertime; + control::BlockParamFloat _param_openlooploiter_roll; + control::BlockParamFloat _param_openlooploiter_pitch; + control::BlockParamFloat _param_openlooploiter_thrust; enum GPSFState { GPSF_STATE_NONE = 0, GPSF_STATE_LOITER = 1, GPSF_STATE_TERMINATE = 2, + GPSF_STATE_END = 3, } _gpsf_state; + hrt_abstime _timestamp_activation; //*< timestamp when this mode was activated */ + /** * Set the GPSF item */ diff --git a/src/modules/navigator/gpsfailure_params.c b/src/modules/navigator/gpsfailure_params.c new file mode 100644 index 000000000..39d179eed --- /dev/null +++ b/src/modules/navigator/gpsfailure_params.c @@ -0,0 +1,97 @@ +/**************************************************************************** + * + * Copyright (c) 2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * AS IS AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file gpsfailure_params.c + * + * Parameters for GPSF navigation mode + * + * @author Thomas Gubler + */ + +#include + +#include + +/* + * GPS Failure Navigation Mode parameters, accessible via MAVLink + */ + +/** + * Loiter time + * + * The amount of time in seconds the system should do open loop loiter and wait for gps recovery + * before it goes into flight termination. + * + * @unit seconds + * @min 0.0 + * @group GPSF + */ +PARAM_DEFINE_FLOAT(NAV_GPSF_LT, 30.0f); + +/** + * Open loop loiter roll + * + * Roll in degrees during the open loop loiter + * + * @unit deg + * @min 0.0 + * @max 30.0 + * @group GPSF + */ +PARAM_DEFINE_FLOAT(NAV_GPSF_R, 15.0f); + +/** + * Open loop loiter pitch + * + * Pitch in degrees during the open loop loiter + * + * @unit deg + * @min -30.0 + * @max 30.0 + * @group GPSF + */ +PARAM_DEFINE_FLOAT(NAV_GPSF_P, 0.0f); + +/** + * Open loop loiter thrust + * + * Thrust value which is set during the open loop loiter + * + * @min 0.0 + * @max 1.0 + * @group GPSF + */ +PARAM_DEFINE_FLOAT(NAV_GPSF_TR, 0.7f); + + diff --git a/src/modules/navigator/module.mk b/src/modules/navigator/module.mk index f6590605b..9e4ad053c 100644 --- a/src/modules/navigator/module.mk +++ b/src/modules/navigator/module.mk @@ -53,7 +53,8 @@ SRCS = navigator_main.cpp \ datalinkloss.cpp \ enginefailure.cpp \ datalinkloss_params.c \ - gpsfailure.cpp + gpsfailure.cpp \ + gpsfailure_params.c INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index b161c2984..ec6e538e3 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -53,6 +53,7 @@ #include #include #include +#include #include "navigator_mode.h" #include "mission.h" @@ -108,6 +109,12 @@ public: * Publish the mission result so commander and mavlink know what is going on */ void publish_mission_result(); + + /** + * Publish the attitude sp, only to be used in very special modes when position control is deactivated + * Example: mode that is triggered on gps failure + */ + void publish_att_sp(); /** * Setters @@ -125,6 +132,7 @@ public: struct home_position_s* get_home_position() { return &_home_pos; } struct position_setpoint_triplet_s* get_position_setpoint_triplet() { return &_pos_sp_triplet; } struct mission_result_s* get_mission_result() { return &_mission_result; } + struct vehicle_attitude_setpoint_s* get_att_sp() { return &_att_sp; } int get_onboard_mission_sub() { return _onboard_mission_sub; } int get_offboard_mission_sub() { return _offboard_mission_sub; } @@ -155,6 +163,9 @@ private: orb_advert_t _pos_sp_triplet_pub; /**< publish position setpoint triplet */ orb_advert_t _mission_result_pub; + orb_advert_t _att_sp_pub; /**< publish att sp + used only in very special failsafe modes + when pos control is deactivated */ vehicle_status_s _vstatus; /**< vehicle status */ vehicle_control_mode_s _control_mode; /**< vehicle control mode */ @@ -166,6 +177,7 @@ private: position_setpoint_triplet_s _pos_sp_triplet; /**< triplet of position setpoints */ mission_result_s _mission_result; + vehicle_attitude_setpoint_s _att_sp; bool _mission_item_valid; /**< flags if the current mission item is valid */ diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index bc9951bf2..e0913bb57 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -110,6 +110,7 @@ Navigator::Navigator() : _param_update_sub(-1), _pos_sp_triplet_pub(-1), _mission_result_pub(-1), + _att_sp_pub(-1), _vstatus{}, _control_mode{}, _global_pos{}, @@ -119,6 +120,7 @@ Navigator::Navigator() : _nav_caps{}, _pos_sp_triplet{}, _mission_result{}, + _att_sp{}, _mission_item_valid(false), _loop_perf(perf_alloc(PC_ELAPSED, "navigator")), _geofence{}, @@ -609,3 +611,17 @@ Navigator::publish_mission_result() _mission_result.reached = false; _mission_result.finished = false; } + +void +Navigator::publish_att_sp() +{ + /* lazily publish the attitude sp only once available */ + if (_att_sp_pub > 0) { + /* publish att sp*/ + orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &_att_sp); + + } else { + /* advertise and publish */ + _att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_att_sp); + } +} diff --git a/src/modules/uORB/topics/position_setpoint_triplet.h b/src/modules/uORB/topics/position_setpoint_triplet.h index 1c78f5330..e2b1525a2 100644 --- a/src/modules/uORB/topics/position_setpoint_triplet.h +++ b/src/modules/uORB/topics/position_setpoint_triplet.h @@ -98,6 +98,7 @@ struct position_setpoint_triplet_s unsigned nav_state; /**< report the navigation state */ bool geofence_violated; /**< true if the geofence is violated */ + bool flight_termination; /**< true if the navigator demands a flight termination from the commander app */ }; /** -- cgit v1.2.3 From 04ad990254e51ceb68c91e7ef601f1e281fd7062 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 22 Aug 2014 07:08:30 +0200 Subject: Fixed build issues --- src/modules/bottle_drop/bottle_drop.cpp | 43 +++++++++++++++++---------------- 1 file changed, 22 insertions(+), 21 deletions(-) diff --git a/src/modules/bottle_drop/bottle_drop.cpp b/src/modules/bottle_drop/bottle_drop.cpp index ae97cc570..6c3bdb002 100644 --- a/src/modules/bottle_drop/bottle_drop.cpp +++ b/src/modules/bottle_drop/bottle_drop.cpp @@ -140,6 +140,9 @@ private: DROP_STATE_BAY_CLOSED } _drop_state; + struct mission_s _onboard_mission; + orb_advert_t _onboard_mission_pub; + void task_main(); void handle_command(struct vehicle_command_s *cmd); @@ -180,7 +183,9 @@ BottleDrop::BottleDrop() : _alt_clearance(70.0f), _target_position {}, _drop_position {}, - _drop_state(DROP_STATE_INIT) + _drop_state(DROP_STATE_INIT), + _onboard_mission {}, + _onboard_mission_pub(-1) { } @@ -410,10 +415,6 @@ BottleDrop::task_main() flight_vector_e.acceptance_radius = 50; // TODO: make parameter flight_vector_e.autocontinue = true; - struct mission_s onboard_mission; - memset(&onboard_mission, 0, sizeof(onboard_mission)); - orb_advert_t onboard_mission_pub = -1; - struct wind_estimate_s wind; /* wakeup source(s) */ @@ -617,27 +618,27 @@ BottleDrop::task_main() warnx("ERROR: could not save onboard WP"); } - onboard_mission.count = 2; - onboard_mission.current_seq = 0; + _onboard_mission.count = 2; + _onboard_mission.current_seq = 0; - if (onboard_mission_pub > 0) { - orb_publish(ORB_ID(onboard_mission), onboard_mission_pub, &onboard_mission); + if (_onboard_mission_pub > 0) { + orb_publish(ORB_ID(onboard_mission), _onboard_mission_pub, &_onboard_mission); } else { - onboard_mission_pub = orb_advertise(ORB_ID(onboard_mission), &onboard_mission); + _onboard_mission_pub = orb_advertise(ORB_ID(onboard_mission), &_onboard_mission); } _drop_state = DROP_STATE_TARGET_SET; } break; - case DROP_STATE_TARGET_SET - + case DROP_STATE_TARGET_SET: + { float distance_wp2 = get_distance_to_next_waypoint(flight_vector_e.lat, flight_vector_e.lon, _drop_position.lat, _drop_position.lon); if (distance_wp2 < distance_real) { - onboard_mission.current_seq = 0; - orb_publish(ORB_ID(onboard_mission), onboard_mission_pub, &onboard_mission); + _onboard_mission.current_seq = 0; + orb_publish(ORB_ID(onboard_mission), _onboard_mission_pub, &_onboard_mission); } else { // We're close enough - open the bay @@ -674,8 +675,8 @@ BottleDrop::task_main() float distance_wp2 = get_distance_to_next_waypoint(flight_vector_e.lat, flight_vector_e.lon, _drop_position.lat, _drop_position.lon); if (distance_wp2 < distance_real) { - onboard_mission.current_seq = 0; - orb_publish(ORB_ID(onboard_mission), onboard_mission_pub, &onboard_mission); + _onboard_mission.current_seq = 0; + orb_publish(ORB_ID(onboard_mission), _onboard_mission_pub, &_onboard_mission); } } } @@ -692,8 +693,8 @@ BottleDrop::task_main() mavlink_log_info(_mavlink_fd, "#audio: closing bay"); // remove onboard mission - onboard_mission.current_seq = -1; - orb_publish(ORB_ID(onboard_mission), onboard_mission_pub, &onboard_mission); + _onboard_mission.current_seq = -1; + orb_publish(ORB_ID(onboard_mission), _onboard_mission_pub, &_onboard_mission); } break; } @@ -784,10 +785,10 @@ BottleDrop::handle_command(struct vehicle_command_s *cmd) _drop_state = DROP_STATE_INIT; // Abort if mission is present - onboard_mission.current_seq = -1; + _onboard_mission.current_seq = -1; - if (onboard_mission_pub > 0) { - orb_publish(ORB_ID(onboard_mission), onboard_mission_pub, &onboard_mission); + if (_onboard_mission_pub > 0) { + orb_publish(ORB_ID(onboard_mission), _onboard_mission_pub, &_onboard_mission); } } else { -- cgit v1.2.3 From f820010a2b51d03f0099d3b4853e0620593721e6 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Fri, 22 Aug 2014 13:24:31 +0400 Subject: UAVCAN GNSS subscription name fix --- src/modules/uavcan/sensors/gnss.cpp | 4 ++-- src/modules/uavcan/sensors/gnss.hpp | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/modules/uavcan/sensors/gnss.cpp b/src/modules/uavcan/sensors/gnss.cpp index 4fc0743ff..fb89b8365 100644 --- a/src/modules/uavcan/sensors/gnss.cpp +++ b/src/modules/uavcan/sensors/gnss.cpp @@ -47,7 +47,7 @@ UavcanGnssBridge::UavcanGnssBridge(uavcan::INode &node) : _node(node), -_uavcan_sub_status(node), +_sub_fix(node), _report_pub(-1) { } @@ -59,7 +59,7 @@ int UavcanGnssBridge::init() int res = -1; // GNSS fix subscription - res = _uavcan_sub_status.start(FixCbBinder(this, &UavcanGnssBridge::gnss_fix_sub_cb)); + res = _sub_fix.start(FixCbBinder(this, &UavcanGnssBridge::gnss_fix_sub_cb)); if (res < 0) { warnx("GNSS fix sub failed %i", res); diff --git a/src/modules/uavcan/sensors/gnss.hpp b/src/modules/uavcan/sensors/gnss.hpp index eec595ad1..6bdae8de3 100644 --- a/src/modules/uavcan/sensors/gnss.hpp +++ b/src/modules/uavcan/sensors/gnss.hpp @@ -77,7 +77,7 @@ private: * libuavcan related things */ uavcan::INode &_node; - uavcan::Subscriber _uavcan_sub_status; + uavcan::Subscriber _sub_fix; /* * uORB -- cgit v1.2.3 From 54affaf633216c3aef65164c7e43674c8c26f178 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Fri, 22 Aug 2014 13:58:05 +0400 Subject: UAVCAN sensor enable command fix --- src/modules/uavcan/uavcan_main.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index fc5521aa7..71302d928 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -558,6 +558,8 @@ int UavcanNode::sensor_enable(const char *bridge_name) retval = bridge->init(); if (retval >= 0) { _sensor_bridges.add(bridge); + } else { + delete bridge; } } -- cgit v1.2.3 From 29dbe8aed582f833f2994daaaf7ab227f7a7cf45 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Fri, 22 Aug 2014 14:27:32 +0400 Subject: UAVCAN magnetometer driver --- src/modules/uavcan/module.mk | 3 +- src/modules/uavcan/sensors/mag.cpp | 155 +++++++++++++++++++++++++++ src/modules/uavcan/sensors/mag.hpp | 72 +++++++++++++ src/modules/uavcan/sensors/sensor_bridge.cpp | 3 + 4 files changed, 232 insertions(+), 1 deletion(-) create mode 100644 src/modules/uavcan/sensors/mag.cpp create mode 100644 src/modules/uavcan/sensors/mag.hpp diff --git a/src/modules/uavcan/module.mk b/src/modules/uavcan/module.mk index 2b9729045..de411e1e2 100644 --- a/src/modules/uavcan/module.mk +++ b/src/modules/uavcan/module.mk @@ -49,7 +49,8 @@ SRCS += actuators/esc.cpp # Sensors SRCS += sensors/sensor_bridge.cpp \ - sensors/gnss.cpp + sensors/gnss.cpp \ + sensors/mag.cpp # # libuavcan diff --git a/src/modules/uavcan/sensors/mag.cpp b/src/modules/uavcan/sensors/mag.cpp new file mode 100644 index 000000000..41a1a5270 --- /dev/null +++ b/src/modules/uavcan/sensors/mag.cpp @@ -0,0 +1,155 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +/** + * @author Pavel Kirienko + */ + +#include "mag.hpp" + +UavcanMagnetometerBridge::UavcanMagnetometerBridge(uavcan::INode& node) : +device::CDev("uavcan_mag", "/dev/uavcan/mag"), +_sub_mag(node) +{ + _scale.x_scale = 1.0F; + _scale.y_scale = 1.0F; + _scale.z_scale = 1.0F; +} + +UavcanMagnetometerBridge::~UavcanMagnetometerBridge() +{ + if (_class_instance > 0) { + (void)unregister_class_devname(MAG_DEVICE_PATH, _class_instance); + } +} + +const char *UavcanMagnetometerBridge::get_name() const { return "mag"; } + +int UavcanMagnetometerBridge::init() +{ + // Init the libuavcan subscription + int res = _sub_mag.start(MagCbBinder(this, &UavcanMagnetometerBridge::mag_sub_cb)); + if (res < 0) { + log("failed to start uavcan sub: %d", res); + return res; + } + + // Detect our device class + _class_instance = register_class_devname(MAG_DEVICE_PATH); + switch (_class_instance) { + case CLASS_DEVICE_PRIMARY: { + _orb_id = ORB_ID(sensor_mag0); + break; + } + case CLASS_DEVICE_SECONDARY: { + _orb_id = ORB_ID(sensor_mag1); + break; + } + case CLASS_DEVICE_TERTIARY: { + _orb_id = ORB_ID(sensor_mag2); + break; + } + default: { + log("invalid class instance: %d", _class_instance); + (void)unregister_class_devname(MAG_DEVICE_PATH, _class_instance); + return -1; + } + } + + log("inited with class instance %d", _class_instance); + return 0; +} + +int UavcanMagnetometerBridge::ioctl(struct file *filp, int cmd, unsigned long arg) +{ + switch (cmd) { + case MAGIOCSSAMPLERATE: + case MAGIOCGSAMPLERATE: + case MAGIOCSRANGE: + case MAGIOCGRANGE: + case MAGIOCSLOWPASS: + case MAGIOCGLOWPASS: { + return -EINVAL; + } + case MAGIOCSSCALE: { + std::memcpy(&_scale, reinterpret_cast(arg), sizeof(_scale)); + log("new scale/offset: x: %f/%f y: %f/%f z: %f/%f", + double(_scale.x_scale), double(_scale.x_offset), + double(_scale.y_scale), double(_scale.y_offset), + double(_scale.z_scale), double(_scale.z_offset)); + return 0; + } + case MAGIOCGSCALE: { + std::memcpy(reinterpret_cast(arg), &_scale, sizeof(_scale)); + return 0; + } + case MAGIOCCALIBRATE: + case MAGIOCEXSTRAP: + case MAGIOCSELFTEST: { + return -EINVAL; + } + case MAGIOCGEXTERNAL: { + return 1; + } + default: { + return CDev::ioctl(filp, cmd, arg); + } + } +} + +void UavcanMagnetometerBridge::mag_sub_cb(const uavcan::ReceivedDataStructure &msg) +{ + auto report = ::mag_report(); + + report.range_ga = 1.3F; // Arbitrary number, doesn't really mean anything + + report.timestamp = msg.getUtcTimestamp().toUSec(); + if (report.timestamp == 0) { + report.timestamp = msg.getMonotonicTimestamp().toUSec(); + } + + report.x = (msg.magnetic_field[0] - _scale.x_offset) * _scale.x_scale; + report.y = (msg.magnetic_field[1] - _scale.x_offset) * _scale.x_scale; + report.z = (msg.magnetic_field[2] - _scale.x_offset) * _scale.x_scale; + + if (_orb_advert >= 0) { + orb_publish(_orb_id, _orb_advert, &report); + } else { + _orb_advert = orb_advertise(_orb_id, &report); + if (_orb_advert < 0) { + log("ADVERT FAIL"); + } else { + log("advertised"); + } + } +} diff --git a/src/modules/uavcan/sensors/mag.hpp b/src/modules/uavcan/sensors/mag.hpp new file mode 100644 index 000000000..7f23a0b8f --- /dev/null +++ b/src/modules/uavcan/sensors/mag.hpp @@ -0,0 +1,72 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +/** + * @author Pavel Kirienko + */ + +#pragma once + +#include "sensor_bridge.hpp" +#include +#include + +#include + +class UavcanMagnetometerBridge : public IUavcanSensorBridge, public device::CDev +{ +public: + UavcanMagnetometerBridge(uavcan::INode& node); + ~UavcanMagnetometerBridge() override; + + const char *get_name() const override; + + int init() override; + +private: + int ioctl(struct file *filp, int cmd, unsigned long arg) override; + + void mag_sub_cb(const uavcan::ReceivedDataStructure &msg); + + typedef uavcan::MethodBinder&)> + MagCbBinder; + + + uavcan::Subscriber _sub_mag; + mag_scale _scale = {}; + orb_id_t _orb_id = nullptr; + orb_advert_t _orb_advert = -1; + int _class_instance = -1; +}; diff --git a/src/modules/uavcan/sensors/sensor_bridge.cpp b/src/modules/uavcan/sensors/sensor_bridge.cpp index ba27d6c71..08fca73c5 100644 --- a/src/modules/uavcan/sensors/sensor_bridge.cpp +++ b/src/modules/uavcan/sensors/sensor_bridge.cpp @@ -37,11 +37,14 @@ #include "sensor_bridge.hpp" #include "gnss.hpp" +#include "mag.hpp" IUavcanSensorBridge* IUavcanSensorBridge::make(uavcan::INode &node, const char *bridge_name) { if (!std::strncmp("gnss", bridge_name, MaxNameLen)) { return new UavcanGnssBridge(node); + } else if (!std::strncmp("mag", bridge_name, MaxNameLen)) { + return new UavcanMagnetometerBridge(node); } else { return nullptr; } -- cgit v1.2.3 From e32ff6004be5d7bbd4b94b437b6deaa770618259 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Fri, 22 Aug 2014 15:31:08 +0400 Subject: UAVCAN mag driver fix --- src/modules/uavcan/sensors/mag.cpp | 30 ++++++++++++++---------------- 1 file changed, 14 insertions(+), 16 deletions(-) diff --git a/src/modules/uavcan/sensors/mag.cpp b/src/modules/uavcan/sensors/mag.cpp index 41a1a5270..ac43ea1e3 100644 --- a/src/modules/uavcan/sensors/mag.cpp +++ b/src/modules/uavcan/sensors/mag.cpp @@ -93,33 +93,31 @@ int UavcanMagnetometerBridge::init() int UavcanMagnetometerBridge::ioctl(struct file *filp, int cmd, unsigned long arg) { switch (cmd) { - case MAGIOCSSAMPLERATE: - case MAGIOCGSAMPLERATE: - case MAGIOCSRANGE: - case MAGIOCGRANGE: - case MAGIOCSLOWPASS: - case MAGIOCGLOWPASS: { - return -EINVAL; - } case MAGIOCSSCALE: { std::memcpy(&_scale, reinterpret_cast(arg), sizeof(_scale)); - log("new scale/offset: x: %f/%f y: %f/%f z: %f/%f", - double(_scale.x_scale), double(_scale.x_offset), - double(_scale.y_scale), double(_scale.y_offset), - double(_scale.z_scale), double(_scale.z_offset)); return 0; } case MAGIOCGSCALE: { std::memcpy(reinterpret_cast(arg), &_scale, sizeof(_scale)); return 0; } - case MAGIOCCALIBRATE: - case MAGIOCEXSTRAP: case MAGIOCSELFTEST: { - return -EINVAL; + return 0; // Nothing to do } case MAGIOCGEXTERNAL: { - return 1; + return 0; // We don't want anyone to transform the coordinate frame, so we declare it onboard + } + case MAGIOCSSAMPLERATE: { + return 0; // Pretend that this stuff is supported to keep the sensor app happy + } + case MAGIOCCALIBRATE: + case MAGIOCGSAMPLERATE: + case MAGIOCSRANGE: + case MAGIOCGRANGE: + case MAGIOCSLOWPASS: + case MAGIOCEXSTRAP: + case MAGIOCGLOWPASS: { + return -EINVAL; } default: { return CDev::ioctl(filp, cmd, arg); -- cgit v1.2.3 From bdc2ecd9f6d0ae3e66feb8a8e94391b606ee451e Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Fri, 22 Aug 2014 15:41:21 +0400 Subject: Too much Ctrl+C Ctrl+V --- src/modules/uavcan/sensors/mag.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/uavcan/sensors/mag.cpp b/src/modules/uavcan/sensors/mag.cpp index ac43ea1e3..6be9e9bac 100644 --- a/src/modules/uavcan/sensors/mag.cpp +++ b/src/modules/uavcan/sensors/mag.cpp @@ -137,8 +137,8 @@ void UavcanMagnetometerBridge::mag_sub_cb(const uavcan::ReceivedDataStructure= 0) { orb_publish(_orb_id, _orb_advert, &report); -- cgit v1.2.3 From 6ebd59c633db0d610f63eeb06c5c867da34740e0 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Fri, 22 Aug 2014 15:52:35 +0400 Subject: UAVCAN: improved sensor bridge factory --- src/modules/uavcan/sensors/gnss.cpp | 4 ++-- src/modules/uavcan/sensors/gnss.hpp | 4 +++- src/modules/uavcan/sensors/mag.cpp | 4 ++-- src/modules/uavcan/sensors/mag.hpp | 7 ++++--- src/modules/uavcan/sensors/sensor_bridge.cpp | 4 ++-- src/modules/uavcan/sensors/sensor_bridge.hpp | 2 +- src/modules/uavcan/uavcan_main.cpp | 4 ++-- 7 files changed, 16 insertions(+), 13 deletions(-) diff --git a/src/modules/uavcan/sensors/gnss.cpp b/src/modules/uavcan/sensors/gnss.cpp index fb89b8365..6b69d163f 100644 --- a/src/modules/uavcan/sensors/gnss.cpp +++ b/src/modules/uavcan/sensors/gnss.cpp @@ -45,6 +45,8 @@ #define MM_PER_CM 10 // Millimeters per centimeter +const char *const UavcanGnssBridge::NAME = "gnss"; + UavcanGnssBridge::UavcanGnssBridge(uavcan::INode &node) : _node(node), _sub_fix(node), @@ -52,8 +54,6 @@ _report_pub(-1) { } -const char *UavcanGnssBridge::get_name() const { return "gnss"; } - int UavcanGnssBridge::init() { int res = -1; diff --git a/src/modules/uavcan/sensors/gnss.hpp b/src/modules/uavcan/sensors/gnss.hpp index 6bdae8de3..db3a515fa 100644 --- a/src/modules/uavcan/sensors/gnss.hpp +++ b/src/modules/uavcan/sensors/gnss.hpp @@ -56,9 +56,11 @@ class UavcanGnssBridge : public IUavcanSensorBridge { public: + static const char *const NAME; + UavcanGnssBridge(uavcan::INode& node); - const char *get_name() const override; + const char *get_name() const override { return NAME; } int init() override; diff --git a/src/modules/uavcan/sensors/mag.cpp b/src/modules/uavcan/sensors/mag.cpp index 6be9e9bac..4f8a5e104 100644 --- a/src/modules/uavcan/sensors/mag.cpp +++ b/src/modules/uavcan/sensors/mag.cpp @@ -37,6 +37,8 @@ #include "mag.hpp" +const char *const UavcanMagnetometerBridge::NAME = "mag"; + UavcanMagnetometerBridge::UavcanMagnetometerBridge(uavcan::INode& node) : device::CDev("uavcan_mag", "/dev/uavcan/mag"), _sub_mag(node) @@ -53,8 +55,6 @@ UavcanMagnetometerBridge::~UavcanMagnetometerBridge() } } -const char *UavcanMagnetometerBridge::get_name() const { return "mag"; } - int UavcanMagnetometerBridge::init() { // Init the libuavcan subscription diff --git a/src/modules/uavcan/sensors/mag.hpp b/src/modules/uavcan/sensors/mag.hpp index 7f23a0b8f..4bc5129a2 100644 --- a/src/modules/uavcan/sensors/mag.hpp +++ b/src/modules/uavcan/sensors/mag.hpp @@ -46,10 +46,12 @@ class UavcanMagnetometerBridge : public IUavcanSensorBridge, public device::CDev { public: + static const char *const NAME; + UavcanMagnetometerBridge(uavcan::INode& node); ~UavcanMagnetometerBridge() override; - const char *get_name() const override; + const char *get_name() const override { return NAME; } int init() override; @@ -63,8 +65,7 @@ private: (const uavcan::ReceivedDataStructure&)> MagCbBinder; - - uavcan::Subscriber _sub_mag; + uavcan::Subscriber _sub_mag; mag_scale _scale = {}; orb_id_t _orb_id = nullptr; orb_advert_t _orb_advert = -1; diff --git a/src/modules/uavcan/sensors/sensor_bridge.cpp b/src/modules/uavcan/sensors/sensor_bridge.cpp index 08fca73c5..2bd662d5c 100644 --- a/src/modules/uavcan/sensors/sensor_bridge.cpp +++ b/src/modules/uavcan/sensors/sensor_bridge.cpp @@ -41,9 +41,9 @@ IUavcanSensorBridge* IUavcanSensorBridge::make(uavcan::INode &node, const char *bridge_name) { - if (!std::strncmp("gnss", bridge_name, MaxNameLen)) { + if (!std::strncmp(UavcanGnssBridge::NAME, bridge_name, MAX_NAME_LEN)) { return new UavcanGnssBridge(node); - } else if (!std::strncmp("mag", bridge_name, MaxNameLen)) { + } else if (!std::strncmp(UavcanMagnetometerBridge::NAME, bridge_name, MAX_NAME_LEN)) { return new UavcanMagnetometerBridge(node); } else { return nullptr; diff --git a/src/modules/uavcan/sensors/sensor_bridge.hpp b/src/modules/uavcan/sensors/sensor_bridge.hpp index 1667c2b57..7bd811813 100644 --- a/src/modules/uavcan/sensors/sensor_bridge.hpp +++ b/src/modules/uavcan/sensors/sensor_bridge.hpp @@ -46,7 +46,7 @@ class IUavcanSensorBridge : uavcan::Noncopyable, public ListNode { public: - static constexpr unsigned MaxNameLen = 20; + static constexpr unsigned MAX_NAME_LEN = 20; virtual ~IUavcanSensorBridge() { } diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index 71302d928..aca4587ff 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -535,7 +535,7 @@ int UavcanNode::sensor_enable(const char *bridge_name) { auto pos = _sensor_bridges.getHead(); while (pos != nullptr) { - if (!std::strncmp(pos->get_name(), bridge_name, IUavcanSensorBridge::MaxNameLen)) { + if (!std::strncmp(pos->get_name(), bridge_name, IUavcanSensorBridge::MAX_NAME_LEN)) { warnx("sensor bridge '%s' already exists", bridge_name); retval = -1; goto leave; @@ -553,7 +553,7 @@ int UavcanNode::sensor_enable(const char *bridge_name) goto leave; } - assert(!std::strncmp(bridge->get_name(), bridge_name, IUavcanSensorBridge::MaxNameLen)); + assert(!std::strncmp(bridge->get_name(), bridge_name, IUavcanSensorBridge::MAX_NAME_LEN)); retval = bridge->init(); if (retval >= 0) { -- cgit v1.2.3 From 0f7c3926143a917b3e444f0f2c6c2cb673f073e4 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 22 Aug 2014 14:30:34 +0200 Subject: Initialize altitude is relative field for drop locations --- src/modules/bottle_drop/bottle_drop.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/modules/bottle_drop/bottle_drop.cpp b/src/modules/bottle_drop/bottle_drop.cpp index 5d2343cfb..8e96c3e09 100644 --- a/src/modules/bottle_drop/bottle_drop.cpp +++ b/src/modules/bottle_drop/bottle_drop.cpp @@ -404,16 +404,18 @@ BottleDrop::task_main() memset(&update, 0, sizeof(update)); int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update)); - struct mission_item_s flight_vector_s; - struct mission_item_s flight_vector_e; + struct mission_item_s flight_vector_s {}; + struct mission_item_s flight_vector_e {}; flight_vector_s.nav_cmd = NAV_CMD_WAYPOINT; flight_vector_s.acceptance_radius = 50; // TODO: make parameter flight_vector_s.autocontinue = true; + flight_vector_s.altitude_is_relative = false; flight_vector_e.nav_cmd = NAV_CMD_WAYPOINT; flight_vector_e.acceptance_radius = 50; // TODO: make parameter flight_vector_e.autocontinue = true; + flight_vector_s.altitude_is_relative = false; struct mission_s onboard_mission; memset(&onboard_mission, 0, sizeof(onboard_mission)); -- cgit v1.2.3 From bdd4f028eec79f09335f888b4857db44968aa07d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 22 Aug 2014 14:36:31 +0200 Subject: Fixed copy-paste errors in drop waypoint coordinates --- src/modules/bottle_drop/bottle_drop.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/bottle_drop/bottle_drop.cpp b/src/modules/bottle_drop/bottle_drop.cpp index 058e90829..5b7f7bd7f 100644 --- a/src/modules/bottle_drop/bottle_drop.cpp +++ b/src/modules/bottle_drop/bottle_drop.cpp @@ -587,10 +587,10 @@ BottleDrop::task_main() // Compute flight vector - map_projection_reproject(&ref, x_drop + 2 * turn_radius * wind_direction_n, y_drop + 2 * turn_radius * wind_direction_n, + map_projection_reproject(&ref, x_drop + 2 * turn_radius * wind_direction_n, y_drop + 2 * turn_radius * wind_direction_e, &(flight_vector_s.lat), &(flight_vector_s.lon)); flight_vector_s.altitude = _drop_position.alt; - map_projection_reproject(&ref, x_drop - turn_radius * wind_direction_e, y_drop - turn_radius * wind_direction_e, + map_projection_reproject(&ref, x_drop - turn_radius * wind_direction_n, y_drop - turn_radius * wind_direction_e, &flight_vector_e.lat, &flight_vector_e.lon); flight_vector_e.altitude = _drop_position.alt; //warnx("Flight vector: starting point = %.7f %.7f , end point = %.7f %.7f", flight_vector_s.lat,flight_vector_s.lon,flight_vector_e.lat,flight_vector_e.lon); //////////////////////////////////////////////////////////////////// DEBUGGING -- cgit v1.2.3 From 3f37efc046c9f3d2d6ee4a39f67ed5e20a6866ac Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 22 Aug 2014 14:41:35 +0200 Subject: Fix a bunch of stupid mistakes on coordinates / distances, set number of waypoints to zero once reached --- src/modules/bottle_drop/bottle_drop.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/modules/bottle_drop/bottle_drop.cpp b/src/modules/bottle_drop/bottle_drop.cpp index 81d1238cf..2bd1f679b 100644 --- a/src/modules/bottle_drop/bottle_drop.cpp +++ b/src/modules/bottle_drop/bottle_drop.cpp @@ -636,7 +636,7 @@ BottleDrop::task_main() case DROP_STATE_TARGET_SET: { - float distance_wp2 = get_distance_to_next_waypoint(flight_vector_e.lat, flight_vector_e.lon, _drop_position.lat, _drop_position.lon); + float distance_wp2 = get_distance_to_next_waypoint(_global_pos.lat, _global_pos.lon, flight_vector_e.lat, flight_vector_e.lon); if (distance_wp2 < distance_real) { _onboard_mission.current_seq = 0; @@ -674,7 +674,7 @@ BottleDrop::task_main() mavlink_log_info(_mavlink_fd, "#audio: payload dropped"); } else { - float distance_wp2 = get_distance_to_next_waypoint(flight_vector_e.lat, flight_vector_e.lon, _drop_position.lat, _drop_position.lon); + float distance_wp2 = get_distance_to_next_waypoint(_global_pos.lat, _global_pos.lon, flight_vector_e.lat, flight_vector_e.lon); if (distance_wp2 < distance_real) { _onboard_mission.current_seq = 0; @@ -696,6 +696,7 @@ BottleDrop::task_main() // remove onboard mission _onboard_mission.current_seq = -1; + _onboard_mission.count = 0; orb_publish(ORB_ID(onboard_mission), _onboard_mission_pub, &_onboard_mission); } break; -- cgit v1.2.3 From 6d80ebfc8933091c1f2cf70a42107bf80d3db74f Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 22 Aug 2014 16:09:20 +0200 Subject: update mavlink version --- mavlink/include/mavlink/v1.0 | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mavlink/include/mavlink/v1.0 b/mavlink/include/mavlink/v1.0 index 4d7487c2b..c8a6e736e 160000 --- a/mavlink/include/mavlink/v1.0 +++ b/mavlink/include/mavlink/v1.0 @@ -1 +1 @@ -Subproject commit 4d7487c2bc5f5ccf87bca82970fb2c08d6d8bd50 +Subproject commit c8a6e736e2bf71e14b023cf26733c3499d6d515b -- cgit v1.2.3 From d604985e080f1c04278f5f28acf0eaaf3b334041 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 22 Aug 2014 17:22:35 +0200 Subject: Bottle drop: Fix signs and comments --- src/modules/bottle_drop/bottle_drop.cpp | 48 +++++++-------------------------- 1 file changed, 9 insertions(+), 39 deletions(-) diff --git a/src/modules/bottle_drop/bottle_drop.cpp b/src/modules/bottle_drop/bottle_drop.cpp index 2bd1f679b..3eec5a879 100644 --- a/src/modules/bottle_drop/bottle_drop.cpp +++ b/src/modules/bottle_drop/bottle_drop.cpp @@ -462,7 +462,7 @@ BottleDrop::task_main() hrt_abstime last_run = hrt_absolute_time(); float dt_runs = 1e6f / sleeptime_us; - /* switch to faster updates during the drop */ + // switch to faster updates during the drop while (_drop_state > DROP_STATE_INIT) { // Get wind estimate @@ -474,17 +474,17 @@ BottleDrop::task_main() // Get vehicle position orb_check(vehicle_global_position_sub, &updated); if (updated) { - /* copy global position */ + // copy global position orb_copy(ORB_ID(vehicle_global_position), vehicle_global_position_sub, &_global_pos); } // Get parameter updates orb_check(parameter_update_sub, &updated); if (updated) { - /* copy global position */ + // copy global position orb_copy(ORB_ID(parameter_update), parameter_update_sub, &update); - /* update all parameters */ + // update all parameters param_get(param_gproperties, &z_0); param_get(param_turn_radius, &turn_radius); param_get(param_precision, &precision); @@ -500,20 +500,10 @@ BottleDrop::task_main() mavlink_log_info(_mavlink_fd, "#audio: drop distance %.2f", (double)distance_real); } - //warnx("absolut wind speed = %.4f", vr); //////////////////////////////////////////////////////////////////// DEBUGGING - - - //warnx("Initialization complete\n"); //////////////////////////////////////////////////////////////////// DEBUGGING - switch (_drop_state) { case DROP_STATE_TARGET_VALID: { - //warnx("approval given\n"); //////////////////////////////////////////////////////////////////// DEBUGGING - // drop here - //open_now = true; - //drop = false; - //drop_start = hrt_absolute_time(); // Update drop point at 10 Hz if (counter % 10 == 0) { @@ -547,18 +537,18 @@ BottleDrop::task_main() x = x + vx * dt_freefall_prediction; vrx = vx + vw; - //Drag force + // drag force v = sqrtf(vz * vz + vrx * vrx); Fd = 0.5f * rho * A * cd * (v * v); Fdx = Fd * vrx / v; Fdz = Fd * vz / v; - //acceleration + // acceleration az = g - Fdz / m; ax = -Fdx / m; } - // Compute drop vector + // compute drop vector x = groundspeed_body * t_signal + x; } @@ -580,9 +570,6 @@ BottleDrop::task_main() y_drop = y_t + x * wind_direction_e; map_projection_reproject(&ref, x_drop, y_drop, &_drop_position.lat, &_drop_position.lon); _drop_position.alt = _target_position.alt + _alt_clearance; - //warnx("drop point: lat = %.7f , lon = %.7f", drop_position.lat, drop_position.lon); //////////////////////////////////////////////////////////////////// DEBUGGING - - // Compute flight vector map_projection_reproject(&ref, x_drop + 2 * turn_radius * wind_direction_n, y_drop + 2 * turn_radius * wind_direction_e, @@ -591,25 +578,8 @@ BottleDrop::task_main() map_projection_reproject(&ref, x_drop - turn_radius * wind_direction_n, y_drop - turn_radius * wind_direction_e, &flight_vector_e.lat, &flight_vector_e.lon); flight_vector_e.altitude = _drop_position.alt; - //warnx("Flight vector: starting point = %.7f %.7f , end point = %.7f %.7f", flight_vector_s.lat,flight_vector_s.lon,flight_vector_e.lat,flight_vector_e.lon); //////////////////////////////////////////////////////////////////// DEBUGGING - - // Drop Cancellation if terms are not met - - // warnx("latitude:%.2f", _global_pos.lat); - // warnx("longitude:%.2f", _global_pos.lon); - // warnx("drop_position.lat:%.2f", drop_position.lat); - // warnx("drop_position.lon:%.2f", drop_position.lon); - - //warnx("position to-be: = %.7f %.7f" ,x_f_NED, y_f_NED ); //////////////////////////////////////////////////////////////////// DEBUGGING - - // if (counter % 10 ==0) { - // warnx("x: %.4f", x); - // warnx("drop_position.lat: %.4f, drop_position.lon: %.4f", drop_position.lat, drop_position.lon); - // warnx("latitude %.4f, longitude: %.4f", _global_pos.lat, _global_pos.lon); - // warnx("future_distance: %.2f, precision: %.2f", future_distance, precision); - // } - /* Save WPs in datamanager */ + // Save WPs in datamanager const ssize_t len = sizeof(struct mission_item_s); if (dm_write(DM_KEY_WAYPOINTS_ONBOARD, 0, DM_PERSIST_IN_FLIGHT_RESET, &flight_vector_s, len) != len) { @@ -662,7 +632,7 @@ BottleDrop::task_main() x_f = x_l + _global_pos.vel_n * dt_runs; y_f = y_l + _global_pos.vel_e * dt_runs; map_projection_reproject(&ref, x_f, y_f, &x_f_NED, &y_f_NED); - future_distance = fabsf(get_distance_to_next_waypoint(x_f_NED, y_f_NED, _drop_position.lat, _drop_position.lon)); + future_distance = get_distance_to_next_waypoint(x_f_NED, y_f_NED, _drop_position.lat, _drop_position.lon); warnx("Distance real: %.2f", (double)distance_real); -- cgit v1.2.3 From 2a6ab537b2f8687fdb125d9e5d46338ff85220ad Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Fri, 22 Aug 2014 20:04:31 +0400 Subject: UAVCAN update --- uavcan | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/uavcan b/uavcan index 446577e4f..6c070852d 160000 --- a/uavcan +++ b/uavcan @@ -1 +1 @@ -Subproject commit 446577e4fb979ee4c629081368233eaa5683d086 +Subproject commit 6c070852d76b18c2d57612f66b5bd00e63118c94 -- cgit v1.2.3 From 6870cd4d3d68941945d303b707c4b05bd5d1e6e4 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Fri, 22 Aug 2014 20:15:04 +0400 Subject: UAVCAN baro driver --- src/modules/uavcan/module.mk | 3 +- src/modules/uavcan/sensors/baro.cpp | 147 +++++++++++++++++++++++++++ src/modules/uavcan/sensors/baro.hpp | 73 +++++++++++++ src/modules/uavcan/sensors/sensor_bridge.cpp | 3 + 4 files changed, 225 insertions(+), 1 deletion(-) create mode 100644 src/modules/uavcan/sensors/baro.cpp create mode 100644 src/modules/uavcan/sensors/baro.hpp diff --git a/src/modules/uavcan/module.mk b/src/modules/uavcan/module.mk index de411e1e2..26ff7102d 100644 --- a/src/modules/uavcan/module.mk +++ b/src/modules/uavcan/module.mk @@ -50,7 +50,8 @@ SRCS += actuators/esc.cpp # Sensors SRCS += sensors/sensor_bridge.cpp \ sensors/gnss.cpp \ - sensors/mag.cpp + sensors/mag.cpp \ + sensors/baro.cpp # # libuavcan diff --git a/src/modules/uavcan/sensors/baro.cpp b/src/modules/uavcan/sensors/baro.cpp new file mode 100644 index 000000000..3afcc3f1c --- /dev/null +++ b/src/modules/uavcan/sensors/baro.cpp @@ -0,0 +1,147 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +/** + * @author Pavel Kirienko + */ + +#include "baro.hpp" +#include + +const char *const UavcanBarometerBridge::NAME = "baro"; + +UavcanBarometerBridge::UavcanBarometerBridge(uavcan::INode& node) : +device::CDev("uavcan_baro", "/dev/uavcan/baro"), +_sub_air_data(node) +{ +} + +UavcanBarometerBridge::~UavcanBarometerBridge() +{ + if (_class_instance > 0) { + (void)unregister_class_devname(BARO_DEVICE_PATH, _class_instance); + } +} + +int UavcanBarometerBridge::init() +{ + // Init the libuavcan subscription + int res = _sub_air_data.start(AirDataCbBinder(this, &UavcanBarometerBridge::air_data_sub_cb)); + if (res < 0) { + log("failed to start uavcan sub: %d", res); + return res; + } + + // Detect our device class + _class_instance = register_class_devname(BARO_DEVICE_PATH); + switch (_class_instance) { + case CLASS_DEVICE_PRIMARY: { + _orb_id = ORB_ID(sensor_baro0); + break; + } + case CLASS_DEVICE_SECONDARY: { + _orb_id = ORB_ID(sensor_baro1); + break; + } + default: { + log("invalid class instance: %d", _class_instance); + (void)unregister_class_devname(BARO_DEVICE_PATH, _class_instance); + return -1; + } + } + + log("inited with class instance %d", _class_instance); + return 0; +} + +int UavcanBarometerBridge::ioctl(struct file *filp, int cmd, unsigned long arg) +{ + switch (cmd) { + case BAROIOCSMSLPRESSURE: { + if ((arg < 80000) || (arg > 120000)) { + return -EINVAL; + } else { + log("new msl pressure %u", _msl_pressure); + _msl_pressure = arg; + return OK; + } + } + case BAROIOCGMSLPRESSURE: { + return _msl_pressure; + } + default: { + return CDev::ioctl(filp, cmd, arg); + } + } +} + +void UavcanBarometerBridge::air_data_sub_cb(const uavcan::ReceivedDataStructure &msg) +{ + auto report = ::baro_report(); + + report.timestamp = msg.getUtcTimestamp().toUSec(); + if (report.timestamp == 0) { + report.timestamp = msg.getMonotonicTimestamp().toUSec(); + } + + report.temperature = msg.static_temperature; + report.pressure = msg.static_pressure / 100.0F; // Convert to millibar + + /* + * Altitude computation + * Refer to the MS5611 driver for details + */ + const double T1 = 15.0 + 273.15; // temperature at base height in Kelvin + const double a = -6.5 / 1000; // temperature gradient in degrees per metre + const double g = 9.80665; // gravity constant in m/s/s + const double R = 287.05; // ideal gas constant in J/kg/K + + const double p1 = _msl_pressure / 1000.0; // current pressure at MSL in kPa + const double p = double(msg.static_pressure) / 1000.0; // measured pressure in kPa + + report.altitude = (((std::pow((p / p1), (-(a * R) / g))) * T1) - T1) / a; + + /* + * Publish + */ + if (_orb_advert >= 0) { + orb_publish(_orb_id, _orb_advert, &report); + } else { + _orb_advert = orb_advertise(_orb_id, &report); + if (_orb_advert < 0) { + log("ADVERT FAIL"); + } else { + log("advertised"); + } + } +} diff --git a/src/modules/uavcan/sensors/baro.hpp b/src/modules/uavcan/sensors/baro.hpp new file mode 100644 index 000000000..f6aa01216 --- /dev/null +++ b/src/modules/uavcan/sensors/baro.hpp @@ -0,0 +1,73 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +/** + * @author Pavel Kirienko + */ + +#pragma once + +#include "sensor_bridge.hpp" +#include +#include + +#include + +class UavcanBarometerBridge : public IUavcanSensorBridge, public device::CDev +{ +public: + static const char *const NAME; + + UavcanBarometerBridge(uavcan::INode& node); + ~UavcanBarometerBridge() override; + + const char *get_name() const override { return NAME; } + + int init() override; + +private: + int ioctl(struct file *filp, int cmd, unsigned long arg) override; + + void air_data_sub_cb(const uavcan::ReceivedDataStructure &msg); + + typedef uavcan::MethodBinder&)> + AirDataCbBinder; + + uavcan::Subscriber _sub_air_data; + unsigned _msl_pressure = 101325; + orb_id_t _orb_id = nullptr; + orb_advert_t _orb_advert = -1; + int _class_instance = -1; +}; diff --git a/src/modules/uavcan/sensors/sensor_bridge.cpp b/src/modules/uavcan/sensors/sensor_bridge.cpp index 2bd662d5c..f826c8fd2 100644 --- a/src/modules/uavcan/sensors/sensor_bridge.cpp +++ b/src/modules/uavcan/sensors/sensor_bridge.cpp @@ -38,6 +38,7 @@ #include "sensor_bridge.hpp" #include "gnss.hpp" #include "mag.hpp" +#include "baro.hpp" IUavcanSensorBridge* IUavcanSensorBridge::make(uavcan::INode &node, const char *bridge_name) { @@ -45,6 +46,8 @@ IUavcanSensorBridge* IUavcanSensorBridge::make(uavcan::INode &node, const char * return new UavcanGnssBridge(node); } else if (!std::strncmp(UavcanMagnetometerBridge::NAME, bridge_name, MAX_NAME_LEN)) { return new UavcanMagnetometerBridge(node); + } else if (!std::strncmp(UavcanBarometerBridge::NAME, bridge_name, MAX_NAME_LEN)) { + return new UavcanBarometerBridge(node); } else { return nullptr; } -- cgit v1.2.3 From 7132141cc478e1b9cdde41207c03f2c622f7831a Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Fri, 22 Aug 2014 20:33:35 +0400 Subject: UAVCAN: Printing all known sensor bridge names with usage info --- src/modules/uavcan/sensors/sensor_bridge.cpp | 12 ++++++++++++ src/modules/uavcan/sensors/sensor_bridge.hpp | 5 +++++ src/modules/uavcan/uavcan_main.cpp | 3 +++ 3 files changed, 20 insertions(+) diff --git a/src/modules/uavcan/sensors/sensor_bridge.cpp b/src/modules/uavcan/sensors/sensor_bridge.cpp index f826c8fd2..5752d5524 100644 --- a/src/modules/uavcan/sensors/sensor_bridge.cpp +++ b/src/modules/uavcan/sensors/sensor_bridge.cpp @@ -36,12 +36,17 @@ */ #include "sensor_bridge.hpp" +#include + #include "gnss.hpp" #include "mag.hpp" #include "baro.hpp" IUavcanSensorBridge* IUavcanSensorBridge::make(uavcan::INode &node, const char *bridge_name) { + /* + * TODO: make a linked list of known implementations at startup? + */ if (!std::strncmp(UavcanGnssBridge::NAME, bridge_name, MAX_NAME_LEN)) { return new UavcanGnssBridge(node); } else if (!std::strncmp(UavcanMagnetometerBridge::NAME, bridge_name, MAX_NAME_LEN)) { @@ -52,3 +57,10 @@ IUavcanSensorBridge* IUavcanSensorBridge::make(uavcan::INode &node, const char * return nullptr; } } + +void IUavcanSensorBridge::print_known_names(const char *prefix) +{ + printf("%s%s\n", prefix, UavcanGnssBridge::NAME); + printf("%s%s\n", prefix, UavcanMagnetometerBridge::NAME); + printf("%s%s\n", prefix, UavcanBarometerBridge::NAME); +} diff --git a/src/modules/uavcan/sensors/sensor_bridge.hpp b/src/modules/uavcan/sensors/sensor_bridge.hpp index 7bd811813..976d9a03d 100644 --- a/src/modules/uavcan/sensors/sensor_bridge.hpp +++ b/src/modules/uavcan/sensors/sensor_bridge.hpp @@ -67,4 +67,9 @@ public: * @return nullptr if such bridge can't be created. */ static IUavcanSensorBridge* make(uavcan::INode &node, const char *bridge_name); + + /** + * Prints all valid bridge names into stdout via printf(), one name per line with prefix. + */ + static void print_known_names(const char *prefix); }; diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index aca4587ff..8607af145 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -590,6 +590,9 @@ static void print_usage() "\tuavcan start [can_bitrate]\n" "\tuavcan sensor enable \n" "\tuavcan sensor list"); + + warnx("known sensor bridges:"); + IUavcanSensorBridge::print_known_names("\t"); } extern "C" __EXPORT int uavcan_main(int argc, char *argv[]); -- cgit v1.2.3 From 406a52e6399b171cfe454b85774f12f9add40aee Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 22 Aug 2014 20:24:04 +0200 Subject: fix typo --- src/modules/commander/commander.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 109ab403a..243546787 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1314,7 +1314,7 @@ int commander_thread_main(int argc, char *argv[]) if (pos_sp_triplet.geofence_violated || pos_sp_triplet.flight_termination) { //XXX: make this configurable to select different actions (e.g. navigation modes) /* this will only trigger if geofence is activated via param and a geofence file is present, also there is a circuit breaker to disable the actual flight termination in the px4io driver */ - armed.fosrce_failsafe = true; + armed.force_failsafe = true; status_changed = true; warnx("Flight termination"); } // no reset is done here on purpose, on geofence violation we want to stay in flighttermination -- cgit v1.2.3 From 708ee8ae3aa4d4d5eac9e2fd86ed9673c126fe33 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 22 Aug 2014 20:30:06 +0200 Subject: autolaunch: added param for delay --- src/lib/launchdetection/CatapultLaunchMethod.cpp | 19 ++++++++++++++++--- src/lib/launchdetection/CatapultLaunchMethod.h | 4 ++++ src/lib/launchdetection/launchdetection_params.c | 9 +++++++++ 3 files changed, 29 insertions(+), 3 deletions(-) diff --git a/src/lib/launchdetection/CatapultLaunchMethod.cpp b/src/lib/launchdetection/CatapultLaunchMethod.cpp index c555a0a69..c4425bbe0 100644 --- a/src/lib/launchdetection/CatapultLaunchMethod.cpp +++ b/src/lib/launchdetection/CatapultLaunchMethod.cpp @@ -51,7 +51,8 @@ CatapultLaunchMethod::CatapultLaunchMethod(SuperBlock *parent) : integrator(0.0f), launchDetected(false), threshold_accel(this, "A"), - threshold_time(this, "T") + threshold_time(this, "T"), + delay(this, "DELAY") { } @@ -78,21 +79,33 @@ void CatapultLaunchMethod::update(float accel_x) // (double)integrator, (double)threshold_accel.get(), (double)threshold_time.get(), (double)accel_x, (double)dt); /* reset integrator */ integrator = 0.0f; - launchDetected = false; + } + + if (launchDetected) { + delayCounter += dt; + if (delayCounter > delay.get()) { + delayPassed = true; + } } } bool CatapultLaunchMethod::getLaunchDetected() { - return launchDetected; + if (delay.get() > 0.0f) { + return delayPassed; + } else { + return launchDetected; + } } void CatapultLaunchMethod::reset() { integrator = 0.0f; + delayCounter = 0.0f; launchDetected = false; + delayPassed = false; } } diff --git a/src/lib/launchdetection/CatapultLaunchMethod.h b/src/lib/launchdetection/CatapultLaunchMethod.h index 23757f6f3..a64d482f6 100644 --- a/src/lib/launchdetection/CatapultLaunchMethod.h +++ b/src/lib/launchdetection/CatapultLaunchMethod.h @@ -63,10 +63,14 @@ public: private: hrt_abstime last_timestamp; float integrator; + float delayCounter; + bool launchDetected; + bool delayPassed; control::BlockParamFloat threshold_accel; control::BlockParamFloat threshold_time; + control::BlockParamFloat delay; }; diff --git a/src/lib/launchdetection/launchdetection_params.c b/src/lib/launchdetection/launchdetection_params.c index 8df8c696c..02f01bc66 100644 --- a/src/lib/launchdetection/launchdetection_params.c +++ b/src/lib/launchdetection/launchdetection_params.c @@ -77,6 +77,15 @@ PARAM_DEFINE_FLOAT(LAUN_CAT_A, 30.0f); */ PARAM_DEFINE_FLOAT(LAUN_CAT_T, 0.05f); +/** + * Catapult delay + * + * + * + * @min 0 + * @group Launch detection + */ +PARAM_DEFINE_FLOAT(LAUN_CAT_DELAY, 0.0f); /** * Throttle setting while detecting launch. * -- cgit v1.2.3 From b22acadb8a255cfdf7a8c77ea0ea84587ff7d5e0 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 22 Aug 2014 21:02:33 +0200 Subject: DL loss && gps fail: flight termination --- src/modules/commander/commander.cpp | 13 ++++++++++++- 1 file changed, 12 insertions(+), 1 deletion(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 243546787..8ae5a441a 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1316,7 +1316,7 @@ int commander_thread_main(int argc, char *argv[]) /* this will only trigger if geofence is activated via param and a geofence file is present, also there is a circuit breaker to disable the actual flight termination in the px4io driver */ armed.force_failsafe = true; status_changed = true; - warnx("Flight termination"); + warnx("Flight termination because of navigator request or geofence"); } // no reset is done here on purpose, on geofence violation we want to stay in flighttermination } @@ -1579,6 +1579,17 @@ int commander_thread_main(int argc, char *argv[]) } } + /* At this point the data link and the gps system have been checked + * If both failed we want to terminate the flight */ + if ((status.data_link_lost && status.gps_failure) || + (status.data_link_lost_cmd && status.gps_failure_cmd)) { + armed.force_failsafe = true; + status_changed = true; + warnx("Flight termination because of data link loss && gps failure"); + mavlink_log_critical(mavlink_fd, "DL and GPS lost: flight termination"); + } + + hrt_abstime t1 = hrt_absolute_time(); /* print new state */ -- cgit v1.2.3 From d50135b611605891cb1fb8841490c41474c3ec31 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 22 Aug 2014 21:40:58 +0200 Subject: rc loss && gps loss: flight termination --- src/modules/commander/commander.cpp | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 8ae5a441a..9968ab8e6 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1579,6 +1579,7 @@ int commander_thread_main(int argc, char *argv[]) } } + /* Check for failure combinations which lead to flight termination */ /* At this point the data link and the gps system have been checked * If both failed we want to terminate the flight */ if ((status.data_link_lost && status.gps_failure) || @@ -1589,6 +1590,21 @@ int commander_thread_main(int argc, char *argv[]) mavlink_log_critical(mavlink_fd, "DL and GPS lost: flight termination"); } + /* At this point the rc signal and the gps system have been checked + * If we are in manual (controlled with RC): + * if both failed we want to terminate the flight */ + if ((status.main_state == MAIN_STATE_ACRO || + status.main_state == MAIN_STATE_MANUAL || + status.main_state == MAIN_STATE_ALTCTL || + status.main_state == MAIN_STATE_POSCTL) && + ((status.rc_signal_lost && status.rc_signal_lost) || + (status.rc_signal_lost && status.gps_failure_cmd))) { + armed.force_failsafe = true; + status_changed = true; + warnx("Flight termination because of RC signal loss && gps failure"); + mavlink_log_critical(mavlink_fd, "RC and GPS lost: flight termination"); + } + hrt_abstime t1 = hrt_absolute_time(); -- cgit v1.2.3 From 4d75222b67b8ae93f28c1bbec782ce7fd0ab9919 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 22 Aug 2014 21:41:24 +0200 Subject: switch to rc loss mode if rc loss commanded --- src/modules/commander/state_machine_helper.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 404bafb04..d105e4bdf 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -451,7 +451,7 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en case MAIN_STATE_ALTCTL: case MAIN_STATE_POSCTL: /* require RC for all manual modes */ - if (status->rc_signal_lost && armed) { + if ((status->rc_signal_lost || status->rc_signal_lost_cmd) && armed) { status->failsafe = true; if (status->condition_global_position_valid && status->condition_home_position_valid) { -- cgit v1.2.3 From 6ae8800ad09caf9194240ec75067af5ef56a5a23 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 22 Aug 2014 23:05:19 +0200 Subject: move and rename params airfield home is general --- src/modules/navigator/datalinkloss.cpp | 6 ++-- src/modules/navigator/datalinkloss_params.c | 33 ---------------------- src/modules/navigator/navigator_params.c | 43 +++++++++++++++++++++++++++++ 3 files changed, 46 insertions(+), 36 deletions(-) diff --git a/src/modules/navigator/datalinkloss.cpp b/src/modules/navigator/datalinkloss.cpp index d8a1de229..893d6d93a 100644 --- a/src/modules/navigator/datalinkloss.cpp +++ b/src/modules/navigator/datalinkloss.cpp @@ -61,9 +61,9 @@ DataLinkLoss::DataLinkLoss(Navigator *navigator, const char *name) : _param_commsholdlat(this, "CH_LAT"), _param_commsholdlon(this, "CH_LON"), _param_commsholdalt(this, "CH_ALT"), - _param_airfieldhomelat(this, "AH_LAT"), - _param_airfieldhomelon(this, "AH_LON"), - _param_airfieldhomealt(this, "AH_ALT"), + _param_airfieldhomelat(this, "NAV_AH_LAT", false), + _param_airfieldhomelon(this, "NAV_AH_LON", false), + _param_airfieldhomealt(this, "NAV_AH_ALT", false), _param_numberdatalinklosses(this, "N"), _dll_state(DLL_STATE_NONE) { diff --git a/src/modules/navigator/datalinkloss_params.c b/src/modules/navigator/datalinkloss_params.c index 77a8763cb..02f7ca4c3 100644 --- a/src/modules/navigator/datalinkloss_params.c +++ b/src/modules/navigator/datalinkloss_params.c @@ -91,39 +91,6 @@ PARAM_DEFINE_INT32(NAV_DLL_CH_LON, 1518453890); */ PARAM_DEFINE_FLOAT(NAV_DLL_CH_ALT, 600.0f); -/** - * Airfield home Lat - * - * Latitude of airfield home waypoint - * - * @unit degrees * 1e7 - * @min 0.0 - * @group DLL - */ -PARAM_DEFINE_INT32(NAV_DLL_AH_LAT, -265847810); - -/** - * Airfield home Lon - * - * Longitude of airfield home waypoint - * - * @unit degrees * 1e7 - * @min 0.0 - * @group DLL - */ -PARAM_DEFINE_INT32(NAV_DLL_AH_LON, 1518423250); - -/** - * Airfield home alt - * - * Altitude of airfield home waypoint - * - * @unit m - * @min 0.0 - * @group DLL - */ -PARAM_DEFINE_FLOAT(NAV_DLL_AH_ALT, 600.0f); - /** * Number of allowed Datalink timeouts * diff --git a/src/modules/navigator/navigator_params.c b/src/modules/navigator/navigator_params.c index afaf1c3c3..1f40e634e 100644 --- a/src/modules/navigator/navigator_params.c +++ b/src/modules/navigator/navigator_params.c @@ -75,3 +75,46 @@ PARAM_DEFINE_FLOAT(NAV_ACC_RAD, 25.0f); * @group Mission */ PARAM_DEFINE_INT32(NAV_DLL_OBC, 0); + +/** + * Set OBC mode for rc loss + * + * If set to 1 the behaviour on data link loss is set to a mode according to the OBC rules + * + * @min 0 + * @group Mission + */ +PARAM_DEFINE_INT32(NAV_RCL_OBC, 0); + +/** + * Airfield home Lat + * + * Latitude of airfield home waypoint + * + * @unit degrees * 1e7 + * @min 0.0 + * @group DLL + */ +PARAM_DEFINE_INT32(NAV_AH_LAT, -265847810); + +/** + * Airfield home Lon + * + * Longitude of airfield home waypoint + * + * @unit degrees * 1e7 + * @min 0.0 + * @group DLL + */ +PARAM_DEFINE_INT32(NAV_AH_LON, 1518423250); + +/** + * Airfield home alt + * + * Altitude of airfield home waypoint + * + * @unit m + * @min 0.0 + * @group DLL + */ +PARAM_DEFINE_FLOAT(NAV_AH_ALT, 600.0f); -- cgit v1.2.3 From fd3746a233b0ef16758e0171da0ee7e71ff58887 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 22 Aug 2014 23:06:14 +0200 Subject: add OBC RC loss mode to navigator --- src/modules/navigator/gpsfailure.cpp | 2 +- src/modules/navigator/module.mk | 4 +- src/modules/navigator/navigator.h | 8 +- src/modules/navigator/navigator_main.cpp | 13 ++- src/modules/navigator/rcloss.cpp | 172 +++++++++++++++++++++++++++++++ src/modules/navigator/rcloss.h | 88 ++++++++++++++++ src/modules/navigator/rcloss_params.c | 59 +++++++++++ 7 files changed, 339 insertions(+), 7 deletions(-) create mode 100644 src/modules/navigator/rcloss.cpp create mode 100644 src/modules/navigator/rcloss.h create mode 100644 src/modules/navigator/rcloss_params.c diff --git a/src/modules/navigator/gpsfailure.cpp b/src/modules/navigator/gpsfailure.cpp index 02e766ffb..f0bbbcf1c 100644 --- a/src/modules/navigator/gpsfailure.cpp +++ b/src/modules/navigator/gpsfailure.cpp @@ -141,7 +141,7 @@ GpsFailure::set_gpsf_item() case GPSF_STATE_TERMINATE: { /* Request flight termination from the commander */ pos_sp_triplet->flight_termination = true; - warnx("request flight termination"); + warnx("gps fail: request flight termination"); } default: break; diff --git a/src/modules/navigator/module.mk b/src/modules/navigator/module.mk index 9e4ad053c..c075903b7 100644 --- a/src/modules/navigator/module.mk +++ b/src/modules/navigator/module.mk @@ -51,8 +51,10 @@ SRCS = navigator_main.cpp \ geofence.cpp \ geofence_params.c \ datalinkloss.cpp \ - enginefailure.cpp \ datalinkloss_params.c \ + rcloss.cpp \ + rcloss_params.c \ + enginefailure.cpp \ gpsfailure.cpp \ gpsfailure_params.c diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index ec6e538e3..709e3e724 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -63,12 +63,13 @@ #include "datalinkloss.h" #include "enginefailure.h" #include "gpsfailure.h" +#include "rcloss.h" #include "geofence.h" /** * Number of navigation modes that need on_active/on_inactive calls */ -#define NAVIGATOR_MODE_ARRAY_SIZE 7 +#define NAVIGATOR_MODE_ARRAY_SIZE 8 class Navigator : public control::SuperBlock { @@ -109,7 +110,7 @@ public: * Publish the mission result so commander and mavlink know what is going on */ void publish_mission_result(); - + /** * Publish the attitude sp, only to be used in very special modes when position control is deactivated * Example: mode that is triggered on gps failure @@ -193,6 +194,8 @@ private: Mission _mission; /**< class that handles the missions */ Loiter _loiter; /**< class that handles loiter */ RTL _rtl; /**< class that handles RTL */ + RCLoss _rcLoss; /**< class that handles RTL according to + OBC rules (rc loss mode) */ Offboard _offboard; /**< class that handles offboard */ DataLinkLoss _dataLinkLoss; /**< class that handles the OBC datalink loss mode */ EngineFailure _engineFailure; /**< class that handles the engine failure mode @@ -207,6 +210,7 @@ private: control::BlockParamFloat _param_loiter_radius; /**< loiter radius for fixedwing */ control::BlockParamFloat _param_acceptance_radius; /**< acceptance for takeoff */ control::BlockParamInt _param_datalinkloss_obc; /**< if true: obc mode on data link loss enabled */ + control::BlockParamInt _param_rcloss_obc; /**< if true: obc mode on rc loss enabled */ /** * Retrieve global position */ diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index e0913bb57..c569ee7b5 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -131,6 +131,7 @@ Navigator::Navigator() : _mission(this, "MIS"), _loiter(this, "LOI"), _rtl(this, "RTL"), + _rcLoss(this, "RCL"), _offboard(this, "OFF"), _dataLinkLoss(this, "DLL"), _engineFailure(this, "EF"), @@ -139,7 +140,8 @@ Navigator::Navigator() : _pos_sp_triplet_updated(false), _param_loiter_radius(this, "LOITER_RAD"), _param_acceptance_radius(this, "ACC_RAD"), - _param_datalinkloss_obc(this, "DLL_OBC") + _param_datalinkloss_obc(this, "DLL_OBC"), + _param_rcloss_obc(this, "RCL_OBC") { /* Create a list of our possible navigation types */ _navigation_mode_array[0] = &_mission; @@ -149,6 +151,7 @@ Navigator::Navigator() : _navigation_mode_array[4] = &_dataLinkLoss; _navigation_mode_array[5] = &_engineFailure; _navigation_mode_array[6] = &_gpsFailure; + _navigation_mode_array[7] = &_rcLoss; updateParams(); } @@ -413,7 +416,11 @@ Navigator::task_main() _navigation_mode = &_loiter; break; case NAVIGATION_STATE_AUTO_RTL: - _navigation_mode = &_rtl; + if (_param_rcloss_obc.get() != 0) { + _navigation_mode = &_rcLoss; + } else { + _navigation_mode = &_rtl; + } break; case NAVIGATION_STATE_AUTO_RTGS: //XXX OBC: differentiate between rc loss and dl loss here /* Use complex data link loss mode only when enabled via param @@ -421,7 +428,7 @@ Navigator::task_main() if (_param_datalinkloss_obc.get() != 0) { _navigation_mode = &_dataLinkLoss; } else { - _navigation_mode = &_rtl; /* TODO: change this to something else */ + _navigation_mode = &_rtl; } break; case NAVIGATION_STATE_AUTO_LANDENGFAIL: diff --git a/src/modules/navigator/rcloss.cpp b/src/modules/navigator/rcloss.cpp new file mode 100644 index 000000000..b8b659efe --- /dev/null +++ b/src/modules/navigator/rcloss.cpp @@ -0,0 +1,172 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ +/** + * @file rcloss.cpp + * Helper class for RC Loss Mode according to the OBC rules + * + * @author Thomas Gubler + */ + +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include + +#include "navigator.h" +#include "datalinkloss.h" + +#define DELAY_SIGMA 0.01f + +RCLoss::RCLoss(Navigator *navigator, const char *name) : + MissionBlock(navigator, name), + _param_loitertime(this, "LT"), + _rcl_state(RCL_STATE_NONE) +{ + /* load initial params */ + updateParams(); + /* initial reset */ + on_inactive(); +} + +RCLoss::~RCLoss() +{ +} + +void +RCLoss::on_inactive() +{ + /* reset RCL state only if setpoint moved */ + if (!_navigator->get_can_loiter_at_sp()) { + _rcl_state = RCL_STATE_NONE; + } +} + +void +RCLoss::on_activation() +{ + _rcl_state = RCL_STATE_NONE; + updateParams(); + advance_rcl(); + set_rcl_item(); +} + +void +RCLoss::on_active() +{ + if (is_mission_item_reached()) { + updateParams(); + advance_rcl(); + set_rcl_item(); + } +} + +void +RCLoss::set_rcl_item() +{ + struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); + + set_previous_pos_setpoint(); + _navigator->set_can_loiter_at_sp(false); + + switch (_rcl_state) { + case RCL_STATE_LOITER: { + _mission_item.lat = _navigator->get_global_position()->lat; + _mission_item.lon = _navigator->get_global_position()->lon; + _mission_item.altitude_is_relative = false; + _mission_item.yaw = NAN; + _mission_item.loiter_radius = _navigator->get_loiter_radius(); + _mission_item.loiter_direction = 1; + _mission_item.nav_cmd = NAV_CMD_LOITER_TIME_LIMIT; + _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); + _mission_item.time_inside = _param_loitertime.get() < 0.0f ? 0.0f : _param_loitertime.get(); + _mission_item.pitch_min = 0.0f; + _mission_item.autocontinue = true; + _mission_item.origin = ORIGIN_ONBOARD; + + _navigator->set_can_loiter_at_sp(true); + break; + } + case RCL_STATE_TERMINATE: { + /* Request flight termination from the commander */ + pos_sp_triplet->flight_termination = true; + warnx("gps fail: request flight termination"); + pos_sp_triplet->previous.valid = false; + pos_sp_triplet->current.valid = false; + pos_sp_triplet->next.valid = false; + } + default: + break; + } + + reset_mission_item_reached(); + + /* convert mission item to current position setpoint and make it valid */ + mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current); + pos_sp_triplet->next.valid = false; + + _navigator->set_position_setpoint_triplet_updated(); +} + +void +RCLoss::advance_rcl() +{ + switch (_rcl_state) { + case RCL_STATE_NONE: + /* Check the number of data link losses. If above home fly home directly */ + warnx("RC loss, OBC mode, loiter"); + mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: rc los, loitering"); + _rcl_state = RCL_STATE_LOITER; + break; + case RCL_STATE_LOITER: + _rcl_state = RCL_STATE_TERMINATE; + warnx("time is up, no RC regain, terminating"); + mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RC not regained, terminating"); + _navigator->get_mission_result()->stay_in_failsafe = true; + _navigator->publish_mission_result(); + break; + case RCL_STATE_TERMINATE: + warnx("rcl end"); + _rcl_state = RCL_STATE_END; + break; + default: + break; + } +} diff --git a/src/modules/navigator/rcloss.h b/src/modules/navigator/rcloss.h new file mode 100644 index 000000000..bcb74d877 --- /dev/null +++ b/src/modules/navigator/rcloss.h @@ -0,0 +1,88 @@ +/*************************************************************************** + * + * Copyright (c) 2013-2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ +/** + * @file rcloss.h + * Helper class for RC Loss Mode acording to the OBC rules + * + * @author Thomas Gubler + */ + +#ifndef NAVIGATOR_RCLOSS_H +#define NAVIGATOR_RCLOSS_H + +#include +#include + +#include + +#include "navigator_mode.h" +#include "mission_block.h" + +class Navigator; + +class RCLoss : public MissionBlock +{ +public: + RCLoss(Navigator *navigator, const char *name); + + ~RCLoss(); + + virtual void on_inactive(); + + virtual void on_activation(); + + virtual void on_active(); + +private: + /* Params */ + control::BlockParamFloat _param_loitertime; + + enum RCLState { + RCL_STATE_NONE = 0, + RCL_STATE_LOITER = 1, + RCL_STATE_TERMINATE = 2, + RCL_STATE_END = 3 + } _rcl_state; + + /** + * Set the RCL item + */ + void set_rcl_item(); + + /** + * Move to next RCL item + */ + void advance_rcl(); + +}; +#endif diff --git a/src/modules/navigator/rcloss_params.c b/src/modules/navigator/rcloss_params.c new file mode 100644 index 000000000..83d23cf49 --- /dev/null +++ b/src/modules/navigator/rcloss_params.c @@ -0,0 +1,59 @@ +/**************************************************************************** + * + * Copyright (c) 2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * AS IS AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file rcloss_params.c + * + * Parameters for RC Loss (OBC) + * + * @author Thomas Gubler + */ + +#include + +#include + +/* + * OBC RC Loss mode parameters, accessible via MAVLink + */ + +/** + * Loiter Time + * + * The amount of time in seconds the system should loiter at current position before termination + * + * @unit seconds + * @min 0.0 + * @group RCL + */ +PARAM_DEFINE_FLOAT(NAV_RCL_LT, 120.0f); -- cgit v1.2.3 From 8d3dd7363dbf9a29d88d2abb3364739749894684 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 22 Aug 2014 23:15:11 +0200 Subject: catapult launch detection: fix integration logic --- src/lib/launchdetection/CatapultLaunchMethod.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/lib/launchdetection/CatapultLaunchMethod.cpp b/src/lib/launchdetection/CatapultLaunchMethod.cpp index c555a0a69..9d479832f 100644 --- a/src/lib/launchdetection/CatapultLaunchMethod.cpp +++ b/src/lib/launchdetection/CatapultLaunchMethod.cpp @@ -66,10 +66,10 @@ void CatapultLaunchMethod::update(float accel_x) last_timestamp = hrt_absolute_time(); if (accel_x > threshold_accel.get()) { - integrator += accel_x * dt; + integrator += dt; // warnx("*** integrator %.3f, threshold_accel %.3f, threshold_time %.3f, accel_x %.3f, dt %.3f", // (double)integrator, (double)threshold_accel.get(), (double)threshold_time.get(), (double)accel_x, (double)dt); - if (integrator > threshold_accel.get() * threshold_time.get()) { + if (integrator > threshold_time.get()) { launchDetected = true; } -- cgit v1.2.3 From 5a5617cb597b16b789a6fa175410d667f45cf907 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 23 Aug 2014 11:42:53 +0200 Subject: launchdetector: return state of detection The launchdetector now has a intermediate state (controlsenabled) which is meant to be interpreted by the controller as: "perform attitude control but do not yet power up the motor". This can be used in the floating phase during a bungee launch for example. --- src/lib/launchdetection/CatapultLaunchMethod.cpp | 70 ++++++++++++++---------- src/lib/launchdetection/CatapultLaunchMethod.h | 13 ++--- src/lib/launchdetection/LaunchDetector.cpp | 27 +++++++-- src/lib/launchdetection/LaunchDetector.h | 8 ++- src/lib/launchdetection/LaunchMethod.h | 13 ++++- src/lib/launchdetection/launchdetection_params.c | 8 ++- 6 files changed, 91 insertions(+), 48 deletions(-) diff --git a/src/lib/launchdetection/CatapultLaunchMethod.cpp b/src/lib/launchdetection/CatapultLaunchMethod.cpp index 2b876b629..65ae461db 100644 --- a/src/lib/launchdetection/CatapultLaunchMethod.cpp +++ b/src/lib/launchdetection/CatapultLaunchMethod.cpp @@ -49,10 +49,10 @@ CatapultLaunchMethod::CatapultLaunchMethod(SuperBlock *parent) : SuperBlock(parent, "CAT"), last_timestamp(hrt_absolute_time()), integrator(0.0f), - launchDetected(false), - threshold_accel(this, "A"), - threshold_time(this, "T"), - delay(this, "DELAY") + state(LAUNCHDETECTION_RES_NONE), + thresholdAccel(this, "A"), + thresholdTime(this, "T"), + motorDelay(this, "MDEL") { } @@ -66,46 +66,56 @@ void CatapultLaunchMethod::update(float accel_x) float dt = (float)hrt_elapsed_time(&last_timestamp) * 1e-6f; last_timestamp = hrt_absolute_time(); - if (accel_x > threshold_accel.get()) { - integrator += dt; -// warnx("*** integrator %.3f, threshold_accel %.3f, threshold_time %.3f, accel_x %.3f, dt %.3f", -// (double)integrator, (double)threshold_accel.get(), (double)threshold_time.get(), (double)accel_x, (double)dt); - if (integrator > threshold_time.get()) { - launchDetected = true; + switch (state) { + case LAUNCHDETECTION_RES_NONE: + /* Detect a acceleration that is longer and stronger as the minimum given by the params */ + if (accel_x > thresholdAccel.get()) { + integrator += dt; + if (integrator > thresholdTime.get()) { + if (motorDelay.get() > 0.0f) { + state = LAUNCHDETECTION_RES_DETECTED_ENABLECONTROL; + warnx("Launch detected: state: enablecontrol, waiting %.2fs until using full" + " throttle", (double)motorDelay.get()); + } else { + /* No motor delay set: go directly to enablemotors state */ + state = LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS; + warnx("Launch detected: state: enablemotors (delay not activated)"); + } + } + } else { + /* reset */ + reset(); } + break; + + case LAUNCHDETECTION_RES_DETECTED_ENABLECONTROL: + /* Vehicle is currently controlling attitude but not with full throttle. Waiting undtil delay is + * over to allow full throttle */ + motorDelayCounter += dt; + if (motorDelayCounter > motorDelay.get()) { + warnx("Launch detected: state enablemotors"); + state = LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS; + } + break; - } else { -// warnx("integrator %.3f, threshold_accel %.3f, threshold_time %.3f, accel_x %.3f, dt %.3f", -// (double)integrator, (double)threshold_accel.get(), (double)threshold_time.get(), (double)accel_x, (double)dt); - /* reset integrator */ - integrator = 0.0f; - } + default: + break; - if (launchDetected) { - delayCounter += dt; - if (delayCounter > delay.get()) { - delayPassed = true; - } } } -bool CatapultLaunchMethod::getLaunchDetected() +LaunchDetectionResult CatapultLaunchMethod::getLaunchDetected() const { - if (delay.get() > 0.0f) { - return delayPassed; - } else { - return launchDetected; - } + return state; } void CatapultLaunchMethod::reset() { integrator = 0.0f; - delayCounter = 0.0f; - launchDetected = false; - delayPassed = false; + motorDelayCounter = 0.0f; + state = LAUNCHDETECTION_RES_NONE; } } diff --git a/src/lib/launchdetection/CatapultLaunchMethod.h b/src/lib/launchdetection/CatapultLaunchMethod.h index a64d482f6..d918c3a76 100644 --- a/src/lib/launchdetection/CatapultLaunchMethod.h +++ b/src/lib/launchdetection/CatapultLaunchMethod.h @@ -57,20 +57,19 @@ public: ~CatapultLaunchMethod(); void update(float accel_x); - bool getLaunchDetected(); + LaunchDetectionResult getLaunchDetected() const; void reset(); private: hrt_abstime last_timestamp; float integrator; - float delayCounter; + float motorDelayCounter; - bool launchDetected; - bool delayPassed; + LaunchDetectionResult state; - control::BlockParamFloat threshold_accel; - control::BlockParamFloat threshold_time; - control::BlockParamFloat delay; + control::BlockParamFloat thresholdAccel; + control::BlockParamFloat thresholdTime; + control::BlockParamFloat motorDelay; }; diff --git a/src/lib/launchdetection/LaunchDetector.cpp b/src/lib/launchdetection/LaunchDetector.cpp index 3bf47bbb0..2958c0a31 100644 --- a/src/lib/launchdetection/LaunchDetector.cpp +++ b/src/lib/launchdetection/LaunchDetector.cpp @@ -46,6 +46,7 @@ namespace launchdetection LaunchDetector::LaunchDetector() : SuperBlock(NULL, "LAUN"), + activeLaunchDetectionMethodIndex(-1), launchdetection_on(this, "ALL_ON"), throttlePreTakeoff(this, "THR_PRE") { @@ -65,7 +66,14 @@ LaunchDetector::~LaunchDetector() void LaunchDetector::reset() { /* Reset all detectors */ - launchMethods[0]->reset(); + for (uint8_t i = 0; i < sizeof(launchMethods)/sizeof(LaunchMethod); i++) { + launchMethods[i]->reset(); + } + + /* Reset active launchdetector */ + activeLaunchDetectionMethodIndex = -1; + + } void LaunchDetector::update(float accel_x) @@ -77,17 +85,24 @@ void LaunchDetector::update(float accel_x) } } -bool LaunchDetector::getLaunchDetected() +LaunchDetectionResult LaunchDetector::getLaunchDetected() { if (launchdetection_on.get() == 1) { - for (uint8_t i = 0; i < sizeof(launchMethods)/sizeof(LaunchMethod); i++) { - if(launchMethods[i]->getLaunchDetected()) { - return true; + if (activeLaunchDetectionMethodIndex < 0) { + /* None of the active launchmethods has detected a launch, check all launchmethods */ + for (uint8_t i = 0; i < sizeof(launchMethods)/sizeof(LaunchMethod); i++) { + if(launchMethods[i]->getLaunchDetected() != LAUNCHDETECTION_RES_NONE) { + warnx("selecting launchmethod %d", i); + activeLaunchDetectionMethodIndex = i; // from now on only check this method + return launchMethods[i]->getLaunchDetected(); + } } + } else { + return launchMethods[activeLaunchDetectionMethodIndex]->getLaunchDetected(); } } - return false; + return LAUNCHDETECTION_RES_NONE; } } diff --git a/src/lib/launchdetection/LaunchDetector.h b/src/lib/launchdetection/LaunchDetector.h index 1a214b66e..b48e724ba 100644 --- a/src/lib/launchdetection/LaunchDetector.h +++ b/src/lib/launchdetection/LaunchDetector.h @@ -59,7 +59,7 @@ public: void reset(); void update(float accel_x); - bool getLaunchDetected(); + LaunchDetectionResult getLaunchDetected(); bool launchDetectionEnabled() { return (bool)launchdetection_on.get(); }; float getThrottlePreTakeoff() {return throttlePreTakeoff.get(); } @@ -67,6 +67,12 @@ public: // virtual bool getLaunchDetected(); protected: private: + int activeLaunchDetectionMethodIndex; /**< holds a index to the launchMethod in the array launchMethods + which detected a Launch. If no launchMethod has detected a launch yet the + value is -1. Once one launchMetthod has detected a launch only this + method is checked for further adavancing in the state machine (e.g. when + to power up the motors) */ + LaunchMethod* launchMethods[1]; control::BlockParamInt launchdetection_on; control::BlockParamFloat throttlePreTakeoff; diff --git a/src/lib/launchdetection/LaunchMethod.h b/src/lib/launchdetection/LaunchMethod.h index e04467f6a..d2f091cea 100644 --- a/src/lib/launchdetection/LaunchMethod.h +++ b/src/lib/launchdetection/LaunchMethod.h @@ -44,11 +44,22 @@ namespace launchdetection { +enum LaunchDetectionResult { + LAUNCHDETECTION_RES_NONE = 0, /**< No launch has been detected */ + LAUNCHDETECTION_RES_DETECTED_ENABLECONTROL = 1, /**< Launch has been detected, the controller should + control the attitude. However any motors should not throttle + up and still be set to 'throttlePreTakeoff'. + For instance this is used to have a delay for the motor + when launching a fixed wing aircraft from a bungee */ + LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS = 2 /**< Launch has been detected, teh controller should control + attitude and also throttle up the motors. */ +}; + class LaunchMethod { public: virtual void update(float accel_x) = 0; - virtual bool getLaunchDetected() = 0; + virtual LaunchDetectionResult getLaunchDetected() const = 0; virtual void reset() = 0; protected: diff --git a/src/lib/launchdetection/launchdetection_params.c b/src/lib/launchdetection/launchdetection_params.c index 02f01bc66..d35eb11f6 100644 --- a/src/lib/launchdetection/launchdetection_params.c +++ b/src/lib/launchdetection/launchdetection_params.c @@ -78,14 +78,16 @@ PARAM_DEFINE_FLOAT(LAUN_CAT_A, 30.0f); PARAM_DEFINE_FLOAT(LAUN_CAT_T, 0.05f); /** - * Catapult delay - * + * Motor delay * + * Delay between starting attitude control and powering up the thorttle (giving throttle control to the controller) + * Before this timespan is up the throttle will be set to LAUN_THR_PRE, set to 0 to deactivate * + * @unit seconds * @min 0 * @group Launch detection */ -PARAM_DEFINE_FLOAT(LAUN_CAT_DELAY, 0.0f); +PARAM_DEFINE_FLOAT(LAUN_CAT_MDEL, 0.0f); /** * Throttle setting while detecting launch. * -- cgit v1.2.3 From 40d47fb069cb60303102f3bfba21f6dc5e0aa5a9 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 23 Aug 2014 11:45:20 +0200 Subject: fw pos control: use new lauchdetector states --- .../fw_pos_control_l1/fw_pos_control_l1_main.cpp | 73 +++++++++++++--------- 1 file changed, 43 insertions(+), 30 deletions(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 350ce6dec..6dd458516 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 @@ -102,6 +102,8 @@ static int _control_task = -1; /**< task handle for sensor task */ */ extern "C" __EXPORT int fw_pos_control_l1_main(int argc, char *argv[]); +using namespace launchdetection; + class FixedwingPositionControl { public: @@ -171,7 +173,7 @@ private: bool land_onslope; /* takeoff/launch states */ - bool launch_detected; + LaunchDetectionResult launch_detection_state; bool usePreTakeoffThrust; bool last_manual; ///< true if the last iteration was in manual mode (used to determine when a reset is needed) @@ -438,7 +440,7 @@ FixedwingPositionControl::FixedwingPositionControl() : land_stayonground(false), land_motor_lim(false), land_onslope(false), - launch_detected(false), + launch_detection_state(LAUNCHDETECTION_RES_NONE), usePreTakeoffThrust(false), last_manual(false), landingslope(), @@ -1076,39 +1078,51 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } else if (pos_sp_triplet.current.type == SETPOINT_TYPE_TAKEOFF) { /* Perform launch detection */ - if(!launch_detected) { //do not do further checks once a launch was detected - if (launchDetector.launchDetectionEnabled()) { - static hrt_abstime last_sent = 0; - if(hrt_absolute_time() - last_sent > 4e6) { - mavlink_log_info(_mavlink_fd, "#audio: Launchdetection running"); - last_sent = hrt_absolute_time(); - } + if (launchDetector.launchDetectionEnabled() && + launch_detection_state != LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS) { + /* Inform user that launchdetection is running */ + static hrt_abstime last_sent = 0; + if(hrt_absolute_time() - last_sent > 4e6) { + mavlink_log_info(_mavlink_fd, "#audio: Launchdetection running"); + last_sent = hrt_absolute_time(); + } + + /* Detect launch */ + launchDetector.update(_sensor_combined.accelerometer_m_s2[0]); + + /* update our copy of the laucn detection state */ + launch_detection_state = launchDetector.getLaunchDetected(); + /* Depending on launch detection state set control flags */ + if(launch_detection_state == LAUNCHDETECTION_RES_NONE) { /* Tell the attitude controller to stop integrating while we are waiting * for the launch */ _att_sp.roll_reset_integral = true; _att_sp.pitch_reset_integral = true; _att_sp.yaw_reset_integral = true; - /* Detect launch */ - launchDetector.update(_sensor_combined.accelerometer_m_s2[0]); - if (launchDetector.getLaunchDetected()) { - launch_detected = true; - mavlink_log_info(_mavlink_fd, "#audio: Takeoff"); - } - } else { - /* no takeoff detection --> fly */ - launch_detected = true; - warnx("launchdetection off"); + usePreTakeoffThrust = true; + } else if (launch_detection_state == LAUNCHDETECTION_RES_DETECTED_ENABLECONTROL) { + usePreTakeoffThrust = true; + } else { + usePreTakeoffThrust = false; } + } else { + /* no takeoff detection --> fly */ + usePreTakeoffThrust = false; + launch_detection_state = LAUNCHDETECTION_RES_DETECTED_ENABLECONTROL; } - _l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed_2d); - _att_sp.roll_body = _l1_control.nav_roll(); - _att_sp.yaw_body = _l1_control.nav_bearing(); + if (launch_detection_state != LAUNCHDETECTION_RES_NONE) { + /* Launch has been detected, hence we have to control the plane. The usePreTakeoffThrust + * flag determines how much thrust we apply */ - if (launch_detected) { - usePreTakeoffThrust = false; + _l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed_2d); + _att_sp.roll_body = _l1_control.nav_roll(); + _att_sp.yaw_body = _l1_control.nav_bearing(); + + float takeoff_throttle = usePreTakeoffThrust ? launchDetector.getThrottlePreTakeoff() : + _parameters.throttle_max; /* apply minimum pitch and limit roll if target altitude is not within 10 meters */ if (_parameters.climbout_diff > 0.001f && altitude_error > _parameters.climbout_diff) { @@ -1119,7 +1133,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi eas2tas, math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max), - _parameters.throttle_min, _parameters.throttle_max, + _parameters.throttle_min, takeoff_throttle, _parameters.throttle_cruise, true, math::max(math::radians(pos_sp_triplet.current.pitch_min), @@ -1138,17 +1152,15 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max), _parameters.throttle_min, - _parameters.throttle_max, + takeoff_throttle, _parameters.throttle_cruise, false, math::radians(_parameters.pitch_limit_min), _global_pos.alt, ground_speed); } - - } else { - usePreTakeoffThrust = true; } + } /* reset landing state */ @@ -1182,6 +1194,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } } + /* making sure again that the correct thrust is used, without depending on library calls */ if (usePreTakeoffThrust) { _att_sp.thrust = launchDetector.getThrottlePreTakeoff(); } @@ -1342,7 +1355,7 @@ FixedwingPositionControl::task_main() void FixedwingPositionControl::reset_takeoff_state() { - launch_detected = false; + launch_detection_state = LAUNCHDETECTION_RES_NONE; usePreTakeoffThrust = false; launchDetector.reset(); } -- cgit v1.2.3 From 9f5004be2d282a0bb8f2618545d0c6174df2e29a Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 23 Aug 2014 12:13:30 +0200 Subject: fw pos control: set default roll, pitch while waiting for launch --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 6dd458516..9cbe5483b 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 @@ -1143,7 +1143,8 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi TECS_MODE_TAKEOFF); /* limit roll motion to ensure enough lift */ - _att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-15.0f), math::radians(15.0f)); + _att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-15.0f), + math::radians(15.0f)); } else { tecs_update_pitch_throttle(_pos_sp_triplet.current.alt, @@ -1159,6 +1160,11 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi _global_pos.alt, ground_speed); } + } else { + /* Set default roll and pitch setpoints during detection phase */ + _att_sp.roll_body = 0.0f; + _att_sp.pitch_body = math::max(math::radians(pos_sp_triplet.current.pitch_min), + math::radians(10.0f)); } } -- cgit v1.2.3 From cfbbe60291028c60a5dd0a8f2b8bad265a7ee839 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 23 Aug 2014 12:41:48 +0200 Subject: fw pos control: launchdetection logic cleanup --- .../fw_pos_control_l1/fw_pos_control_l1_main.cpp | 44 +++++++++------------- 1 file changed, 18 insertions(+), 26 deletions(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 9cbe5483b..da81b0dba 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -174,7 +174,6 @@ private: /* takeoff/launch states */ LaunchDetectionResult launch_detection_state; - bool usePreTakeoffThrust; bool last_manual; ///< true if the last iteration was in manual mode (used to determine when a reset is needed) @@ -441,7 +440,6 @@ FixedwingPositionControl::FixedwingPositionControl() : land_motor_lim(false), land_onslope(false), launch_detection_state(LAUNCHDETECTION_RES_NONE), - usePreTakeoffThrust(false), last_manual(false), landingslope(), flare_curve_alt_rel_last(0.0f), @@ -1092,37 +1090,24 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* update our copy of the laucn detection state */ launch_detection_state = launchDetector.getLaunchDetected(); - - /* Depending on launch detection state set control flags */ - if(launch_detection_state == LAUNCHDETECTION_RES_NONE) { - /* Tell the attitude controller to stop integrating while we are waiting - * for the launch */ - _att_sp.roll_reset_integral = true; - _att_sp.pitch_reset_integral = true; - _att_sp.yaw_reset_integral = true; - - usePreTakeoffThrust = true; - } else if (launch_detection_state == LAUNCHDETECTION_RES_DETECTED_ENABLECONTROL) { - usePreTakeoffThrust = true; - } else { - usePreTakeoffThrust = false; - } } else { /* no takeoff detection --> fly */ - usePreTakeoffThrust = false; - launch_detection_state = LAUNCHDETECTION_RES_DETECTED_ENABLECONTROL; + launch_detection_state = LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS; } + /* Set control values depending on the detection state */ if (launch_detection_state != LAUNCHDETECTION_RES_NONE) { - /* Launch has been detected, hence we have to control the plane. The usePreTakeoffThrust - * flag determines how much thrust we apply */ + /* Launch has been detected, hence we have to control the plane. */ _l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed_2d); _att_sp.roll_body = _l1_control.nav_roll(); _att_sp.yaw_body = _l1_control.nav_bearing(); - float takeoff_throttle = usePreTakeoffThrust ? launchDetector.getThrottlePreTakeoff() : - _parameters.throttle_max; + /* Select throttle: only in LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS we want to use + * full throttle, otherwise we use the preTakeOff Throttle */ + float takeoff_throttle = launch_detection_state != + LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS ? + launchDetector.getThrottlePreTakeoff() : _parameters.throttle_max; /* apply minimum pitch and limit roll if target altitude is not within 10 meters */ if (_parameters.climbout_diff > 0.001f && altitude_error > _parameters.climbout_diff) { @@ -1161,6 +1146,12 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi ground_speed); } } else { + /* Tell the attitude controller to stop integrating while we are waiting + * for the launch */ + _att_sp.roll_reset_integral = true; + _att_sp.pitch_reset_integral = true; + _att_sp.yaw_reset_integral = true; + /* Set default roll and pitch setpoints during detection phase */ _att_sp.roll_body = 0.0f; _att_sp.pitch_body = math::max(math::radians(pos_sp_triplet.current.pitch_min), @@ -1200,8 +1191,10 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } } - /* making sure again that the correct thrust is used, without depending on library calls */ - if (usePreTakeoffThrust) { + /* Copy thrust and pitch values from tecs + * making sure again that the correct thrust is used, without depending on library calls */ + if (pos_sp_triplet.current.type == SETPOINT_TYPE_TAKEOFF && + launch_detection_state != LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS) { _att_sp.thrust = launchDetector.getThrottlePreTakeoff(); } else { @@ -1362,7 +1355,6 @@ FixedwingPositionControl::task_main() void FixedwingPositionControl::reset_takeoff_state() { launch_detection_state = LAUNCHDETECTION_RES_NONE; - usePreTakeoffThrust = false; launchDetector.reset(); } -- cgit v1.2.3 From 29715102894121f711fd40acfcf6ec8fb4fa6630 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 23 Aug 2014 13:00:28 +0200 Subject: commander: flight termination, require arming --- src/modules/commander/commander.cpp | 50 +++++++++++++++++++------------------ 1 file changed, 26 insertions(+), 24 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 9968ab8e6..692e5e8ec 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1311,7 +1311,7 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(position_setpoint_triplet), pos_sp_triplet_sub, &pos_sp_triplet); /* Check for geofence violation */ - if (pos_sp_triplet.geofence_violated || pos_sp_triplet.flight_termination) { + if (armed.armed && (pos_sp_triplet.geofence_violated || pos_sp_triplet.flight_termination)) { //XXX: make this configurable to select different actions (e.g. navigation modes) /* this will only trigger if geofence is activated via param and a geofence file is present, also there is a circuit breaker to disable the actual flight termination in the px4io driver */ armed.force_failsafe = true; @@ -1580,29 +1580,31 @@ int commander_thread_main(int argc, char *argv[]) } /* Check for failure combinations which lead to flight termination */ - /* At this point the data link and the gps system have been checked - * If both failed we want to terminate the flight */ - if ((status.data_link_lost && status.gps_failure) || - (status.data_link_lost_cmd && status.gps_failure_cmd)) { - armed.force_failsafe = true; - status_changed = true; - warnx("Flight termination because of data link loss && gps failure"); - mavlink_log_critical(mavlink_fd, "DL and GPS lost: flight termination"); - } - - /* At this point the rc signal and the gps system have been checked - * If we are in manual (controlled with RC): - * if both failed we want to terminate the flight */ - if ((status.main_state == MAIN_STATE_ACRO || - status.main_state == MAIN_STATE_MANUAL || - status.main_state == MAIN_STATE_ALTCTL || - status.main_state == MAIN_STATE_POSCTL) && - ((status.rc_signal_lost && status.rc_signal_lost) || - (status.rc_signal_lost && status.gps_failure_cmd))) { - armed.force_failsafe = true; - status_changed = true; - warnx("Flight termination because of RC signal loss && gps failure"); - mavlink_log_critical(mavlink_fd, "RC and GPS lost: flight termination"); + if (armed.armed) { + /* At this point the data link and the gps system have been checked + * If both failed we want to terminate the flight */ + if ((status.data_link_lost && status.gps_failure) || + (status.data_link_lost_cmd && status.gps_failure_cmd)) { + armed.force_failsafe = true; + status_changed = true; + warnx("Flight termination because of data link loss && gps failure"); + mavlink_log_critical(mavlink_fd, "DL and GPS lost: flight termination"); + } + + /* At this point the rc signal and the gps system have been checked + * If we are in manual (controlled with RC): + * if both failed we want to terminate the flight */ + if ((status.main_state == MAIN_STATE_ACRO || + status.main_state == MAIN_STATE_MANUAL || + status.main_state == MAIN_STATE_ALTCTL || + status.main_state == MAIN_STATE_POSCTL) && + ((status.rc_signal_lost && status.rc_signal_lost) || + (status.rc_signal_lost && status.gps_failure_cmd))) { + armed.force_failsafe = true; + status_changed = true; + warnx("Flight termination because of RC signal loss && gps failure"); + mavlink_log_critical(mavlink_fd, "RC and GPS lost: flight termination"); + } } -- cgit v1.2.3 From cc12e6051d1ae8100c035e935fe6dd1a453ce887 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 23 Aug 2014 13:13:51 +0200 Subject: obc rcloss: set altitude --- src/modules/navigator/rcloss.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/modules/navigator/rcloss.cpp b/src/modules/navigator/rcloss.cpp index b8b659efe..427001a01 100644 --- a/src/modules/navigator/rcloss.cpp +++ b/src/modules/navigator/rcloss.cpp @@ -110,6 +110,7 @@ RCLoss::set_rcl_item() case RCL_STATE_LOITER: { _mission_item.lat = _navigator->get_global_position()->lat; _mission_item.lon = _navigator->get_global_position()->lon; + _mission_item.altitude = _navigator->get_global_position()->alt; _mission_item.altitude_is_relative = false; _mission_item.yaw = NAN; _mission_item.loiter_radius = _navigator->get_loiter_radius(); @@ -127,7 +128,7 @@ RCLoss::set_rcl_item() case RCL_STATE_TERMINATE: { /* Request flight termination from the commander */ pos_sp_triplet->flight_termination = true; - warnx("gps fail: request flight termination"); + warnx("rc not recovered: request flight termination"); pos_sp_triplet->previous.valid = false; pos_sp_triplet->current.valid = false; pos_sp_triplet->next.valid = false; @@ -152,7 +153,7 @@ RCLoss::advance_rcl() case RCL_STATE_NONE: /* Check the number of data link losses. If above home fly home directly */ warnx("RC loss, OBC mode, loiter"); - mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: rc los, loitering"); + mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: rc loss, loitering"); _rcl_state = RCL_STATE_LOITER; break; case RCL_STATE_LOITER: -- cgit v1.2.3 From ffd2fa7386f9fe3ab017d98a2b0e8e21b0975833 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 23 Aug 2014 13:25:50 +0200 Subject: commander: fix check for rc && gps loss --- src/modules/commander/commander.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 692e5e8ec..c4b76b391 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1598,8 +1598,8 @@ int commander_thread_main(int argc, char *argv[]) status.main_state == MAIN_STATE_MANUAL || status.main_state == MAIN_STATE_ALTCTL || status.main_state == MAIN_STATE_POSCTL) && - ((status.rc_signal_lost && status.rc_signal_lost) || - (status.rc_signal_lost && status.gps_failure_cmd))) { + ((status.rc_signal_lost && status.gps_failure) || + (status.rc_signal_lost_cmd && status.gps_failure_cmd))) { armed.force_failsafe = true; status_changed = true; warnx("Flight termination because of RC signal loss && gps failure"); -- cgit v1.2.3 From 3c10b78e2044ec2cbe36d4de35c7ac8a936521ae Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 23 Aug 2014 14:02:22 +0200 Subject: stae machine helper: remove unnecessary check for RC loss --- src/modules/commander/commander.cpp | 2 +- src/modules/commander/state_machine_helper.cpp | 15 +-------------- 2 files changed, 2 insertions(+), 15 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index c4b76b391..0bf5cfe34 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -566,7 +566,7 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s status_local->rc_signal_lost_cmd = false; if ((int)cmd->param2 <= 0) { /* reset all commanded failure modes */ - warnx("revert to normal mode"); + warnx("reset all non-flighttermination failsafe commands"); } else if ((int)cmd->param2 == 1) { /* trigger engine failure mode */ status_local->engine_failure_cmd = true; diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index d105e4bdf..4506942ab 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -548,22 +548,9 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en break; case MAIN_STATE_AUTO_LOITER: - /* go into failsafe on a engine failure or if datalink and RC is lost */ + /* go into failsafe on a engine failure */ if (status->engine_failure) { status->nav_state = NAVIGATION_STATE_AUTO_LANDENGFAIL; - } else if ((status->data_link_lost && data_link_loss_enabled) && status->rc_signal_lost) { - status->failsafe = true; - - if (status->condition_global_position_valid && status->condition_home_position_valid) { - status->nav_state = NAVIGATION_STATE_AUTO_RTL; - } else if (status->condition_local_position_valid) { - status->nav_state = NAVIGATION_STATE_LAND; - } else if (status->condition_local_altitude_valid) { - status->nav_state = NAVIGATION_STATE_DESCEND; - } else { - status->nav_state = NAVIGATION_STATE_TERMINATION; - } - /* also go into failsafe if just datalink is lost */ } else if (status->data_link_lost && data_link_loss_enabled) { status->failsafe = true; -- cgit v1.2.3 From 6a8971e28f492073a951d96065df30034853bea7 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 23 Aug 2014 17:23:59 +0400 Subject: New UAVCAN initialization logic --- ROMFS/px4fmu_common/init.d/rc.uavcan | 16 ++++++++ ROMFS/px4fmu_common/init.d/rcS | 12 ++++-- src/modules/uavcan/module.mk | 3 +- src/modules/uavcan/uavcan_main.cpp | 50 ++++++++---------------- src/modules/uavcan/uavcan_params.c | 73 ++++++++++++++++++++++++++++++++++++ 5 files changed, 114 insertions(+), 40 deletions(-) create mode 100644 ROMFS/px4fmu_common/init.d/rc.uavcan create mode 100644 src/modules/uavcan/uavcan_params.c diff --git a/ROMFS/px4fmu_common/init.d/rc.uavcan b/ROMFS/px4fmu_common/init.d/rc.uavcan new file mode 100644 index 000000000..abb76b400 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/rc.uavcan @@ -0,0 +1,16 @@ +#!nsh +# +# UAVCAN initialization script. +# + +if param compare UAVCAN_ENABLE 1 +then + if uavcan start + then + sleep 1 # Sensor autodetection delay + echo "[init] UAVCAN started" + else + echo "[init] ERROR: Could not start UAVCAN" + tone_alarm $TUNE_OUT_ERROR + fi +fi diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index c9e6a27ca..195771905 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -304,11 +304,10 @@ then then if [ $OUTPUT_MODE == uavcan_esc ] then - if uavcan start 1 + if param compare UAVCAN_ENABLE 0 then - echo "CAN UP" - else - echo "CAN ERR" + echo "[init] OVERRIDING UAVCAN_ENABLE = 1" + param set UAVCAN_ENABLE 1 fi fi @@ -447,6 +446,11 @@ then mavlink start $MAVLINK_FLAGS + # + # UAVCAN + # + sh /etc/init.d/rc.uavcan + # # Sensors, Logging, GPS # diff --git a/src/modules/uavcan/module.mk b/src/modules/uavcan/module.mk index 26ff7102d..93a1bf96f 100644 --- a/src/modules/uavcan/module.mk +++ b/src/modules/uavcan/module.mk @@ -42,7 +42,8 @@ MAXOPTIMIZATION = -Os # Main SRCS += uavcan_main.cpp \ - uavcan_clock.cpp + uavcan_clock.cpp \ + uavcan_params.c # Actuators SRCS += actuators/esc.cpp diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index 8607af145..a15f83696 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -38,6 +38,7 @@ #include #include #include +#include #include #include #include @@ -587,7 +588,7 @@ void UavcanNode::sensor_print_enabled() static void print_usage() { warnx("usage: \n" - "\tuavcan start [can_bitrate]\n" + "\tuavcan start\n" "\tuavcan sensor enable \n" "\tuavcan sensor list"); @@ -599,52 +600,32 @@ extern "C" __EXPORT int uavcan_main(int argc, char *argv[]); int uavcan_main(int argc, char *argv[]) { - constexpr unsigned DEFAULT_CAN_BITRATE = 1000000; - if (argc < 2) { print_usage(); ::exit(1); } if (!std::strcmp(argv[1], "start")) { - if (argc < 3) { - print_usage(); - ::exit(1); + if (UavcanNode::instance()) { + errx(1, "already started"); } - /* - * Node ID - */ - const int node_id = atoi(argv[2]); + // Node ID + int32_t node_id = 0; + (void)param_get(param_find("UAVCAN_NODE_ID"), &node_id); if (node_id < 0 || node_id > uavcan::NodeID::Max || !uavcan::NodeID(node_id).isUnicast()) { warnx("Invalid Node ID %i", node_id); ::exit(1); } - /* - * CAN bitrate - */ - unsigned bitrate = 0; - - if (argc > 3) { - bitrate = atol(argv[3]); - } + // CAN bitrate + int32_t bitrate = 0; + (void)param_get(param_find("UAVCAN_BITRATE"), &bitrate); - if (bitrate <= 0) { - bitrate = DEFAULT_CAN_BITRATE; - } - - if (UavcanNode::instance()) { - errx(1, "already started"); - } - - /* - * Start - */ + // Start warnx("Node ID %u, bitrate %u", node_id, bitrate); return UavcanNode::start(node_id, bitrate); - } /* commands below require the app to be started */ @@ -655,14 +636,13 @@ int uavcan_main(int argc, char *argv[]) } if (!std::strcmp(argv[1], "status") || !std::strcmp(argv[1], "info")) { - - inst->print_info(); - ::exit(0); + inst->print_info(); + ::exit(0); } if (!std::strcmp(argv[1], "stop")) { - delete inst; - ::exit(0); + delete inst; + ::exit(0); } if (!std::strcmp(argv[1], "sensor")) { diff --git a/src/modules/uavcan/uavcan_params.c b/src/modules/uavcan/uavcan_params.c new file mode 100644 index 000000000..e6ea8a8fb --- /dev/null +++ b/src/modules/uavcan/uavcan_params.c @@ -0,0 +1,73 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +/** + * @author Pavel Kirienko + */ + +#include +#include + +/** + * Enable UAVCAN. + * + * Enables support for UAVCAN-interfaced actuators and sensors. + * + * @min 0 + * @max 1 + * @group UAVCAN + */ +PARAM_DEFINE_INT32(UAVCAN_ENABLE, 0); + +/** + * UAVCAN Node ID. + * + * Read the specs at http://uavcan.org to learn more about Node ID. + * + * @min 1 + * @max 125 + * @group UAVCAN + */ +PARAM_DEFINE_INT32(UAVCAN_NODE_ID, 1); + +/** + * UAVCAN CAN bus bitrate. + * + * @min 20000 + * @max 1000000 + * @group UAVCAN + */ +PARAM_DEFINE_INT32(UAVCAN_BITRATE, 1000000); + + + -- cgit v1.2.3 From bcca3cae748ffea2df51907992e4a3c7ca673fd2 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 23 Aug 2014 16:03:24 +0200 Subject: Run full update straight after reset, filter wind speed dynamically --- .../ekf_att_pos_estimator_main.cpp | 22 ++---------- .../ekf_att_pos_estimator/estimator_23states.cpp | 40 +++++++++++++++------- .../ekf_att_pos_estimator/estimator_23states.h | 1 + 3 files changed, 31 insertions(+), 32 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index c384b2566..b7e118d47 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 @@ -1220,30 +1220,12 @@ FixedwingEstimator::task_main() } else if (_ekf->statesInitialised) { // We're apparently initialized in this case now - int check = check_filter_state(); - - if (check) { - // Let the system re-initialize itself - continue; - } + // check (and reset the filter as needed) + (void)check_filter_state(); // Run the strapdown INS equations every IMU update _ekf->UpdateStrapdownEquationsNED(); - #if 0 - // debug code - could be tunred into a filter mnitoring/watchdog function - float tempQuat[4]; - - for (uint8_t j = 0; j <= 3; j++) tempQuat[j] = states[j]; - - quat2eul(eulerEst, tempQuat); - for (uint8_t j = 0; j <= 2; j++) eulerDif[j] = eulerEst[j] - ahrsEul[j]; - - if (eulerDif[2] > pi) eulerDif[2] -= 2 * pi; - - if (eulerDif[2] < -pi) eulerDif[2] += 2 * pi; - - #endif // store the predicted states for subsequent use by measurement fusion _ekf->StoreStates(IMUmsec); // Check if on ground - status is used by covariance prediction diff --git a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp index d176ccee2..249cc419e 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp @@ -74,6 +74,7 @@ AttPosEKF::AttPosEKF() : windSpdFiltNorth(0.0f), windSpdFiltEast(0.0f), windSpdFiltAltitude(0.0f), + windSpdFiltClimb(0.0f), fusionModeGPS(0), innovVelPos{}, varInnovVelPos{}, @@ -1660,22 +1661,30 @@ void AttPosEKF::FuseAirspeed() // Perform fusion of True Airspeed measurement if (useAirspeed && fuseVtasData && (VtasPred > 1.0f) && (VtasMeas > 8.0f)) { - // Lowpass the output of the wind estimate - we want a long-term - // stable estimate, but not start to load into the overall dynamics - // of the system (which adjusting covariances would do) - - // Nominally damp to 0.02% of the noise, but reduce the damping for strong altitude variations - // assuming equal wind speeds on the same altitude and varying wind speeds on - // different altitudes - float windFiltCoeff = 0.0002f; float altDiff = fabsf(windSpdFiltAltitude - hgtMea); - // Change filter coefficient based on altitude - windFiltCoeff += ConstrainFloat(0.00001f * altDiff, windFiltCoeff, 0.1f); + if (isfinite(windSpdFiltClimb)) { + windSpdFiltClimb = ((1.0f - 0.0002f) * windSpdFiltClimb) + (0.0002f * states[6]); + } else { + windSpdFiltClimb = states[6]; + } + + if (altDiff < 20.0f) { + // Lowpass the output of the wind estimate - we want a long-term + // stable estimate, but not start to load into the overall dynamics + // of the system (which adjusting covariances would do) + + // Change filter coefficient based on altitude change rate + float windFiltCoeff = ConstrainFloat(fabsf(windSpdFiltClimb) / 1000.0f, 0.00005f, 0.2f); + + windSpdFiltNorth = ((1.0f - windFiltCoeff) * windSpdFiltNorth) + (windFiltCoeff * vwn); + windSpdFiltEast = ((1.0f - windFiltCoeff) * windSpdFiltEast) + (windFiltCoeff * vwe); + } else { + windSpdFiltNorth = vwn; + windSpdFiltEast = vwe; + } - windSpdFiltNorth = ((1.0f - windFiltCoeff) * windSpdFiltNorth) + (windFiltCoeff * vwn); - windSpdFiltEast = ((1.0f - windFiltCoeff) * windSpdFiltEast) + (windFiltCoeff * vwe); windSpdFiltAltitude = hgtMea; // Calculate observation jacobians @@ -3097,6 +3106,13 @@ void AttPosEKF::ZeroVariables() dVelIMU.zero(); lastGyroOffset.zero(); + windSpdFiltNorth = 0.0f; + windSpdFiltEast = 0.0f; + // setting the altitude to zero will give us a higher + // gain to adjust faster in the first step + windSpdFiltAltitude = 0.0f; + windSpdFiltClimb = 0.0f; + for (unsigned i = 0; i < data_buffer_size; i++) { for (unsigned j = 0; j < n_states; j++) { diff --git a/src/modules/ekf_att_pos_estimator/estimator_23states.h b/src/modules/ekf_att_pos_estimator/estimator_23states.h index 6349b03f0..f8bb7a9c4 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_23states.h +++ b/src/modules/ekf_att_pos_estimator/estimator_23states.h @@ -152,6 +152,7 @@ public: float windSpdFiltNorth; // average wind speed north component float windSpdFiltEast; // average wind speed east component float windSpdFiltAltitude; // the last altitude used to filter wind speed + float windSpdFiltClimb; // filtered climb rate uint8_t fusionModeGPS; // 0 = GPS outputs 3D velocity, 1 = GPS outputs 2D velocity, 2 = GPS outputs no velocity float innovVelPos[6]; // innovation output float varInnovVelPos[6]; // innovation variance output -- cgit v1.2.3 From 40fe9ab969247315e3065d0423034554cf66b885 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sat, 23 Aug 2014 18:03:01 +0200 Subject: meas_airspeed: don't take the aboslute value --- src/drivers/meas_airspeed/meas_airspeed.cpp | 18 +++++++----------- 1 file changed, 7 insertions(+), 11 deletions(-) diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp index 159706278..c136c6641 100644 --- a/src/drivers/meas_airspeed/meas_airspeed.cpp +++ b/src/drivers/meas_airspeed/meas_airspeed.cpp @@ -228,8 +228,10 @@ MEASAirspeed::collect() // the raw value still should be compensated for the known offset diff_press_pa_raw -= _diff_pres_offset; - float diff_press_pa = fabsf(diff_press_pa_raw); - + /* don't take the absolute value because the calibration takes this into account and warns the user if the + * tubes are connected backwards */ + float diff_press_pa = diff_press_pa_raw; + /* note that we return both the absolute value with offset applied and a raw value without the offset applied. This @@ -241,14 +243,8 @@ MEASAirspeed::collect() With the above calculation the MS4525 sensor will produce a positive number when the top port is used as a dynamic port and bottom port is used as the static port - - Also note that the _diff_pres_offset is applied before the - fabsf() not afterwards. It needs to be done this way to - prevent a bias at low speeds, but this also means that when - setting a offset you must set it based on the raw value, not - the offset value */ - + struct differential_pressure_s report; /* track maximum differential pressure measured (so we can work out top speed). */ @@ -345,7 +341,7 @@ MEASAirspeed::cycle() /** correct for 5V rail voltage if the system_power ORB topic is available - + See http://uav.tridgell.net/MS4525/MS4525-offset.png for a graph of offset versus voltage for 3 sensors */ @@ -394,7 +390,7 @@ MEASAirspeed::voltage_correction(float &diff_press_pa, float &temperature) if (voltage_diff < -1.0f) { voltage_diff = -1.0f; } - temperature -= voltage_diff * temp_slope; + temperature -= voltage_diff * temp_slope; #endif // CONFIG_ARCH_BOARD_PX4FMU_V2 } -- cgit v1.2.3 From c6fb75f66fa94444d07056f271980e3f05008f94 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sat, 23 Aug 2014 18:44:09 +0200 Subject: airspeed_calibration: stop talking about Pa and and hashtags --- src/drivers/meas_airspeed/meas_airspeed.cpp | 8 +++++--- src/modules/commander/airspeed_calibration.cpp | 18 ++++++++++-------- 2 files changed, 15 insertions(+), 11 deletions(-) diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp index c136c6641..bcdf4670a 100644 --- a/src/drivers/meas_airspeed/meas_airspeed.cpp +++ b/src/drivers/meas_airspeed/meas_airspeed.cpp @@ -232,6 +232,8 @@ MEASAirspeed::collect() * tubes are connected backwards */ float diff_press_pa = diff_press_pa_raw; + warnx("diff preasure: %.4f", (double)diff_press_pa); + /* note that we return both the absolute value with offset applied and a raw value without the offset applied. This @@ -259,9 +261,9 @@ MEASAirspeed::collect() report.differential_pressure_filtered_pa = _filter.apply(diff_press_pa); /* the dynamics of the filter can make it overshoot into the negative range */ - if (report.differential_pressure_filtered_pa < 0.0f) { - report.differential_pressure_filtered_pa = _filter.reset(diff_press_pa); - } + //if (report.differential_pressure_filtered_pa < 0.0f) { + // report.differential_pressure_filtered_pa = _filter.reset(diff_press_pa); + //} report.differential_pressure_raw_pa = diff_press_pa_raw; report.max_differential_pressure_pa = _max_differential_pressure_pa; diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp index 0e58c68b6..339b11bbe 100644 --- a/src/modules/commander/airspeed_calibration.cpp +++ b/src/modules/commander/airspeed_calibration.cpp @@ -180,11 +180,13 @@ int do_airspeed_calibration(int mavlink_fd) return ERROR; } - mavlink_log_critical(mavlink_fd, "Offset of %d Pa, create airflow now!", (int)diff_pres_offset); + mavlink_log_critical(mavlink_fd, "Offset of %d Pascal", (int)diff_pres_offset); /* wait 500 ms to ensure parameter propagated through the system */ usleep(500 * 1000); + mavlink_log_critical(mavlink_fd, "Create airflow now"); + calibration_counter = 0; const unsigned maxcount = 3000; @@ -204,18 +206,18 @@ int do_airspeed_calibration(int mavlink_fd) calibration_counter++; if (fabsf(diff_pres.differential_pressure_raw_pa) < 50.0f) { - if (calibration_counter % 100 == 0) { - mavlink_log_critical(mavlink_fd, "Missing airflow! (%d, wanted: 50 Pa, #h101)", - (int)diff_pres.differential_pressure_raw_pa); + if (calibration_counter % 500 == 0) { + mavlink_log_info(mavlink_fd, "Create airflow! (%d, wanted: 50 Pa)", + (int)diff_pres.differential_pressure_raw_pa); } continue; } /* do not allow negative values */ if (diff_pres.differential_pressure_raw_pa < 0.0f) { - mavlink_log_info(mavlink_fd, "negative pressure: ERROR (%d Pa)", - (int)diff_pres.differential_pressure_raw_pa); - mavlink_log_critical(mavlink_fd, "%d Pa: swap static and dynamic ports!", (int)diff_pres.differential_pressure_raw_pa); + mavlink_log_critical(mavlink_fd, "Swap static and dynamic ports!"); + mavlink_log_info(mavlink_fd, "ERROR: Negative pressure difference detected! (%d Pa)", + (int)diff_pres.differential_pressure_raw_pa); close(diff_pres_sub); /* the user setup is wrong, wipe the calibration to force a proper re-calibration */ @@ -236,7 +238,7 @@ int do_airspeed_calibration(int mavlink_fd) mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name); return ERROR; } else { - mavlink_log_info(mavlink_fd, "positive pressure: OK (%d Pa)", + mavlink_log_info(mavlink_fd, "Positive pressure: OK (%d Pa)", (int)diff_pres.differential_pressure_raw_pa); break; } -- cgit v1.2.3 From 6480747c69203f70b4f28d3a357f49e05df89c85 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sat, 23 Aug 2014 18:49:00 +0200 Subject: Revert "airspeed_calibration: stop talking about Pa and and hashtags" This reverts commit c6fb75f66fa94444d07056f271980e3f05008f94. --- src/drivers/meas_airspeed/meas_airspeed.cpp | 8 +++----- src/modules/commander/airspeed_calibration.cpp | 18 ++++++++---------- 2 files changed, 11 insertions(+), 15 deletions(-) diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp index bcdf4670a..c136c6641 100644 --- a/src/drivers/meas_airspeed/meas_airspeed.cpp +++ b/src/drivers/meas_airspeed/meas_airspeed.cpp @@ -232,8 +232,6 @@ MEASAirspeed::collect() * tubes are connected backwards */ float diff_press_pa = diff_press_pa_raw; - warnx("diff preasure: %.4f", (double)diff_press_pa); - /* note that we return both the absolute value with offset applied and a raw value without the offset applied. This @@ -261,9 +259,9 @@ MEASAirspeed::collect() report.differential_pressure_filtered_pa = _filter.apply(diff_press_pa); /* the dynamics of the filter can make it overshoot into the negative range */ - //if (report.differential_pressure_filtered_pa < 0.0f) { - // report.differential_pressure_filtered_pa = _filter.reset(diff_press_pa); - //} + if (report.differential_pressure_filtered_pa < 0.0f) { + report.differential_pressure_filtered_pa = _filter.reset(diff_press_pa); + } report.differential_pressure_raw_pa = diff_press_pa_raw; report.max_differential_pressure_pa = _max_differential_pressure_pa; diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp index 339b11bbe..0e58c68b6 100644 --- a/src/modules/commander/airspeed_calibration.cpp +++ b/src/modules/commander/airspeed_calibration.cpp @@ -180,13 +180,11 @@ int do_airspeed_calibration(int mavlink_fd) return ERROR; } - mavlink_log_critical(mavlink_fd, "Offset of %d Pascal", (int)diff_pres_offset); + mavlink_log_critical(mavlink_fd, "Offset of %d Pa, create airflow now!", (int)diff_pres_offset); /* wait 500 ms to ensure parameter propagated through the system */ usleep(500 * 1000); - mavlink_log_critical(mavlink_fd, "Create airflow now"); - calibration_counter = 0; const unsigned maxcount = 3000; @@ -206,18 +204,18 @@ int do_airspeed_calibration(int mavlink_fd) calibration_counter++; if (fabsf(diff_pres.differential_pressure_raw_pa) < 50.0f) { - if (calibration_counter % 500 == 0) { - mavlink_log_info(mavlink_fd, "Create airflow! (%d, wanted: 50 Pa)", - (int)diff_pres.differential_pressure_raw_pa); + if (calibration_counter % 100 == 0) { + mavlink_log_critical(mavlink_fd, "Missing airflow! (%d, wanted: 50 Pa, #h101)", + (int)diff_pres.differential_pressure_raw_pa); } continue; } /* do not allow negative values */ if (diff_pres.differential_pressure_raw_pa < 0.0f) { - mavlink_log_critical(mavlink_fd, "Swap static and dynamic ports!"); - mavlink_log_info(mavlink_fd, "ERROR: Negative pressure difference detected! (%d Pa)", - (int)diff_pres.differential_pressure_raw_pa); + mavlink_log_info(mavlink_fd, "negative pressure: ERROR (%d Pa)", + (int)diff_pres.differential_pressure_raw_pa); + mavlink_log_critical(mavlink_fd, "%d Pa: swap static and dynamic ports!", (int)diff_pres.differential_pressure_raw_pa); close(diff_pres_sub); /* the user setup is wrong, wipe the calibration to force a proper re-calibration */ @@ -238,7 +236,7 @@ int do_airspeed_calibration(int mavlink_fd) mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name); return ERROR; } else { - mavlink_log_info(mavlink_fd, "Positive pressure: OK (%d Pa)", + mavlink_log_info(mavlink_fd, "positive pressure: OK (%d Pa)", (int)diff_pres.differential_pressure_raw_pa); break; } -- cgit v1.2.3 From a1b4d72d1f8bd3cd25f9fcfd4dfd4c4ec5bcad01 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sat, 23 Aug 2014 18:52:56 +0200 Subject: airspeed_calibration: stop talking about Pa and and hashtags (now the correct files) --- src/modules/commander/airspeed_calibration.cpp | 18 ++++++++++-------- 1 file changed, 10 insertions(+), 8 deletions(-) diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp index 0e58c68b6..339b11bbe 100644 --- a/src/modules/commander/airspeed_calibration.cpp +++ b/src/modules/commander/airspeed_calibration.cpp @@ -180,11 +180,13 @@ int do_airspeed_calibration(int mavlink_fd) return ERROR; } - mavlink_log_critical(mavlink_fd, "Offset of %d Pa, create airflow now!", (int)diff_pres_offset); + mavlink_log_critical(mavlink_fd, "Offset of %d Pascal", (int)diff_pres_offset); /* wait 500 ms to ensure parameter propagated through the system */ usleep(500 * 1000); + mavlink_log_critical(mavlink_fd, "Create airflow now"); + calibration_counter = 0; const unsigned maxcount = 3000; @@ -204,18 +206,18 @@ int do_airspeed_calibration(int mavlink_fd) calibration_counter++; if (fabsf(diff_pres.differential_pressure_raw_pa) < 50.0f) { - if (calibration_counter % 100 == 0) { - mavlink_log_critical(mavlink_fd, "Missing airflow! (%d, wanted: 50 Pa, #h101)", - (int)diff_pres.differential_pressure_raw_pa); + if (calibration_counter % 500 == 0) { + mavlink_log_info(mavlink_fd, "Create airflow! (%d, wanted: 50 Pa)", + (int)diff_pres.differential_pressure_raw_pa); } continue; } /* do not allow negative values */ if (diff_pres.differential_pressure_raw_pa < 0.0f) { - mavlink_log_info(mavlink_fd, "negative pressure: ERROR (%d Pa)", - (int)diff_pres.differential_pressure_raw_pa); - mavlink_log_critical(mavlink_fd, "%d Pa: swap static and dynamic ports!", (int)diff_pres.differential_pressure_raw_pa); + mavlink_log_critical(mavlink_fd, "Swap static and dynamic ports!"); + mavlink_log_info(mavlink_fd, "ERROR: Negative pressure difference detected! (%d Pa)", + (int)diff_pres.differential_pressure_raw_pa); close(diff_pres_sub); /* the user setup is wrong, wipe the calibration to force a proper re-calibration */ @@ -236,7 +238,7 @@ int do_airspeed_calibration(int mavlink_fd) mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name); return ERROR; } else { - mavlink_log_info(mavlink_fd, "positive pressure: OK (%d Pa)", + mavlink_log_info(mavlink_fd, "Positive pressure: OK (%d Pa)", (int)diff_pres.differential_pressure_raw_pa); break; } -- cgit v1.2.3 From e09bc02c37d5e36a3c7d3feffca83e9ddfe1ed79 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sat, 23 Aug 2014 18:53:45 +0200 Subject: meas_airspeed: don't reset the filter below 0 --- src/drivers/meas_airspeed/meas_airspeed.cpp | 5 ----- 1 file changed, 5 deletions(-) diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp index c136c6641..479fa3ccf 100644 --- a/src/drivers/meas_airspeed/meas_airspeed.cpp +++ b/src/drivers/meas_airspeed/meas_airspeed.cpp @@ -258,11 +258,6 @@ MEASAirspeed::collect() report.differential_pressure_pa = diff_press_pa; report.differential_pressure_filtered_pa = _filter.apply(diff_press_pa); - /* the dynamics of the filter can make it overshoot into the negative range */ - if (report.differential_pressure_filtered_pa < 0.0f) { - report.differential_pressure_filtered_pa = _filter.reset(diff_press_pa); - } - report.differential_pressure_raw_pa = diff_press_pa_raw; report.max_differential_pressure_pa = _max_differential_pressure_pa; -- cgit v1.2.3 From 4e0d7c6b0e52d3eecba65f4415d4c7372dfd8a49 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 23 Aug 2014 23:14:59 +0400 Subject: UAVCAN: redundant sensors support --- src/modules/uavcan/module.mk | 6 +- src/modules/uavcan/sensors/baro.cpp | 49 +++------------ src/modules/uavcan/sensors/baro.hpp | 7 +-- src/modules/uavcan/sensors/gnss.cpp | 15 +++++ src/modules/uavcan/sensors/gnss.hpp | 18 +++--- src/modules/uavcan/sensors/mag.cpp | 51 +++------------ src/modules/uavcan/sensors/mag.hpp | 7 +-- src/modules/uavcan/sensors/sensor_bridge.cpp | 93 +++++++++++++++++++++++++++- src/modules/uavcan/sensors/sensor_bridge.hpp | 54 ++++++++++++++++ 9 files changed, 188 insertions(+), 112 deletions(-) diff --git a/src/modules/uavcan/module.mk b/src/modules/uavcan/module.mk index 93a1bf96f..f92bc754f 100644 --- a/src/modules/uavcan/module.mk +++ b/src/modules/uavcan/module.mk @@ -49,9 +49,9 @@ SRCS += uavcan_main.cpp \ SRCS += actuators/esc.cpp # Sensors -SRCS += sensors/sensor_bridge.cpp \ - sensors/gnss.cpp \ - sensors/mag.cpp \ +SRCS += sensors/sensor_bridge.cpp \ + sensors/gnss.cpp \ + sensors/mag.cpp \ sensors/baro.cpp # diff --git a/src/modules/uavcan/sensors/baro.cpp b/src/modules/uavcan/sensors/baro.cpp index 3afcc3f1c..ef4f0dbba 100644 --- a/src/modules/uavcan/sensors/baro.cpp +++ b/src/modules/uavcan/sensors/baro.cpp @@ -38,49 +38,26 @@ #include "baro.hpp" #include +static const orb_id_t BARO_TOPICS[2] = { + ORB_ID(sensor_baro0), + ORB_ID(sensor_baro1) +}; + const char *const UavcanBarometerBridge::NAME = "baro"; UavcanBarometerBridge::UavcanBarometerBridge(uavcan::INode& node) : -device::CDev("uavcan_baro", "/dev/uavcan/baro"), +UavcanCDevSensorBridgeBase("uavcan_baro", "/dev/uavcan/baro", BARO_DEVICE_PATH, BARO_TOPICS), _sub_air_data(node) { } -UavcanBarometerBridge::~UavcanBarometerBridge() -{ - if (_class_instance > 0) { - (void)unregister_class_devname(BARO_DEVICE_PATH, _class_instance); - } -} - int UavcanBarometerBridge::init() { - // Init the libuavcan subscription int res = _sub_air_data.start(AirDataCbBinder(this, &UavcanBarometerBridge::air_data_sub_cb)); if (res < 0) { log("failed to start uavcan sub: %d", res); return res; } - - // Detect our device class - _class_instance = register_class_devname(BARO_DEVICE_PATH); - switch (_class_instance) { - case CLASS_DEVICE_PRIMARY: { - _orb_id = ORB_ID(sensor_baro0); - break; - } - case CLASS_DEVICE_SECONDARY: { - _orb_id = ORB_ID(sensor_baro1); - break; - } - default: { - log("invalid class instance: %d", _class_instance); - (void)unregister_class_devname(BARO_DEVICE_PATH, _class_instance); - return -1; - } - } - - log("inited with class instance %d", _class_instance); return 0; } @@ -131,17 +108,5 @@ void UavcanBarometerBridge::air_data_sub_cb(const uavcan::ReceivedDataStructure< report.altitude = (((std::pow((p / p1), (-(a * R) / g))) * T1) - T1) / a; - /* - * Publish - */ - if (_orb_advert >= 0) { - orb_publish(_orb_id, _orb_advert, &report); - } else { - _orb_advert = orb_advertise(_orb_id, &report); - if (_orb_advert < 0) { - log("ADVERT FAIL"); - } else { - log("advertised"); - } - } + publish(msg.getSrcNodeID().get(), &report); } diff --git a/src/modules/uavcan/sensors/baro.hpp b/src/modules/uavcan/sensors/baro.hpp index f6aa01216..9d470219e 100644 --- a/src/modules/uavcan/sensors/baro.hpp +++ b/src/modules/uavcan/sensors/baro.hpp @@ -39,17 +39,15 @@ #include "sensor_bridge.hpp" #include -#include #include -class UavcanBarometerBridge : public IUavcanSensorBridge, public device::CDev +class UavcanBarometerBridge : public UavcanCDevSensorBridgeBase { public: static const char *const NAME; UavcanBarometerBridge(uavcan::INode& node); - ~UavcanBarometerBridge() override; const char *get_name() const override { return NAME; } @@ -67,7 +65,4 @@ private: uavcan::Subscriber _sub_air_data; unsigned _msl_pressure = 101325; - orb_id_t _orb_id = nullptr; - orb_advert_t _orb_advert = -1; - int _class_instance = -1; }; diff --git a/src/modules/uavcan/sensors/gnss.cpp b/src/modules/uavcan/sensors/gnss.cpp index 6b69d163f..f2bb28087 100644 --- a/src/modules/uavcan/sensors/gnss.cpp +++ b/src/modules/uavcan/sensors/gnss.cpp @@ -73,8 +73,23 @@ int UavcanGnssBridge::init() return res; } +unsigned UavcanGnssBridge::get_num_redundant_channels() const +{ + return (_receiver_node_id < 0) ? 0 : 1; +} + void UavcanGnssBridge::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure &msg) { + // This bridge does not support redundant GNSS receivers yet. + if (_receiver_node_id < 0) { + _receiver_node_id = msg.getSrcNodeID().get(); + warnx("GNSS receiver node ID: %d", _receiver_node_id); + } else { + if (_receiver_node_id != msg.getSrcNodeID().get()) { + return; // This GNSS receiver is the redundant one, ignore it. + } + } + _report.timestamp_position = hrt_absolute_time(); _report.lat = msg.lat_1e7; _report.lon = msg.lon_1e7; diff --git a/src/modules/uavcan/sensors/gnss.hpp b/src/modules/uavcan/sensors/gnss.hpp index db3a515fa..9488c5fe5 100644 --- a/src/modules/uavcan/sensors/gnss.hpp +++ b/src/modules/uavcan/sensors/gnss.hpp @@ -64,27 +64,23 @@ public: int init() override; + unsigned get_num_redundant_channels() const override; + private: /** * GNSS fix message will be reported via this callback. */ void gnss_fix_sub_cb(const uavcan::ReceivedDataStructure &msg); - typedef uavcan::MethodBinder&)> FixCbBinder; - /* - * libuavcan related things - */ - uavcan::INode &_node; - uavcan::Subscriber _sub_fix; + uavcan::INode &_node; + uavcan::Subscriber _sub_fix; + int _receiver_node_id = -1; - /* - * uORB - */ - struct vehicle_gps_position_s _report; ///< uORB topic for gnss position - orb_advert_t _report_pub; ///< uORB pub for gnss position + struct vehicle_gps_position_s _report; ///< uORB topic for gnss position + orb_advert_t _report_pub; ///< uORB pub for gnss position }; diff --git a/src/modules/uavcan/sensors/mag.cpp b/src/modules/uavcan/sensors/mag.cpp index 4f8a5e104..aaa3a4463 100644 --- a/src/modules/uavcan/sensors/mag.cpp +++ b/src/modules/uavcan/sensors/mag.cpp @@ -37,10 +37,16 @@ #include "mag.hpp" +static const orb_id_t MAG_TOPICS[3] = { + ORB_ID(sensor_mag0), + ORB_ID(sensor_mag1), + ORB_ID(sensor_mag2) +}; + const char *const UavcanMagnetometerBridge::NAME = "mag"; UavcanMagnetometerBridge::UavcanMagnetometerBridge(uavcan::INode& node) : -device::CDev("uavcan_mag", "/dev/uavcan/mag"), +UavcanCDevSensorBridgeBase("uavcan_mag", "/dev/uavcan/mag", MAG_DEVICE_PATH, MAG_TOPICS), _sub_mag(node) { _scale.x_scale = 1.0F; @@ -48,45 +54,13 @@ _sub_mag(node) _scale.z_scale = 1.0F; } -UavcanMagnetometerBridge::~UavcanMagnetometerBridge() -{ - if (_class_instance > 0) { - (void)unregister_class_devname(MAG_DEVICE_PATH, _class_instance); - } -} - int UavcanMagnetometerBridge::init() { - // Init the libuavcan subscription int res = _sub_mag.start(MagCbBinder(this, &UavcanMagnetometerBridge::mag_sub_cb)); if (res < 0) { log("failed to start uavcan sub: %d", res); return res; } - - // Detect our device class - _class_instance = register_class_devname(MAG_DEVICE_PATH); - switch (_class_instance) { - case CLASS_DEVICE_PRIMARY: { - _orb_id = ORB_ID(sensor_mag0); - break; - } - case CLASS_DEVICE_SECONDARY: { - _orb_id = ORB_ID(sensor_mag1); - break; - } - case CLASS_DEVICE_TERTIARY: { - _orb_id = ORB_ID(sensor_mag2); - break; - } - default: { - log("invalid class instance: %d", _class_instance); - (void)unregister_class_devname(MAG_DEVICE_PATH, _class_instance); - return -1; - } - } - - log("inited with class instance %d", _class_instance); return 0; } @@ -140,14 +114,5 @@ void UavcanMagnetometerBridge::mag_sub_cb(const uavcan::ReceivedDataStructure= 0) { - orb_publish(_orb_id, _orb_advert, &report); - } else { - _orb_advert = orb_advertise(_orb_id, &report); - if (_orb_advert < 0) { - log("ADVERT FAIL"); - } else { - log("advertised"); - } - } + publish(msg.getSrcNodeID().get(), &report); } diff --git a/src/modules/uavcan/sensors/mag.hpp b/src/modules/uavcan/sensors/mag.hpp index 4bc5129a2..6d413a8f7 100644 --- a/src/modules/uavcan/sensors/mag.hpp +++ b/src/modules/uavcan/sensors/mag.hpp @@ -38,18 +38,16 @@ #pragma once #include "sensor_bridge.hpp" -#include #include #include -class UavcanMagnetometerBridge : public IUavcanSensorBridge, public device::CDev +class UavcanMagnetometerBridge : public UavcanCDevSensorBridgeBase { public: static const char *const NAME; UavcanMagnetometerBridge(uavcan::INode& node); - ~UavcanMagnetometerBridge() override; const char *get_name() const override { return NAME; } @@ -67,7 +65,4 @@ private: uavcan::Subscriber _sub_mag; mag_scale _scale = {}; - orb_id_t _orb_id = nullptr; - orb_advert_t _orb_advert = -1; - int _class_instance = -1; }; diff --git a/src/modules/uavcan/sensors/sensor_bridge.cpp b/src/modules/uavcan/sensors/sensor_bridge.cpp index 5752d5524..05d589b58 100644 --- a/src/modules/uavcan/sensors/sensor_bridge.cpp +++ b/src/modules/uavcan/sensors/sensor_bridge.cpp @@ -36,12 +36,15 @@ */ #include "sensor_bridge.hpp" -#include +#include #include "gnss.hpp" #include "mag.hpp" #include "baro.hpp" +/* + * IUavcanSensorBridge + */ IUavcanSensorBridge* IUavcanSensorBridge::make(uavcan::INode &node, const char *bridge_name) { /* @@ -64,3 +67,91 @@ void IUavcanSensorBridge::print_known_names(const char *prefix) printf("%s%s\n", prefix, UavcanMagnetometerBridge::NAME); printf("%s%s\n", prefix, UavcanBarometerBridge::NAME); } + +/* + * UavcanCDevSensorBridgeBase + */ +UavcanCDevSensorBridgeBase::~UavcanCDevSensorBridgeBase() +{ + for (unsigned i = 0; i < _max_channels; i++) { + if (_channels[i].redunancy_channel_id >= 0) { + (void)unregister_class_devname(_class_devname, _channels[i].class_instance); + } + } + delete [] _orb_topics; + delete [] _channels; +} + +void UavcanCDevSensorBridgeBase::publish(const int redundancy_channel_id, const void *report) +{ + Channel *channel = nullptr; + + // Checking if such channel already exists + for (unsigned i = 0; i < _max_channels; i++) { + if (_channels[i].redunancy_channel_id == redundancy_channel_id) { + channel = _channels + i; + break; + } + } + + // No such channel - try to create one + if (channel == nullptr) { + if (_out_of_channels) { + return; // Give up immediately - saves some CPU time + } + + log("adding channel %d...", redundancy_channel_id); + + // Search for the first free channel + for (unsigned i = 0; i < _max_channels; i++) { + if (_channels[i].redunancy_channel_id < 0) { + channel = _channels + i; + break; + } + } + + // No free channels left + if (channel == nullptr) { + _out_of_channels = true; + log("out of channels"); + return; + } + + // Ask the CDev helper which class instance we can take + const int class_instance = register_class_devname(_class_devname); + if (class_instance < 0 || class_instance >= int(_max_channels)) { + _out_of_channels = true; + log("out of class instances"); + (void)unregister_class_devname(_class_devname, class_instance); + return; + } + + // Publish to the appropriate topic, abort on failure + channel->orb_id = _orb_topics[class_instance]; + channel->redunancy_channel_id = redundancy_channel_id; + channel->class_instance = class_instance; + + channel->orb_advert = orb_advertise(channel->orb_id, report); + if (channel->orb_advert < 0) { + log("ADVERTISE FAILED"); + *channel = Channel(); + return; + } + + log("channel %d class instance %d ok", channel->redunancy_channel_id, channel->class_instance); + } + assert(channel != nullptr); + + (void)orb_publish(channel->orb_id, channel->orb_advert, report); +} + +unsigned UavcanCDevSensorBridgeBase::get_num_redundant_channels() const +{ + unsigned out = 0; + for (unsigned i = 0; i < _max_channels; i++) { + if (_channels[i].redunancy_channel_id >= 0) { + out += 1; + } + } + return out; +} diff --git a/src/modules/uavcan/sensors/sensor_bridge.hpp b/src/modules/uavcan/sensors/sensor_bridge.hpp index 976d9a03d..d27ccb8a0 100644 --- a/src/modules/uavcan/sensors/sensor_bridge.hpp +++ b/src/modules/uavcan/sensors/sensor_bridge.hpp @@ -39,6 +39,8 @@ #include #include +#include +#include /** * A sensor bridge class must implement this interface. @@ -61,6 +63,11 @@ public: */ virtual int init() = 0; + /** + * Returns number of active redundancy channels. + */ + virtual unsigned get_num_redundant_channels() const = 0; + /** * Sensor bridge factory. * Creates a bridge object by its ASCII name, e.g. "gnss", "mag". @@ -73,3 +80,50 @@ public: */ static void print_known_names(const char *prefix); }; + +/** + * This is the base class for redundant sensors with an independent ORB topic per each redundancy channel. + * For example, sensor_mag0, sensor_mag1, etc. + */ +class UavcanCDevSensorBridgeBase : public IUavcanSensorBridge, public device::CDev +{ + struct Channel + { + int redunancy_channel_id = -1; + orb_id_t orb_id = nullptr; + orb_advert_t orb_advert = -1; + int class_instance = -1; + }; + + const unsigned _max_channels; + const char *const _class_devname; + orb_id_t *const _orb_topics; + Channel *const _channels; + bool _out_of_channels = false; + +protected: + template + UavcanCDevSensorBridgeBase(const char *name, const char *devname, const char *class_devname, + const orb_id_t (&orb_topics)[MaxChannels]) : + device::CDev(name, devname), + _max_channels(MaxChannels), + _class_devname(class_devname), + _orb_topics(new orb_id_t[MaxChannels]), + _channels(new Channel[MaxChannels]) + { + memcpy(_orb_topics, orb_topics, sizeof(orb_id_t) * MaxChannels); + } + + /** + * Sends one measurement into appropriate ORB topic. + * New redundancy channels will be registered automatically. + * @param redundancy_channel_id Redundant sensor identifier (e.g. UAVCAN Node ID) + * @param report ORB message object + */ + void publish(const int redundancy_channel_id, const void *report); + +public: + virtual ~UavcanCDevSensorBridgeBase(); + + unsigned get_num_redundant_channels() const override; +}; -- cgit v1.2.3 From ce73be514e0add0356c120c19e43f6818af236ad Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 23 Aug 2014 23:30:49 +0400 Subject: UAVCAN: Proper CDev initialization from sensor bridges --- src/modules/uavcan/sensors/baro.cpp | 7 ++++++- src/modules/uavcan/sensors/mag.cpp | 7 ++++++- 2 files changed, 12 insertions(+), 2 deletions(-) diff --git a/src/modules/uavcan/sensors/baro.cpp b/src/modules/uavcan/sensors/baro.cpp index ef4f0dbba..80c5e3828 100644 --- a/src/modules/uavcan/sensors/baro.cpp +++ b/src/modules/uavcan/sensors/baro.cpp @@ -53,7 +53,12 @@ _sub_air_data(node) int UavcanBarometerBridge::init() { - int res = _sub_air_data.start(AirDataCbBinder(this, &UavcanBarometerBridge::air_data_sub_cb)); + int res = device::CDev::init(); + if (res < 0) { + return res; + } + + res = _sub_air_data.start(AirDataCbBinder(this, &UavcanBarometerBridge::air_data_sub_cb)); if (res < 0) { log("failed to start uavcan sub: %d", res); return res; diff --git a/src/modules/uavcan/sensors/mag.cpp b/src/modules/uavcan/sensors/mag.cpp index aaa3a4463..8e6a9a22f 100644 --- a/src/modules/uavcan/sensors/mag.cpp +++ b/src/modules/uavcan/sensors/mag.cpp @@ -56,7 +56,12 @@ _sub_mag(node) int UavcanMagnetometerBridge::init() { - int res = _sub_mag.start(MagCbBinder(this, &UavcanMagnetometerBridge::mag_sub_cb)); + int res = device::CDev::init(); + if (res < 0) { + return res; + } + + res = _sub_mag.start(MagCbBinder(this, &UavcanMagnetometerBridge::mag_sub_cb)); if (res < 0) { log("failed to start uavcan sub: %d", res); return res; -- cgit v1.2.3 From 0f124963d4f0c07afa94d96b779a0d28b0fbd66f Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 23 Aug 2014 23:43:01 +0400 Subject: UAVCAN: Minor improvement of the GNSS bridge --- src/modules/uavcan/sensors/gnss.cpp | 57 ++++++++++++++++++------------------- src/modules/uavcan/sensors/gnss.hpp | 1 - 2 files changed, 28 insertions(+), 30 deletions(-) diff --git a/src/modules/uavcan/sensors/gnss.cpp b/src/modules/uavcan/sensors/gnss.cpp index f2bb28087..b3a9cb99b 100644 --- a/src/modules/uavcan/sensors/gnss.cpp +++ b/src/modules/uavcan/sensors/gnss.cpp @@ -66,9 +66,6 @@ int UavcanGnssBridge::init() return res; } - // Clear the uORB GPS report - memset(&_report, 0, sizeof(_report)); - warnx("gnss sensor bridge init ok"); return res; } @@ -90,12 +87,14 @@ void UavcanGnssBridge::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure 0) ? sqrtf(horizontal_pos_variance) : -1.0F; + report.eph = (horizontal_pos_variance > 0) ? sqrtf(horizontal_pos_variance) : -1.0F; // Vertical position uncertainty - _report.epv = (pos_cov[8] > 0) ? sqrtf(pos_cov[8]) : -1.0F; + report.epv = (pos_cov[8] > 0) ? sqrtf(pos_cov[8]) : -1.0F; } else { - _report.eph = -1.0F; - _report.epv = -1.0F; + report.eph = -1.0F; + report.epv = -1.0F; } if (valid_velocity_covariance) { float vel_cov[9]; msg.velocity_covariance.unpackSquareMatrix(vel_cov); - _report.s_variance_m_s = math::max(math::max(vel_cov[0], vel_cov[4]), vel_cov[8]); + report.s_variance_m_s = math::max(math::max(vel_cov[0], vel_cov[4]), vel_cov[8]); /* There is a nonlinear relationship between the velocity vector and the heading. * Use Jacobian to transform velocity covariance to heading covariance @@ -136,36 +135,36 @@ void UavcanGnssBridge::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure 0) { - orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &_report); + orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &report); } else { - _report_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report); + _report_pub = orb_advertise(ORB_ID(vehicle_gps_position), &report); } } diff --git a/src/modules/uavcan/sensors/gnss.hpp b/src/modules/uavcan/sensors/gnss.hpp index 9488c5fe5..c2b6e4195 100644 --- a/src/modules/uavcan/sensors/gnss.hpp +++ b/src/modules/uavcan/sensors/gnss.hpp @@ -80,7 +80,6 @@ private: uavcan::Subscriber _sub_fix; int _receiver_node_id = -1; - struct vehicle_gps_position_s _report; ///< uORB topic for gnss position orb_advert_t _report_pub; ///< uORB pub for gnss position }; -- cgit v1.2.3 From e9da8303161a1e1f012b7c5d770249c3d606fcbf Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 24 Aug 2014 00:06:47 +0400 Subject: UAVCAN: initializing all bridges by default --- src/modules/uavcan/sensors/gnss.cpp | 7 +- src/modules/uavcan/sensors/sensor_bridge.cpp | 24 +----- src/modules/uavcan/sensors/sensor_bridge.hpp | 7 +- src/modules/uavcan/uavcan_main.cpp | 111 ++++++--------------------- src/modules/uavcan/uavcan_main.hpp | 6 +- 5 files changed, 29 insertions(+), 126 deletions(-) diff --git a/src/modules/uavcan/sensors/gnss.cpp b/src/modules/uavcan/sensors/gnss.cpp index b3a9cb99b..8548660fe 100644 --- a/src/modules/uavcan/sensors/gnss.cpp +++ b/src/modules/uavcan/sensors/gnss.cpp @@ -56,17 +56,12 @@ _report_pub(-1) int UavcanGnssBridge::init() { - int res = -1; - - // GNSS fix subscription - res = _sub_fix.start(FixCbBinder(this, &UavcanGnssBridge::gnss_fix_sub_cb)); + int res = _sub_fix.start(FixCbBinder(this, &UavcanGnssBridge::gnss_fix_sub_cb)); if (res < 0) { warnx("GNSS fix sub failed %i", res); return res; } - - warnx("gnss sensor bridge init ok"); return res; } diff --git a/src/modules/uavcan/sensors/sensor_bridge.cpp b/src/modules/uavcan/sensors/sensor_bridge.cpp index 05d589b58..a69514d77 100644 --- a/src/modules/uavcan/sensors/sensor_bridge.cpp +++ b/src/modules/uavcan/sensors/sensor_bridge.cpp @@ -45,27 +45,11 @@ /* * IUavcanSensorBridge */ -IUavcanSensorBridge* IUavcanSensorBridge::make(uavcan::INode &node, const char *bridge_name) +void IUavcanSensorBridge::make_all(uavcan::INode &node, List &list) { - /* - * TODO: make a linked list of known implementations at startup? - */ - if (!std::strncmp(UavcanGnssBridge::NAME, bridge_name, MAX_NAME_LEN)) { - return new UavcanGnssBridge(node); - } else if (!std::strncmp(UavcanMagnetometerBridge::NAME, bridge_name, MAX_NAME_LEN)) { - return new UavcanMagnetometerBridge(node); - } else if (!std::strncmp(UavcanBarometerBridge::NAME, bridge_name, MAX_NAME_LEN)) { - return new UavcanBarometerBridge(node); - } else { - return nullptr; - } -} - -void IUavcanSensorBridge::print_known_names(const char *prefix) -{ - printf("%s%s\n", prefix, UavcanGnssBridge::NAME); - printf("%s%s\n", prefix, UavcanMagnetometerBridge::NAME); - printf("%s%s\n", prefix, UavcanBarometerBridge::NAME); + list.add(new UavcanBarometerBridge(node)); + list.add(new UavcanMagnetometerBridge(node)); + list.add(new UavcanGnssBridge(node)); } /* diff --git a/src/modules/uavcan/sensors/sensor_bridge.hpp b/src/modules/uavcan/sensors/sensor_bridge.hpp index d27ccb8a0..a13d0ab35 100644 --- a/src/modules/uavcan/sensors/sensor_bridge.hpp +++ b/src/modules/uavcan/sensors/sensor_bridge.hpp @@ -73,12 +73,7 @@ public: * Creates a bridge object by its ASCII name, e.g. "gnss", "mag". * @return nullptr if such bridge can't be created. */ - static IUavcanSensorBridge* make(uavcan::INode &node, const char *bridge_name); - - /** - * Prints all valid bridge names into stdout via printf(), one name per line with prefix. - */ - static void print_known_names(const char *prefix); + static void make_all(uavcan::INode &node, List &list); }; /** diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index a15f83696..19b54b9cf 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -217,11 +217,12 @@ int UavcanNode::init(uavcan::NodeID node_id) { int ret = -1; - /* do regular cdev init */ + // Do regular cdev init ret = CDev::init(); - if (ret != OK) + if (ret != OK) { return ret; + } _node.setName("org.pixhawk.pixhawk"); @@ -229,10 +230,24 @@ int UavcanNode::init(uavcan::NodeID node_id) fill_node_info(); - /* initializing the bridges UAVCAN <--> uORB */ + // Actuators ret = _esc_controller.init(); - if (ret < 0) + if (ret < 0) { return ret; + } + + // Sensor bridges + IUavcanSensorBridge::make_all(_node, _sensor_bridges); + auto br = _sensor_bridges.getHead(); + while (br != nullptr) { + ret = br->init(); + if (ret < 0) { + warnx("cannot init sensor bridge '%s' (%d)", br->get_name(), ret); + return ret; + } + warnx("sensor bridge '%s' init ok", br->get_name()); + br = br->getSibling(); + } return _node.start(); } @@ -522,62 +537,10 @@ UavcanNode::print_info() warnx("not running, start first"); } - warnx("actuators control groups: sub: %u / req: %u / fds: %u", (unsigned)_groups_subscribed, (unsigned)_groups_required, _poll_fds_num); - warnx("mixer: %s", (_mixers == nullptr) ? "NONE" : "OK"); -} - -int UavcanNode::sensor_enable(const char *bridge_name) -{ - int retval = -1; - - (void)pthread_mutex_lock(&_node_mutex); - - // Checking if such bridge already exists - { - auto pos = _sensor_bridges.getHead(); - while (pos != nullptr) { - if (!std::strncmp(pos->get_name(), bridge_name, IUavcanSensorBridge::MAX_NAME_LEN)) { - warnx("sensor bridge '%s' already exists", bridge_name); - retval = -1; - goto leave; - } - pos = pos->getSibling(); - } - } - - // Creating and initing - { - auto bridge = IUavcanSensorBridge::make(get_node(), bridge_name); - if (bridge == nullptr) { - warnx("cannot create sensor bridge '%s'", bridge_name); - retval = -1; - goto leave; - } - - assert(!std::strncmp(bridge->get_name(), bridge_name, IUavcanSensorBridge::MAX_NAME_LEN)); - - retval = bridge->init(); - if (retval >= 0) { - _sensor_bridges.add(bridge); - } else { - delete bridge; - } - } - -leave: - (void)pthread_mutex_unlock(&_node_mutex); - return retval; -} - -void UavcanNode::sensor_print_enabled() -{ (void)pthread_mutex_lock(&_node_mutex); - auto pos = _sensor_bridges.getHead(); - while (pos != nullptr) { - warnx("%s", pos->get_name()); - pos = pos->getSibling(); - } + warnx("actuators control groups: sub: %u / req: %u / fds: %u", (unsigned)_groups_subscribed, (unsigned)_groups_required, _poll_fds_num); + warnx("mixer: %s", (_mixers == nullptr) ? "NONE" : "OK"); (void)pthread_mutex_unlock(&_node_mutex); } @@ -588,12 +551,7 @@ void UavcanNode::sensor_print_enabled() static void print_usage() { warnx("usage: \n" - "\tuavcan start\n" - "\tuavcan sensor enable \n" - "\tuavcan sensor list"); - - warnx("known sensor bridges:"); - IUavcanSensorBridge::print_known_names("\t"); + "\tuavcan {start|status|stop}"); } extern "C" __EXPORT int uavcan_main(int argc, char *argv[]); @@ -645,31 +603,6 @@ int uavcan_main(int argc, char *argv[]) ::exit(0); } - if (!std::strcmp(argv[1], "sensor")) { - if (argc < 3) { - print_usage(); - ::exit(1); - } - - if (!std::strcmp(argv[2], "list")) { - inst->sensor_print_enabled(); - ::exit(0); - } - - if (argc < 4) { - print_usage(); - ::exit(1); - } - - if (!std::strcmp(argv[2], "enable")) { - const int res = inst->sensor_enable(argv[3]); - if (res < 0) { - warnx("failed to enable sensor '%s': error %d", argv[3], res); - } - ::exit(0); - } - } - print_usage(); ::exit(1); } diff --git a/src/modules/uavcan/uavcan_main.hpp b/src/modules/uavcan/uavcan_main.hpp index bca1aa530..be7db9741 100644 --- a/src/modules/uavcan/uavcan_main.hpp +++ b/src/modules/uavcan/uavcan_main.hpp @@ -89,10 +89,6 @@ public: void print_info(); - int sensor_enable(const char *bridge_name); - - void sensor_print_enabled(); - static UavcanNode* instance() { return _instance; } private: @@ -115,7 +111,7 @@ private: UavcanEscController _esc_controller; - List _sensor_bridges; ///< Append-only list of active sensor bridges + List _sensor_bridges; ///< List of active sensor bridges MixerGroup *_mixers = nullptr; -- cgit v1.2.3 From 701bd803ce2b7b212b53704453ee9a493d473d2d Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 24 Aug 2014 00:20:57 +0400 Subject: UAVCAN status reporting and proper termination --- src/modules/uavcan/uavcan_main.cpp | 21 +++++++++++++++++++-- 1 file changed, 19 insertions(+), 2 deletions(-) diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index 19b54b9cf..482fec1e0 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -106,6 +106,14 @@ UavcanNode::~UavcanNode() ::close(_armed_sub); + // Removing the sensor bridges + auto br = _sensor_bridges.getHead(); + while (br != nullptr) { + auto next = br->getSibling(); + delete br; + br = next; + } + _instance = nullptr; } @@ -539,8 +547,17 @@ UavcanNode::print_info() (void)pthread_mutex_lock(&_node_mutex); - warnx("actuators control groups: sub: %u / req: %u / fds: %u", (unsigned)_groups_subscribed, (unsigned)_groups_required, _poll_fds_num); - warnx("mixer: %s", (_mixers == nullptr) ? "NONE" : "OK"); + // ESC mixer status + warnx("ESC actuators control groups: sub: %u / req: %u / fds: %u", + (unsigned)_groups_subscribed, (unsigned)_groups_required, _poll_fds_num); + warnx("ESC mixer: %s", (_mixers == nullptr) ? "NONE" : "OK"); + + // Sensor bridges + auto br = _sensor_bridges.getHead(); + while (br != nullptr) { + warnx("Sensor '%s': channels: %u", br->get_name(), br->get_num_redundant_channels()); + br = br->getSibling(); + } (void)pthread_mutex_unlock(&_node_mutex); } -- cgit v1.2.3 From 3a029926b432d531f8e85ff73c0578750c171d75 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 23 Aug 2014 22:36:40 +0200 Subject: vfr_hud mavlink msg: use baro alt The vfr_hud message demands the AMSL altitude and not the wgs84 altitude. Use the baro altitude for now. This can be changed to an output of the position estimator later. --- src/modules/mavlink/mavlink_messages.cpp | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index af4d46a36..5a92004a6 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -774,6 +774,9 @@ private: MavlinkOrbSubscription *_airspeed_sub; uint64_t _airspeed_time; + MavlinkOrbSubscription *_sensor_combined_sub; + uint64_t _sensor_combined_time; + /* do not allow top copying this class */ MavlinkStreamVFRHUD(MavlinkStreamVFRHUD &); MavlinkStreamVFRHUD& operator = (const MavlinkStreamVFRHUD &); @@ -789,7 +792,9 @@ protected: _act_sub(_mavlink->add_orb_subscription(ORB_ID(actuator_controls_0))), _act_time(0), _airspeed_sub(_mavlink->add_orb_subscription(ORB_ID(airspeed))), - _airspeed_time(0) + _airspeed_time(0), + _sensor_combined_sub(_mavlink->add_orb_subscription(ORB_ID(sensor_combined))), + _sensor_combined_time(0) {} void send(const hrt_abstime t) @@ -799,12 +804,14 @@ protected: struct actuator_armed_s armed; struct actuator_controls_s act; struct airspeed_s airspeed; + struct sensor_combined_s sensor_combined; bool updated = _att_sub->update(&_att_time, &att); updated |= _pos_sub->update(&_pos_time, &pos); updated |= _armed_sub->update(&_armed_time, &armed); updated |= _act_sub->update(&_act_time, &act); updated |= _airspeed_sub->update(&_airspeed_time, &airspeed); + updated |= _sensor_combined_sub->update(&_sensor_combined_time, &sensor_combined); if (updated) { mavlink_vfr_hud_t msg; @@ -813,7 +820,7 @@ protected: msg.groundspeed = sqrtf(pos.vel_n * pos.vel_n + pos.vel_e * pos.vel_e); msg.heading = _wrap_2pi(att.yaw) * M_RAD_TO_DEG_F; msg.throttle = armed.armed ? act.control[3] * 100.0f : 0.0f; - msg.alt = pos.alt; + msg.alt = sensor_combined.baro_alt_meter; msg.climb = -pos.vel_d; _mavlink->send_message(MAVLINK_MSG_ID_VFR_HUD, &msg); -- cgit v1.2.3 From 1fa49aaea98c18a00ec5bc6e227b46ac19fe66a1 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 24 Aug 2014 01:41:54 +0400 Subject: UAVCAN: clarification --- ROMFS/px4fmu_common/init.d/rc.uavcan | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.uavcan b/ROMFS/px4fmu_common/init.d/rc.uavcan index abb76b400..9a470a6b8 100644 --- a/ROMFS/px4fmu_common/init.d/rc.uavcan +++ b/ROMFS/px4fmu_common/init.d/rc.uavcan @@ -7,7 +7,9 @@ if param compare UAVCAN_ENABLE 1 then if uavcan start then - sleep 1 # Sensor autodetection delay + # First sensor publisher to initialize takes lowest instance ID + # This delay ensures that UAVCAN-interfaced sensors would be allocated on lowest instance IDs + sleep 1 echo "[init] UAVCAN started" else echo "[init] ERROR: Could not start UAVCAN" -- cgit v1.2.3 From 3866b5a5fe1c6bc9d8b56ef2d603b8c88f1a295d Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 24 Aug 2014 03:02:52 +0400 Subject: Resource leak fix --- src/modules/uavcan/sensors/sensor_bridge.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/modules/uavcan/sensors/sensor_bridge.cpp b/src/modules/uavcan/sensors/sensor_bridge.cpp index a69514d77..a98596f9c 100644 --- a/src/modules/uavcan/sensors/sensor_bridge.cpp +++ b/src/modules/uavcan/sensors/sensor_bridge.cpp @@ -118,6 +118,7 @@ void UavcanCDevSensorBridgeBase::publish(const int redundancy_channel_id, const channel->orb_advert = orb_advertise(channel->orb_id, report); if (channel->orb_advert < 0) { log("ADVERTISE FAILED"); + (void)unregister_class_devname(_class_devname, class_instance); *channel = Channel(); return; } -- cgit v1.2.3 From 8262739b6219f54e7ea31de93cd81304df311ab9 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 24 Aug 2014 11:14:15 +0200 Subject: geofence: can select gps instead of global position --- src/modules/navigator/geofence.cpp | 23 ++++++++++++++++++- src/modules/navigator/geofence.h | 14 +++++++++++- src/modules/navigator/geofence_params.c | 17 ++++++++++++-- src/modules/navigator/navigator.h | 9 ++++++++ src/modules/navigator/navigator_main.cpp | 38 +++++++++++++++++++++++++------- 5 files changed, 89 insertions(+), 12 deletions(-) diff --git a/src/modules/navigator/geofence.cpp b/src/modules/navigator/geofence.cpp index 7897c7ba0..4a02a0c3f 100644 --- a/src/modules/navigator/geofence.cpp +++ b/src/modules/navigator/geofence.cpp @@ -63,7 +63,8 @@ Geofence::Geofence() : _altitude_max(0), _verticesCount(0), _param_geofence_on(this, "ON"), - _param_altitude_mode(this, "ALTMODE") + _param_altitude_mode(this, "ALTMODE"), + _param_source(this, "SOURCE") { /* Load initial params */ updateParams(); @@ -85,6 +86,26 @@ bool Geofence::inside(const struct vehicle_global_position_s &global_position, f return inside(global_position.lat, global_position.lon, baro_altitude_amsl); } + +bool Geofence::inside(const struct vehicle_global_position_s &global_position, + const struct vehicle_gps_position_s &gps_position,float baro_altitude_amsl) { + if (getAltitudeMode() == Geofence::GF_ALT_MODE_WGS84) { + if (getSource() == Geofence::GF_SOURCE_GLOBALPOS) { + return inside(global_position); + } else { + return inside((double)gps_position.lat * 1.0e-7, (double)gps_position.lon * 1.0e-7, + (double)gps_position.alt * 1.0e-3); + } + } else { + if (getSource() == Geofence::GF_SOURCE_GLOBALPOS) { + return inside(global_position, baro_altitude_amsl); + } else { + return inside((double)gps_position.lat * 1.0e-7, (double)gps_position.lon * 1.0e-7, + baro_altitude_amsl); + } + } +} + bool Geofence::inside(double lat, double lon, float altitude) { /* Return true if geofence is disabled */ diff --git a/src/modules/navigator/geofence.h b/src/modules/navigator/geofence.h index e2c0f08d8..91c74572e 100644 --- a/src/modules/navigator/geofence.h +++ b/src/modules/navigator/geofence.h @@ -43,6 +43,7 @@ #include #include +#include #include #include #include @@ -57,10 +58,16 @@ public: /* Altitude mode, corresponding to the param GF_ALTMODE */ enum { - GF_ALT_MODE_GPS = 0, + GF_ALT_MODE_WGS84 = 0, GF_ALT_MODE_AMSL = 1 }; + /* Source, corresponding to the param GF_SOURCE */ + enum { + GF_SOURCE_GLOBALPOS = 0, + GF_SOURCE_GPS = 1 + }; + /** * Return whether system is inside geofence. * @@ -71,6 +78,8 @@ public: bool inside(const struct vehicle_global_position_s &global_position); bool inside(const struct vehicle_global_position_s &global_position, float baro_altitude_amsl); bool inside(double lat, double lon, float altitude); + bool inside(const struct vehicle_global_position_s &global_position, + const struct vehicle_gps_position_s &gps_position,float baro_altitude_amsl); int clearDm(); @@ -88,6 +97,8 @@ public: bool isEmpty() {return _verticesCount == 0;} int getAltitudeMode() { return _param_altitude_mode.get(); } + + int getSource() { return _param_source.get(); } private: orb_advert_t _fence_pub; /**< publish fence topic */ @@ -100,6 +111,7 @@ private: /* Params */ control::BlockParamInt _param_geofence_on; control::BlockParamInt _param_altitude_mode; + control::BlockParamInt _param_source; }; diff --git a/src/modules/navigator/geofence_params.c b/src/modules/navigator/geofence_params.c index 29b42cd54..32902ee97 100644 --- a/src/modules/navigator/geofence_params.c +++ b/src/modules/navigator/geofence_params.c @@ -63,10 +63,23 @@ PARAM_DEFINE_INT32(GF_ON, 1); * Geofence altitude mode * * Select which altitude reference should be used - * 0 = GPS, 1 = AMSL + * 0 = WGS84, 1 = AMSL * * @min 0 * @max 1 * @group Geofence */ -PARAM_DEFINE_INT32(GF_ALTMODE, 1); +PARAM_DEFINE_INT32(GF_ALTMODE, 0); + +/** + * Geofence source + * + * Select which position source should be used. Selecting GPS instead of global position makes sure that there is + * no dependence on the position estimator + * 0 = global position, 1 = GPS + * + * @min 0 + * @max 1 + * @group Geofence + */ +PARAM_DEFINE_INT32(GF_SOURCE, 0); diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index 709e3e724..840b43f1b 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -51,6 +51,7 @@ #include #include #include +#include #include #include #include @@ -129,6 +130,7 @@ public: struct vehicle_status_s* get_vstatus() { return &_vstatus; } struct vehicle_control_mode_s* get_control_mode() { return &_control_mode; } struct vehicle_global_position_s* get_global_position() { return &_global_pos; } + struct vehicle_gps_position_s* get_gps_position() { return &_gps_pos; } struct sensor_combined_s* get_sensor_combined() { return &_sensor_combined; } struct home_position_s* get_home_position() { return &_home_pos; } struct position_setpoint_triplet_s* get_position_setpoint_triplet() { return &_pos_sp_triplet; } @@ -152,6 +154,7 @@ private: int _mavlink_fd; /**< the file descriptor to send messages over mavlink */ int _global_pos_sub; /**< global position subscription */ + int _gps_pos_sub; /**< gps position subscription */ int _sensor_combined_sub; /**< sensor combined subscription */ int _home_pos_sub; /**< home position subscription */ int _vstatus_sub; /**< vehicle status subscription */ @@ -171,6 +174,7 @@ private: vehicle_status_s _vstatus; /**< vehicle status */ vehicle_control_mode_s _control_mode; /**< vehicle control mode */ vehicle_global_position_s _global_pos; /**< global vehicle position */ + vehicle_gps_position_s _gps_pos; /**< gps position */ sensor_combined_s _sensor_combined; /**< sensor values */ home_position_s _home_pos; /**< home position for RTL */ mission_item_s _mission_item; /**< current mission item */ @@ -216,6 +220,11 @@ private: */ void global_position_update(); + /** + * Retrieve gps position + */ + void gps_position_update(); + /** * Retrieve sensor values */ diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index c569ee7b5..81ceaaca2 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -100,6 +100,7 @@ Navigator::Navigator() : _navigator_task(-1), _mavlink_fd(-1), _global_pos_sub(-1), + _gps_pos_sub(-1), _home_pos_sub(-1), _vstatus_sub(-1), _capabilities_sub(-1), @@ -114,6 +115,7 @@ Navigator::Navigator() : _vstatus{}, _control_mode{}, _global_pos{}, + _gps_pos{}, _sensor_combined{}, _home_pos{}, _mission_item{}, @@ -187,6 +189,12 @@ Navigator::global_position_update() orb_copy(ORB_ID(vehicle_global_position), _global_pos_sub, &_global_pos); } +void +Navigator::gps_position_update() +{ + orb_copy(ORB_ID(vehicle_gps_position), _gps_pos_sub, &_gps_pos); +} + void Navigator::sensor_combined_update() { @@ -263,6 +271,7 @@ Navigator::task_main() /* do subscriptions */ _global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); + _gps_pos_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); _sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined)); _capabilities_sub = orb_subscribe(ORB_ID(navigation_capabilities)); _vstatus_sub = orb_subscribe(ORB_ID(vehicle_status)); @@ -277,6 +286,7 @@ Navigator::task_main() vehicle_status_update(); vehicle_control_mode_update(); global_position_update(); + gps_position_update(); sensor_combined_update(); home_position_update(); navigation_capabilities_update(); @@ -289,7 +299,7 @@ Navigator::task_main() const hrt_abstime mavlink_open_interval = 500000; /* wakeup source(s) */ - struct pollfd fds[7]; + struct pollfd fds[8]; /* Setup of loop */ fds[0].fd = _global_pos_sub; @@ -306,6 +316,8 @@ Navigator::task_main() fds[5].events = POLLIN; fds[6].fd = _sensor_combined_sub; fds[6].events = POLLIN; + fds[7].fd = _gps_pos_sub; + fds[7].events = POLLIN; while (!_task_should_exit) { @@ -330,6 +342,16 @@ Navigator::task_main() _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); } + bool have_geofence_position_data = false; + + /* gps updated */ + if (fds[7].revents & POLLIN) { + gps_position_update(); + if (_geofence.getSource() == Geofence::GF_SOURCE_GPS) { + have_geofence_position_data = true; + } + } + /* sensors combined updated */ if (fds[6].revents & POLLIN) { sensor_combined_update(); @@ -364,14 +386,14 @@ Navigator::task_main() /* global position updated */ if (fds[0].revents & POLLIN) { global_position_update(); - - /* Check geofence violation */ - bool inside = false; - if (_geofence.getAltitudeMode() == Geofence::GF_ALT_MODE_GPS) { - inside = _geofence.inside(_global_pos); - } else { - inside = _geofence.inside(_global_pos, _sensor_combined.baro_alt_meter); + if (_geofence.getSource() == Geofence::GF_SOURCE_GLOBALPOS) { + have_geofence_position_data = true; } + } + + /* Check geofence violation */ + if (have_geofence_position_data) { + bool inside = _geofence.inside(_global_pos, _gps_pos, _sensor_combined.baro_alt_meter); if (!inside) { /* inform other apps via the sp triplet */ _pos_sp_triplet.geofence_violated = true; -- cgit v1.2.3 From 81adc52671d920ffe184948267fcc1f9fbb027cc Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 24 Aug 2014 11:30:02 +0200 Subject: geofence: add counter threshold for subsequent detections --- src/modules/navigator/geofence.cpp | 22 +++++++++++++++++++++- src/modules/navigator/geofence.h | 8 ++++++-- src/modules/navigator/geofence_params.c | 11 +++++++++++ 3 files changed, 38 insertions(+), 3 deletions(-) diff --git a/src/modules/navigator/geofence.cpp b/src/modules/navigator/geofence.cpp index 4a02a0c3f..a3805dc0f 100644 --- a/src/modules/navigator/geofence.cpp +++ b/src/modules/navigator/geofence.cpp @@ -64,7 +64,9 @@ Geofence::Geofence() : _verticesCount(0), _param_geofence_on(this, "ON"), _param_altitude_mode(this, "ALTMODE"), - _param_source(this, "SOURCE") + _param_source(this, "SOURCE"), + _param_counter_threshold(this, "COUNT"), + _outside_counter(0) { /* Load initial params */ updateParams(); @@ -107,6 +109,24 @@ bool Geofence::inside(const struct vehicle_global_position_s &global_position, } bool Geofence::inside(double lat, double lon, float altitude) +{ + bool inside_fence = inside_polygon(lat, lon, altitude); + + if (inside_fence) { + _outside_counter = 0; + return inside_fence; + } { + _outside_counter++; + if(_outside_counter > _param_counter_threshold.get()) { + return inside_fence; + } else { + return true; + } + } +} + + +bool Geofence::inside_polygon(double lat, double lon, float altitude) { /* Return true if geofence is disabled */ if (_param_geofence_on.get() != 1) diff --git a/src/modules/navigator/geofence.h b/src/modules/navigator/geofence.h index 91c74572e..65ebb0c3d 100644 --- a/src/modules/navigator/geofence.h +++ b/src/modules/navigator/geofence.h @@ -77,9 +77,10 @@ public: */ bool inside(const struct vehicle_global_position_s &global_position); bool inside(const struct vehicle_global_position_s &global_position, float baro_altitude_amsl); - bool inside(double lat, double lon, float altitude); bool inside(const struct vehicle_global_position_s &global_position, const struct vehicle_gps_position_s &gps_position,float baro_altitude_amsl); + bool inside(double lat, double lon, float altitude); + bool inside_polygon(double lat, double lon, float altitude); int clearDm(); @@ -97,7 +98,7 @@ public: bool isEmpty() {return _verticesCount == 0;} int getAltitudeMode() { return _param_altitude_mode.get(); } - + int getSource() { return _param_source.get(); } private: @@ -112,6 +113,9 @@ private: control::BlockParamInt _param_geofence_on; control::BlockParamInt _param_altitude_mode; control::BlockParamInt _param_source; + control::BlockParamInt _param_counter_threshold; + + uint8_t _outside_counter; }; diff --git a/src/modules/navigator/geofence_params.c b/src/modules/navigator/geofence_params.c index 32902ee97..fca3918e1 100644 --- a/src/modules/navigator/geofence_params.c +++ b/src/modules/navigator/geofence_params.c @@ -83,3 +83,14 @@ PARAM_DEFINE_INT32(GF_ALTMODE, 0); * @group Geofence */ PARAM_DEFINE_INT32(GF_SOURCE, 0); + +/** + * Geofence counter limit + * + * Set how many subsequent position measurements outside of the fence are needed before geofence violation is triggered + * + * @min -1 + * @max 10 + * @group Geofence + */ +PARAM_DEFINE_INT32(GF_COUNT, -1); -- cgit v1.2.3 From 127948f32f24e36c0c03a047b57fcd64287d9967 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 24 Aug 2014 11:49:45 +0200 Subject: Remove absolute pressure field as its not useful and confusing anywary --- src/drivers/ets_airspeed/ets_airspeed.cpp | 20 ++++++-------------- src/drivers/meas_airspeed/meas_airspeed.cpp | 22 +++++----------------- src/modules/sensors/sensors.cpp | 14 +++++++------- src/modules/uORB/topics/differential_pressure.h | 1 - 4 files changed, 18 insertions(+), 39 deletions(-) diff --git a/src/drivers/ets_airspeed/ets_airspeed.cpp b/src/drivers/ets_airspeed/ets_airspeed.cpp index f98d615a2..0f77bb805 100644 --- a/src/drivers/ets_airspeed/ets_airspeed.cpp +++ b/src/drivers/ets_airspeed/ets_airspeed.cpp @@ -155,7 +155,6 @@ ETSAirspeed::collect() } uint16_t diff_pres_pa_raw = val[1] << 8 | val[0]; - uint16_t diff_pres_pa; if (diff_pres_pa_raw == 0) { // a zero value means the pressure sensor cannot give us a // value. We need to return, and not report a value or the @@ -166,28 +165,21 @@ ETSAirspeed::collect() return -1; } - if (diff_pres_pa_raw < _diff_pres_offset + MIN_ACCURATE_DIFF_PRES_PA) { - diff_pres_pa = 0; - } else { - diff_pres_pa = diff_pres_pa_raw - _diff_pres_offset; - } - // The raw value still should be compensated for the known offset diff_pres_pa_raw -= _diff_pres_offset; // Track maximum differential pressure measured (so we can work out top speed). - if (diff_pres_pa > _max_differential_pressure_pa) { - _max_differential_pressure_pa = diff_pres_pa; + if (diff_pres_pa_raw > _max_differential_pressure_pa) { + _max_differential_pressure_pa = diff_pres_pa_raw; } differential_pressure_s report; report.timestamp = hrt_absolute_time(); report.error_count = perf_event_count(_comms_errors); - report.differential_pressure_pa = (float)diff_pres_pa; // XXX we may want to smooth out the readings to remove noise. - report.differential_pressure_filtered_pa = (float)diff_pres_pa; - report.differential_pressure_raw_pa = (float)diff_pres_pa_raw; + report.differential_pressure_filtered_pa = diff_pres_pa_raw; + report.differential_pressure_raw_pa = diff_pres_pa_raw; report.temperature = -1000.0f; report.max_differential_pressure_pa = _max_differential_pressure_pa; @@ -369,7 +361,7 @@ test() err(1, "immediate read failed"); warnx("single read"); - warnx("diff pressure: %f pa", (double)report.differential_pressure_pa); + warnx("diff pressure: %f pa", (double)report.differential_pressure_filtered_pa); /* start the sensor polling at 2Hz */ if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) @@ -394,7 +386,7 @@ test() err(1, "periodic read failed"); warnx("periodic read %u", i); - warnx("diff pressure: %f pa", (double)report.differential_pressure_pa); + warnx("diff pressure: %f pa", (double)report.differential_pressure_filtered_pa); } /* reset the sensor polling to its default rate */ diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp index 479fa3ccf..1d9a463ad 100644 --- a/src/drivers/meas_airspeed/meas_airspeed.cpp +++ b/src/drivers/meas_airspeed/meas_airspeed.cpp @@ -228,18 +228,7 @@ MEASAirspeed::collect() // the raw value still should be compensated for the known offset diff_press_pa_raw -= _diff_pres_offset; - /* don't take the absolute value because the calibration takes this into account and warns the user if the - * tubes are connected backwards */ - float diff_press_pa = diff_press_pa_raw; - /* - note that we return both the absolute value with offset - applied and a raw value without the offset applied. This - makes it possible for higher level code to detect if the - user has the tubes connected backwards, and also makes it - possible to correctly use offsets calculated by a higher - level airspeed driver. - With the above calculation the MS4525 sensor will produce a positive number when the top port is used as a dynamic port and bottom port is used as the static port @@ -248,15 +237,14 @@ MEASAirspeed::collect() struct differential_pressure_s report; /* track maximum differential pressure measured (so we can work out top speed). */ - if (diff_press_pa > _max_differential_pressure_pa) { - _max_differential_pressure_pa = diff_press_pa; + if (diff_press_pa_raw > _max_differential_pressure_pa) { + _max_differential_pressure_pa = diff_press_pa_raw; } report.timestamp = hrt_absolute_time(); report.error_count = perf_event_count(_comms_errors); report.temperature = temperature; - report.differential_pressure_pa = diff_press_pa; - report.differential_pressure_filtered_pa = _filter.apply(diff_press_pa); + report.differential_pressure_filtered_pa = _filter.apply(diff_press_pa_raw); report.differential_pressure_raw_pa = diff_press_pa_raw; report.max_differential_pressure_pa = _max_differential_pressure_pa; @@ -514,7 +502,7 @@ test() } warnx("single read"); - warnx("diff pressure: %d pa", (int)report.differential_pressure_pa); + warnx("diff pressure: %d pa", (int)report.differential_pressure_filtered_pa); /* start the sensor polling at 2Hz */ if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) { @@ -542,7 +530,7 @@ test() } warnx("periodic read %u", i); - warnx("diff pressure: %d pa", (int)report.differential_pressure_pa); + warnx("diff pressure: %d pa", (int)report.differential_pressure_filtered_pa); warnx("temperature: %d C (0x%02x)", (int)report.temperature, (unsigned) report.temperature); } diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index f40034d79..cdcb428dd 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -1229,16 +1229,18 @@ Sensors::diff_pres_poll(struct sensor_combined_s &raw) if (updated) { orb_copy(ORB_ID(differential_pressure), _diff_pres_sub, &_diff_pres); - raw.differential_pressure_pa = _diff_pres.differential_pressure_pa; + raw.differential_pressure_pa = _diff_pres.differential_pressure_raw_pa; raw.differential_pressure_timestamp = _diff_pres.timestamp; raw.differential_pressure_filtered_pa = _diff_pres.differential_pressure_filtered_pa; float air_temperature_celsius = (_diff_pres.temperature > -300.0f) ? _diff_pres.temperature : (raw.baro_temp_celcius - PCB_TEMP_ESTIMATE_DEG); _airspeed.timestamp = _diff_pres.timestamp; - _airspeed.indicated_airspeed_m_s = calc_indicated_airspeed(_diff_pres.differential_pressure_filtered_pa); - _airspeed.true_airspeed_m_s = calc_true_airspeed(_diff_pres.differential_pressure_filtered_pa + raw.baro_pres_mbar * 1e2f, - raw.baro_pres_mbar * 1e2f, air_temperature_celsius); + + /* don't risk to feed negative airspeed into the system */ + _airspeed.indicated_airspeed_m_s = math::max(0.0f, calc_indicated_airspeed(_diff_pres.differential_pressure_filtered_pa)); + _airspeed.true_airspeed_m_s = math::max(0.0f, calc_true_airspeed(_diff_pres.differential_pressure_filtered_pa + raw.baro_pres_mbar * 1e2f, + raw.baro_pres_mbar * 1e2f, air_temperature_celsius)); _airspeed.air_temperature_celsius = air_temperature_celsius; /* announce the airspeed if needed, just publish else */ @@ -1457,12 +1459,10 @@ Sensors::adc_poll(struct sensor_combined_s &raw) if (voltage > 0.4f && (_parameters.diff_pres_analog_scale > 0.0f)) { float diff_pres_pa_raw = voltage * _parameters.diff_pres_analog_scale - _parameters.diff_pres_offset_pa; - float diff_pres_pa = (diff_pres_pa_raw > 0.0f) ? diff_pres_pa_raw : 0.0f; _diff_pres.timestamp = t; - _diff_pres.differential_pressure_pa = diff_pres_pa; _diff_pres.differential_pressure_raw_pa = diff_pres_pa_raw; - _diff_pres.differential_pressure_filtered_pa = (_diff_pres.differential_pressure_filtered_pa * 0.9f) + (diff_pres_pa * 0.1f); + _diff_pres.differential_pressure_filtered_pa = (_diff_pres.differential_pressure_filtered_pa * 0.9f) + (diff_pres_pa_raw * 0.1f); _diff_pres.temperature = -1000.0f; /* announce the airspeed if needed, just publish else */ diff --git a/src/modules/uORB/topics/differential_pressure.h b/src/modules/uORB/topics/differential_pressure.h index cd48d3cb2..7342fcf04 100644 --- a/src/modules/uORB/topics/differential_pressure.h +++ b/src/modules/uORB/topics/differential_pressure.h @@ -54,7 +54,6 @@ struct differential_pressure_s { uint64_t timestamp; /**< Microseconds since system boot, needed to integrate */ uint64_t error_count; /**< Number of errors detected by driver */ - float differential_pressure_pa; /**< Differential pressure reading */ float differential_pressure_raw_pa; /**< Raw differential pressure reading (may be negative) */ float differential_pressure_filtered_pa; /**< Low pass filtered differential pressure reading */ float max_differential_pressure_pa; /**< Maximum differential pressure reading */ -- cgit v1.2.3 From ef0a0a1a6e86aff79a0fd8829ca5c86244fa39bc Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 24 Aug 2014 12:05:12 +0200 Subject: Fix drop offset: We want to drop so that the wind carries the bottle into the drop zone --- src/modules/bottle_drop/bottle_drop.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/bottle_drop/bottle_drop.cpp b/src/modules/bottle_drop/bottle_drop.cpp index 3eec5a879..65eca1a62 100644 --- a/src/modules/bottle_drop/bottle_drop.cpp +++ b/src/modules/bottle_drop/bottle_drop.cpp @@ -566,8 +566,8 @@ BottleDrop::task_main() wind_direction_e = wind.windspeed_east / windspeed_norm; } - x_drop = x_t + x * wind_direction_n; - y_drop = y_t + x * wind_direction_e; + x_drop = x_t - x * wind_direction_n; + y_drop = y_t - x * wind_direction_e; map_projection_reproject(&ref, x_drop, y_drop, &_drop_position.lat, &_drop_position.lon); _drop_position.alt = _target_position.alt + _alt_clearance; -- cgit v1.2.3 From 92cc44fe1e09f426087e91fff88effde04b32c5d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 24 Aug 2014 12:07:59 +0200 Subject: avoid changing the reset logic --- src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index b7e118d47..8fdc40d41 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 @@ -1221,7 +1221,12 @@ FixedwingEstimator::task_main() // We're apparently initialized in this case now // check (and reset the filter as needed) - (void)check_filter_state(); + int check = check_filter_state(); + + if (check) { + // Let the system re-initialize itself + continue; + } // Run the strapdown INS equations every IMU update _ekf->UpdateStrapdownEquationsNED(); -- cgit v1.2.3 From c037cfe6f2daef8ff96cad965c4b040a9d8c62f9 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 24 Aug 2014 12:29:30 +0200 Subject: datalink loss (obc): add termination after loitering at airfield home --- src/modules/navigator/datalinkloss.cpp | 25 ++++++++++++++++++++++++- src/modules/navigator/datalinkloss.h | 3 +++ src/modules/navigator/datalinkloss_params.c | 11 +++++++++++ src/modules/navigator/rcloss.cpp | 1 + 4 files changed, 39 insertions(+), 1 deletion(-) diff --git a/src/modules/navigator/datalinkloss.cpp b/src/modules/navigator/datalinkloss.cpp index 893d6d93a..3310984b0 100644 --- a/src/modules/navigator/datalinkloss.cpp +++ b/src/modules/navigator/datalinkloss.cpp @@ -64,6 +64,7 @@ DataLinkLoss::DataLinkLoss(Navigator *navigator, const char *name) : _param_airfieldhomelat(this, "NAV_AH_LAT", false), _param_airfieldhomelon(this, "NAV_AH_LON", false), _param_airfieldhomealt(this, "NAV_AH_ALT", false), + _param_airfieldhomewaittime(this, "AH_T"), _param_numberdatalinklosses(this, "N"), _dll_state(DLL_STATE_NONE) { @@ -140,7 +141,8 @@ DataLinkLoss::set_dll_item() _mission_item.yaw = NAN; _mission_item.loiter_radius = _navigator->get_loiter_radius(); _mission_item.loiter_direction = 1; - _mission_item.nav_cmd = NAV_CMD_LOITER_UNLIMITED; + _mission_item.nav_cmd = NAV_CMD_LOITER_TIME_LIMIT; + _mission_item.time_inside = _param_airfieldhomewaittime.get() < 0.0f ? 0.0f : _param_airfieldhomewaittime.get(); _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); _mission_item.pitch_min = 0.0f; _mission_item.autocontinue = true; @@ -149,6 +151,15 @@ DataLinkLoss::set_dll_item() _navigator->set_can_loiter_at_sp(true); break; } + case DLL_STATE_TERMINATE: { + /* Request flight termination from the commander */ + pos_sp_triplet->flight_termination = true; + warnx("not switched to manual: request flight termination"); + pos_sp_triplet->previous.valid = false; + pos_sp_triplet->current.valid = false; + pos_sp_triplet->next.valid = false; + break; + } default: break; } @@ -185,6 +196,18 @@ DataLinkLoss::advance_dll() _navigator->get_mission_result()->stay_in_failsafe = true; _navigator->publish_mission_result(); break; + case DLL_STATE_FLYTOAIRFIELDHOMEWP: + _dll_state = DLL_STATE_TERMINATE; + warnx("time is up, state should have been changed manually by now"); + mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: no manual control, terminating"); + _navigator->get_mission_result()->stay_in_failsafe = true; + _navigator->publish_mission_result(); + break; + case DLL_STATE_TERMINATE: + warnx("dll end"); + _dll_state = DLL_STATE_END; + break; + default: break; } diff --git a/src/modules/navigator/datalinkloss.h b/src/modules/navigator/datalinkloss.h index 96b4ce010..d0c9ad09a 100644 --- a/src/modules/navigator/datalinkloss.h +++ b/src/modules/navigator/datalinkloss.h @@ -72,12 +72,15 @@ private: control::BlockParamInt _param_airfieldhomelat; // * 1e7 control::BlockParamInt _param_airfieldhomelon; // * 1e7 control::BlockParamFloat _param_airfieldhomealt; + control::BlockParamFloat _param_airfieldhomewaittime; control::BlockParamInt _param_numberdatalinklosses; enum DLLState { DLL_STATE_NONE = 0, DLL_STATE_FLYTOCOMMSHOLDWP = 1, DLL_STATE_FLYTOAIRFIELDHOMEWP = 2, + DLL_STATE_TERMINATE = 3, + DLL_STATE_END = 4 } _dll_state; /** diff --git a/src/modules/navigator/datalinkloss_params.c b/src/modules/navigator/datalinkloss_params.c index 02f7ca4c3..db307c904 100644 --- a/src/modules/navigator/datalinkloss_params.c +++ b/src/modules/navigator/datalinkloss_params.c @@ -91,6 +91,17 @@ PARAM_DEFINE_INT32(NAV_DLL_CH_LON, 1518453890); */ PARAM_DEFINE_FLOAT(NAV_DLL_CH_ALT, 600.0f); +/** + * Aifield hole wait time + * + * The amount of time in seconds the system should wait at the airfield home waypoint + * + * @unit seconds + * @min 0.0 + * @group DLL + */ +PARAM_DEFINE_FLOAT(NAV_DLL_AH_T, 120.0f); + /** * Number of allowed Datalink timeouts * diff --git a/src/modules/navigator/rcloss.cpp b/src/modules/navigator/rcloss.cpp index 427001a01..54f1813f5 100644 --- a/src/modules/navigator/rcloss.cpp +++ b/src/modules/navigator/rcloss.cpp @@ -132,6 +132,7 @@ RCLoss::set_rcl_item() pos_sp_triplet->previous.valid = false; pos_sp_triplet->current.valid = false; pos_sp_triplet->next.valid = false; + break; } default: break; -- cgit v1.2.3 From ae7c99393606373e3a549946481fe07de6fb4c84 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 24 Aug 2014 12:40:19 +0200 Subject: datalink loss: add param to allow skipping of comms hold wp --- src/modules/navigator/datalinkloss.cpp | 24 +++++++++++++++++------- src/modules/navigator/datalinkloss.h | 1 + src/modules/navigator/datalinkloss_params.c | 14 +++++++++++++- 3 files changed, 31 insertions(+), 8 deletions(-) diff --git a/src/modules/navigator/datalinkloss.cpp b/src/modules/navigator/datalinkloss.cpp index 3310984b0..4e3d25840 100644 --- a/src/modules/navigator/datalinkloss.cpp +++ b/src/modules/navigator/datalinkloss.cpp @@ -66,6 +66,7 @@ DataLinkLoss::DataLinkLoss(Navigator *navigator, const char *name) : _param_airfieldhomealt(this, "NAV_AH_ALT", false), _param_airfieldhomewaittime(this, "AH_T"), _param_numberdatalinklosses(this, "N"), + _param_skipcommshold(this, "CHSK"), _dll_state(DLL_STATE_NONE) { /* load initial params */ @@ -179,14 +180,23 @@ DataLinkLoss::advance_dll() switch (_dll_state) { case DLL_STATE_NONE: /* Check the number of data link losses. If above home fly home directly */ - if (_navigator->get_vstatus()->data_link_lost_counter > _param_numberdatalinklosses.get()) { - warnx("%d data link losses, limit is %d, fly to airfield home", _navigator->get_vstatus()->data_link_lost_counter, _param_numberdatalinklosses.get()); - mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: too many DL losses, fly to home"); - _dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP; + if (!_param_skipcommshold.get()) { + /* if number of data link losses limit is not reached fly to comms hold wp */ + if (_navigator->get_vstatus()->data_link_lost_counter > _param_numberdatalinklosses.get()) { + warnx("%d data link losses, limit is %d, fly to airfield home", + _navigator->get_vstatus()->data_link_lost_counter, _param_numberdatalinklosses.get()); + mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: too many DL losses, fly to home"); + _dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP; + } else { + warnx("fly to comms hold, datalink loss counter: %d", _navigator->get_vstatus()->data_link_lost_counter); + mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: fly to comms hold"); + _dll_state = DLL_STATE_FLYTOCOMMSHOLDWP; + } } else { - warnx("fly to comms hold, datalink loss counter: %d", _navigator->get_vstatus()->data_link_lost_counter); - mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: fly to comms hold"); - _dll_state = DLL_STATE_FLYTOCOMMSHOLDWP; + /* comms hold wp not active, fly to airfield home directly */ + warnx("Skipping comms hold wp. Flying directly to airfield home"); + mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: fly to airfield home, comms hold skipped"); + _dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP; } break; case DLL_STATE_FLYTOCOMMSHOLDWP: diff --git a/src/modules/navigator/datalinkloss.h b/src/modules/navigator/datalinkloss.h index d0c9ad09a..31e0e3907 100644 --- a/src/modules/navigator/datalinkloss.h +++ b/src/modules/navigator/datalinkloss.h @@ -74,6 +74,7 @@ private: control::BlockParamFloat _param_airfieldhomealt; control::BlockParamFloat _param_airfieldhomewaittime; control::BlockParamInt _param_numberdatalinklosses; + control::BlockParamInt _param_skipcommshold; enum DLLState { DLL_STATE_NONE = 0, diff --git a/src/modules/navigator/datalinkloss_params.c b/src/modules/navigator/datalinkloss_params.c index db307c904..6780c0c88 100644 --- a/src/modules/navigator/datalinkloss_params.c +++ b/src/modules/navigator/datalinkloss_params.c @@ -107,8 +107,20 @@ PARAM_DEFINE_FLOAT(NAV_DLL_AH_T, 120.0f); * * After more than this number of data link timeouts the aircraft returns home directly * - * @group commander + * @group DLL * @min 0 * @max 1000 */ PARAM_DEFINE_INT32(NAV_DLL_N, 2); + +/** + * Skip comms hold wp + * + * If set to 1 the system will skip the comms hold wp on data link loss and will directly fly to + * airfield home + * + * @group DLL + * @min 0 + * @max 1 + */ +PARAM_DEFINE_INT32(NAV_DLL_CHSK, 0); -- cgit v1.2.3 From bf8956d2e87b29af25392b74a24ea0da8d422d4b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 24 Aug 2014 13:26:28 +0200 Subject: Be only reasonably strict on avionics supply voltage. --- src/modules/commander/state_machine_helper.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index f8589d24b..c9fc9218c 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -183,9 +183,9 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s // Fail transition if power levels on the avionics rail // are measured but are insufficient if (status->condition_power_input_valid && ((status->avionics_power_rail_voltage > 0.0f) && - (status->avionics_power_rail_voltage < 4.9f))) { + (status->avionics_power_rail_voltage < 4.75f))) { - mavlink_log_critical(mavlink_fd, "NOT ARMING: Avionics power low: %6.2f V.", (double)status->avionics_power_rail_voltage); + mavlink_log_critical(mavlink_fd, "NOT ARMING: Avionics power low: %6.2f Volt", (double)status->avionics_power_rail_voltage); feedback_provided = true; valid_transition = false; } -- cgit v1.2.3 From 64d3c48770860586d4755b1d2e865e607af4b25e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 24 Aug 2014 13:32:46 +0200 Subject: Add warning for non-standard avionics rail voltages --- src/modules/commander/state_machine_helper.cpp | 19 +++++++++++++------ 1 file changed, 13 insertions(+), 6 deletions(-) diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index c9fc9218c..684c61a17 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -182,12 +182,19 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s // Fail transition if power levels on the avionics rail // are measured but are insufficient - if (status->condition_power_input_valid && ((status->avionics_power_rail_voltage > 0.0f) && - (status->avionics_power_rail_voltage < 4.75f))) { - - mavlink_log_critical(mavlink_fd, "NOT ARMING: Avionics power low: %6.2f Volt", (double)status->avionics_power_rail_voltage); - feedback_provided = true; - valid_transition = false; + if (status->condition_power_input_valid && (status->avionics_power_rail_voltage > 0.0f)) { + // Check avionics rail voltages + if (status->avionics_power_rail_voltage < 4.75f) { + mavlink_log_critical(mavlink_fd, "NOT ARMING: Avionics power low: %6.2f Volt", (double)status->avionics_power_rail_voltage); + feedback_provided = true; + valid_transition = false; + } else if (status->avionics_power_rail_voltage < 4.9f) { + mavlink_log_critical(mavlink_fd, "CAUTION: Avionics power low: %6.2f Volt", (double)status->avionics_power_rail_voltage); + feedback_provided = true; + } else if (status->avionics_power_rail_voltage > 5.4f) { + mavlink_log_critical(mavlink_fd, "CAUTION: Avionics power high: %6.2f Volt", (double)status->avionics_power_rail_voltage); + feedback_provided = true; + } } } -- cgit v1.2.3 From 2418969b8705a859e49480c338770fc476cd0c24 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 24 Aug 2014 15:35:02 +0400 Subject: UAVCAN update --- uavcan | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/uavcan b/uavcan index 6c070852d..5bfa1999f 160000 --- a/uavcan +++ b/uavcan @@ -1 +1 @@ -Subproject commit 6c070852d76b18c2d57612f66b5bd00e63118c94 +Subproject commit 5bfa1999f41e4983a947efa0029efd2de6873beb -- cgit v1.2.3 From 163224eda20657b867142df380cb7e5311c82e97 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 24 Aug 2014 13:37:00 +0200 Subject: Updated MAVLink --- mavlink/include/mavlink/v1.0 | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mavlink/include/mavlink/v1.0 b/mavlink/include/mavlink/v1.0 index 4d7487c2b..2423e47b4 160000 --- a/mavlink/include/mavlink/v1.0 +++ b/mavlink/include/mavlink/v1.0 @@ -1 +1 @@ -Subproject commit 4d7487c2bc5f5ccf87bca82970fb2c08d6d8bd50 +Subproject commit 2423e47b4f9169e77f7194b36fa2118e018c94e2 -- cgit v1.2.3 From c0975af375c168be98804f025192bbb30710355d Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 24 Aug 2014 13:44:43 +0200 Subject: commander: check if baro is healthy --- src/modules/commander/commander.cpp | 19 +++++++++++++++++++ src/modules/uORB/topics/vehicle_status.h | 2 ++ 2 files changed, 21 insertions(+) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 0bf5cfe34..ddfbd65a1 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1081,6 +1081,25 @@ int commander_thread_main(int argc, char *argv[]) if (updated) { orb_copy(ORB_ID(sensor_combined), sensor_sub, &sensors); + /* Check if the barometer is healthy and issue a warning in the GCS if not so. + * Because the barometer is used for calculating AMSL altitude which is used to ensure + * vertical separation from other airtraffic the operator has to know when the + * barometer is inoperational. + * */ + if (hrt_elapsed_time(&sensors.baro_timestamp) < FAILSAFE_DEFAULT_TIMEOUT) { + /* handle the case where baro was regained */ + if (status.barometer_failure) { + status.barometer_failure = false; + status_changed = true; + mavlink_log_critical(mavlink_fd, "baro healthy"); + } + } else { + if (!status.barometer_failure) { + status.barometer_failure = true; + status_changed = true; + mavlink_log_critical(mavlink_fd, "baro failed"); + } + } } orb_check(diff_pres_sub, &updated); diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index b465f8407..ad92f5b26 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -211,6 +211,8 @@ struct vehicle_status_s { bool gps_failure; /** Set to true if a gps failure is detected */ bool gps_failure_cmd; /** Set to true if a gps failure mode is commanded */ + bool barometer_failure; /** Set to true if a barometer failure is detected */ + bool offboard_control_signal_found_once; bool offboard_control_signal_lost; bool offboard_control_signal_weak; -- cgit v1.2.3 From 1a582e8be0993dadb3b1a70d8ccab8ed3ededfce Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 24 Aug 2014 16:36:35 +0400 Subject: UAVCAN update for #1306 --- uavcan | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/uavcan b/uavcan index 5bfa1999f..75153eb64 160000 --- a/uavcan +++ b/uavcan @@ -1 +1 @@ -Subproject commit 5bfa1999f41e4983a947efa0029efd2de6873beb +Subproject commit 75153eb6436d0cc00679056ff8e916c6d99057ad -- 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(-) 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 bdccd69030e56381d906afeabc8305dbe18e2de6 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 24 Aug 2014 15:27:31 +0200 Subject: geofence: make some functions private, correctly update params --- src/modules/navigator/geofence.cpp | 2 ++ src/modules/navigator/geofence.h | 7 ++++--- src/modules/navigator/mission_feasibility_checker.cpp | 2 +- 3 files changed, 7 insertions(+), 4 deletions(-) diff --git a/src/modules/navigator/geofence.cpp b/src/modules/navigator/geofence.cpp index a3805dc0f..5504239c5 100644 --- a/src/modules/navigator/geofence.cpp +++ b/src/modules/navigator/geofence.cpp @@ -91,6 +91,8 @@ bool Geofence::inside(const struct vehicle_global_position_s &global_position, f bool Geofence::inside(const struct vehicle_global_position_s &global_position, const struct vehicle_gps_position_s &gps_position,float baro_altitude_amsl) { + updateParams(); + if (getAltitudeMode() == Geofence::GF_ALT_MODE_WGS84) { if (getSource() == Geofence::GF_SOURCE_GLOBALPOS) { return inside(global_position); diff --git a/src/modules/navigator/geofence.h b/src/modules/navigator/geofence.h index 65ebb0c3d..65e5b4042 100644 --- a/src/modules/navigator/geofence.h +++ b/src/modules/navigator/geofence.h @@ -75,11 +75,8 @@ public: * @param craft pointer craft coordinates * @return true: system is inside fence, false: system is outside fence */ - bool inside(const struct vehicle_global_position_s &global_position); - bool inside(const struct vehicle_global_position_s &global_position, float baro_altitude_amsl); bool inside(const struct vehicle_global_position_s &global_position, const struct vehicle_gps_position_s &gps_position,float baro_altitude_amsl); - bool inside(double lat, double lon, float altitude); bool inside_polygon(double lat, double lon, float altitude); int clearDm(); @@ -116,6 +113,10 @@ private: control::BlockParamInt _param_counter_threshold; uint8_t _outside_counter; + + bool inside(double lat, double lon, float altitude); + bool inside(const struct vehicle_global_position_s &global_position); + bool inside(const struct vehicle_global_position_s &global_position, float baro_altitude_amsl); }; diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp index 606521f20..937e4fa5a 100644 --- a/src/modules/navigator/mission_feasibility_checker.cpp +++ b/src/modules/navigator/mission_feasibility_checker.cpp @@ -109,7 +109,7 @@ bool MissionFeasibilityChecker::checkGeofence(dm_item_t dm_current, size_t nMiss return false; } - if (!geofence.inside(missionitem.lat, missionitem.lon, missionitem.altitude)) { + if (!geofence.inside_polygon(missionitem.lat, missionitem.lon, missionitem.altitude)) { mavlink_log_info(_mavlink_fd, "#audio: Geofence violation waypoint %d", i); return false; } -- cgit v1.2.3 From a432d0493c0761da075c7734c0f54f44d6121e78 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 24 Aug 2014 17:44:36 +0200 Subject: correctly initialize stay in failsafe flag --- src/modules/navigator/navigator_mode.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/navigator/navigator_mode.cpp b/src/modules/navigator/navigator_mode.cpp index d91f7ab18..3807c5ea8 100644 --- a/src/modules/navigator/navigator_mode.cpp +++ b/src/modules/navigator/navigator_mode.cpp @@ -64,7 +64,7 @@ NavigatorMode::run(bool active) { /* first run */ _first_run = false; /* Reset stay in failsafe flag */ - _navigator->get_mission_result()->stay_in_failsafe = true; + _navigator->get_mission_result()->stay_in_failsafe = false; _navigator->publish_mission_result(); on_activation(); -- cgit v1.2.3 From 04ccf5d8c955d1074bede184eeb35ebed897b55a Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 24 Aug 2014 19:44:54 +0400 Subject: UAVCAN update --- uavcan | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/uavcan b/uavcan index 75153eb64..c7872def1 160000 --- a/uavcan +++ b/uavcan @@ -1 +1 @@ -Subproject commit 75153eb6436d0cc00679056ff8e916c6d99057ad +Subproject commit c7872def16e82a8b318d571829fe9622e2d35ff0 -- cgit v1.2.3 From 06317046f2da215db328be660f900d265cdf9102 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 24 Aug 2014 17:45:15 +0200 Subject: move flight termination and geofence flags from setpoint triplet to mission result --- src/modules/commander/commander.cpp | 18 ++++++------- src/modules/navigator/datalinkloss.cpp | 31 ++++++++++++---------- src/modules/navigator/gpsfailure.cpp | 3 ++- src/modules/navigator/navigator_main.cpp | 16 +++++------ src/modules/navigator/rcloss.cpp | 3 ++- src/modules/uORB/topics/mission_result.h | 8 +++--- .../uORB/topics/position_setpoint_triplet.h | 2 -- 7 files changed, 41 insertions(+), 40 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index ddfbd65a1..f05f970e5 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1328,15 +1328,6 @@ int commander_thread_main(int argc, char *argv[]) if (updated) { orb_copy(ORB_ID(position_setpoint_triplet), pos_sp_triplet_sub, &pos_sp_triplet); - - /* Check for geofence violation */ - if (armed.armed && (pos_sp_triplet.geofence_violated || pos_sp_triplet.flight_termination)) { - //XXX: make this configurable to select different actions (e.g. navigation modes) - /* this will only trigger if geofence is activated via param and a geofence file is present, also there is a circuit breaker to disable the actual flight termination in the px4io driver */ - armed.force_failsafe = true; - status_changed = true; - warnx("Flight termination because of navigator request or geofence"); - } // no reset is done here on purpose, on geofence violation we want to stay in flighttermination } if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) { @@ -1429,6 +1420,15 @@ int commander_thread_main(int argc, char *argv[]) if (updated) { orb_copy(ORB_ID(mission_result), mission_result_sub, &mission_result); + + /* Check for geofence violation */ + if (armed.armed && (mission_result.geofence_violated || mission_result.flight_termination)) { + //XXX: make this configurable to select different actions (e.g. navigation modes) + /* this will only trigger if geofence is activated via param and a geofence file is present, also there is a circuit breaker to disable the actual flight termination in the px4io driver */ + armed.force_failsafe = true; + status_changed = true; + warnx("Flight termination because of navigator request or geofence"); + } // no reset is done here on purpose, on geofence violation we want to stay in flighttermination } /* RC input check */ diff --git a/src/modules/navigator/datalinkloss.cpp b/src/modules/navigator/datalinkloss.cpp index 4e3d25840..66f1c8c73 100644 --- a/src/modules/navigator/datalinkloss.cpp +++ b/src/modules/navigator/datalinkloss.cpp @@ -154,7 +154,8 @@ DataLinkLoss::set_dll_item() } case DLL_STATE_TERMINATE: { /* Request flight termination from the commander */ - pos_sp_triplet->flight_termination = true; + _navigator->get_mission_result()->flight_termination = true; + _navigator->publish_mission_result(); warnx("not switched to manual: request flight termination"); pos_sp_triplet->previous.valid = false; pos_sp_triplet->current.valid = false; @@ -180,23 +181,25 @@ DataLinkLoss::advance_dll() switch (_dll_state) { case DLL_STATE_NONE: /* Check the number of data link losses. If above home fly home directly */ - if (!_param_skipcommshold.get()) { - /* if number of data link losses limit is not reached fly to comms hold wp */ - if (_navigator->get_vstatus()->data_link_lost_counter > _param_numberdatalinklosses.get()) { - warnx("%d data link losses, limit is %d, fly to airfield home", - _navigator->get_vstatus()->data_link_lost_counter, _param_numberdatalinklosses.get()); - mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: too many DL losses, fly to home"); - _dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP; - } else { + /* if number of data link losses limit is not reached fly to comms hold wp */ + if (_navigator->get_vstatus()->data_link_lost_counter > _param_numberdatalinklosses.get()) { + warnx("%d data link losses, limit is %d, fly to airfield home", + _navigator->get_vstatus()->data_link_lost_counter, _param_numberdatalinklosses.get()); + mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: too many DL losses, fly to airfield home"); + _navigator->get_mission_result()->stay_in_failsafe = true; + _navigator->publish_mission_result(); + _dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP; + } else { + if (!_param_skipcommshold.get()) { warnx("fly to comms hold, datalink loss counter: %d", _navigator->get_vstatus()->data_link_lost_counter); mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: fly to comms hold"); _dll_state = DLL_STATE_FLYTOCOMMSHOLDWP; + } else { + /* comms hold wp not active, fly to airfield home directly */ + warnx("Skipping comms hold wp. Flying directly to airfield home"); + mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: fly to airfield home, comms hold skipped"); + _dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP; } - } else { - /* comms hold wp not active, fly to airfield home directly */ - warnx("Skipping comms hold wp. Flying directly to airfield home"); - mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: fly to airfield home, comms hold skipped"); - _dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP; } break; case DLL_STATE_FLYTOCOMMSHOLDWP: diff --git a/src/modules/navigator/gpsfailure.cpp b/src/modules/navigator/gpsfailure.cpp index f0bbbcf1c..cd55f60b0 100644 --- a/src/modules/navigator/gpsfailure.cpp +++ b/src/modules/navigator/gpsfailure.cpp @@ -140,7 +140,8 @@ GpsFailure::set_gpsf_item() switch (_gpsf_state) { case GPSF_STATE_TERMINATE: { /* Request flight termination from the commander */ - pos_sp_triplet->flight_termination = true; + _navigator->get_mission_result()->flight_termination = true; + _navigator->publish_mission_result(); warnx("gps fail: request flight termination"); } default: diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 81ceaaca2..c173ecd50 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -395,11 +395,9 @@ Navigator::task_main() if (have_geofence_position_data) { bool inside = _geofence.inside(_global_pos, _gps_pos, _sensor_combined.baro_alt_meter); if (!inside) { - /* inform other apps via the sp triplet */ - _pos_sp_triplet.geofence_violated = true; - if (_pos_sp_triplet.geofence_violated != true) { - _pos_sp_triplet_updated = true; - } + /* inform other apps via the mission result */ + _mission_result.geofence_violated = true; + publish_mission_result(); /* Issue a warning about the geofence violation once */ if (!_geofence_violation_warning_sent) { @@ -407,11 +405,9 @@ Navigator::task_main() _geofence_violation_warning_sent = true; } } else { - /* inform other apps via the sp triplet */ - _pos_sp_triplet.geofence_violated = false; - if (_pos_sp_triplet.geofence_violated != false) { - _pos_sp_triplet_updated = true; - } + /* inform other apps via the mission result */ + _mission_result.geofence_violated = false; + publish_mission_result(); /* Reset the _geofence_violation_warning_sent field */ _geofence_violation_warning_sent = false; } diff --git a/src/modules/navigator/rcloss.cpp b/src/modules/navigator/rcloss.cpp index 54f1813f5..651e31184 100644 --- a/src/modules/navigator/rcloss.cpp +++ b/src/modules/navigator/rcloss.cpp @@ -127,7 +127,8 @@ RCLoss::set_rcl_item() } case RCL_STATE_TERMINATE: { /* Request flight termination from the commander */ - pos_sp_triplet->flight_termination = true; + _navigator->get_mission_result()->flight_termination = true; + _navigator->publish_mission_result(); warnx("rc not recovered: request flight termination"); pos_sp_triplet->previous.valid = false; pos_sp_triplet->current.valid = false; diff --git a/src/modules/uORB/topics/mission_result.h b/src/modules/uORB/topics/mission_result.h index 65ddfb4ad..c7d25d1f0 100644 --- a/src/modules/uORB/topics/mission_result.h +++ b/src/modules/uORB/topics/mission_result.h @@ -55,9 +55,11 @@ struct mission_result_s { unsigned seq_reached; /**< Sequence of the mission item which has been reached */ unsigned seq_current; /**< Sequence of the current mission item */ - bool reached; /**< true if mission has been reached */ - bool finished; /**< true if mission has been completed */ - bool stay_in_failsafe; /**< true if the commander should not switch out of the failsafe mode*/ + bool reached; /**< true if mission has been reached */ + bool finished; /**< true if mission has been completed */ + bool stay_in_failsafe; /**< true if the commander should not switch out of the failsafe mode*/ + bool geofence_violated; /**< true if the geofence is violated */ + bool flight_termination; /**< true if the navigator demands a flight termination from the commander app */ }; /** diff --git a/src/modules/uORB/topics/position_setpoint_triplet.h b/src/modules/uORB/topics/position_setpoint_triplet.h index e2b1525a2..ec2131abd 100644 --- a/src/modules/uORB/topics/position_setpoint_triplet.h +++ b/src/modules/uORB/topics/position_setpoint_triplet.h @@ -97,8 +97,6 @@ struct position_setpoint_triplet_s struct position_setpoint_s next; unsigned nav_state; /**< report the navigation state */ - bool geofence_violated; /**< true if the geofence is violated */ - bool flight_termination; /**< true if the navigator demands a flight termination from the commander app */ }; /** -- cgit v1.2.3 From 98e74ed0e74e02f866fc7538c7d4153ae3c36743 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 24 Aug 2014 22:18:01 +0200 Subject: commander: limit the output of a warnx --- src/modules/commander/commander.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index f05f970e5..2fcf7bebb 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1427,7 +1427,11 @@ int commander_thread_main(int argc, char *argv[]) /* this will only trigger if geofence is activated via param and a geofence file is present, also there is a circuit breaker to disable the actual flight termination in the px4io driver */ armed.force_failsafe = true; status_changed = true; - warnx("Flight termination because of navigator request or geofence"); + static bool flight_termination_printed = false; + if (!flight_termination_printed) { + warnx("Flight termination because of navigator request or geofence"); + flight_termination_printed = true; + } } // no reset is done here on purpose, on geofence violation we want to stay in flighttermination } -- cgit v1.2.3 From 2f5c0cbd133deb492102ea8515563f471531acce Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 24 Aug 2014 23:05:28 +0200 Subject: Deal with zero airspeed measurements --- src/modules/fw_att_control/fw_att_control_main.cpp | 19 +++++++++++-------- 1 file changed, 11 insertions(+), 8 deletions(-) diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index 0cea13cc4..ad873203e 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -145,6 +145,7 @@ private: perf_counter_t _nonfinite_output_perf; /**< performance counter for non finite output */ bool _setpoint_valid; /**< flag if the position control setpoint is valid */ + bool _debug; /**< if set to true, print debug output */ struct { float tconst; @@ -324,7 +325,8 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : _nonfinite_input_perf(perf_alloc(PC_COUNT, "fw att control nonfinite input")), _nonfinite_output_perf(perf_alloc(PC_COUNT, "fw att control nonfinite output")), /* states */ - _setpoint_valid(false) + _setpoint_valid(false), + _debug(false) { /* safely initialize structs */ _att = {}; @@ -700,7 +702,8 @@ FixedwingAttitudeControl::task_main() perf_count(_nonfinite_input_perf); } } else { - airspeed = _airspeed.true_airspeed_m_s; + /* prevent numerical drama by requiring 0.5 m/s minimal speed */ + airspeed = math::max(0.5f, _airspeed.true_airspeed_m_s); } /* @@ -785,7 +788,7 @@ FixedwingAttitudeControl::task_main() speed_body_v = _att.R[0][1] * _global_pos.vel_n + _att.R[1][1] * _global_pos.vel_e + _att.R[2][1] * _global_pos.vel_d; speed_body_w = _att.R[0][2] * _global_pos.vel_n + _att.R[1][2] * _global_pos.vel_e + _att.R[2][2] * _global_pos.vel_d; } else { - if (loop_counter % 10 == 0) { + if (_debug && loop_counter % 10 == 0) { warnx("Did not get a valid R\n"); } } @@ -808,7 +811,7 @@ FixedwingAttitudeControl::task_main() _roll_ctrl.reset_integrator(); perf_count(_nonfinite_output_perf); - if (loop_counter % 10 == 0) { + if (_debug && loop_counter % 10 == 0) { warnx("roll_u %.4f", (double)roll_u); } } @@ -821,7 +824,7 @@ FixedwingAttitudeControl::task_main() if (!isfinite(pitch_u)) { _pitch_ctrl.reset_integrator(); perf_count(_nonfinite_output_perf); - if (loop_counter % 10 == 0) { + if (_debug && loop_counter % 10 == 0) { warnx("pitch_u %.4f, _yaw_ctrl.get_desired_rate() %.4f," " airspeed %.4f, airspeed_scaling %.4f," " roll_sp %.4f, pitch_sp %.4f," @@ -845,7 +848,7 @@ FixedwingAttitudeControl::task_main() if (!isfinite(yaw_u)) { _yaw_ctrl.reset_integrator(); perf_count(_nonfinite_output_perf); - if (loop_counter % 10 == 0) { + if (_debug && loop_counter % 10 == 0) { warnx("yaw_u %.4f", (double)yaw_u); } } @@ -853,13 +856,13 @@ FixedwingAttitudeControl::task_main() /* throttle passed through */ _actuators.control[3] = (isfinite(throttle_sp)) ? throttle_sp : 0.0f; if (!isfinite(throttle_sp)) { - if (loop_counter % 10 == 0) { + if (_debug && loop_counter % 10 == 0) { warnx("throttle_sp %.4f", (double)throttle_sp); } } } else { perf_count(_nonfinite_input_perf); - if (loop_counter % 10 == 0) { + if (_debug && loop_counter % 10 == 0) { warnx("Non-finite setpoint roll_sp: %.4f, pitch_sp %.4f", (double)roll_sp, (double)pitch_sp); } } -- 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(-) 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 b150c3092c2bfe532618678b91c1eab42095c486 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 25 Aug 2014 09:33:39 +0200 Subject: mavlink_main: raise rates of onboard mode --- src/modules/mavlink/mavlink_main.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index ed7e879d3..93f4fec92 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1401,9 +1401,9 @@ Mavlink::task_main(int argc, char *argv[]) case MAVLINK_MODE_ONBOARD: configure_stream("SYS_STATUS", 1.0f); - configure_stream("ATTITUDE", 15.0f); - configure_stream("GLOBAL_POSITION_INT", 15.0f); - configure_stream("CAMERA_CAPTURE", 1.0f); + configure_stream("ATTITUDE", 50.0f); + configure_stream("GLOBAL_POSITION_INT", 50.0f); + configure_stream("CAMERA_CAPTURE", 2.0f); break; default: -- cgit v1.2.3 From 9825ed8f3cd037b8fd131a911ba350a29203ef0c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 25 Aug 2014 10:21:26 +0200 Subject: Attempt at fixing programming timeouts --- Tools/px_uploader.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/Tools/px_uploader.py b/Tools/px_uploader.py index d8f4884bc..b46db00b5 100755 --- a/Tools/px_uploader.py +++ b/Tools/px_uploader.py @@ -178,9 +178,9 @@ class uploader(object): MAVLINK_REBOOT_ID1 = bytearray(b'\xfe\x21\x72\xff\x00\x4c\x00\x00\x80\x3f\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\xf6\x00\x01\x00\x00\x48\xf0') MAVLINK_REBOOT_ID0 = bytearray(b'\xfe\x21\x45\xff\x00\x4c\x00\x00\x80\x3f\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\xf6\x00\x00\x00\x00\xd7\xac') - def __init__(self, portname, baudrate): + def __init__(self, portname, baudrate, interCharTimeout=0.001, timeout=0.5): # open the port, keep the default timeout short so we can poll quickly - self.port = serial.Serial(portname, baudrate, timeout=2.0) + self.port = serial.Serial(portname, baudrate) self.otp = b'' self.sn = b'' @@ -195,7 +195,7 @@ class uploader(object): def __recv(self, count=1): c = self.port.read(count) if len(c) < 1: - raise RuntimeError("timeout waiting for data") + raise RuntimeError("timeout waiting for data (%u bytes)", count) # print("recv " + binascii.hexlify(c)) return c -- cgit v1.2.3 From 73ecbbe13d1f231bf2a9c2ccaafe29065352d75c Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 25 Aug 2014 11:12:01 +0200 Subject: config_px4fmu-v2_default: include px4flow driver by default --- makefiles/config_px4fmu-v2_default.mk | 1 + 1 file changed, 1 insertion(+) diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index d0a40445d..5f146686c 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -41,6 +41,7 @@ MODULES += drivers/frsky_telemetry MODULES += modules/sensors MODULES += drivers/mkblctrl MODULES += drivers/pca8574 +MODULES += drivers/px4flow # Needs to be burned to the ground and re-written; for now, -- cgit v1.2.3 From 60799e51558e6259a7a2d20d765a4f8d29b88cc5 Mon Sep 17 00:00:00 2001 From: Nuno Marques Date: Mon, 25 Aug 2014 11:07:30 +0100 Subject: sdlog2: add vision log struct --- src/modules/sdlog2/sdlog2_messages.h | 11 +++++++++++ 1 file changed, 11 insertions(+) diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index fb7609435..82d83b5c1 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -391,6 +391,16 @@ struct log_TEL_s { uint64_t heartbeat_time; }; +/* --- VISN - VISION POSITION --- */ +#define LOG_VISN_MSG 38 +struct log_VISN_s { + float x; + float y; + float z; + float roll; + float pitch; + float yaw; +}; /********** SYSTEM MESSAGES, ID > 0x80 **********/ @@ -449,6 +459,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(EST1, "ffffffffffffffff", "s12,s13,s14,s15,s16,s17,s18,s19,s20,s21,s22,s23,s24,s25,s26,s27"), LOG_FORMAT(PWR, "fffBBBBB", "Periph5V,Servo5V,RSSI,UsbOk,BrickOk,ServoOk,PeriphOC,HipwrOC"), LOG_FORMAT(VICN, "ffffff", "X,Y,Z,Roll,Pitch,Yaw"), + LOG_FORMAT(VISN, "ffffff", "X,Y,Z,Roll,Pitch,Yaw"), LOG_FORMAT(GS0A, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"), LOG_FORMAT(GS0B, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"), LOG_FORMAT(GS1A, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"), -- cgit v1.2.3 From ec8438bdcab31c566f91869140505b506a4fcaf8 Mon Sep 17 00:00:00 2001 From: Nuno Marques Date: Mon, 25 Aug 2014 11:20:55 +0100 Subject: sdlog2: added vision estimate logging --- src/modules/sdlog2/sdlog2.c | 25 +++++++++++++++++-------- 1 file changed, 17 insertions(+), 8 deletions(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 6c4b49452..59765415a 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -934,6 +934,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct vehicle_global_position_s global_pos; struct position_setpoint_triplet_s triplet; struct vehicle_vicon_position_s vicon_pos; + struct vehicle_vision_position_s vision_pos; struct optical_flow_s flow; struct rc_channels_s rc; struct differential_pressure_s diff_pres; @@ -984,6 +985,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct log_EST1_s log_EST1; struct log_PWR_s log_PWR; struct log_VICN_s log_VICN; + struct log_VISN_s log_VISN; struct log_GS0A_s log_GS0A; struct log_GS0B_s log_GS0B; struct log_GS1A_s log_GS1A; @@ -1013,6 +1015,7 @@ int sdlog2_thread_main(int argc, char *argv[]) int gps_pos_sub; int sat_info_sub; int vicon_pos_sub; + int vision_pos_sub; int flow_sub; int rc_sub; int airspeed_sub; @@ -1043,6 +1046,7 @@ int sdlog2_thread_main(int argc, char *argv[]) subs.global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); subs.triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet)); subs.vicon_pos_sub = orb_subscribe(ORB_ID(vehicle_vicon_position)); + subs.vision_pos_sub = orb_subscribe(ORB_ID(vehicle_vision_position)); subs.flow_sub = orb_subscribe(ORB_ID(optical_flow)); subs.rc_sub = orb_subscribe(ORB_ID(rc_channels)); subs.airspeed_sub = orb_subscribe(ORB_ID(airspeed)); @@ -1459,6 +1463,18 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.body.log_VICN.yaw = buf.vicon_pos.yaw; LOGBUFFER_WRITE_AND_COUNT(VICN); } + + /* --- VISION POSITION --- */ + if (copy_if_updated(ORB_ID(vehicle_vision_position), subs.vision_pos_sub, &buf.vision_pos)) { + log_msg.msg_type = LOG_VISN_MSG; + log_msg.body.log_VISN.x = buf.vision_pos.x; + log_msg.body.log_VISN.y = buf.vision_pos.y; + log_msg.body.log_VISN.z = buf.vision_pos.z; + log_msg.body.log_VISN.pitch = buf.vision_pos.pitch; + log_msg.body.log_VISN.roll = buf.vision_pos.roll; + log_msg.body.log_VISN.yaw = buf.vision_pos.yaw; + LOGBUFFER_WRITE_AND_COUNT(VISN); + } /* --- FLOW --- */ if (copy_if_updated(ORB_ID(optical_flow), subs.flow_sub, &buf.flow)) { @@ -1565,14 +1581,7 @@ int sdlog2_thread_main(int argc, char *argv[]) } } - /* --- BOTTOM DISTANCE --- */ - if (copy_if_updated(ORB_ID(sensor_range_finder), subs.range_finder_sub, &buf.range_finder)) { - log_msg.msg_type = LOG_DIST_MSG; - log_msg.body.log_DIST.bottom = buf.range_finder.distance; - log_msg.body.log_DIST.bottom_rate = 0.0f; - log_msg.body.log_DIST.flags = (buf.range_finder.valid ? 1 : 0); - LOGBUFFER_WRITE_AND_COUNT(DIST); - } + /* --- ESTIMATOR STATUS --- */ if (copy_if_updated(ORB_ID(estimator_status), subs.estimator_status_sub, &buf.estimator_status)) { -- cgit v1.2.3 From c591444c1e2e4124d28a03653c0ccdbcd305c1c2 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 25 Aug 2014 12:48:14 +0200 Subject: fw pos control: set pitch sp correctly while waiting for launch --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index da81b0dba..b3a78ad82 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 @@ -1192,7 +1192,8 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } /* Copy thrust and pitch values from tecs - * making sure again that the correct thrust is used, without depending on library calls */ + * making sure again that the correct thrust is used, + * without depending on library calls */ if (pos_sp_triplet.current.type == SETPOINT_TYPE_TAKEOFF && launch_detection_state != LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS) { _att_sp.thrust = launchDetector.getThrottlePreTakeoff(); @@ -1200,7 +1201,12 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi else { _att_sp.thrust = math::min(_mTecs.getEnabled() ? _mTecs.getThrottleSetpoint() : _tecs.get_throttle_demand(), throttle_max); } - _att_sp.pitch_body = _mTecs.getEnabled() ? _mTecs.getPitchSetpoint() : _tecs.get_pitch_demand(); + /* During a takeoff waypoint while waiting for launch the pitch sp is set + * already (not by tecs) */ + if (!(pos_sp_triplet.current.type == SETPOINT_TYPE_TAKEOFF && + launch_detection_state == LAUNCHDETECTION_RES_NONE)) { + _att_sp.pitch_body = _mTecs.getEnabled() ? _mTecs.getPitchSetpoint() : _tecs.get_pitch_demand(); + } if (_control_mode.flag_control_position_enabled) { last_manual = false; -- cgit v1.2.3 From d0f5eca5be3d3257b1350337b58f020063b65eb9 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 25 Aug 2014 13:13:07 +0200 Subject: px4flow: removed flow report in driver, just use uORB topic --- src/drivers/drv_px4flow.h | 31 ---------------- src/drivers/px4flow/px4flow.cpp | 68 ++++++++++++++++++------------------ src/modules/mavlink/mavlink_main.cpp | 2 +- 3 files changed, 35 insertions(+), 66 deletions(-) diff --git a/src/drivers/drv_px4flow.h b/src/drivers/drv_px4flow.h index 76ec55c3e..ab640837b 100644 --- a/src/drivers/drv_px4flow.h +++ b/src/drivers/drv_px4flow.h @@ -46,37 +46,6 @@ #define PX4FLOW_DEVICE_PATH "/dev/px4flow" -/** - * @addtogroup topics - * @{ - */ - -/** - * Optical flow in NED body frame in SI units. - * - * @see http://en.wikipedia.org/wiki/International_System_of_Units - * - * @warning If possible the usage of the raw flow and performing rotation-compensation - * using the autopilot angular rate estimate is recommended. - */ -struct px4flow_report { - - uint64_t timestamp; /**< in microseconds since system start */ - - int16_t flow_raw_x; /**< flow in pixels in X direction, not rotation-compensated */ - int16_t flow_raw_y; /**< flow in pixels in Y direction, not rotation-compensated */ - float flow_comp_x_m; /**< speed over ground in meters per second, rotation-compensated */ - float flow_comp_y_m; /**< speed over ground in meters per second, rotation-compensated */ - float ground_distance_m; /**< Altitude / distance to ground in meters */ - uint8_t quality; /**< Quality of the measurement, 0: bad quality, 255: maximum quality */ - uint8_t sensor_id; /**< id of the sensor emitting the flow value */ - -}; - -/** - * @} - */ - /* * ObjDev tag for px4flow data. */ diff --git a/src/drivers/px4flow/px4flow.cpp b/src/drivers/px4flow/px4flow.cpp index f214b5964..60ad3c1af 100644 --- a/src/drivers/px4flow/px4flow.cpp +++ b/src/drivers/px4flow/px4flow.cpp @@ -37,7 +37,7 @@ * * Driver for the PX4FLOW module connected via I2C. */ - + #include #include @@ -68,7 +68,7 @@ #include #include -//#include +#include #include @@ -80,7 +80,7 @@ /* PX4FLOW Registers addresses */ #define PX4FLOW_REG 0x00 /* Measure Register */ -#define PX4FLOW_CONVERSION_INTERVAL 8000 /* 8ms 125Hz +#define PX4FLOW_CONVERSION_INTERVAL 8000 /* 8ms 125Hz */ /* oddly, ERROR is not defined for c++ */ #ifdef ERROR @@ -115,17 +115,17 @@ class PX4FLOW : public device::I2C public: PX4FLOW(int bus = PX4FLOW_BUS, int address = I2C_FLOW_ADDRESS); virtual ~PX4FLOW(); - + virtual int init(); - + virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); virtual int ioctl(struct file *filp, int cmd, unsigned long arg); - + /** * Diagnostics - print some basic information about the driver. */ void print_info(); - + protected: virtual int probe(); @@ -136,13 +136,13 @@ private: bool _sensor_ok; int _measure_ticks; bool _collect_phase; - + orb_advert_t _px4flow_topic; perf_counter_t _sample_perf; perf_counter_t _comms_errors; perf_counter_t _buffer_overflows; - + /** * Test whether the device supported by the driver is present at a * specific address. @@ -151,7 +151,7 @@ private: * @return True if the device is present. */ int probe_address(uint8_t address); - + /** * Initialise the automatic measurement state machine and start it. * @@ -159,12 +159,12 @@ private: * to make it more aggressive about resetting the bus in case of errors. */ void start(); - + /** * Stop the automatic measurement state machine. */ void stop(); - + /** * Perform a poll cycle; collect from the previous measurement * and start a new one. @@ -179,8 +179,8 @@ private: * @param arg Instance pointer for the driver that is polling. */ static void cycle_trampoline(void *arg); - - + + }; /* @@ -201,7 +201,7 @@ PX4FLOW::PX4FLOW(int bus, int address) : { // enable debug() calls _debug_enabled = true; - + // work_cancel in the dtor will explode if we don't do this... memset(&_work, 0, sizeof(_work)); } @@ -226,13 +226,13 @@ PX4FLOW::init() goto out; /* allocate basic report buffers */ - _reports = new RingBuffer(2, sizeof(px4flow_report)); + _reports = new RingBuffer(2, sizeof(struct optical_flow_s)); if (_reports == nullptr) goto out; /* get a publish handle on the px4flow topic */ - struct px4flow_report zero_report; + struct optical_flow_s zero_report; memset(&zero_report, 0, sizeof(zero_report)); _px4flow_topic = orb_advertise(ORB_ID(optical_flow), &zero_report); @@ -323,24 +323,24 @@ PX4FLOW::ioctl(struct file *filp, int cmd, unsigned long arg) /* lower bound is mandatory, upper bound is a sanity check */ if ((arg < 1) || (arg > 100)) return -EINVAL; - + irqstate_t flags = irqsave(); if (!_reports->resize(arg)) { irqrestore(flags); return -ENOMEM; } irqrestore(flags); - + return OK; } case SENSORIOCGQUEUEDEPTH: return _reports->size(); - + case SENSORIOCRESET: /* XXX implement this */ return -EINVAL; - + default: /* give it to the superclass */ return I2C::ioctl(filp, cmd, arg); @@ -350,8 +350,8 @@ PX4FLOW::ioctl(struct file *filp, int cmd, unsigned long arg) ssize_t PX4FLOW::read(struct file *filp, char *buffer, size_t buflen) { - unsigned count = buflen / sizeof(struct px4flow_report); - struct px4flow_report *rbuf = reinterpret_cast(buffer); + unsigned count = buflen / sizeof(struct optical_flow_s); + struct optical_flow_s *rbuf = reinterpret_cast(buffer); int ret = 0; /* buffer must be large enough */ @@ -425,7 +425,7 @@ PX4FLOW::measure() return ret; } ret = OK; - + return ret; } @@ -433,14 +433,14 @@ int PX4FLOW::collect() { int ret = -EIO; - + /* read from the sensor */ uint8_t val[22] = {0, 0,0, 0,0, 0,0, 0,0, 0,0, 0,0, 0,0, 0,0, 0,0, 0,0, 0}; - + perf_begin(_sample_perf); - + ret = transfer(nullptr, 0, &val[0], 22); - + if (ret < 0) { log("error reading from sensor: %d", ret); @@ -448,7 +448,7 @@ PX4FLOW::collect() perf_end(_sample_perf); return ret; } - + // f.frame_count = val[1] << 8 | val[0]; // f.pixel_flow_x_sum= val[3] << 8 | val[2]; // f.pixel_flow_y_sum= val[5] << 8 | val[4]; @@ -466,7 +466,7 @@ PX4FLOW::collect() int16_t flowcy = val[9] << 8 | val[8]; int16_t gdist = val[21] << 8 | val[20]; - struct px4flow_report report; + struct optical_flow_s report; report.flow_comp_x_m = float(flowcx)/1000.0f; report.flow_comp_y_m = float(flowcy)/1000.0f; report.flow_raw_x= val[3] << 8 | val[2]; @@ -503,7 +503,7 @@ PX4FLOW::start() /* schedule a cycle to start things */ work_queue(HPWORK, &_work, (worker_t)&PX4FLOW::cycle_trampoline, this, 1); - + /* notify about state change */ struct subsystem_info_s info = { true, @@ -644,7 +644,7 @@ start() fail: - if (g_dev != nullptr) + if (g_dev != nullptr) { delete g_dev; g_dev = nullptr; @@ -678,7 +678,7 @@ void stop() void test() { - struct px4flow_report report; + struct optical_flow_s report; ssize_t sz; int ret; @@ -777,7 +777,7 @@ px4flow_main(int argc, char *argv[]) */ if (!strcmp(argv[1], "start")) px4flow::start(); - + /* * Stop the driver */ diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 93f4fec92..940e64144 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1396,7 +1396,7 @@ Mavlink::task_main(int argc, char *argv[]) configure_stream("POSITION_TARGET_GLOBAL_INT", 3.0f); configure_stream("ATTITUDE_TARGET", 3.0f); configure_stream("DISTANCE_SENSOR", 0.5f); - configure_stream("OPTICAL_FLOW", 0.5f); + configure_stream("OPTICAL_FLOW", 20.0f); break; case MAVLINK_MODE_ONBOARD: -- 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(-) 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 0994006f96f2c030acd31bb14b32d2fd2127c6dd Mon Sep 17 00:00:00 2001 From: Nuno Marques Date: Mon, 25 Aug 2014 14:16:13 +0100 Subject: sdlog2: update vision log fields --- src/modules/sdlog2/sdlog2.c | 17 +++++++++++------ src/modules/sdlog2/sdlog2_messages.h | 12 ++++++++---- 2 files changed, 19 insertions(+), 10 deletions(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 59765415a..ec4197e79 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -76,6 +76,7 @@ #include #include #include +#include #include #include #include @@ -934,7 +935,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct vehicle_global_position_s global_pos; struct position_setpoint_triplet_s triplet; struct vehicle_vicon_position_s vicon_pos; - struct vehicle_vision_position_s vision_pos; + struct vision_position_estimate_s vision_pos; struct optical_flow_s flow; struct rc_channels_s rc; struct differential_pressure_s diff_pres; @@ -1046,7 +1047,7 @@ int sdlog2_thread_main(int argc, char *argv[]) subs.global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); subs.triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet)); subs.vicon_pos_sub = orb_subscribe(ORB_ID(vehicle_vicon_position)); - subs.vision_pos_sub = orb_subscribe(ORB_ID(vehicle_vision_position)); + subs.vision_pos_sub = orb_subscribe(ORB_ID(vision_position_estimate)); subs.flow_sub = orb_subscribe(ORB_ID(optical_flow)); subs.rc_sub = orb_subscribe(ORB_ID(rc_channels)); subs.airspeed_sub = orb_subscribe(ORB_ID(airspeed)); @@ -1465,14 +1466,18 @@ int sdlog2_thread_main(int argc, char *argv[]) } /* --- VISION POSITION --- */ - if (copy_if_updated(ORB_ID(vehicle_vision_position), subs.vision_pos_sub, &buf.vision_pos)) { + if (copy_if_updated(ORB_ID(vision_position_estimate), subs.vision_pos_sub, &buf.vision_pos)) { log_msg.msg_type = LOG_VISN_MSG; log_msg.body.log_VISN.x = buf.vision_pos.x; log_msg.body.log_VISN.y = buf.vision_pos.y; log_msg.body.log_VISN.z = buf.vision_pos.z; - log_msg.body.log_VISN.pitch = buf.vision_pos.pitch; - log_msg.body.log_VISN.roll = buf.vision_pos.roll; - log_msg.body.log_VISN.yaw = buf.vision_pos.yaw; + log_msg.body.log_VISN.vx = buf.vision_pos.vx; + log_msg.body.log_VISN.vy = buf.vision_pos.vy; + log_msg.body.log_VISN.vz = buf.vision_pos.vz; + log_msg.body.log_VISN.qx = buf.vision_pos.q[0]; + log_msg.body.log_VISN.qy = buf.vision_pos.q[1]; + log_msg.body.log_VISN.qz = buf.vision_pos.q[2]; + log_msg.body.log_VISN.qw = buf.vision_pos.q[3]; LOGBUFFER_WRITE_AND_COUNT(VISN); } diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 82d83b5c1..6741c7e25 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -397,9 +397,13 @@ struct log_VISN_s { float x; float y; float z; - float roll; - float pitch; - float yaw; + float vx; + float vy; + float vz; + float qx; + float qy; + float qz; + float qw; }; /********** SYSTEM MESSAGES, ID > 0x80 **********/ @@ -459,7 +463,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(EST1, "ffffffffffffffff", "s12,s13,s14,s15,s16,s17,s18,s19,s20,s21,s22,s23,s24,s25,s26,s27"), LOG_FORMAT(PWR, "fffBBBBB", "Periph5V,Servo5V,RSSI,UsbOk,BrickOk,ServoOk,PeriphOC,HipwrOC"), LOG_FORMAT(VICN, "ffffff", "X,Y,Z,Roll,Pitch,Yaw"), - LOG_FORMAT(VISN, "ffffff", "X,Y,Z,Roll,Pitch,Yaw"), + LOG_FORMAT(VISN, "ffffffffff", "X,Y,Z,VX,VY,VZ,QuatX,QuatY,QuatZ,QuatW"), LOG_FORMAT(GS0A, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"), LOG_FORMAT(GS0B, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"), LOG_FORMAT(GS1A, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"), -- cgit v1.2.3 From 3ef374c4264969928ce958ff3053f6238909479b Mon Sep 17 00:00:00 2001 From: Nuno Marques Date: Mon, 25 Aug 2014 14:20:17 +0100 Subject: sdlog2: added BOTTOM_DISTANCE again --- src/modules/sdlog2/sdlog2.c | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index ec4197e79..02cc39dfc 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1586,7 +1586,14 @@ int sdlog2_thread_main(int argc, char *argv[]) } } - + /* --- BOTTOM DISTANCE --- */ +- if (copy_if_updated(ORB_ID(sensor_range_finder), subs.range_finder_sub, &buf.range_finder)) { +- log_msg.msg_type = LOG_DIST_MSG; +- log_msg.body.log_DIST.bottom = buf.range_finder.distance; +- log_msg.body.log_DIST.bottom_rate = 0.0f; +- log_msg.body.log_DIST.flags = (buf.range_finder.valid ? 1 : 0); +- LOGBUFFER_WRITE_AND_COUNT(DIST); +- } /* --- ESTIMATOR STATUS --- */ if (copy_if_updated(ORB_ID(estimator_status), subs.estimator_status_sub, &buf.estimator_status)) { -- cgit v1.2.3 From ebd56aa2c12c3c55e4eb349429a765355afe6360 Mon Sep 17 00:00:00 2001 From: Nuno Marques Date: Mon, 25 Aug 2014 15:07:14 +0100 Subject: sdlog2: minor corrections --- src/modules/sdlog2/sdlog2.c | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 02cc39dfc..07655808c 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -935,7 +935,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct vehicle_global_position_s global_pos; struct position_setpoint_triplet_s triplet; struct vehicle_vicon_position_s vicon_pos; - struct vision_position_estimate_s vision_pos; + struct vision_position_estimate vision_pos; struct optical_flow_s flow; struct rc_channels_s rc; struct differential_pressure_s diff_pres; @@ -1587,13 +1587,13 @@ int sdlog2_thread_main(int argc, char *argv[]) } /* --- BOTTOM DISTANCE --- */ -- if (copy_if_updated(ORB_ID(sensor_range_finder), subs.range_finder_sub, &buf.range_finder)) { -- log_msg.msg_type = LOG_DIST_MSG; -- log_msg.body.log_DIST.bottom = buf.range_finder.distance; -- log_msg.body.log_DIST.bottom_rate = 0.0f; -- log_msg.body.log_DIST.flags = (buf.range_finder.valid ? 1 : 0); -- LOGBUFFER_WRITE_AND_COUNT(DIST); -- } + if (copy_if_updated(ORB_ID(sensor_range_finder), subs.range_finder_sub, &buf.range_finder)) { + log_msg.msg_type = LOG_DIST_MSG; + log_msg.body.log_DIST.bottom = buf.range_finder.distance; + log_msg.body.log_DIST.bottom_rate = 0.0f; + log_msg.body.log_DIST.flags = (buf.range_finder.valid ? 1 : 0); + LOGBUFFER_WRITE_AND_COUNT(DIST); + } /* --- ESTIMATOR STATUS --- */ if (copy_if_updated(ORB_ID(estimator_status), subs.estimator_status_sub, &buf.estimator_status)) { -- cgit v1.2.3 From d6810ae1f8e2b8c86d54acfe80094265e97a3232 Mon Sep 17 00:00:00 2001 From: Nuno Marques Date: Mon, 25 Aug 2014 15:37:51 +0100 Subject: sdlog2: minor improvements --- src/modules/sdlog2/sdlog2.c | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 07655808c..dbda8ea6f 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1589,11 +1589,11 @@ int sdlog2_thread_main(int argc, char *argv[]) /* --- BOTTOM DISTANCE --- */ if (copy_if_updated(ORB_ID(sensor_range_finder), subs.range_finder_sub, &buf.range_finder)) { log_msg.msg_type = LOG_DIST_MSG; - log_msg.body.log_DIST.bottom = buf.range_finder.distance; + log_msg.body.log_DIST.bottom = buf.range_finder.distance; log_msg.body.log_DIST.bottom_rate = 0.0f; log_msg.body.log_DIST.flags = (buf.range_finder.valid ? 1 : 0); - LOGBUFFER_WRITE_AND_COUNT(DIST); - } + LOGBUFFER_WRITE_AND_COUNT(DIST); + } /* --- ESTIMATOR STATUS --- */ if (copy_if_updated(ORB_ID(estimator_status), subs.estimator_status_sub, &buf.estimator_status)) { -- cgit v1.2.3 From eab701b896fa316132aff78a34362ca77549e581 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Tue, 26 Aug 2014 00:50:19 +0400 Subject: Improved UAVCAN status reporting --- src/modules/uavcan/sensors/gnss.cpp | 10 ++++++++ src/modules/uavcan/sensors/gnss.hpp | 2 ++ src/modules/uavcan/sensors/sensor_bridge.cpp | 36 ++++++++++++++++++++-------- src/modules/uavcan/sensors/sensor_bridge.hpp | 15 ++++++++---- src/modules/uavcan/uavcan_main.cpp | 10 ++++---- 5 files changed, 55 insertions(+), 18 deletions(-) diff --git a/src/modules/uavcan/sensors/gnss.cpp b/src/modules/uavcan/sensors/gnss.cpp index 8548660fe..0d67aad47 100644 --- a/src/modules/uavcan/sensors/gnss.cpp +++ b/src/modules/uavcan/sensors/gnss.cpp @@ -70,6 +70,16 @@ unsigned UavcanGnssBridge::get_num_redundant_channels() const return (_receiver_node_id < 0) ? 0 : 1; } +void UavcanGnssBridge::print_status() const +{ + printf("RX errors: %d, receiver node id: ", _sub_fix.getFailureCount()); + if (_receiver_node_id < 0) { + printf("N/A\n"); + } else { + printf("%d\n", _receiver_node_id); + } +} + void UavcanGnssBridge::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure &msg) { // This bridge does not support redundant GNSS receivers yet. diff --git a/src/modules/uavcan/sensors/gnss.hpp b/src/modules/uavcan/sensors/gnss.hpp index c2b6e4195..e8466b401 100644 --- a/src/modules/uavcan/sensors/gnss.hpp +++ b/src/modules/uavcan/sensors/gnss.hpp @@ -66,6 +66,8 @@ public: unsigned get_num_redundant_channels() const override; + void print_status() const override; + private: /** * GNSS fix message will be reported via this callback. diff --git a/src/modules/uavcan/sensors/sensor_bridge.cpp b/src/modules/uavcan/sensors/sensor_bridge.cpp index a98596f9c..9608ce680 100644 --- a/src/modules/uavcan/sensors/sensor_bridge.cpp +++ b/src/modules/uavcan/sensors/sensor_bridge.cpp @@ -58,7 +58,7 @@ void IUavcanSensorBridge::make_all(uavcan::INode &node, List= 0) { + if (_channels[i].node_id >= 0) { (void)unregister_class_devname(_class_devname, _channels[i].class_instance); } } @@ -66,13 +66,15 @@ UavcanCDevSensorBridgeBase::~UavcanCDevSensorBridgeBase() delete [] _channels; } -void UavcanCDevSensorBridgeBase::publish(const int redundancy_channel_id, const void *report) +void UavcanCDevSensorBridgeBase::publish(const int node_id, const void *report) { + assert(report != nullptr); + Channel *channel = nullptr; // Checking if such channel already exists for (unsigned i = 0; i < _max_channels; i++) { - if (_channels[i].redunancy_channel_id == redundancy_channel_id) { + if (_channels[i].node_id == node_id) { channel = _channels + i; break; } @@ -84,11 +86,11 @@ void UavcanCDevSensorBridgeBase::publish(const int redundancy_channel_id, const return; // Give up immediately - saves some CPU time } - log("adding channel %d...", redundancy_channel_id); + log("adding channel %d...", node_id); // Search for the first free channel for (unsigned i = 0; i < _max_channels; i++) { - if (_channels[i].redunancy_channel_id < 0) { + if (_channels[i].node_id < 0) { channel = _channels + i; break; } @@ -111,9 +113,9 @@ void UavcanCDevSensorBridgeBase::publish(const int redundancy_channel_id, const } // Publish to the appropriate topic, abort on failure - channel->orb_id = _orb_topics[class_instance]; - channel->redunancy_channel_id = redundancy_channel_id; - channel->class_instance = class_instance; + channel->orb_id = _orb_topics[class_instance]; + channel->node_id = node_id; + channel->class_instance = class_instance; channel->orb_advert = orb_advertise(channel->orb_id, report); if (channel->orb_advert < 0) { @@ -123,7 +125,7 @@ void UavcanCDevSensorBridgeBase::publish(const int redundancy_channel_id, const return; } - log("channel %d class instance %d ok", channel->redunancy_channel_id, channel->class_instance); + log("channel %d class instance %d ok", channel->node_id, channel->class_instance); } assert(channel != nullptr); @@ -134,9 +136,23 @@ unsigned UavcanCDevSensorBridgeBase::get_num_redundant_channels() const { unsigned out = 0; for (unsigned i = 0; i < _max_channels; i++) { - if (_channels[i].redunancy_channel_id >= 0) { + if (_channels[i].node_id >= 0) { out += 1; } } return out; } + +void UavcanCDevSensorBridgeBase::print_status() const +{ + printf("devname: %s\n", _class_devname); + + for (unsigned i = 0; i < _max_channels; i++) { + if (_channels[i].node_id >= 0) { + printf("channel %d: node id %d --> class instance %d\n", + i, _channels[i].node_id, _channels[i].class_instance); + } else { + printf("channel %d: empty\n", i); + } + } +} diff --git a/src/modules/uavcan/sensors/sensor_bridge.hpp b/src/modules/uavcan/sensors/sensor_bridge.hpp index a13d0ab35..1316f7ecc 100644 --- a/src/modules/uavcan/sensors/sensor_bridge.hpp +++ b/src/modules/uavcan/sensors/sensor_bridge.hpp @@ -68,6 +68,11 @@ public: */ virtual unsigned get_num_redundant_channels() const = 0; + /** + * Prints current status in a human readable format to stdout. + */ + virtual void print_status() const = 0; + /** * Sensor bridge factory. * Creates a bridge object by its ASCII name, e.g. "gnss", "mag". @@ -84,7 +89,7 @@ class UavcanCDevSensorBridgeBase : public IUavcanSensorBridge, public device::CD { struct Channel { - int redunancy_channel_id = -1; + int node_id = -1; orb_id_t orb_id = nullptr; orb_advert_t orb_advert = -1; int class_instance = -1; @@ -112,13 +117,15 @@ protected: /** * Sends one measurement into appropriate ORB topic. * New redundancy channels will be registered automatically. - * @param redundancy_channel_id Redundant sensor identifier (e.g. UAVCAN Node ID) - * @param report ORB message object + * @param node_id Sensor's Node ID + * @param report Pointer to ORB message object */ - void publish(const int redundancy_channel_id, const void *report); + void publish(const int node_id, const void *report); public: virtual ~UavcanCDevSensorBridgeBase(); unsigned get_num_redundant_channels() const override; + + void print_status() const override; }; diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index 482fec1e0..95c6ba13e 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -548,14 +548,16 @@ UavcanNode::print_info() (void)pthread_mutex_lock(&_node_mutex); // ESC mixer status - warnx("ESC actuators control groups: sub: %u / req: %u / fds: %u", - (unsigned)_groups_subscribed, (unsigned)_groups_required, _poll_fds_num); - warnx("ESC mixer: %s", (_mixers == nullptr) ? "NONE" : "OK"); + printf("ESC actuators control groups: sub: %u / req: %u / fds: %u\n", + (unsigned)_groups_subscribed, (unsigned)_groups_required, _poll_fds_num); + printf("ESC mixer: %s\n", (_mixers == nullptr) ? "NONE" : "OK"); // Sensor bridges auto br = _sensor_bridges.getHead(); while (br != nullptr) { - warnx("Sensor '%s': channels: %u", br->get_name(), br->get_num_redundant_channels()); + printf("Sensor '%s':\n", br->get_name()); + br->print_status(); + printf("\n"); br = br->getSibling(); } -- cgit v1.2.3 From 8a9da209d194b4f35000935379901ed6091091f9 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 25 Aug 2014 23:17:56 +0200 Subject: limit warnx output on flight termination --- src/modules/commander/commander.cpp | 12 ++++++++++-- 1 file changed, 10 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 2fcf7bebb..8b065560f 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1610,7 +1610,11 @@ int commander_thread_main(int argc, char *argv[]) (status.data_link_lost_cmd && status.gps_failure_cmd)) { armed.force_failsafe = true; status_changed = true; - warnx("Flight termination because of data link loss && gps failure"); + static bool flight_termination_printed = false; + if (!flight_termination_printed) { + warnx("Flight termination because of data link loss && gps failure"); + flight_termination_printed = true; + } mavlink_log_critical(mavlink_fd, "DL and GPS lost: flight termination"); } @@ -1625,7 +1629,11 @@ int commander_thread_main(int argc, char *argv[]) (status.rc_signal_lost_cmd && status.gps_failure_cmd))) { armed.force_failsafe = true; status_changed = true; - warnx("Flight termination because of RC signal loss && gps failure"); + static bool flight_termination_printed = false; + if (!flight_termination_printed) { + warnx("Flight termination because of RC signal loss && gps failure"); + flight_termination_printed = true; + } mavlink_log_critical(mavlink_fd, "RC and GPS lost: flight termination"); } } -- cgit v1.2.3 From a54ef70a207cd892a9ef406df6f1aa0732035537 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 26 Aug 2014 08:14:52 +0200 Subject: Decrease I2C timeout in config so it matches the previous 500 us timeout as close as possible. This is necessary after fixing the NuttX I2C timeout logic --- nuttx-configs/px4fmu-v1/nsh/defconfig | 3 +-- nuttx-configs/px4fmu-v2/nsh/defconfig | 3 +-- nuttx-configs/px4io-v1/nsh/defconfig | 2 ++ 3 files changed, 4 insertions(+), 4 deletions(-) diff --git a/nuttx-configs/px4fmu-v1/nsh/defconfig b/nuttx-configs/px4fmu-v1/nsh/defconfig index d7697cd67..cd9c05b13 100644 --- a/nuttx-configs/px4fmu-v1/nsh/defconfig +++ b/nuttx-configs/px4fmu-v1/nsh/defconfig @@ -287,8 +287,7 @@ CONFIG_STM32_USART_SINGLEWIRE=y # # CONFIG_STM32_I2C_DYNTIMEO is not set CONFIG_STM32_I2CTIMEOSEC=0 -CONFIG_STM32_I2CTIMEOMS=10 -CONFIG_STM32_I2CTIMEOTICKS=500 +CONFIG_STM32_I2CTIMEOMS=1 # CONFIG_STM32_I2C_DUTY16_9 is not set # diff --git a/nuttx-configs/px4fmu-v2/nsh/defconfig b/nuttx-configs/px4fmu-v2/nsh/defconfig index bd956f7bf..e31f40cd2 100644 --- a/nuttx-configs/px4fmu-v2/nsh/defconfig +++ b/nuttx-configs/px4fmu-v2/nsh/defconfig @@ -323,8 +323,7 @@ CONFIG_STM32_USART_SINGLEWIRE=y # # CONFIG_STM32_I2C_DYNTIMEO is not set CONFIG_STM32_I2CTIMEOSEC=0 -CONFIG_STM32_I2CTIMEOMS=10 -CONFIG_STM32_I2CTIMEOTICKS=500 +CONFIG_STM32_I2CTIMEOMS=1 # CONFIG_STM32_I2C_DUTY16_9 is not set # diff --git a/nuttx-configs/px4io-v1/nsh/defconfig b/nuttx-configs/px4io-v1/nsh/defconfig index 512c6a0f2..7c76be7ec 100755 --- a/nuttx-configs/px4io-v1/nsh/defconfig +++ b/nuttx-configs/px4io-v1/nsh/defconfig @@ -133,6 +133,8 @@ CONFIG_STM32_USART2=y CONFIG_STM32_USART3=y CONFIG_STM32_I2C1=y CONFIG_STM32_I2C2=n +CONFIG_STM32_I2CTIMEOSEC=0 +CONFIG_STM32_I2CTIMEOMS=1 CONFIG_STM32_BKP=n CONFIG_STM32_PWR=n CONFIG_STM32_DAC=n -- cgit v1.2.3 From 65dab36910ed44acbfd589c52bfe8d308f0199e5 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 26 Aug 2014 10:13:52 +0200 Subject: Improve startup and payload handling --- ROMFS/px4fmu_common/init.d/3035_viper | 2 ++ ROMFS/px4fmu_common/init.d/rc.interface | 5 +++++ ROMFS/px4fmu_common/init.d/rcS | 9 ++++----- ROMFS/px4fmu_common/mixers/FMU_AERT.mix | 11 ++++++----- ROMFS/px4fmu_common/mixers/Viper.mix | 13 ++++++------- 5 files changed, 23 insertions(+), 17 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/3035_viper b/ROMFS/px4fmu_common/init.d/3035_viper index a4c1e832d..3714b612f 100644 --- a/ROMFS/px4fmu_common/init.d/3035_viper +++ b/ROMFS/px4fmu_common/init.d/3035_viper @@ -8,3 +8,5 @@ sh /etc/init.d/rc.fw_defaults set MIXER Viper + +set FAILSAFE "-c567 -p 1000" diff --git a/ROMFS/px4fmu_common/init.d/rc.interface b/ROMFS/px4fmu_common/init.d/rc.interface index 1de0abb58..e44cd0953 100644 --- a/ROMFS/px4fmu_common/init.d/rc.interface +++ b/ROMFS/px4fmu_common/init.d/rc.interface @@ -77,4 +77,9 @@ then pwm max -c $PWM_OUTPUTS -p $PWM_MAX fi fi + + if [ $FAILSAFE != none ] + then + pwm failsafe -d $OUTPUT_DEV $FAILSAFE + fi fi diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 195771905..ea04ece34 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -66,6 +66,9 @@ then # sercon + # Try to get an USB console + nshterm /dev/ttyACM0 & + # # Start the ORB (first app to start) # @@ -96,11 +99,9 @@ then # if rgbled start then - echo "[init] RGB Led" else if blinkm start then - echo "[init] BlinkM" blinkm systemstate fi fi @@ -129,6 +130,7 @@ then set LOAD_DEFAULT_APPS yes set GPS yes set GPS_FAKE no + set FAILSAFE none # # Set DO_AUTOCONFIG flag to use it in AUTOSTART scripts @@ -279,9 +281,6 @@ then fi fi - # Try to get an USB console - nshterm /dev/ttyACM0 & - # # Start the datamanager (and do not abort boot if it fails) # diff --git a/ROMFS/px4fmu_common/mixers/FMU_AERT.mix b/ROMFS/px4fmu_common/mixers/FMU_AERT.mix index 0ec663e35..7fed488af 100644 --- a/ROMFS/px4fmu_common/mixers/FMU_AERT.mix +++ b/ROMFS/px4fmu_common/mixers/FMU_AERT.mix @@ -64,21 +64,22 @@ O: 10000 10000 0 -10000 10000 S: 0 3 0 20000 -10000 -10000 10000 -Gimbal / flaps / payload mixer for last four channels +Gimbal / flaps / payload mixer for last four channels, +using the payload control group ----------------------------------------------------- M: 1 O: 10000 10000 0 -10000 10000 -S: 0 4 10000 10000 0 -10000 10000 +S: 2 0 10000 10000 0 -10000 10000 M: 1 O: 10000 10000 0 -10000 10000 -S: 0 5 10000 10000 0 -10000 10000 +S: 2 1 10000 10000 0 -10000 10000 M: 1 O: 10000 10000 0 -10000 10000 -S: 0 6 10000 10000 0 -10000 10000 +S: 2 2 10000 10000 0 -10000 10000 M: 1 O: 10000 10000 0 -10000 10000 -S: 0 7 10000 10000 0 -10000 10000 +S: 2 3 10000 10000 0 -10000 10000 diff --git a/ROMFS/px4fmu_common/mixers/Viper.mix b/ROMFS/px4fmu_common/mixers/Viper.mix index 79c4447be..5a0381bd8 100755 --- a/ROMFS/px4fmu_common/mixers/Viper.mix +++ b/ROMFS/px4fmu_common/mixers/Viper.mix @@ -52,21 +52,20 @@ M: 1 O: 10000 10000 0 -10000 10000 S: 0 3 0 20000 -10000 -10000 10000 -Gimbal / flaps / payload mixer for last four channels +Inputs to the mixer come from channel group 2 (payload), channels 0 +(bay servo 1), 1 (bay servo 2) and 3 (drop release). ----------------------------------------------------- M: 1 O: 10000 10000 0 -10000 10000 -S: 0 4 10000 10000 0 -10000 10000 +S: 2 0 10000 10000 0 -10000 10000 M: 1 O: 10000 10000 0 -10000 10000 -S: 0 5 10000 10000 0 -10000 10000 +S: 2 1 10000 10000 0 -10000 10000 M: 1 O: 10000 10000 0 -10000 10000 -S: 0 6 10000 10000 0 -10000 10000 +S: 2 2 -10000 -10000 0 -10000 10000 + -M: 1 -O: 10000 10000 0 -10000 10000 -S: 0 7 10000 10000 0 -10000 10000 -- cgit v1.2.3 From 4af4e4e1e5d7209819c1fb43508ddd1ebed4f9c7 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 26 Aug 2014 10:14:36 +0200 Subject: Support additional payload commands and let commander ignore them --- src/modules/commander/commander.cpp | 5 +++++ src/modules/uORB/topics/vehicle_command.h | 19 ++++++++++++------- 2 files changed, 17 insertions(+), 7 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 74deda8cc..a5a772825 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; diff --git a/src/modules/uORB/topics/vehicle_command.h b/src/modules/uORB/topics/vehicle_command.h index 7db33d98b..44aa50572 100644 --- a/src/modules/uORB/topics/vehicle_command.h +++ b/src/modules/uORB/topics/vehicle_command.h @@ -1,9 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. - * Author: @author Thomas Gubler - * @author Julian Oes - * @author Lorenz Meier + * Copyright (C) 2012-2014 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -37,6 +34,10 @@ /** * @file vehicle_command.h * Definition of the vehicle command uORB topic. + * + * @author Thomas Gubler + * @author Julian Oes + * @author Lorenz Meier */ #ifndef TOPIC_VEHICLE_COMMAND_H_ @@ -52,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| */ @@ -87,7 +91,8 @@ enum VEHICLE_CMD { VEHICLE_CMD_MISSION_START = 300, /* start running a mission |first_item: the first mission item to run| last_item: the last mission item to run (after this item is run, the mission ends)| */ VEHICLE_CMD_COMPONENT_ARM_DISARM = 400, /* Arms / Disarms a component |1 to arm, 0 to disarm| */ VEHICLE_CMD_START_RX_PAIR = 500, /* Starts receiver pairing |0:Spektrum| 0:Spektrum DSM2, 1:Spektrum DSMX| */ - VEHICLE_CMD_ENUM_END = 501, /* | */ + VEHICLE_CMD_PAYLOAD_PREPARE_DEPLOY = 30001, /**< Prepare a payload deployment in the flight plan */ + VEHICLE_CMD_PAYLOAD_CONTROL_DEPLOY = 30002 /**< Control a pre-programmed payload deployment */ }; /** @@ -115,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 87b2375be436aa92d361e0131be9ff63ae43fe36 Mon Sep 17 00:00:00 2001 From: Holger Steinhaus Date: Tue, 26 Aug 2014 14:34:14 +0200 Subject: Ignore single channels during PWM output --- src/drivers/drv_pwm_output.h | 5 +++++ src/drivers/px4fmu/fmu.cpp | 4 +++- src/modules/px4iofirmware/registers.c | 4 +++- 3 files changed, 11 insertions(+), 2 deletions(-) diff --git a/src/drivers/drv_pwm_output.h b/src/drivers/drv_pwm_output.h index 84815fdfb..5aff6825b 100644 --- a/src/drivers/drv_pwm_output.h +++ b/src/drivers/drv_pwm_output.h @@ -94,6 +94,11 @@ __BEGIN_DECLS */ #define PWM_LOWEST_MAX 1700 +/** + * Do not output a channel with this value + */ +#define PWM_IGNORE_THIS_CHANNEL UINT16_MAX + /** * Servo output signal type, value is actual servo output pulse * width in microseconds. diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 82977a032..122a3cd17 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -1272,7 +1272,9 @@ PX4FMU::write(file *filp, const char *buffer, size_t len) memcpy(values, buffer, count * 2); for (uint8_t i = 0; i < count; i++) { - up_pwm_servo_set(i, values[i]); + if (values[i] != PWM_IGNORE_THIS_CHANNEL) { + up_pwm_servo_set(i, values[i]); + } } return count * 2; diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index 43161aa70..0da778b6f 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -285,7 +285,9 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num while ((offset < PX4IO_CONTROL_CHANNELS) && (num_values > 0)) { /* XXX range-check value? */ - r_page_servos[offset] = *values; + if (*values != PWM_IGNORE_THIS_CHANNEL) { + r_page_servos[offset] = *values; + } offset++; num_values--; -- cgit v1.2.3 From ccdfab245bac02565521f2d76e604f54ba3f5bd5 Mon Sep 17 00:00:00 2001 From: Holger Steinhaus Date: Tue, 26 Aug 2014 20:26:47 +0200 Subject: mixer_multirotor: topic for motor limit notification --- src/modules/uORB/objects_common.cpp | 3 + src/modules/uORB/topics/multirotor_motor_limits.h | 69 +++++++++++++++++++++++ 2 files changed, 72 insertions(+) create mode 100644 src/modules/uORB/topics/multirotor_motor_limits.h diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index f7d5f8737..b921865eb 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -192,6 +192,9 @@ ORB_DEFINE(actuator_outputs_1, struct actuator_outputs_s); ORB_DEFINE(actuator_outputs_2, struct actuator_outputs_s); ORB_DEFINE(actuator_outputs_3, struct actuator_outputs_s); +#include "topics/multirotor_motor_limits.h" +ORB_DEFINE(multirotor_motor_limits, struct multirotor_motor_limits_s); + #include "topics/telemetry_status.h" ORB_DEFINE(telemetry_status_0, struct telemetry_status_s); ORB_DEFINE(telemetry_status_1, struct telemetry_status_s); diff --git a/src/modules/uORB/topics/multirotor_motor_limits.h b/src/modules/uORB/topics/multirotor_motor_limits.h new file mode 100644 index 000000000..44c51e4d9 --- /dev/null +++ b/src/modules/uORB/topics/multirotor_motor_limits.h @@ -0,0 +1,69 @@ +/**************************************************************************** + * + * Copyright (C) 2012-2013 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file multirotor_motor_limits.h + * + * Definition of multirotor_motor_limits topic + */ + +#ifndef MULTIROTOR_MOTOR_LIMITS_H_ +#define MULTIROTOR_MOTOR_LIMITS_H_ + +#include "../uORB.h" +#include + +/** + * @addtogroup topics + * @{ + */ + +/** + * Motor limits + */ +struct multirotor_motor_limits_s { + uint8_t roll_pitch : 1; // roll/pitch limit reached + uint8_t yaw : 1; // yaw limit reached + uint8_t throttle_lower : 1; // lower throttle limit reached + uint8_t throttle_upper : 1; // upper throttle limit reached + uint8_t reserved : 4; +}; + +/** + * @} + */ + +/* register this as object request broker structure */ +ORB_DECLARE(multirotor_motor_limits); + +#endif -- cgit v1.2.3 From 5aafa1b021a80868a3b5aebde13cd54d407e6e41 Mon Sep 17 00:00:00 2001 From: Holger Steinhaus Date: Tue, 26 Aug 2014 20:36:02 +0200 Subject: mixer_multirotor: motor limit notification for PX4FMU --- src/modules/systemlib/mixer/mixer.h | 6 ++++++ src/modules/systemlib/mixer/mixer_multirotor.cpp | 22 +++++++++++++++++++++- 2 files changed, 27 insertions(+), 1 deletion(-) diff --git a/src/modules/systemlib/mixer/mixer.h b/src/modules/systemlib/mixer/mixer.h index cdf60febc..17989558e 100644 --- a/src/modules/systemlib/mixer/mixer.h +++ b/src/modules/systemlib/mixer/mixer.h @@ -130,6 +130,9 @@ #include #include "drivers/drv_mixer.h" + +#include + #include "mixer_load.h" /** @@ -531,6 +534,9 @@ private: float _yaw_scale; float _idle_speed; + orb_advert_t _limits_pub; + multirotor_motor_limits_s _limits; + unsigned _rotor_count; const Rotor *_rotors; diff --git a/src/modules/systemlib/mixer/mixer_multirotor.cpp b/src/modules/systemlib/mixer/mixer_multirotor.cpp index 4b22a46d0..57e17b67d 100644 --- a/src/modules/systemlib/mixer/mixer_multirotor.cpp +++ b/src/modules/systemlib/mixer/mixer_multirotor.cpp @@ -36,7 +36,8 @@ * * Multi-rotor mixers. */ - +#include +#include #include #include @@ -302,6 +303,11 @@ MultirotorMixer::mix(float *outputs, unsigned space) float min_out = 0.0f; float max_out = 0.0f; + _limits.roll_pitch = false; + _limits.yaw = false; + _limits.throttle_upper = false; + _limits.throttle_lower = false; + /* perform initial mix pass yielding unbounded outputs, ignore yaw */ for (unsigned i = 0; i < _rotor_count; i++) { float out = roll * _rotors[i].roll_scale + @@ -311,6 +317,7 @@ MultirotorMixer::mix(float *outputs, unsigned space) /* limit yaw if it causes outputs clipping */ if (out >= 0.0f && out < -yaw * _rotors[i].yaw_scale) { yaw = -out / _rotors[i].yaw_scale; + _limits.yaw = true; } /* calculate min and max output values */ @@ -332,6 +339,7 @@ MultirotorMixer::mix(float *outputs, unsigned space) for (unsigned i = 0; i < _rotor_count; i++) { outputs[i] = scale_in * (roll * _rotors[i].roll_scale + pitch * _rotors[i].pitch_scale) + thrust; } + _limits.roll_pitch = true; } else { /* roll/pitch mixed without limiting, add yaw control */ @@ -344,6 +352,7 @@ MultirotorMixer::mix(float *outputs, unsigned space) float scale_out; if (max_out > 1.0f) { scale_out = 1.0f / max_out; + _limits.throttle_upper = true; } else { scale_out = 1.0f; @@ -351,9 +360,20 @@ MultirotorMixer::mix(float *outputs, unsigned space) /* scale outputs to range _idle_speed..1, and do final limiting */ for (unsigned i = 0; i < _rotor_count; i++) { + if (outputs[i] < _idle_speed) { + _limits.throttle_lower = true; + } outputs[i] = constrain(_idle_speed + (outputs[i] * (1.0f - _idle_speed) * scale_out), _idle_speed, 1.0f); } +#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1) || defined(CONFIG_ARCH_BOARD_PX4FMU_V2) + /* publish/advertise motor limits if running on FMU */ + if (_limits_pub > 0) { + orb_publish(ORB_ID(multirotor_motor_limits), _limits_pub, &_limits); + } else { + _limits_pub = orb_advertise(ORB_ID(multirotor_motor_limits), &_limits); + } +#endif return _rotor_count; } -- cgit v1.2.3 From 6ece4cf7c44869d2fd6e027ad6239baf3af860df Mon Sep 17 00:00:00 2001 From: Holger Steinhaus Date: Tue, 26 Aug 2014 21:08:16 +0200 Subject: ORB topic for drive testing requests --- src/modules/uORB/objects_common.cpp | 3 ++ src/modules/uORB/topics/test_motor.h | 70 ++++++++++++++++++++++++++++++++++++ 2 files changed, 73 insertions(+) create mode 100644 src/modules/uORB/topics/test_motor.h diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index f7d5f8737..b8cdb45eb 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -198,6 +198,9 @@ ORB_DEFINE(telemetry_status_1, struct telemetry_status_s); ORB_DEFINE(telemetry_status_2, struct telemetry_status_s); ORB_DEFINE(telemetry_status_3, struct telemetry_status_s); +#include "topics/test_motor.h" +ORB_DEFINE(test_motor, struct test_motor_s); + #include "topics/debug_key_value.h" ORB_DEFINE(debug_key_value, struct debug_key_value_s); diff --git a/src/modules/uORB/topics/test_motor.h b/src/modules/uORB/topics/test_motor.h new file mode 100644 index 000000000..491096934 --- /dev/null +++ b/src/modules/uORB/topics/test_motor.h @@ -0,0 +1,70 @@ +/**************************************************************************** + * + * Copyright (C) 2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file test_motor.h + * + * Test a single drive motor while the vehicle is disarmed + * + * Publishing values to this topic causes a single motor to spin, e.g. for + * ground test purposes. This topic will be ignored while the vehicle is armed. + * + */ + +#ifndef TOPIC_TEST_MOTOR_H +#define TOPIC_TEST_MOTOR_H + +#include +#include "../uORB.h" + +#define NUM_MOTOR_OUTPUTS 8 + +/** + * @addtogroup topics + * @{ + */ + +struct test_motor_s { + uint64_t timestamp; /**< output timestamp in us since system boot */ + unsigned motor_number; /**< number of motor to spin */ + float value; /**< output data, in natural output units */ +}; + +/** + * @} + */ + +/* actuator output sets; this list can be expanded as more drivers emerge */ +ORB_DECLARE(test_motor); + +#endif -- cgit v1.2.3 From f8ba5f8dae146529e3ff28f4330cdc4daaa6f260 Mon Sep 17 00:00:00 2001 From: Holger Steinhaus Date: Tue, 26 Aug 2014 21:10:39 +0200 Subject: cmd line utility for controlling drive tests --- src/systemcmds/motor_test/module.mk | 41 +++++++++++ src/systemcmds/motor_test/motor_test.c | 128 +++++++++++++++++++++++++++++++++ 2 files changed, 169 insertions(+) create mode 100644 src/systemcmds/motor_test/module.mk create mode 100644 src/systemcmds/motor_test/motor_test.c diff --git a/src/systemcmds/motor_test/module.mk b/src/systemcmds/motor_test/module.mk new file mode 100644 index 000000000..eb36d2ded --- /dev/null +++ b/src/systemcmds/motor_test/module.mk @@ -0,0 +1,41 @@ +############################################################################ +# +# Copyright (c) 2014 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +# +# Build the motor_test tool. +# + +MODULE_COMMAND = motor_test +SRCS = motor_test.c + +MODULE_STACKSIZE = 4096 diff --git a/src/systemcmds/motor_test/motor_test.c b/src/systemcmds/motor_test/motor_test.c new file mode 100644 index 000000000..079f99674 --- /dev/null +++ b/src/systemcmds/motor_test/motor_test.c @@ -0,0 +1,128 @@ +/**************************************************************************** + * + * Copyright (C) 2014 PX4 Development Team. All rights reserved. + * Author: Holger Steinhaus + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file motor_test.c + * + * Tool for drive testing + */ + +#include + +#include +#include +#include +#include + +#include +#include +#include + + +#include "systemlib/systemlib.h" +#include "systemlib/err.h" + + +__EXPORT int motor_test_main(int argc, char *argv[]); +static void motor_test(unsigned channel, float value); +static void usage(const char *reason); + +void motor_test(unsigned channel, float value) +{ + orb_advert_t _test_motor_pub; + struct test_motor_s _test_motor; + + _test_motor.motor_number = channel; + _test_motor.timestamp = hrt_absolute_time(); + _test_motor.value = value; + + if (_test_motor_pub > 0) { + /* publish armed state */ + orb_publish(ORB_ID(test_motor), _test_motor_pub, &_test_motor); + } else { + /* advertise and publish */ + _test_motor_pub = orb_advertise(ORB_ID(test_motor), &_test_motor); + } +} + +static void usage(const char *reason) +{ + if (reason != NULL) + warnx("%s", reason); + + errx(1, + "usage:\n" + "motor_test\n" + " -m Motor to test (0..7)\n" + " -p Power (0..100)\n"); +} + +int motor_test_main(int argc, char *argv[]) +{ + unsigned long channel, lval; + float value; + int ch; + + if (argc != 5) { + usage("please specify motor and power"); + } + + while ((ch = getopt(argc, argv, "m:p:")) != EOF) { + switch (ch) { + + case 'm': + /* Read in channels supplied as one int and convert to mask: 1234 -> 0xF */ + channel = strtoul(optarg, NULL, 0); + break; + + case 'p': + /* Read in custom low value */ + lval = strtoul(optarg, NULL, 0); + + if (lval > 100) + usage("value invalid"); + + value = (float)lval/100.f; + break; + default: + usage(NULL); + } + } + + motor_test(channel, value); + + printf("motor %d set to %.2f\n", channel, value); + + exit(0); +} -- cgit v1.2.3 From 3d528a2c979e7d0df1171afc1f038759c7b01383 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 26 Aug 2014 22:22:59 +0200 Subject: introduce new nav state to allow normal rtl with RC switch --- src/modules/commander/state_machine_helper.cpp | 2 +- src/modules/navigator/navigator_main.cpp | 5 ++++- src/modules/uORB/topics/vehicle_status.h | 1 + 3 files changed, 6 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 098ff1a3d..e3b5d30e4 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -462,7 +462,7 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en status->failsafe = true; if (status->condition_global_position_valid && status->condition_home_position_valid) { - status->nav_state = NAVIGATION_STATE_AUTO_RTL; + status->nav_state = NAVIGATION_STATE_AUTO_RCRECOVER; } else if (status->condition_local_position_valid) { status->nav_state = NAVIGATION_STATE_LAND; } else if (status->condition_local_altitude_valid) { diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index c173ecd50..9a8c54e7e 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -433,13 +433,16 @@ Navigator::task_main() case NAVIGATION_STATE_AUTO_LOITER: _navigation_mode = &_loiter; break; - case NAVIGATION_STATE_AUTO_RTL: + case NAVIGATION_STATE_AUTO_RCRECOVER: if (_param_rcloss_obc.get() != 0) { _navigation_mode = &_rcLoss; } else { _navigation_mode = &_rtl; } break; + case NAVIGATION_STATE_AUTO_RTL: + _navigation_mode = &_rtl; + break; case NAVIGATION_STATE_AUTO_RTGS: //XXX OBC: differentiate between rc loss and dl loss here /* Use complex data link loss mode only when enabled via param * otherwise use rtl */ diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index ad92f5b26..9dccb2309 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -102,6 +102,7 @@ typedef enum { NAVIGATION_STATE_AUTO_MISSION, /**< Auto mission mode */ NAVIGATION_STATE_AUTO_LOITER, /**< Auto loiter mode */ NAVIGATION_STATE_AUTO_RTL, /**< Auto return to launch mode */ + NAVIGATION_STATE_AUTO_RCRECOVER, /**< RC recover mode */ NAVIGATION_STATE_AUTO_RTGS, /**< Auto return to groundstation on data link loss */ NAVIGATION_STATE_AUTO_LANDENGFAIL, /**< Auto land on engine failure */ NAVIGATION_STATE_AUTO_LANDGPSFAIL, /**< Auto land on gps failure (e.g. open loop loiter down) */ -- cgit v1.2.3 From 5e5322c593c6bd8ccd894f47ab8fd88b72e51677 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 26 Aug 2014 22:46:09 +0200 Subject: fix flight termination circuit breaker name, tested --- src/drivers/px4io/px4io.cpp | 2 +- src/modules/systemlib/circuit_breaker.c | 2 +- src/modules/systemlib/circuit_breaker.h | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index d5f569d71..b5c5ef17c 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1170,7 +1170,7 @@ PX4IO::io_set_arming_state() } /* Do not set failsafe if circuit breaker is enabled */ - if (armed.force_failsafe && !circuit_breaker_enabled("CBRK_FLIGHTTERMINATION", CBRK_FLIGHTTERMINATION_KEY)) { + if (armed.force_failsafe && !circuit_breaker_enabled("CBRK_FLIGHTTERM", CBRK_FLIGHTTERM_KEY)) { set |= PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE; } else { clear |= PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE; diff --git a/src/modules/systemlib/circuit_breaker.c b/src/modules/systemlib/circuit_breaker.c index 8a0b51b71..b0f95aedf 100644 --- a/src/modules/systemlib/circuit_breaker.c +++ b/src/modules/systemlib/circuit_breaker.c @@ -106,7 +106,7 @@ PARAM_DEFINE_INT32(CBRK_AIRSPD_CHK, 0); * @max 121212 * @group Circuit Breaker */ -PARAM_DEFINE_INT32(CBRK_FLIGHTTERMINATION, 0); +PARAM_DEFINE_INT32(CBRK_FLIGHTTERM, 0); bool circuit_breaker_enabled(const char* breaker, int32_t magic) { diff --git a/src/modules/systemlib/circuit_breaker.h b/src/modules/systemlib/circuit_breaker.h index 54c4fced6..445a89d3a 100644 --- a/src/modules/systemlib/circuit_breaker.h +++ b/src/modules/systemlib/circuit_breaker.h @@ -53,7 +53,7 @@ #define CBRK_RATE_CTRL_KEY 140253 #define CBRK_IO_SAFETY_KEY 22027 #define CBRK_AIRSPD_CHK_KEY 162128 -#define CBRK_FLIGHTTERMINATION_KEY 121212 +#define CBRK_FLIGHTTERM_KEY 121212 #include -- cgit v1.2.3 From 9cc1f1ab9db4af9af18e6879ba82cbcfa8e588f3 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 26 Aug 2014 23:12:28 +0200 Subject: flight termination on gps failure && datalink loss: do not activate in manual modes --- src/modules/commander/commander.cpp | 11 ++++++++--- 1 file changed, 8 insertions(+), 3 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 8b065560f..5673037b8 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1605,9 +1605,14 @@ int commander_thread_main(int argc, char *argv[]) /* Check for failure combinations which lead to flight termination */ if (armed.armed) { /* At this point the data link and the gps system have been checked - * If both failed we want to terminate the flight */ - if ((status.data_link_lost && status.gps_failure) || - (status.data_link_lost_cmd && status.gps_failure_cmd)) { + * If we are not in a manual (RC stick controlled mode) + * and both failed we want to terminate the flight */ + if (status.main_state != MAIN_STATE_MANUAL && + status.main_state != MAIN_STATE_ACRO && + status.main_state != MAIN_STATE_ALTCTL && + status.main_state != MAIN_STATE_POSCTL && + ((status.data_link_lost && status.gps_failure) || + (status.data_link_lost_cmd && status.gps_failure_cmd))) { armed.force_failsafe = true; status_changed = true; static bool flight_termination_printed = false; -- cgit v1.2.3 From b928897ab525a79eb2fad202fc28ef0235adeb50 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 27 Aug 2014 07:58:36 +0200 Subject: mavlink: code style only fix --- src/modules/mavlink/mavlink_messages.cpp | 26 +++++++++++++------------- 1 file changed, 13 insertions(+), 13 deletions(-) diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 5a92004a6..a2f3828ff 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -886,8 +886,8 @@ protected: msg.eph = cm_uint16_from_m_float(gps.eph); msg.epv = cm_uint16_from_m_float(gps.epv); msg.vel = cm_uint16_from_m_float(gps.vel_m_s), - msg.cog = _wrap_2pi(gps.cog_rad) * M_RAD_TO_DEG_F * 1e2f, - msg.satellites_visible = gps.satellites_used; + msg.cog = _wrap_2pi(gps.cog_rad) * M_RAD_TO_DEG_F * 1e2f, + msg.satellites_visible = gps.satellites_used; _mavlink->send_message(MAVLINK_MSG_ID_GPS_RAW_INT, &msg); } @@ -957,11 +957,11 @@ protected: msg.lat = pos.lat * 1e7; msg.lon = pos.lon * 1e7; msg.alt = pos.alt * 1000.0f; - msg.relative_alt = (pos.alt - home.alt) * 1000.0f; - msg.vx = pos.vel_n * 100.0f; - msg.vy = pos.vel_e * 100.0f; - msg.vz = pos.vel_d * 100.0f; - msg.hdg = _wrap_2pi(pos.yaw) * M_RAD_TO_DEG_F * 100.0f; + msg.relative_alt = (pos.alt - home.alt) * 1000.0f; + msg.vx = pos.vel_n * 100.0f; + msg.vy = pos.vel_e * 100.0f; + msg.vz = pos.vel_d * 100.0f; + msg.hdg = _wrap_2pi(pos.yaw) * M_RAD_TO_DEG_F * 100.0f; _mavlink->send_message(MAVLINK_MSG_ID_GLOBAL_POSITION_INT, &msg); } @@ -1022,9 +1022,9 @@ protected: msg.x = pos.x; msg.y = pos.y; msg.z = pos.z; - msg.vx = pos.vx; - msg.vy = pos.vy; - msg.vz = pos.vz; + msg.vx = pos.vx; + msg.vy = pos.vy; + msg.vz = pos.vz; _mavlink->send_message(MAVLINK_MSG_ID_LOCAL_POSITION_NED, &msg); } @@ -1085,9 +1085,9 @@ protected: msg.x = pos.x; msg.y = pos.y; msg.z = pos.z; - msg.roll = pos.roll; - msg.pitch = pos.pitch; - msg.yaw = pos.yaw; + msg.roll = pos.roll; + msg.pitch = pos.pitch; + msg.yaw = pos.yaw; _mavlink->send_message(MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE, &msg); } -- cgit v1.2.3 From c79bc7073eb65f08b724a3893d183d20781f06e2 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 27 Aug 2014 07:59:34 +0200 Subject: Support for termination failsafe in PWM out driver --- src/drivers/drv_pwm_output.h | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/src/drivers/drv_pwm_output.h b/src/drivers/drv_pwm_output.h index 5aff6825b..bc586f395 100644 --- a/src/drivers/drv_pwm_output.h +++ b/src/drivers/drv_pwm_output.h @@ -205,10 +205,13 @@ ORB_DECLARE(output_pwm); #define PWM_SERVO_GET_DISABLE_LOCKDOWN _IOC(_PWM_SERVO_BASE, 22) /** force safety switch off (to disable use of safety switch) */ -#define PWM_SERVO_SET_FORCE_SAFETY_OFF _IOC(_PWM_SERVO_BASE, 23) +#define PWM_SERVO_SET_FORCE_SAFETY_OFF _IOC(_PWM_SERVO_BASE, 23) /** force failsafe mode (failsafe values are set immediately even if failsafe condition not met) */ -#define PWM_SERVO_SET_FORCE_FAILSAFE _IOC(_PWM_SERVO_BASE, 24) +#define PWM_SERVO_SET_FORCE_FAILSAFE _IOC(_PWM_SERVO_BASE, 24) + +/** make failsafe non-recoverable (termination) if it occurs */ +#define PWM_SERVO_SET_TERMINATION_FAILSAFE _IOC(_PWM_SERVO_BASE, 25) /* * -- cgit v1.2.3 From 1fbdca4ee988d5816eebbd9fef95ce498bacfd14 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 27 Aug 2014 07:59:53 +0200 Subject: Add command to run termination failsafe --- src/systemcmds/pwm/pwm.c | 21 ++++++++++++++++++++- 1 file changed, 20 insertions(+), 1 deletion(-) diff --git a/src/systemcmds/pwm/pwm.c b/src/systemcmds/pwm/pwm.c index c8d698b86..478c2a772 100644 --- a/src/systemcmds/pwm/pwm.c +++ b/src/systemcmds/pwm/pwm.c @@ -654,9 +654,28 @@ pwm_main(int argc, char *argv[]) } } exit(0); + } else if (!strcmp(argv[1], "terminatefail")) { + + if (argc < 3) { + errx(1, "arg missing [on|off]"); + } else { + + if (!strcmp(argv[2], "on")) { + /* force failsafe */ + ret = ioctl(fd, PWM_SERVO_SET_TERMINATION_FAILSAFE, 1); + } else { + /* force failsafe */ + ret = ioctl(fd, PWM_SERVO_SET_TERMINATION_FAILSAFE, 0); + } + + if (ret != OK) { + warnx("FAILED setting termination failsafe %s", argv[2]); + } + } + exit(0); } - usage("specify arm|disarm|rate|failsafe|disarmed|min|max|test|info|forcefail"); + usage("specify arm|disarm|rate|failsafe\n\t\tdisarmed|min|max|test|info|forcefail|terminatefail"); return 0; } -- cgit v1.2.3 From 49846d476f77290093c24097e05d5a8d60d1a4f1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 27 Aug 2014 08:00:12 +0200 Subject: IO firmware supports termination failsafe --- src/modules/px4iofirmware/mixer.cpp | 10 ++++++++++ src/modules/px4iofirmware/protocol.h | 3 ++- src/modules/px4iofirmware/registers.c | 18 ++++++++++++++++-- 3 files changed, 28 insertions(+), 3 deletions(-) diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index 606c639f9..1eacda97a 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -154,6 +154,16 @@ mixer_tick(void) } } + /* + * Check if failsafe termination is set - if yes, + * set the force failsafe flag once entering the first + * failsafe condition. + */ + if ((r_setup_arming & PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE) && + (source == MIX_FAILSAFE)) { + r_setup_arming |= PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE; + } + /* * Check if we should force failsafe - and do it if we have to */ diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h index 050783687..eae7f9567 100644 --- a/src/modules/px4iofirmware/protocol.h +++ b/src/modules/px4iofirmware/protocol.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -180,6 +180,7 @@ #define PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED (1 << 6) /* Disable the IO-internal evaluation of the RC */ #define PX4IO_P_SETUP_ARMING_LOCKDOWN (1 << 7) /* If set, the system operates normally, but won't actuate any servos */ #define PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE (1 << 8) /* If set, the system will always output the failsafe values */ +#define PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE (1 << 9) /* If set, the system will never return from a failsafe, but remain in failsafe once triggered. */ #define PX4IO_P_SETUP_PWM_RATES 2 /* bitmask, 0 = low rate, 1 = high rate */ #define PX4IO_P_SETUP_PWM_DEFAULTRATE 3 /* 'low' PWM frame output rate in Hz */ diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index 0da778b6f..7a5a5e484 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -190,7 +190,8 @@ volatile uint16_t r_page_setup[] = PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE | \ PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED | \ PX4IO_P_SETUP_ARMING_LOCKDOWN | \ - PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE) + PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE | \ + PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE) #define PX4IO_P_SETUP_RATES_VALID ((1 << PX4IO_SERVO_COUNT) - 1) #define PX4IO_P_SETUP_RELAYS_VALID ((1 << PX4IO_RELAY_CHANNELS) - 1) @@ -518,6 +519,19 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) r_status_flags |= PX4IO_P_STATUS_FLAGS_INIT_OK; } + /* + * If the failsafe termination flag is set, do not allow the autopilot to unset it + */ + value |= (r_setup_arming & PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE); + + /* + * If failsafe termination is enabled and force failsafe bit is set, do not allow + * the autopilot to clear it. + */ + if (r_setup_arming & PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE) { + value |= (r_setup_arming & PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE); + } + r_setup_arming = value; break; -- cgit v1.2.3 From a7109609ecfeba0a11121c1b83a46b1463f55931 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 27 Aug 2014 08:00:28 +0200 Subject: support termination failsafe in IO driver --- src/drivers/px4io/px4io.cpp | 22 +++++++++++++++++++++- 1 file changed, 21 insertions(+), 1 deletion(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 97919538f..3fdb1dd1f 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1175,6 +1175,14 @@ PX4IO::io_set_arming_state() clear |= PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE; } + // XXX this is for future support in the commander + // but can be removed if unneeded + // if (armed.termination_failsafe) { + // set |= PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE; + // } else { + // clear |= PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE; + // } + if (armed.ready_to_arm) { set |= PX4IO_P_SETUP_ARMING_IO_ARM_OK; @@ -2038,7 +2046,8 @@ PX4IO::print_status(bool extended_status) ((arming & PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK) ? " INAIR_RESTART_OK" : ""), ((arming & PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE) ? " ALWAYS_PWM_ENABLE" : ""), ((arming & PX4IO_P_SETUP_ARMING_LOCKDOWN) ? " LOCKDOWN" : ""), - ((arming & PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE) ? " FORCE_FAILSAFE" : "") + ((arming & PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE) ? " FORCE_FAILSAFE" : ""), + ((arming & PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE) ? " TERM_FAILSAFE" : "") ); #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 printf("rates 0x%04x default %u alt %u relays 0x%04x\n", @@ -2262,6 +2271,17 @@ PX4IO::ioctl(file * filep, int cmd, unsigned long arg) } break; + case PWM_SERVO_SET_TERMINATION_FAILSAFE: + /* if failsafe occurs, do not allow the system to recover */ + if (arg == 0) { + /* clear termination failsafe flag */ + ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE, 0); + } else { + /* set termination failsafe flag */ + ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, 0, PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE); + } + break; + case DSM_BIND_START: /* only allow DSM2, DSM-X and DSM-X with more than 7 channels */ -- cgit v1.2.3 From 91d50301c61cf495e83cab59621ef83cff24da3a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 27 Aug 2014 10:46:10 +0200 Subject: Do not enter RC override if FMU is lost and termination failsafe mode requested --- src/modules/px4iofirmware/mixer.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index 1eacda97a..0c65b7642 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -139,7 +139,9 @@ mixer_tick(void) (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) && (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) && !(r_setup_arming & PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED) && - !(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK)) { + !(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) && + /* do not enter manual override if we asked for termination failsafe and FMU is lost */ + !(r_setup_arming & PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE)) { /* if allowed, mix from RC inputs directly */ source = MIX_OVERRIDE; -- cgit v1.2.3 From 0eea110f6fb4e95238a496f3a71b1cb6741625f7 Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Wed, 27 Aug 2014 17:14:49 -0700 Subject: Modified to use new FILE_TRANSFER_PROTOCOL message - Also corrected system/component id transmit/check --- src/modules/mavlink/mavlink_ftp.cpp | 4 +-- src/modules/mavlink/mavlink_ftp.h | 49 ++++++++++++++++++-------------- src/modules/mavlink/mavlink_receiver.cpp | 2 +- 3 files changed, 30 insertions(+), 25 deletions(-) diff --git a/src/modules/mavlink/mavlink_ftp.cpp b/src/modules/mavlink/mavlink_ftp.cpp index 00c8df18c..f4a7bc101 100644 --- a/src/modules/mavlink/mavlink_ftp.cpp +++ b/src/modules/mavlink/mavlink_ftp.cpp @@ -114,7 +114,7 @@ MavlinkFTP::_worker(Request *req) uint32_t messageCRC; // basic sanity checks; must validate length before use - if ((hdr->magic != kProtocolMagic) || (hdr->size > kMaxDataLength)) { + if (hdr->size > kMaxDataLength) { errorCode = kErrNoRequest; goto out; } @@ -199,7 +199,7 @@ MavlinkFTP::_reply(Request *req) { auto hdr = req->header(); - hdr->magic = kProtocolMagic; + hdr->seqNumber = req->header()->seqNumber + 1; // message is assumed to be already constructed in the request buffer, so generate the CRC hdr->crc32 = 0; diff --git a/src/modules/mavlink/mavlink_ftp.h b/src/modules/mavlink/mavlink_ftp.h index 43de89de9..cf9ea0c07 100644 --- a/src/modules/mavlink/mavlink_ftp.h +++ b/src/modules/mavlink/mavlink_ftp.h @@ -38,9 +38,6 @@ * * MAVLink remote file server. * - * Messages are wrapped in ENCAPSULATED_DATA messages. Every message includes - * a session ID and sequence number. - * * A limited number of requests (currently 2) may be outstanding at a time. * Additional messages will be discarded. * @@ -74,16 +71,18 @@ private: static MavlinkFTP *_server; + /// @brief This structure is sent in the payload portion of the file transfer protocol mavlink message. + /// This structure is layed out such that it should not require any special compiler packing to not consume extra space. struct RequestHeader - { - uint8_t magic; - uint8_t session; - uint8_t opcode; - uint8_t size; - uint32_t crc32; - uint32_t offset; + { + uint16_t seqNumber; ///< sequence number for message + unsigned int session:4; ///< Session id for read and write commands + unsigned int opcode:4; ///< Command opcode + uint8_t size; ///< Size of data + uint32_t crc32; ///< CRC for entire Request structure, with crc32 set to 0 + uint32_t offset; ///< Offsets for List and Read commands uint8_t data[]; - }; + }; enum Opcode : uint8_t { @@ -131,10 +130,11 @@ private: }; bool decode(Mavlink *mavlink, mavlink_message_t *fromMessage) { - if (fromMessage->msgid == MAVLINK_MSG_ID_ENCAPSULATED_DATA) { + if (fromMessage->msgid == MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL) { + _systemId = fromMessage->sysid; _mavlink = mavlink; - mavlink_msg_encapsulated_data_decode(fromMessage, &_message); - return true; + mavlink_msg_file_transfer_protocol_decode(fromMessage, &_message); + return _message.target_system == _mavlink->get_system_id(); } return false; } @@ -145,8 +145,14 @@ private: // flat memory architecture, as we're operating between threads here. mavlink_message_t msg; msg.checksum = 0; - unsigned len = mavlink_msg_encapsulated_data_pack_chan(_mavlink->get_system_id(), _mavlink->get_component_id(), - _mavlink->get_channel(), &msg, sequence()+1, rawData()); + unsigned len = mavlink_msg_file_transfer_protocol_pack_chan(_mavlink->get_system_id(), // Sender system id + _mavlink->get_component_id(), // Sender component id + _mavlink->get_channel(), // Channel to send on + &msg, // Message to pack payload into + 0, // Target network + _systemId, // Target system id + 0, // Target component id + rawData()); // Payload to pack into message _mavlink->lockMessageBufferMutex(); bool success = _mavlink->message_buffer_write(&msg, len); @@ -167,26 +173,25 @@ private: #endif } - uint8_t *rawData() { return &_message.data[0]; } - RequestHeader *header() { return reinterpret_cast(&_message.data[0]); } + uint8_t *rawData() { return &_message.payload[0]; } + RequestHeader *header() { return reinterpret_cast(&_message.payload[0]); } uint8_t *requestData() { return &(header()->data[0]); } unsigned dataSize() { return header()->size + sizeof(RequestHeader); } - uint16_t sequence() const { return _message.seqnr; } mavlink_channel_t channel() { return _mavlink->get_channel(); } char *dataAsCString(); private: Mavlink *_mavlink; - mavlink_encapsulated_data_t _message; + mavlink_file_transfer_protocol_t _message; + uint8_t _systemId; }; - static const uint8_t kProtocolMagic = 'f'; static const char kDirentFile = 'F'; static const char kDirentDir = 'D'; static const char kDirentUnknown = 'U'; - static const uint8_t kMaxDataLength = MAVLINK_MSG_ENCAPSULATED_DATA_FIELD_DATA_LEN - sizeof(RequestHeader); + static const uint8_t kMaxDataLength = MAVLINK_MSG_FILE_TRANSFER_PROTOCOL_FIELD_PAYLOAD_LEN - sizeof(RequestHeader); /// Request worker; runs on the low-priority work queue to service /// remote requests. diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index a602344fd..bed1bd789 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -172,7 +172,7 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) handle_message_request_data_stream(msg); break; - case MAVLINK_MSG_ID_ENCAPSULATED_DATA: + case MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL: MavlinkFTP::getServer()->handle_message(_mavlink, msg); break; -- cgit v1.2.3 From fce0a3b728f0aca12f9afb678f36cacde865e976 Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Wed, 27 Aug 2014 21:39:55 -0700 Subject: Gave up on using bitfields --- src/modules/mavlink/mavlink_ftp.cpp | 6 ++++++ src/modules/mavlink/mavlink_ftp.h | 17 +++++++++-------- 2 files changed, 15 insertions(+), 8 deletions(-) diff --git a/src/modules/mavlink/mavlink_ftp.cpp b/src/modules/mavlink/mavlink_ftp.cpp index f4a7bc101..5b65dc369 100644 --- a/src/modules/mavlink/mavlink_ftp.cpp +++ b/src/modules/mavlink/mavlink_ftp.cpp @@ -122,6 +122,9 @@ MavlinkFTP::_worker(Request *req) // check request CRC to make sure this is one of ours messageCRC = hdr->crc32; hdr->crc32 = 0; + hdr->padding[0] = 0; + hdr->padding[1] = 0; + hdr->padding[2] = 0; if (crc32(req->rawData(), req->dataSize()) != messageCRC) { errorCode = kErrNoRequest; goto out; @@ -203,6 +206,9 @@ MavlinkFTP::_reply(Request *req) // message is assumed to be already constructed in the request buffer, so generate the CRC hdr->crc32 = 0; + hdr->padding[0] = 0; + hdr->padding[1] = 0; + hdr->padding[2] = 0; hdr->crc32 = crc32(req->rawData(), req->dataSize()); // then pack and send the reply back to the request source diff --git a/src/modules/mavlink/mavlink_ftp.h b/src/modules/mavlink/mavlink_ftp.h index cf9ea0c07..1dd8f102e 100644 --- a/src/modules/mavlink/mavlink_ftp.h +++ b/src/modules/mavlink/mavlink_ftp.h @@ -71,16 +71,17 @@ private: static MavlinkFTP *_server; - /// @brief This structure is sent in the payload portion of the file transfer protocol mavlink message. - /// This structure is layed out such that it should not require any special compiler packing to not consume extra space. + /// @brief Trying to pack structures across differing compilers is questionable for Clients, so we pad the + /// structure ourselves to 32 bit alignment which should get us what we want. struct RequestHeader { - uint16_t seqNumber; ///< sequence number for message - unsigned int session:4; ///< Session id for read and write commands - unsigned int opcode:4; ///< Command opcode - uint8_t size; ///< Size of data - uint32_t crc32; ///< CRC for entire Request structure, with crc32 set to 0 - uint32_t offset; ///< Offsets for List and Read commands + uint16_t seqNumber; ///< sequence number for message + uint8_t session; ///< Session id for read and write commands + uint8_t opcode; ///< Command opcode + uint8_t size; ///< Size of data + uint8_t padding[3]; + uint32_t crc32; ///< CRC for entire Request structure, with crc32 and padding set to 0 + uint32_t offset; ///< Offsets for List and Read commands uint8_t data[]; }; -- cgit v1.2.3 From c85d7aae06b49f43e32dde05c00bcee1310aee9f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 28 Aug 2014 20:36:59 +0200 Subject: Make sure we got to valid input at least once before kicking in failsafe --- src/modules/px4iofirmware/mixer.cpp | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index 0c65b7642..17751bd6d 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -71,6 +71,7 @@ extern "C" { static bool mixer_servos_armed = false; static bool should_arm = false; static bool should_always_enable_pwm = false; +static bool input_valid_initialized = false; /* the input was valid at least once */ static volatile bool in_mixer = false; /* selected control values and count for mixing */ @@ -162,7 +163,8 @@ mixer_tick(void) * failsafe condition. */ if ((r_setup_arming & PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE) && - (source == MIX_FAILSAFE)) { + (source == MIX_FAILSAFE) && + input_valid_initialized) { r_setup_arming |= PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE; } @@ -180,6 +182,9 @@ mixer_tick(void) r_status_flags |= PX4IO_P_STATUS_FLAGS_FAILSAFE; } else { r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_FAILSAFE); + + /* we got valid input, kick off the full failsafe checks */ + input_valid_initialized = true; } /* -- cgit v1.2.3 From 6773eb988017063366dce95bbfec8fae1f1913f0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 28 Aug 2014 21:39:33 +0200 Subject: Introduce FMU initialised status flag, only run failsafe trap if initialized once --- src/modules/px4iofirmware/mixer.cpp | 67 +++++++++++++++++++----------------- src/modules/px4iofirmware/protocol.h | 1 + 2 files changed, 36 insertions(+), 32 deletions(-) diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index 17751bd6d..3e19333d8 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -58,7 +58,7 @@ extern "C" { /* * Maximum interval in us before FMU signal is considered lost */ -#define FMU_INPUT_DROP_LIMIT_US 200000 +#define FMU_INPUT_DROP_LIMIT_US 500000 /* XXX need to move the RC_CHANNEL_FUNCTION out of rc_channels.h and into systemlib */ #define ROLL 0 @@ -71,7 +71,6 @@ extern "C" { static bool mixer_servos_armed = false; static bool should_arm = false; static bool should_always_enable_pwm = false; -static bool input_valid_initialized = false; /* the input was valid at least once */ static volatile bool in_mixer = false; /* selected control values and count for mixing */ @@ -99,7 +98,8 @@ mixer_tick(void) { /* check that we are receiving fresh data from the FMU */ - if (hrt_elapsed_time(&system_state.fmu_data_received_time) > FMU_INPUT_DROP_LIMIT_US) { + if ((system_state.fmu_data_received_time == 0) || + hrt_elapsed_time(&system_state.fmu_data_received_time) > FMU_INPUT_DROP_LIMIT_US) { /* too long without FMU input, time to go to failsafe */ if (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) { @@ -110,6 +110,9 @@ mixer_tick(void) } else { r_status_flags |= PX4IO_P_STATUS_FLAGS_FMU_OK; + + /* this flag is never cleared once OK */ + r_status_flags |= PX4IO_P_STATUS_FLAGS_FMU_INITIALIZED; } /* default to failsafe mixing - it will be forced below if flag is set */ @@ -157,14 +160,41 @@ mixer_tick(void) } } + /* + * Decide whether the servos should be armed right now. + * + * We must be armed, and we must have a PWM source; either raw from + * FMU or from the mixer. + * + */ + should_arm = ( + /* IO initialised without error */ (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK) + /* and IO is armed */ && (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) + /* and FMU is armed */ && ( + ((r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) + /* and there is valid input via or mixer */ && (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) ) + /* or direct PWM is set */ || (r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) + /* or failsafe was set manually */ || ((r_setup_arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM) && !(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK)) + ) + ); + + should_always_enable_pwm = (r_setup_arming & PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE) + && (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK) + && (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK); + /* * Check if failsafe termination is set - if yes, * set the force failsafe flag once entering the first * failsafe condition. */ - if ((r_setup_arming & PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE) && + if ( /* if we have requested flight termination style failsafe (noreturn) */ + (r_setup_arming & PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE) && + /* and we ended up in a failsafe condition */ (source == MIX_FAILSAFE) && - input_valid_initialized) { + /* and we should be armed, so we intended to provide outputs */ + should_arm && + /* and FMU is initialized */ + (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_INITIALIZED)) { r_setup_arming |= PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE; } @@ -182,35 +212,8 @@ mixer_tick(void) r_status_flags |= PX4IO_P_STATUS_FLAGS_FAILSAFE; } else { r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_FAILSAFE); - - /* we got valid input, kick off the full failsafe checks */ - input_valid_initialized = true; } - /* - * Decide whether the servos should be armed right now. - * - * We must be armed, and we must have a PWM source; either raw from - * FMU or from the mixer. - * - * XXX correct behaviour for failsafe may require an additional case - * here. - */ - should_arm = ( - /* IO initialised without error */ (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK) - /* and IO is armed */ && (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) - /* and FMU is armed */ && ( - ((r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) - /* and there is valid input via or mixer */ && (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) ) - /* or direct PWM is set */ || (r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) - /* or failsafe was set manually */ || ((r_setup_arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM) && !(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK)) - ) - ); - - should_always_enable_pwm = (r_setup_arming & PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE) - && (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK) - && (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK); - /* * Run the mixers. */ diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h index eae7f9567..4739f6e40 100644 --- a/src/modules/px4iofirmware/protocol.h +++ b/src/modules/px4iofirmware/protocol.h @@ -111,6 +111,7 @@ #define PX4IO_P_STATUS_FLAGS_INIT_OK (1 << 10) /* initialisation of the IO completed without error */ #define PX4IO_P_STATUS_FLAGS_FAILSAFE (1 << 11) /* failsafe is active */ #define PX4IO_P_STATUS_FLAGS_SAFETY_OFF (1 << 12) /* safety is off */ +#define PX4IO_P_STATUS_FLAGS_FMU_INITIALIZED (1 << 13) /* FMU was initialized and OK once */ #define PX4IO_P_STATUS_ALARMS 3 /* alarm flags - alarms latch, write 1 to a bit to clear it */ #define PX4IO_P_STATUS_ALARMS_VBATT_LOW (1 << 0) /* [1] VBatt is very close to regulator dropout */ -- cgit v1.2.3 From a87a2644156e1fc105d151887489cb8eba8f881b Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Fri, 29 Aug 2014 03:19:49 +0400 Subject: UAVCAN update (lots of warning fixes) --- uavcan | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/uavcan b/uavcan index c7872def1..c4c14c60f 160000 --- a/uavcan +++ b/uavcan @@ -1 +1 @@ -Subproject commit c7872def16e82a8b318d571829fe9622e2d35ff0 +Subproject commit c4c14c60fbbd9acd281ee97d5bb2a4027d0ae2d9 -- cgit v1.2.3 From 580b726536bee9b4383e984ab40352a0c67ab850 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Fri, 29 Aug 2014 14:34:29 +0400 Subject: UAVCAN: missing declaration warning fix --- src/modules/uavcan/uavcan_clock.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/uavcan/uavcan_clock.cpp b/src/modules/uavcan/uavcan_clock.cpp index e41d5f953..fe8ba406a 100644 --- a/src/modules/uavcan/uavcan_clock.cpp +++ b/src/modules/uavcan/uavcan_clock.cpp @@ -62,6 +62,8 @@ void adjustUtc(uavcan::UtcDuration adjustment) (void)adjustment; } +uavcan::uint64_t getUtcUSecFromCanInterrupt(); + uavcan::uint64_t getUtcUSecFromCanInterrupt() { return 0; -- 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(-) 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(-) 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 ab022d51338c30207379048c913715a0b8d601c3 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 31 Aug 2014 16:23:37 +0200 Subject: disable underspeed protection when landing also in tecs --- src/lib/external_lgpl/tecs/tecs.cpp | 5 +++++ src/lib/external_lgpl/tecs/tecs.h | 10 ++++++++++ src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 3 +++ 3 files changed, 18 insertions(+) diff --git a/src/lib/external_lgpl/tecs/tecs.cpp b/src/lib/external_lgpl/tecs/tecs.cpp index a57a0481a..d27bf776f 100644 --- a/src/lib/external_lgpl/tecs/tecs.cpp +++ b/src/lib/external_lgpl/tecs/tecs.cpp @@ -252,6 +252,11 @@ void TECS::_update_height_demand(float demand, float state) void TECS::_detect_underspeed(void) { + if (!_detect_underspeed_enabled) { + _underspeed = false; + return; + } + if (((_integ5_state < _TASmin * 0.9f) && (_throttle_dem >= _THRmaxf * 0.95f)) || ((_integ3_state < _hgt_dem_adj) && _underspeed)) { _underspeed = true; diff --git a/src/lib/external_lgpl/tecs/tecs.h b/src/lib/external_lgpl/tecs/tecs.h index bcc2d90e5..36ae4ecaf 100644 --- a/src/lib/external_lgpl/tecs/tecs.h +++ b/src/lib/external_lgpl/tecs/tecs.h @@ -66,6 +66,9 @@ public: _hgt_dem_prev(0.0f), _TAS_dem_adj(0.0f), _STEdotErrLast(0.0f), + _underspeed(false), + _detect_underspeed_enabled(true), + _badDescent(false), _climbOutDem(false), _SPE_dem(0.0f), _SKE_dem(0.0f), @@ -221,6 +224,10 @@ public: _speedrate_p = speedrate_p; } + void set_detect_underspeed_enabled(bool enabled) { + _detect_underspeed_enabled = enabled; + } + private: struct tecs_state _tecs_state; @@ -323,6 +330,9 @@ private: // Underspeed condition bool _underspeed; + // Underspeed detection enabled + bool _detect_underspeed_enabled; + // Bad descent condition caused by unachievable airspeed demand bool _badDescent; 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 350ce6dec..522f5caca 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 @@ -1380,6 +1380,9 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_ _mTecs.updateAltitudeSpeed(flightPathAngle, altitude, alt_sp, _airspeed.true_airspeed_m_s, v_sp, mode, limitOverride); } else { + /* No underspeed protection in landing mode */ + _tecs.set_detect_underspeed_enabled(!(mode == TECS_MODE_LAND || mode == TECS_MODE_LAND_THROTTLELIM)); + /* Using tecs library */ _tecs.update_pitch_throttle(_R_nb, _att.pitch, altitude, alt_sp, v_sp, _airspeed.indicated_airspeed_m_s, eas2tas, -- 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(-) 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 0e954b30c2ef3e31d60e5ef7a5fa985882baef8f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 31 Aug 2014 17:58:43 +0200 Subject: Rely on laser altitude once valid --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 522f5caca..c983ac6f9 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 @@ -823,7 +823,7 @@ float FixedwingPositionControl::get_relative_landingalt(float land_setpoint_alt, * 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 || rel_alt_estimated > 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; } -- cgit v1.2.3 From 7a253d50f8da7a6f8176f784c196b85bcae3c8f1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 31 Aug 2014 17:59:21 +0200 Subject: Add more paranoid checks to sf0x altitude parsing --- src/drivers/sf0x/sf0x.cpp | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/src/drivers/sf0x/sf0x.cpp b/src/drivers/sf0x/sf0x.cpp index 80ecab2ee..d7780e9e8 100644 --- a/src/drivers/sf0x/sf0x.cpp +++ b/src/drivers/sf0x/sf0x.cpp @@ -593,13 +593,19 @@ SF0X::collect() /* wipe out partially read content from last cycle(s), check for dot */ for (unsigned i = 0; i < (lend - 2); i++) { if (_linebuf[i] == '\n') { + /* allocate temporary buffer */ char buf[sizeof(_linebuf)]; + /* copy remainder of buffer (2nd measurement) to temporary buffer */ memcpy(buf, &_linebuf[i+1], (lend + 1) - (i + 1)); + /* copy temporary buffer to beginning of line buffer, + * effectively overwriting a previous temporary + * measurement + */ memcpy(_linebuf, buf, (lend + 1) - (i + 1)); } /* we need a digit before the dot and a dot for a valid number */ - if (i > 0 && _linebuf[i] == '.') { + if (i > 0 && ((_linebuf[i - 1] >= '0') && (_linebuf[i - 1] <= '9')) && (_linebuf[i] == '.')) { valid = true; } } -- cgit v1.2.3 From ee913202991f2a015904b1570bd9f5a62865e5c7 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 31 Aug 2014 19:53:01 +0200 Subject: SF02/F driver: Improve parsing --- src/drivers/sf0x/sf0x.cpp | 40 ++++++++++++++++------------------------ 1 file changed, 16 insertions(+), 24 deletions(-) diff --git a/src/drivers/sf0x/sf0x.cpp b/src/drivers/sf0x/sf0x.cpp index d7780e9e8..cf0419075 100644 --- a/src/drivers/sf0x/sf0x.cpp +++ b/src/drivers/sf0x/sf0x.cpp @@ -520,11 +520,9 @@ 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; - } else if (_linebuf_index > 0) { - /* increment to next read position */ - _linebuf_index++; } /* the buffer for read chars is buflen minus null termination */ @@ -550,18 +548,19 @@ SF0X::collect() return -EAGAIN; } - /* we did increment the index to the next position already, so just add the additional fields */ - _linebuf_index += (ret - 1); + /* let the write pointer point to the next free entry */ + _linebuf_index += ret; _last_read = hrt_absolute_time(); - if (_linebuf_index < 1) { - /* we need at least the two end bytes to make sense of this string */ + /* 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 - 1] != '\r' || _linebuf[_linebuf_index] != '\n') { + } else if (_linebuf[_linebuf_index - 2] != '\r' || _linebuf[_linebuf_index - 1] != '\n') { - if (_linebuf_index >= readlen - 1) { + if (_linebuf_index == readlen) { /* we have a full buffer, but no line ending - abort */ _linebuf_index = 0; perf_count(_comms_errors); @@ -577,9 +576,7 @@ SF0X::collect() bool valid; /* enforce line ending */ - unsigned lend = (_linebuf_index < (sizeof(_linebuf) - 1)) ? _linebuf_index : (sizeof(_linebuf) - 1); - - _linebuf[lend] = '\0'; + _linebuf[_linebuf_index] = '\0'; if (_linebuf[0] == '-' && _linebuf[1] == '-' && _linebuf[2] == '.') { si_units = -1.0f; @@ -591,17 +588,12 @@ SF0X::collect() valid = false; /* wipe out partially read content from last cycle(s), check for dot */ - for (unsigned i = 0; i < (lend - 2); i++) { + for (unsigned i = 0; i < (_linebuf_index - 2); i++) { if (_linebuf[i] == '\n') { - /* allocate temporary buffer */ - char buf[sizeof(_linebuf)]; - /* copy remainder of buffer (2nd measurement) to temporary buffer */ - memcpy(buf, &_linebuf[i+1], (lend + 1) - (i + 1)); - /* copy temporary buffer to beginning of line buffer, - * effectively overwriting a previous temporary - * measurement - */ - memcpy(_linebuf, buf, (lend + 1) - (i + 1)); + /* 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 */ @@ -613,8 +605,8 @@ SF0X::collect() if (valid) { si_units = strtod(_linebuf, &end); - /* we require at least 3 characters for a valid number */ - if (end > _linebuf + 3) { + /* we require at least four characters for a valid number */ + if (end > _linebuf + 4) { valid = true; } else { si_units = -1.0f; -- cgit v1.2.3 From a46e1aa60efcaf8b52db02f783fc4c0a9dc05878 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 31 Aug 2014 20:55:29 +0200 Subject: Fix reschedule logic for SF02/F driver --- src/drivers/sf0x/sf0x.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/drivers/sf0x/sf0x.cpp b/src/drivers/sf0x/sf0x.cpp index cf0419075..b690938e7 100644 --- a/src/drivers/sf0x/sf0x.cpp +++ b/src/drivers/sf0x/sf0x.cpp @@ -707,12 +707,12 @@ SF0X::cycle() int collect_ret = collect(); if (collect_ret == -EAGAIN) { - /* reschedule to grab the missing bits, time to transmit 10 bytes @9600 bps */ + /* reschedule to grab the missing bits, time to transmit 8 bytes @ 9600 bps */ work_queue(HPWORK, &_work, (worker_t)&SF0X::cycle_trampoline, this, - USEC2TICK(1100)); + USEC2TICK(1042 * 8)); return; } -- cgit v1.2.3 From af3b3680e090ad80ffeb6581f1f58f2dd4ae5195 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 31 Aug 2014 21:10:04 +0200 Subject: Fixed SF02/F driver, tested ok --- src/drivers/sf0x/sf0x.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/drivers/sf0x/sf0x.cpp b/src/drivers/sf0x/sf0x.cpp index b690938e7..d382d08d0 100644 --- a/src/drivers/sf0x/sf0x.cpp +++ b/src/drivers/sf0x/sf0x.cpp @@ -606,7 +606,7 @@ SF0X::collect() si_units = strtod(_linebuf, &end); /* we require at least four characters for a valid number */ - if (end > _linebuf + 4) { + if (end > _linebuf + 3) { valid = true; } else { si_units = -1.0f; @@ -615,7 +615,7 @@ SF0X::collect() } } - debug("val (float): %8.4f, raw: %s, valid: %s\n", (double)si_units, _linebuf, ((valid) ? "OK" : "NO")); + 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; -- cgit v1.2.3 From 84d26185df1aa3a727d0db7b06b262e756eb5248 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 31 Aug 2014 21:11:20 +0200 Subject: Better initial config for Phantom --- ROMFS/px4fmu_common/init.d/3031_phantom | 3 +++ 1 file changed, 3 insertions(+) diff --git a/ROMFS/px4fmu_common/init.d/3031_phantom b/ROMFS/px4fmu_common/init.d/3031_phantom index 31dfe7100..53c48d8aa 100644 --- a/ROMFS/px4fmu_common/init.d/3031_phantom +++ b/ROMFS/px4fmu_common/init.d/3031_phantom @@ -30,6 +30,9 @@ then param set FW_RR_P 0.08 param set FW_R_LIM 50 param set FW_R_RMAX 0 + # Bottom of bay and nominal zero-pitch attitude differ + # the payload bay is pitched up about 7 degrees + param set SENS_BOARD_Y_OFF 7.0 fi set MIXER phantom -- cgit v1.2.3 From 22d2e26b9cab22fbe3f418b22e74fb05048083e0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 31 Aug 2014 21:55:31 +0200 Subject: Simply throttle limiting on approach - limit throttle still defaults to 1 --- 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 | 13 +++++++++---- 2 files changed, 15 insertions(+), 4 deletions(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index c983ac6f9..deccab482 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 @@ -569,6 +569,12 @@ FixedwingPositionControl::parameters_update() param_get(_parameter_handles.land_H1_virt, &(_parameters.land_H1_virt)); param_get(_parameter_handles.land_flare_alt_relative, &(_parameters.land_flare_alt_relative)); param_get(_parameter_handles.land_thrust_lim_alt_relative, &(_parameters.land_thrust_lim_alt_relative)); + + /* check if negative value for 2/3 of flare altitude is set for throttle cut */ + if (_parameters.land_thrust_lim_alt_relative < 0.0f) { + _parameters.land_thrust_lim_alt_relative = 0.66f * _parameters.land_flare_alt_relative; + } + 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)); 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 41c374407..890ab3bad 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 @@ -379,18 +379,23 @@ PARAM_DEFINE_FLOAT(FW_LND_ANG, 5.0f); PARAM_DEFINE_FLOAT(FW_LND_HVIRT, 10.0f); /** - * Landing flare altitude (relative) + * Landing flare altitude (relative to landing altitude) * + * @unit meter * @group L1 Control */ -PARAM_DEFINE_FLOAT(FW_LND_FLALT, 15.0f); +PARAM_DEFINE_FLOAT(FW_LND_FLALT, 8.0f); /** - * Landing throttle limit altitude (relative) + * Landing throttle limit altitude (relative landing altitude) * + * Default of -1.0f lets the system default to applying throttle + * limiting at 2/3 of the flare altitude. + * + * @unit meter * @group L1 Control */ -PARAM_DEFINE_FLOAT(FW_LND_TLALT, 5.0f); +PARAM_DEFINE_FLOAT(FW_LND_TLALT, -1.0f); /** * Landing heading hold horizontal distance -- cgit v1.2.3 From 69109b622796dacd6f78ebafa92b10379ba0d7b4 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 1 Sep 2014 00:45:41 +0200 Subject: Compile and link ST24 parser in IO firmware --- src/lib/rc/st24.c | 120 ++++++++++++++++++++++++++++++++++- src/lib/rc/st24.h | 46 ++++++++------ src/modules/px4iofirmware/controls.c | 27 +++++++- src/modules/px4iofirmware/module.mk | 3 +- 4 files changed, 173 insertions(+), 23 deletions(-) diff --git a/src/lib/rc/st24.c b/src/lib/rc/st24.c index addbcd899..956a72bf3 100644 --- a/src/lib/rc/st24.c +++ b/src/lib/rc/st24.c @@ -39,6 +39,23 @@ * @author Lorenz Meier */ +#include +#include "st24.h" + +enum ST24_DECODE_STATE { + ST24_DECODE_STATE_UNSYNCED, + ST24_DECODE_STATE_GOT_STX1, + ST24_DECODE_STATE_GOT_STX2, + ST24_DECODE_STATE_GOT_LEN, + ST24_DECODE_STATE_GOT_TYPE, + ST24_DECODE_STATE_GOT_DATA +}; + +static enum ST24_DECODE_STATE _decode_state = ST24_DECODE_STATE_UNSYNCED; +static unsigned _rxlen; + +static ReceiverFcPacket _rxpacket; + uint8_t st24_common_crc8(uint8_t *ptr, uint8_t len) { uint8_t i, crc ; @@ -66,6 +83,105 @@ uint8_t st24_common_crc8(uint8_t *ptr, uint8_t len) } -uint8_t st24_decode(uint8_t byte, uint8_t *rssi, uint8_t* rx_count, uint16_t *channels, uint16_t max_chan_count) { +uint8_t st24_decode(uint8_t byte, uint8_t *rssi, uint8_t* rx_count, uint16_t *channels, uint16_t max_chan_count) +{ + + bool ret = false; + + switch (_decode_state) { + case ST24_DECODE_STATE_UNSYNCED: + if (byte == ST24_STX1) { + _decode_state = ST24_DECODE_STATE_GOT_STX1; + } + break; + + case ST24_DECODE_STATE_GOT_STX1: + if (byte == ST24_STX2) { + _decode_state = ST24_DECODE_STATE_GOT_STX2; + } else { + _decode_state = ST24_DECODE_STATE_UNSYNCED; + } + break; + + case ST24_DECODE_STATE_GOT_STX2: + _rxpacket.length = byte; + _rxlen = 0; + _decode_state = ST24_DECODE_STATE_GOT_LEN; + break; + + case ST24_DECODE_STATE_GOT_LEN: + _rxpacket.type = byte; + _rxlen++; + _decode_state = ST24_DECODE_STATE_GOT_TYPE; + break; + + case ST24_DECODE_STATE_GOT_TYPE: + if (_rxlen < (_rxpacket.length - 1)) { + _rxpacket.st24_data[_rxlen] = byte; + _rxlen++; + } else { + _decode_state = ST24_DECODE_STATE_GOT_DATA; + } + break; + + case ST24_DECODE_STATE_GOT_DATA: + _rxpacket.crc8 = byte; + _rxlen++; + + if (st24_common_crc8((uint8_t*)&(_rxpacket.length), sizeof(_rxpacket.length) + + sizeof(_rxpacket.st24_data) + sizeof(_rxpacket.type)) == _rxpacket.crc8) { -} \ No newline at end of file + ret = true; + + /* decode the actual packet */ + + switch (_rxpacket.type) { + + case ST24_PACKET_TYPE_CHANNELDATA12: + { + ChannelData12* d = (ChannelData12*)&_rxpacket; + + *rssi = d->rssi; + *rx_count = d->packet_count; + + for (unsigned i = 0; i < 1; i += 2) { + channels[i] = ((uint16_t)d->channel[i]) << 8; + channels[i] |= (0xF & d->channel[i+1]); + + channels[i+1] = ((uint16_t)(0xF & d->channel[i+1])) << 4; + channels[i+1] |= d->channel[i+2]; + } + } + break; + + case ST24_PACKET_TYPE_CHANNELDATA24: + { + ChannelData24* d = (ChannelData12*)&_rxpacket; + + *rssi = d->rssi; + *rx_count = d->packet_count; + + for (unsigned i = 0; i < 1; i += 2) { + channels[i] = ((uint16_t)d->channel[i]) << 8; + channels[i] |= (0xF & d->channel[i+1]); + + channels[i+1] = ((uint16_t)(0xF & d->channel[i+1])) << 4; + channels[i+1] |= d->channel[i+2]; + } + } + break; + + default: + ret = false; + break; + } + + } else { + /* decoding failed */ + _decode_state = ST24_DECODE_STATE_UNSYNCED; + } + break; + } + + return ret; +} diff --git a/src/lib/rc/st24.h b/src/lib/rc/st24.h index 3621e8506..24dbf8e51 100644 --- a/src/lib/rc/st24.h +++ b/src/lib/rc/st24.h @@ -39,22 +39,30 @@ * @author Lorenz Meier */ +#pragma once + +#include + +__BEGIN_DECLS + #define ST24_DATA_LEN_MAX 64 +#define ST24_STX1 0x55 +#define ST24_STX2 0x55 -enum { +enum ST24_PACKET_TYPE { ST24_PACKET_TYPE_CHANNELDATA12 = 0, ST24_PACKET_TYPE_CHANNELDATA24, ST24_PACKET_TYPE_TRANSMITTERGPSDATA -} ST24_PACKET_TYPE; +}; #pragma pack(push, 1) typedef struct { - uint8_t header1; ///< 0x55 for a valid packet - uint8_t header2; ///< 0x55 for a valid packet + uint8_t header1; ///< 0x55 for a valid packet + uint8_t header2; ///< 0x55 for a valid packet uint8_t length; ///< length includes type, data, and crc = sizeof(type)+sizeof(data[payload_len])+sizeof(crc8) - uint8_t type; ///< from enum ST24_PACKET_TYPE + uint8_t type; ///< from enum ST24_PACKET_TYPE uint8_t st24_data[ST24_DATA_LEN_MAX]; - uint8_t crc8; ///< crc8 checksum, calculated by st24_common_crc8 and including fields length, type and st24_data + uint8_t crc8; ///< crc8 checksum, calculated by st24_common_crc8 and including fields length, type and st24_data } ReceiverFcPacket; /** @@ -63,8 +71,8 @@ typedef struct { * This is incoming from the ST24 */ typedef struct { - uint16_t t; ///< packet counter or clock - uint8_t rssi ///< signal strength + uint16_t t; ///< packet counter or clock + uint8_t rssi; ///< signal strength uint8_t packet_count; ///< Number of UART packets sent since reception of last RF frame (this tells something about age / rate) uint8_t channel[18]; ///< channel data, 12 channels (12 bit numbers) } ChannelData12; @@ -74,8 +82,8 @@ typedef struct { * */ typedef struct { - uint16_t t; ///< packet counter or clock - uint8_t rssi ///< signal strength + uint16_t t; ///< packet counter or clock + uint8_t rssi; ///< signal strength uint8_t packet_count; ///< Number of UART packets sent since reception of last RF frame (this tells something about age / rate) uint8_t channel[36]; ///< channel data, 24 channels (12 bit numbers) } ChannelData24; @@ -114,17 +122,17 @@ typedef struct { * */ typedef struct { - uint16_t t; ///< packet counter or clock - int32_t lat; ///< lattitude (degrees) +/- 90 deg - int32_t lon; ///< longitude (degrees) +/- 180 deg - int32_t alt; ///< 0.01m resolution, altitude (meters) + uint16_t t; ///< packet counter or clock + int32_t lat; ///< lattitude (degrees) +/- 90 deg + int32_t lon; ///< longitude (degrees) +/- 180 deg + int32_t alt; ///< 0.01m resolution, altitude (meters) int16_t vx, vy, vz; ///< velocity 0.01m res, +/-320.00 North-East- Down - uint8_t nsat; /// #include #include +#include #include "px4io.h" @@ -51,11 +52,21 @@ #define RC_CHANNEL_LOW_THRESH -8000 /* 10% threshold */ static bool ppm_input(uint16_t *values, uint16_t *num_values, uint16_t *frame_len); +static bool dsm_port_input(void); static perf_counter_t c_gather_dsm; static perf_counter_t c_gather_sbus; static perf_counter_t c_gather_ppm; +static int _dsm_fd; + +bool dsm_port_input() +{ + /* get data from FD and attempt to parse with DSM and ST24 libs */ + + return false; +} + void controls_init(void) { @@ -65,7 +76,7 @@ controls_init(void) system_state.rc_channels_timestamp_valid = 0; /* DSM input (USART1) */ - dsm_init("/dev/ttyS0"); + _dsm_fd = dsm_init("/dev/ttyS0"); /* S.bus input (USART3) */ sbus_init("/dev/ttyS2"); @@ -175,6 +186,18 @@ controls_tick() { } perf_end(c_gather_ppm); + uint8_t st24_rssi, rx_count; + uint8_t st24_maxchans = 18; + bool st24_updated = st24_decode(0, &st24_rssi, &rx_count, r_raw_rc_values, st24_maxchans); + if (st24_updated) { + + rssi = st24_rssi; + + r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_PPM; + r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP); + r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE); + } + /* limit number of channels to allowable data size */ if (r_raw_rc_count > PX4IO_RC_INPUT_CHANNELS) r_raw_rc_count = PX4IO_RC_INPUT_CHANNELS; @@ -191,7 +214,7 @@ controls_tick() { /* * If we received a new frame from any of the RC sources, process it. */ - if (dsm_updated || sbus_updated || ppm_updated) { + if (dsm_updated || sbus_updated || ppm_updated || st24_updated) { /* record a bitmask of channels assigned */ unsigned assigned_channels = 0; diff --git a/src/modules/px4iofirmware/module.mk b/src/modules/px4iofirmware/module.mk index 01869569f..eb99e8a96 100644 --- a/src/modules/px4iofirmware/module.mk +++ b/src/modules/px4iofirmware/module.mk @@ -14,7 +14,8 @@ SRCS = adc.c \ ../systemlib/mixer/mixer_group.cpp \ ../systemlib/mixer/mixer_multirotor.cpp \ ../systemlib/mixer/mixer_simple.cpp \ - ../systemlib/pwm_limit/pwm_limit.c + ../systemlib/pwm_limit/pwm_limit.c \ + ../../lib/rc/st24.c ifeq ($(BOARD),px4io-v1) SRCS += i2c.c -- cgit v1.2.3 From 9f94e4ac8409dc15484ee3bcfb89958890a305e6 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 1 Sep 2014 10:33:59 +0200 Subject: add terrain alt field to global pos topic --- src/modules/uORB/topics/vehicle_global_position.h | 1 + 1 file changed, 1 insertion(+) diff --git a/src/modules/uORB/topics/vehicle_global_position.h b/src/modules/uORB/topics/vehicle_global_position.h index 25137c1c6..6cbe25fbd 100644 --- a/src/modules/uORB/topics/vehicle_global_position.h +++ b/src/modules/uORB/topics/vehicle_global_position.h @@ -72,6 +72,7 @@ struct vehicle_global_position_s { float yaw; /**< Yaw in radians -PI..+PI. */ float eph; /**< Standard deviation of position estimate horizontally */ float epv; /**< Standard deviation of position vertically */ + float terrain_alt; /**< Terrain altitude in m, WGS84 */ }; /** -- cgit v1.2.3 From 0f548ab88cb36be362d14f9e9e76c53e3764e3c9 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 1 Sep 2014 10:38:40 +0200 Subject: add global pos valid flag --- src/modules/uORB/topics/vehicle_global_position.h | 1 + 1 file changed, 1 insertion(+) diff --git a/src/modules/uORB/topics/vehicle_global_position.h b/src/modules/uORB/topics/vehicle_global_position.h index 6cbe25fbd..c3bb3b893 100644 --- a/src/modules/uORB/topics/vehicle_global_position.h +++ b/src/modules/uORB/topics/vehicle_global_position.h @@ -73,6 +73,7 @@ struct vehicle_global_position_s { float eph; /**< Standard deviation of position estimate horizontally */ float epv; /**< Standard deviation of position vertically */ float terrain_alt; /**< Terrain altitude in m, WGS84 */ + bool terrain_alt_valid; /**< Terrain altitude estimate is valid */ }; /** -- cgit v1.2.3 From 1f951b0df9ad475349ba4c7e815483445128cc2a Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 1 Sep 2014 10:42:56 +0200 Subject: add logging for gpos terrain alt --- src/modules/sdlog2/sdlog2.c | 7 ++++++- src/modules/sdlog2/sdlog2_messages.h | 3 ++- 2 files changed, 8 insertions(+), 2 deletions(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index dbda8ea6f..e13593077 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1432,6 +1432,11 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.body.log_GPOS.vel_d = buf.global_pos.vel_d; log_msg.body.log_GPOS.eph = buf.global_pos.eph; log_msg.body.log_GPOS.epv = buf.global_pos.epv; + if (buf.global_pos.terrain_alt_valid) { + log_msg.body.log_GPOS.terrain_alt = buf.global_pos.terrain_alt; + } else { + log_msg.body.log_GPOS.terrain_alt = -1.0f; + } LOGBUFFER_WRITE_AND_COUNT(GPOS); } @@ -1464,7 +1469,7 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.body.log_VICN.yaw = buf.vicon_pos.yaw; LOGBUFFER_WRITE_AND_COUNT(VICN); } - + /* --- VISION POSITION --- */ if (copy_if_updated(ORB_ID(vision_position_estimate), subs.vision_pos_sub, &buf.vision_pos)) { log_msg.msg_type = LOG_VISN_MSG; diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 6741c7e25..d2845eb75 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -220,6 +220,7 @@ struct log_GPOS_s { float vel_d; float eph; float epv; + float terrain_alt; }; /* --- GPSP - GLOBAL POSITION SETPOINT --- */ @@ -449,7 +450,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(AIRS, "fff", "IndSpeed,TrueSpeed,AirTemp"), LOG_FORMAT(ARSP, "fff", "RollRateSP,PitchRateSP,YawRateSP"), LOG_FORMAT(FLOW, "hhfffBB", "RawX,RawY,CompX,CompY,Dist,Q,SensID"), - LOG_FORMAT(GPOS, "LLffffff", "Lat,Lon,Alt,VelN,VelE,VelD,EPH,EPV"), + LOG_FORMAT(GPOS, "LLfffffff", "Lat,Lon,Alt,VelN,VelE,VelD,EPH,EPV,TALT"), LOG_FORMAT(GPSP, "BLLffBfbf", "NavState,Lat,Lon,Alt,Yaw,Type,LoitR,LoitDir,PitMin"), LOG_FORMAT(ESC, "HBBBHHHHHHfH", "count,nESC,Conn,N,Ver,Adr,Volt,Amp,RPM,Temp,SetP,SetPRAW"), LOG_FORMAT(GVSP, "fff", "VX,VY,VZ"), -- 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(-) 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 71a2025f7d904c77b7f6351ccc69acd181a42a55 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 1 Sep 2014 11:35:41 +0200 Subject: Add filter estimates --- .../ekf_att_pos_estimator_main.cpp | 35 +++++++++++++++++++++- 1 file changed, 34 insertions(+), 1 deletion(-) diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index 91d17e787..e34cce078 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 @@ -58,6 +58,7 @@ #include #include #include +#include #ifdef SENSOR_COMBINED_SUB #include #endif @@ -171,6 +172,7 @@ private: #else int _sensor_combined_sub; #endif + int _distance_sub; /**< distance measurement */ int _airspeed_sub; /**< airspeed subscription */ int _baro_sub; /**< barometer subscription */ int _gps_sub; /**< GPS subscription */ @@ -196,7 +198,8 @@ private: struct vehicle_global_position_s _global_pos; /**< global vehicle position */ struct vehicle_local_position_s _local_pos; /**< local vehicle position */ struct vehicle_gps_position_s _gps; /**< GPS position */ - struct wind_estimate_s _wind; /**< Wind estimate */ + struct wind_estimate_s _wind; /**< wind estimate */ + struct range_finder_report _distance; /**< distance estimate */ struct gyro_scale _gyro_offsets; struct accel_scale _accel_offsets; @@ -226,6 +229,7 @@ private: hrt_abstime _filter_start_time; hrt_abstime _last_sensor_timestamp; hrt_abstime _last_run; + hrt_abstime _distance_last_valid; bool _gyro_valid; bool _accel_valid; bool _mag_valid; @@ -342,6 +346,7 @@ FixedwingEstimator::FixedwingEstimator() : #else _sensor_combined_sub(-1), #endif + _distance_sub(-1), _airspeed_sub(-1), _baro_sub(-1), _gps_sub(-1), @@ -399,6 +404,7 @@ FixedwingEstimator::FixedwingEstimator() : _filter_start_time(0), _last_sensor_timestamp(0), _last_run(0), + _distance_last_valid(0), _gyro_valid(false), _accel_valid(false), _mag_valid(false), @@ -549,6 +555,7 @@ FixedwingEstimator::parameters_update() _ekf->gyroProcessNoise = _parameters.gyro_pnoise; _ekf->accelProcessNoise = _parameters.acc_pnoise; _ekf->airspeedMeasurementSigma = _parameters.eas_noise; + _ekf->rngFinderPitch = 0.0f; // XXX base on SENS_BOARD_Y_OFF } return OK; @@ -704,6 +711,7 @@ FixedwingEstimator::task_main() /* * do subscriptions */ + _distance_sub = orb_subscribe(ORB_ID(sensor_range_finder)); _baro_sub = orb_subscribe(ORB_ID(sensor_baro0)); _airspeed_sub = orb_subscribe(ORB_ID(airspeed)); _gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); @@ -753,6 +761,7 @@ FixedwingEstimator::task_main() bool newHgtData = false; bool newAdsData = false; bool newDataMag = false; + bool newRangeData = false; float posNED[3] = {0.0f, 0.0f, 0.0f}; // North, East Down position (m) @@ -1114,6 +1123,19 @@ FixedwingEstimator::task_main() newDataMag = false; } + orb_check(_distance_sub, &newRangeData); + + if (newRangeData) { + orb_copy(ORB_ID(sensor_range_finder), _distance_sub, &_distance); + + if (_distance.valid) { + _ekf->rngMea = _distance.distance; + _distance_last_valid = _distance.timestamp; + } else { + newRangeData = false; + } + } + /* * CHECK IF ITS THE RIGHT TIME TO RUN THINGS ALREADY */ @@ -1334,6 +1356,13 @@ FixedwingEstimator::task_main() _ekf->fuseVtasData = false; } + if (newRangeData) { + _ekf->fuseRngData = true; + _ekf->useRangeFinder = true; + _ekf->RecallStates(_ekf->statesAtRngTime, (IMUmsec - 500.0f)); + _ekf->FuseRangeFinder(); + } + // Output results math::Quaternion q(_ekf->states[0], _ekf->states[1], _ekf->states[2], _ekf->states[3]); @@ -1447,6 +1476,10 @@ FixedwingEstimator::task_main() _global_pos.vel_d = _local_pos.vz; } + /* terrain altitude */ + _global_pos.terrain_alt = _ekf->states[22]; + _global_pos.terrain_alt_valid = (_distance_last_valid > 0) && + (hrt_elapsed_time(&_distance_last_valid) < 20 * 1000 * 1000); _global_pos.yaw = _local_pos.yaw; -- cgit v1.2.3 From 8ad1aa789b6f10769dd9977151c9a39971b1ebaf Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 1 Sep 2014 18:13:29 +0200 Subject: mc_pos_control: reset position setpoint on entering to AUTO mode --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 19 ++++++++++++++----- 1 file changed, 14 insertions(+), 5 deletions(-) diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 847a92151..01fdb4c65 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -180,6 +180,7 @@ private: bool _reset_pos_sp; bool _reset_alt_sp; + bool _mode_auto; math::Vector<3> _pos; math::Vector<3> _pos_sp; @@ -292,7 +293,8 @@ MulticopterPositionControl::MulticopterPositionControl() : _ref_timestamp(0), _reset_pos_sp(true), - _reset_alt_sp(true) + _reset_alt_sp(true), + _mode_auto(false) { memset(&_att, 0, sizeof(_att)); memset(&_att_sp, 0, sizeof(_att_sp)); @@ -684,6 +686,13 @@ MulticopterPositionControl::cross_sphere_line(const math::Vector<3>& sphere_c, f void MulticopterPositionControl::control_auto(float dt) { + if (!_mode_auto) { + _mode_auto = true; + /* reset position setpoint on AUTO mode activation */ + reset_pos_sp(); + reset_alt_sp(); + } + bool updated; orb_check(_pos_sp_triplet_sub, &updated); @@ -807,9 +816,7 @@ MulticopterPositionControl::control_auto(float dt) } } else { - /* no waypoint, loiter, reset position setpoint if needed */ - reset_pos_sp(); - reset_alt_sp(); + /* no waypoint, do nothing, setpoint was already reset */ } } @@ -916,10 +923,12 @@ MulticopterPositionControl::task_main() if (_control_mode.flag_control_manual_enabled) { /* manual control */ control_manual(dt); + _mode_auto = false; } else if (_control_mode.flag_control_offboard_enabled) { /* offboard control */ control_offboard(dt); + _mode_auto = false; } else { /* AUTO */ @@ -1271,9 +1280,9 @@ MulticopterPositionControl::task_main() /* position controller disabled, reset setpoints */ _reset_alt_sp = true; _reset_pos_sp = true; + _mode_auto = false; reset_int_z = true; reset_int_xy = true; - } /* reset altitude controller integral (hovering throttle) to manual throttle after manual throttle control */ -- cgit v1.2.3 From 132c9180ead0c64c2edcc36dec2bf8896679a040 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 1 Sep 2014 18:47:56 +0200 Subject: mc_pos_control: move position offset limiting to separate method --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 28 ++++++++++++++++++++++ 1 file changed, 28 insertions(+) diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 01fdb4c65..ce26f7dc2 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -221,6 +221,11 @@ private: */ void reset_alt_sp(); + /** + * Check if position setpoint is too far from current position and adjust it if needed. + */ + void limit_pos_sp_offset(); + /** * Set position setpoint using manual control */ @@ -544,6 +549,29 @@ MulticopterPositionControl::reset_alt_sp() } } +void +MulticopterPositionControl::limit_pos_sp_offset() +{ + math::Vector<3> pos_sp_offs; + pos_sp_offs.zero(); + + if (_control_mode.flag_control_position_enabled) { + pos_sp_offs(0) = (_pos_sp(0) - _pos(0)) / _params.sp_offs_max(0); + pos_sp_offs(1) = (_pos_sp(1) - _pos(1)) / _params.sp_offs_max(1); + } + + if (_control_mode.flag_control_altitude_enabled) { + pos_sp_offs(2) = (_pos_sp(2) - _pos(2)) / _params.sp_offs_max(2); + } + + float pos_sp_offs_norm = pos_sp_offs.length(); + + if (pos_sp_offs_norm > 1.0f) { + pos_sp_offs /= pos_sp_offs_norm; + _pos_sp = _pos + pos_sp_offs.emult(_params.sp_offs_max); + } +} + void MulticopterPositionControl::control_manual(float dt) { -- cgit v1.2.3 From 4c7cc10b6294c799798d05faad688d9dec2102f3 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 2 Sep 2014 15:37:18 +0200 Subject: Working and replay-tested altitude fusion --- .../ekf_att_pos_estimator_main.cpp | 4 +- .../ekf_att_pos_estimator/estimator_23states.cpp | 719 ++++++++++++--------- .../ekf_att_pos_estimator/estimator_23states.h | 40 +- 3 files changed, 461 insertions(+), 302 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index 87a20a775..97abb76a9 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 @@ -1370,7 +1370,7 @@ FixedwingEstimator::task_main() _ekf->fuseRngData = true; _ekf->useRangeFinder = true; _ekf->RecallStates(_ekf->statesAtRngTime, (IMUmsec - 500.0f)); - _ekf->FuseRangeFinder(); + _ekf->GroundEKF(); } @@ -1487,7 +1487,7 @@ FixedwingEstimator::task_main() } /* terrain altitude */ - _global_pos.terrain_alt = _ekf->states[22]; + _global_pos.terrain_alt = _ekf->hgtRef - _ekf->flowStates[1]; _global_pos.terrain_alt_valid = (_distance_last_valid > 0) && (hrt_elapsed_time(&_distance_last_valid) < 20 * 1000 * 1000); diff --git a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp index 249cc419e..94a6d165f 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp @@ -50,7 +50,7 @@ AttPosEKF::AttPosEKF() : statesAtMagMeasTime{}, statesAtVtasMeasTime{}, statesAtRngTime{}, - statesAtOptFlowTime{}, + statesAtFlowTime{}, correctedDelAng(), correctedDelVel(), summedDelAng(), @@ -118,13 +118,13 @@ AttPosEKF::AttPosEKF() : inhibitWindStates(true), inhibitMagStates(true), - inhibitGndHgtState(true), + inhibitGndState(true), onGround(true), staticMode(true), useAirspeed(true), useCompass(true), - useRangeFinder(false), + useRangeFinder(true), useOpticalFlow(false), ekfDiverged(false), @@ -132,7 +132,24 @@ AttPosEKF::AttPosEKF() : current_ekf_state{}, last_ekf_error{}, numericalProtection(true), - storeIndex(0) + storeIndex(0), + storedOmega{}, + Popt{}, + flowStates{}, + prevPosN(0.0f), + prevPosE(0.0f), + auxFlowObsInnov{}, + auxFlowObsInnovVar{}, + fScaleFactorVar(0.0f), + Tnb_flow{}, + R_LOS(0.0f), + auxFlowTestRatio{}, + auxRngTestRatio(0.0f), + flowInnovGate(0.0f), + auxFlowInnovGate(0.0f), + rngInnovGate(0.0f), + minFlowRng(0.0f), + moCompR_LOS(0.0f) { memset(&last_ekf_error, 0, sizeof(last_ekf_error)); memset(¤t_ekf_state, 0, sizeof(current_ekf_state)); @@ -330,7 +347,7 @@ void AttPosEKF::CovariancePrediction(float dt) } else { for (uint8_t i=16; i<=21; i++) processNoise[i] = 0; } - if (!inhibitGndHgtState) { + if (!inhibitGndState) { processNoise[22] = dt * sqrtf(sq(states[4]) + sq(states[5])) * gndHgtSigma; } else { processNoise[22] = 0; @@ -1218,7 +1235,7 @@ void AttPosEKF::FuseVelposNED() } } // Don't update terrain state if inhibited - if (inhibitGndHgtState) { + if (inhibitGndState) { Kfusion[22] = 0; } @@ -1401,7 +1418,7 @@ void AttPosEKF::FuseMagnetometer() Kfusion[i] = 0; } } - if (!inhibitGndHgtState) { + if (!inhibitGndState) { Kfusion[22] = SK_MX[0]*(P[22][19] + P[22][1]*SH_MAG[0] + P[22][3]*SH_MAG[2] + P[22][0]*SK_MX[3] - P[22][2]*SK_MX[2] - P[22][16]*SK_MX[1] + P[22][17]*SK_MX[5] - P[22][18]*SK_MX[4]); } else { Kfusion[22] = 0; @@ -1475,11 +1492,6 @@ void AttPosEKF::FuseMagnetometer() Kfusion[20] = 0; Kfusion[21] = 0; } - if (!inhibitGndHgtState) { - Kfusion[22] = SK_MY[0]*(P[22][20] + P[22][0]*SH_MAG[2] + P[22][1]*SH_MAG[1] + P[22][2]*SH_MAG[0] - P[22][3]*SK_MY[2] - P[22][17]*SK_MY[1] - P[22][16]*SK_MY[3] + P[22][18]*SK_MY[4]); - } else { - Kfusion[22] = 0; - } varInnovMag[1] = 1.0f/SK_MY[0]; innovMag[1] = MagPred[1] - magData.y; } @@ -1550,11 +1562,6 @@ void AttPosEKF::FuseMagnetometer() Kfusion[20] = 0; Kfusion[21] = 0; } - if (!inhibitGndHgtState) { - Kfusion[22] = SK_MZ[0]*(P[22][21] + P[22][0]*SH_MAG[1] + P[22][3]*SH_MAG[0] - P[22][1]*SK_MZ[2] + P[22][2]*SK_MZ[3] + P[22][18]*SK_MZ[1] + P[22][16]*SK_MZ[5] - P[22][17]*SK_MZ[4]); - } else { - Kfusion[22] = 0; - } varInnovMag[2] = 1.0f/SK_MZ[0]; innovMag[2] = MagPred[2] - magData.z; @@ -1746,11 +1753,6 @@ void AttPosEKF::FuseAirspeed() Kfusion[i] = 0; } } - if (!inhibitGndHgtState) { - Kfusion[22] = SK_TAS*(P[22][4]*SH_TAS[2] - P[22][14]*SH_TAS[2] + P[22][5]*SH_TAS[1] - P[22][15]*SH_TAS[1] + P[22][6]*vd*SH_TAS[0]); - } else { - Kfusion[22] = 0; - } varInnovVtas = 1.0f/SK_TAS; // Calculate the measurement innovation @@ -1882,8 +1884,11 @@ void AttPosEKF::FuseRangeFinder() rngPred = (ptd - pd)/cosRngTilt; innovRng = rngPred - rngMea; - // Check the innovation for consistency and don't fuse if > 5Sigma - if ((innovRng*innovRng*SK_RNG[0]) < 25) + // calculate the innovation consistency test ratio + auxRngTestRatio = sq(innovRng) / (sq(rngInnovGate) * varInnovRng); + + // Check the innovation for consistency and don't fuse if out of bounds + if (auxRngTestRatio < 1.0f) { // correct the state vector states[22] = states[22] - Kfusion[22] * innovRng; @@ -1898,286 +1903,386 @@ void AttPosEKF::FuseRangeFinder() void AttPosEKF::FuseOptFlow() { - static uint8_t obsIndex; - static float SH_LOS[13]; - static float SKK_LOS[15]; - static float SK_LOS[2]; - static float q0 = 0.0f; - static float q1 = 0.0f; - static float q2 = 0.0f; - static float q3 = 1.0f; - static float vn = 0.0f; - static float ve = 0.0f; - static float vd = 0.0f; - static float pd = 0.0f; - static float ptd = 0.0f; - static float R_LOS = 0.01f; - static float losPred[2]; - - // Transformation matrix from nav to body axes - Mat3f Tnb_local; - // Transformation matrix from body to sensor axes - // assume camera is aligned with Z body axis plus a misalignment - // defined by 3 small angles about X, Y and Z body axis - Mat3f Tbs; - Tbs.x.y = a3; - Tbs.y.x = -a3; - Tbs.x.z = -a2; - Tbs.z.x = a2; - Tbs.y.z = a1; - Tbs.z.y = -a1; - // Transformation matrix from navigation to sensor axes - Mat3f Tns; - float H_LOS[n_states]; - for (uint8_t i = 0; i < n_states; i++) { - H_LOS[i] = 0.0f; - } - Vector3f velNED_local; - Vector3f relVelSensor; +// static uint8_t obsIndex; +// static float SH_LOS[13]; +// static float SKK_LOS[15]; +// static float SK_LOS[2]; +// static float q0 = 0.0f; +// static float q1 = 0.0f; +// static float q2 = 0.0f; +// static float q3 = 1.0f; +// static float vn = 0.0f; +// static float ve = 0.0f; +// static float vd = 0.0f; +// static float pd = 0.0f; +// static float ptd = 0.0f; +// static float R_LOS = 0.01f; +// static float losPred[2]; + +// // Transformation matrix from nav to body axes +// Mat3f Tnb_local; +// // Transformation matrix from body to sensor axes +// // assume camera is aligned with Z body axis plus a misalignment +// // defined by 3 small angles about X, Y and Z body axis +// Mat3f Tbs; +// Tbs.x.y = a3; +// Tbs.y.x = -a3; +// Tbs.x.z = -a2; +// Tbs.z.x = a2; +// Tbs.y.z = a1; +// Tbs.z.y = -a1; +// // Transformation matrix from navigation to sensor axes +// Mat3f Tns; +// float H_LOS[n_states]; +// for (uint8_t i = 0; i < n_states; i++) { +// H_LOS[i] = 0.0f; +// } +// Vector3f velNED_local; +// Vector3f relVelSensor; + +// // Perform sequential fusion of optical flow measurements only when in the air and tilt is less than 30 deg. +// if (useOpticalFlow && (fuseOptFlowData || obsIndex == 1) && !onGround && Tbs.z.z > 0.866f && rngMea > 5.0f && rngMea < 39.0f) +// { +// // Sequential fusion of XY components to spread processing load across +// // two prediction time steps. + +// // Calculate observation jacobians and Kalman gains +// if (fuseOptFlowData) +// { +// // Copy required states to local variable names +// q0 = statesAtOptFlowTime[0]; +// q1 = statesAtOptFlowTime[1]; +// q2 = statesAtOptFlowTime[2]; +// q3 = statesAtOptFlowTime[3]; +// vn = statesAtOptFlowTime[4]; +// ve = statesAtOptFlowTime[5]; +// vd = statesAtOptFlowTime[6]; +// pd = statesAtOptFlowTime[9]; +// ptd = statesAtOptFlowTime[22]; +// velNED_local.x = vn; +// velNED_local.y = ve; +// velNED_local.z = vd; + +// // calculate rotation from NED to body axes +// float q00 = sq(q0); +// float q11 = sq(q1); +// float q22 = sq(q2); +// float q33 = sq(q3); +// float q01 = q0 * q1; +// float q02 = q0 * q2; +// float q03 = q0 * q3; +// float q12 = q1 * q2; +// float q13 = q1 * q3; +// float q23 = q2 * q3; +// Tnb_local.x.x = q00 + q11 - q22 - q33; +// Tnb_local.y.y = q00 - q11 + q22 - q33; +// Tnb_local.z.z = q00 - q11 - q22 + q33; +// Tnb_local.y.x = 2*(q12 - q03); +// Tnb_local.z.x = 2*(q13 + q02); +// Tnb_local.x.y = 2*(q12 + q03); +// Tnb_local.z.y = 2*(q23 - q01); +// Tnb_local.x.z = 2*(q13 - q02); +// Tnb_local.y.z = 2*(q23 + q01); + +// // calculate transformation from NED to sensor axes +// Tns = Tbs*Tnb_local; + +// // calculate range from ground plain to centre of sensor fov assuming flat earth +// float range = ConstrainFloat(((ptd - pd)/Tns.z.z),0.5f,100.0f); + +// // calculate relative velocity in sensor frame +// relVelSensor = Tns*velNED_local; + +// // divide velocity by range and include angular rate effects to get predicted angular LOS rates relative to X and Y axes +// losPred[0] = relVelSensor.y/range; +// losPred[1] = -relVelSensor.x/range; + +// //printf("relVelSensor.x=%5.1f, relVelSensor.y=%5.1f\n", relVelSensor.x, relVelSensor.y); +// //printf("Xpred=%5.2f, Xmea=%5.2f, Ypred=%5.2f, Ymea=%5.2f, delAng.x=%4.4f, delAng.y=%4.4f\n", losPred[0], losData[0], losPred[1], losData[1], delAng.x, delAng.y); +// //printf("omegaX=%5.2f, omegaY=%5.2f, velY=%5.1f velX=%5.1f\n, range=%5.1f\n", delAngRel.x/dt, delAngRel.y/dt, relVelSensor.y, relVelSensor.x, range); + +// // Calculate observation jacobians +// SH_LOS[0] = a1*(2*q0*q1 + 2*q2*q3) + a2*(2*q0*q2 - 2*q1*q3) - sq(q0) + sq(q1) + sq(q2) - sq(q3); +// SH_LOS[1] = vd*(a2*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q2 - 2*q1*q3 - a3*(2*q0*q1 + 2*q2*q3)) - ve*(a3*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + 2*q0*q3 + 2*q1*q2 + a2*(2*q0*q1 - 2*q2*q3)) + vn*(a2*(2*q0*q2 + 2*q1*q3) + a3*(2*q0*q3 - 2*q1*q2) - sq(q0) - sq(q1) + sq(q2) + sq(q3)); +// SH_LOS[2] = ve*(a1*(2*q0*q1 - 2*q2*q3) + a3*(2*q0*q3 + 2*q1*q2) - sq(q0) + sq(q1) - sq(q2) + sq(q3)) - vd*(a1*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q1 + 2*q2*q3 + a3*(2*q0*q2 - 2*q1*q3)) + vn*(a3*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) + 2*q0*q3 - 2*q1*q2 - a1*(2*q0*q2 + 2*q1*q3)); +// SH_LOS[3] = 1/(pd - ptd); +// SH_LOS[4] = 2*q1 - 2*a2*q3 + 2*a3*q2; +// SH_LOS[5] = 2*a2*q2 - 2*q0 + 2*a3*q3; +// SH_LOS[6] = 2*q2 + 2*a2*q0 - 2*a3*q1; +// SH_LOS[7] = 1/sq(pd - ptd); +// SH_LOS[8] = 2*q2 + 2*a1*q3 - 2*a3*q1; +// SH_LOS[9] = 2*q3 - 2*a1*q2 + 2*a3*q0; +// SH_LOS[10] = 2*a1*q1 - 2*q0 + 2*a3*q3; +// SH_LOS[11] = 2*q3 + 2*a2*q1 + 2*a3*q0; +// SH_LOS[12] = 2*q1 + 2*a1*q0 + 2*a3*q2; + +// for (uint8_t i = 0; i < n_states; i++) H_LOS[i] = 0; +// H_LOS[0] = - SH_LOS[2]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) - SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[10] - vd*SH_LOS[12] + vn*SH_LOS[9]); +// H_LOS[1] = - SH_LOS[2]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[10] + ve*SH_LOS[12] - vn*SH_LOS[8]); +// H_LOS[2] = SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[9] + ve*SH_LOS[8] + vn*SH_LOS[12]) - SH_LOS[2]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3); +// H_LOS[3] = SH_LOS[2]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[8] - ve*SH_LOS[9] + vn*SH_LOS[10]); +// H_LOS[4] = -SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) + 2*q0*q3 - 2*q1*q2 - a1*(2*q0*q2 + 2*q1*q3)); +// H_LOS[5] = -SH_LOS[0]*SH_LOS[3]*(a1*(2*q0*q1 - 2*q2*q3) + a3*(2*q0*q3 + 2*q1*q2) - sq(q0) + sq(q1) - sq(q2) + sq(q3)); +// H_LOS[6] = SH_LOS[0]*SH_LOS[3]*(a1*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q1 + 2*q2*q3 + a3*(2*q0*q2 - 2*q1*q3)); +// H_LOS[9] = SH_LOS[0]*SH_LOS[2]*SH_LOS[7]; +// H_LOS[22] = -SH_LOS[0]*SH_LOS[2]*SH_LOS[7]; + +// // Calculate Kalman gain +// SKK_LOS[0] = a2*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q2 - 2*q1*q3 - a3*(2*q0*q1 + 2*q2*q3); +// SKK_LOS[1] = a3*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + 2*q0*q3 + 2*q1*q2 + a2*(2*q0*q1 - 2*q2*q3); +// SKK_LOS[2] = a2*(2*q0*q2 + 2*q1*q3) + a3*(2*q0*q3 - 2*q1*q2) - sq(q0) - sq(q1) + sq(q2) + sq(q3); +// SKK_LOS[3] = a1*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q1 + 2*q2*q3 + a3*(2*q0*q2 - 2*q1*q3); +// SKK_LOS[4] = a1*(2*q0*q1 - 2*q2*q3) + a3*(2*q0*q3 + 2*q1*q2) - sq(q0) + sq(q1) - sq(q2) + sq(q3); +// SKK_LOS[5] = a3*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) + 2*q0*q3 - 2*q1*q2 - a1*(2*q0*q2 + 2*q1*q3); +// SKK_LOS[6] = SH_LOS[2]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[8] - ve*SH_LOS[9] + vn*SH_LOS[10]); +// SKK_LOS[7] = SH_LOS[2]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[10] + ve*SH_LOS[12] - vn*SH_LOS[8]); +// SKK_LOS[8] = SH_LOS[2]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[10] - vd*SH_LOS[12] + vn*SH_LOS[9]); +// SKK_LOS[9] = SH_LOS[2]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[9] + ve*SH_LOS[8] + vn*SH_LOS[12]); +// SKK_LOS[10] = SH_LOS[1]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[6] - ve*SH_LOS[11] + vn*SH_LOS[5]); +// SKK_LOS[11] = SH_LOS[1]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[11] + ve*SH_LOS[6] + vn*SH_LOS[4]); +// SKK_LOS[12] = SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6]); +// SKK_LOS[13] = SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) - SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11]); +// SKK_LOS[14] = SH_LOS[0]; + +// SK_LOS[0] = 1/(R_LOS + SKK_LOS[8]*(P[0][0]*SKK_LOS[8] + P[1][0]*SKK_LOS[7] + P[2][0]*SKK_LOS[9] - P[3][0]*SKK_LOS[6] - P[9][0]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][0]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][0]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][0]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][0]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) + SKK_LOS[7]*(P[0][1]*SKK_LOS[8] + P[1][1]*SKK_LOS[7] + P[2][1]*SKK_LOS[9] - P[3][1]*SKK_LOS[6] - P[9][1]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][1]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][1]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][1]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][1]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) + SKK_LOS[9]*(P[0][2]*SKK_LOS[8] + P[1][2]*SKK_LOS[7] + P[2][2]*SKK_LOS[9] - P[3][2]*SKK_LOS[6] - P[9][2]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][2]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][2]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][2]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][2]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) - SKK_LOS[6]*(P[0][3]*SKK_LOS[8] + P[1][3]*SKK_LOS[7] + P[2][3]*SKK_LOS[9] - P[3][3]*SKK_LOS[6] - P[9][3]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][3]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][3]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][3]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][3]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) - SH_LOS[2]*SH_LOS[7]*SKK_LOS[14]*(P[0][9]*SKK_LOS[8] + P[1][9]*SKK_LOS[7] + P[2][9]*SKK_LOS[9] - P[3][9]*SKK_LOS[6] - P[9][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][9]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][9]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][9]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) + SH_LOS[2]*SH_LOS[7]*SKK_LOS[14]*(P[0][22]*SKK_LOS[8] + P[1][22]*SKK_LOS[7] + P[2][22]*SKK_LOS[9] - P[3][22]*SKK_LOS[6] - P[9][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][22]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][22]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][22]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) + SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14]*(P[0][4]*SKK_LOS[8] + P[1][4]*SKK_LOS[7] + P[2][4]*SKK_LOS[9] - P[3][4]*SKK_LOS[6] - P[9][4]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][4]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][4]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][4]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) + SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14]*(P[0][5]*SKK_LOS[8] + P[1][5]*SKK_LOS[7] + P[2][5]*SKK_LOS[9] - P[3][5]*SKK_LOS[6] - P[9][5]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][5]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][5]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][5]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) - SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]*(P[0][6]*SKK_LOS[8] + P[1][6]*SKK_LOS[7] + P[2][6]*SKK_LOS[9] - P[3][6]*SKK_LOS[6] - P[9][6]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][6]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][6]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][6]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14])); +// Kfusion[0] = -SK_LOS[0]*(P[0][0]*SKK_LOS[8] + P[0][1]*SKK_LOS[7] - P[0][3]*SKK_LOS[6] + P[0][2]*SKK_LOS[9] - P[0][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[0][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[0][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[0][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[0][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); +// Kfusion[1] = -SK_LOS[0]*(P[1][0]*SKK_LOS[8] + P[1][1]*SKK_LOS[7] - P[1][3]*SKK_LOS[6] + P[1][2]*SKK_LOS[9] - P[1][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[1][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[1][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[1][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[1][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); +// Kfusion[2] = -SK_LOS[0]*(P[2][0]*SKK_LOS[8] + P[2][1]*SKK_LOS[7] - P[2][3]*SKK_LOS[6] + P[2][2]*SKK_LOS[9] - P[2][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[2][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[2][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[2][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[2][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); +// Kfusion[3] = -SK_LOS[0]*(P[3][0]*SKK_LOS[8] + P[3][1]*SKK_LOS[7] - P[3][3]*SKK_LOS[6] + P[3][2]*SKK_LOS[9] - P[3][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[3][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[3][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[3][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[3][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); +// Kfusion[4] = -SK_LOS[0]*(P[4][0]*SKK_LOS[8] + P[4][1]*SKK_LOS[7] - P[4][3]*SKK_LOS[6] + P[4][2]*SKK_LOS[9] - P[4][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[4][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[4][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); +// Kfusion[5] = -SK_LOS[0]*(P[5][0]*SKK_LOS[8] + P[5][1]*SKK_LOS[7] - P[5][3]*SKK_LOS[6] + P[5][2]*SKK_LOS[9] - P[5][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[5][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[5][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[5][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); +// Kfusion[6] = -SK_LOS[0]*(P[6][0]*SKK_LOS[8] + P[6][1]*SKK_LOS[7] - P[6][3]*SKK_LOS[6] + P[6][2]*SKK_LOS[9] - P[6][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[6][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[6][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[6][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); +// Kfusion[7] = -SK_LOS[0]*(P[7][0]*SKK_LOS[8] + P[7][1]*SKK_LOS[7] - P[7][3]*SKK_LOS[6] + P[7][2]*SKK_LOS[9] - P[7][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[7][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[7][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[7][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[7][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); +// Kfusion[8] = -SK_LOS[0]*(P[8][0]*SKK_LOS[8] + P[8][1]*SKK_LOS[7] - P[8][3]*SKK_LOS[6] + P[8][2]*SKK_LOS[9] - P[8][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[8][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[8][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[8][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[8][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); +// Kfusion[9] = -SK_LOS[0]*(P[9][0]*SKK_LOS[8] + P[9][1]*SKK_LOS[7] - P[9][3]*SKK_LOS[6] + P[9][2]*SKK_LOS[9] - P[9][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[9][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[9][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[9][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[9][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); +// Kfusion[10] = -SK_LOS[0]*(P[10][0]*SKK_LOS[8] + P[10][1]*SKK_LOS[7] - P[10][3]*SKK_LOS[6] + P[10][2]*SKK_LOS[9] - P[10][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[10][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[10][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[10][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[10][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); +// Kfusion[11] = -SK_LOS[0]*(P[11][0]*SKK_LOS[8] + P[11][1]*SKK_LOS[7] - P[11][3]*SKK_LOS[6] + P[11][2]*SKK_LOS[9] - P[11][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[11][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[11][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[11][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[11][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); +// Kfusion[12] = -SK_LOS[0]*(P[12][0]*SKK_LOS[8] + P[12][1]*SKK_LOS[7] - P[12][3]*SKK_LOS[6] + P[12][2]*SKK_LOS[9] - P[12][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[12][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[12][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[12][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[12][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); +// Kfusion[13] = 0.0f;//-SK_LOS[0]*(P[13][0]*SKK_LOS[8] + P[13][1]*SKK_LOS[7] - P[13][3]*SKK_LOS[6] + P[13][2]*SKK_LOS[9] - P[13][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[13][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[13][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[13][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[13][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); +// Kfusion[14] = -SK_LOS[0]*(P[14][0]*SKK_LOS[8] + P[14][1]*SKK_LOS[7] - P[14][3]*SKK_LOS[6] + P[14][2]*SKK_LOS[9] - P[14][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[14][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[14][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[14][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[14][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); +// Kfusion[15] = -SK_LOS[0]*(P[15][0]*SKK_LOS[8] + P[15][1]*SKK_LOS[7] - P[15][3]*SKK_LOS[6] + P[15][2]*SKK_LOS[9] - P[15][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[15][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[15][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[15][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[15][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); +// Kfusion[16] = -SK_LOS[0]*(P[16][0]*SKK_LOS[8] + P[16][1]*SKK_LOS[7] - P[16][3]*SKK_LOS[6] + P[16][2]*SKK_LOS[9] - P[16][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[16][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[16][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[16][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[16][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); +// Kfusion[17] = -SK_LOS[0]*(P[17][0]*SKK_LOS[8] + P[17][1]*SKK_LOS[7] - P[17][3]*SKK_LOS[6] + P[17][2]*SKK_LOS[9] - P[17][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[17][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[17][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[17][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[17][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); +// Kfusion[18] = -SK_LOS[0]*(P[18][0]*SKK_LOS[8] + P[18][1]*SKK_LOS[7] - P[18][3]*SKK_LOS[6] + P[18][2]*SKK_LOS[9] - P[18][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[18][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[18][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[18][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[18][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); +// Kfusion[19] = -SK_LOS[0]*(P[19][0]*SKK_LOS[8] + P[19][1]*SKK_LOS[7] - P[19][3]*SKK_LOS[6] + P[19][2]*SKK_LOS[9] - P[19][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[19][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[19][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[19][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[19][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); +// Kfusion[20] = -SK_LOS[0]*(P[20][0]*SKK_LOS[8] + P[20][1]*SKK_LOS[7] - P[20][3]*SKK_LOS[6] + P[20][2]*SKK_LOS[9] - P[20][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[20][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[20][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[20][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[20][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); +// Kfusion[21] = -SK_LOS[0]*(P[21][0]*SKK_LOS[8] + P[21][1]*SKK_LOS[7] - P[21][3]*SKK_LOS[6] + P[21][2]*SKK_LOS[9] - P[21][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[21][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[21][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[21][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[21][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); +// Kfusion[22] = -SK_LOS[0]*(P[22][0]*SKK_LOS[8] + P[22][1]*SKK_LOS[7] - P[22][3]*SKK_LOS[6] + P[22][2]*SKK_LOS[9] - P[22][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[22][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[22][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); +// varInnovOptFlow[0] = 1.0f/SK_LOS[0]; +// innovOptFlow[0] = losPred[0] - losData[0]; + +// // set the observation index to 1 to fuse the y component next time round and reset the commence fusion flag +// obsIndex = 1; +// fuseOptFlowData = false; +// } +// else if (obsIndex == 1) // we are now fusing the Y measurement +// { +// // Calculate observation jacobians +// for (uint8_t i = 0; i < n_states; i++) H_LOS[i] = 0; +// H_LOS[0] = SH_LOS[1]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[6] - ve*SH_LOS[11] + vn*SH_LOS[5]); +// H_LOS[1] = SH_LOS[1]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[11] + ve*SH_LOS[6] + vn*SH_LOS[4]); +// H_LOS[2] = SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6]); +// H_LOS[3] = SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11]) - SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1); +// H_LOS[4] = SH_LOS[0]*SH_LOS[3]*(a2*(2*q0*q2 + 2*q1*q3) + a3*(2*q0*q3 - 2*q1*q2) - sq(q0) - sq(q1) + sq(q2) + sq(q3)); +// H_LOS[5] = -SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + 2*q0*q3 + 2*q1*q2 + a2*(2*q0*q1 - 2*q2*q3)); +// H_LOS[6] = SH_LOS[0]*SH_LOS[3]*(a2*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q2 - 2*q1*q3 - a3*(2*q0*q1 + 2*q2*q3)); +// H_LOS[9] = -SH_LOS[0]*SH_LOS[1]*SH_LOS[7]; +// H_LOS[22] = SH_LOS[0]*SH_LOS[1]*SH_LOS[7]; + +// // Calculate Kalman gains +// SK_LOS[1] = 1/(R_LOS + SKK_LOS[12]*(P[0][2]*SKK_LOS[10] + P[1][2]*SKK_LOS[11] + P[2][2]*SKK_LOS[12] - P[3][2]*SKK_LOS[13] - P[9][2]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][2]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][2]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][2]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][2]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) - SKK_LOS[13]*(P[0][3]*SKK_LOS[10] + P[1][3]*SKK_LOS[11] + P[2][3]*SKK_LOS[12] - P[3][3]*SKK_LOS[13] - P[9][3]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][3]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][3]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][3]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][3]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) + SKK_LOS[10]*(P[0][0]*SKK_LOS[10] + P[1][0]*SKK_LOS[11] + P[2][0]*(SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[3]*SKK_LOS[14]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6])) - P[3][0]*(SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) - SH_LOS[3]*SKK_LOS[14]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11])) - P[9][0]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][0]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][0]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][0]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][0]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) + SKK_LOS[11]*(P[0][1]*SKK_LOS[10] + P[1][1]*SKK_LOS[11] + P[2][1]*(SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[3]*SKK_LOS[14]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6])) - P[3][1]*(SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) - SH_LOS[3]*SKK_LOS[14]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11])) - P[9][1]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][1]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][1]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][1]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][1]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) - SH_LOS[1]*SH_LOS[7]*SKK_LOS[14]*(P[0][9]*SKK_LOS[10] + P[1][9]*SKK_LOS[11] + P[2][9]*SKK_LOS[12] - P[3][9]*SKK_LOS[13] - P[9][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][9]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][9]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][9]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) + SH_LOS[1]*SH_LOS[7]*SKK_LOS[14]*(P[0][22]*SKK_LOS[10] + P[1][22]*SKK_LOS[11] + P[2][22]*SKK_LOS[12] - P[3][22]*SKK_LOS[13] - P[9][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][22]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][22]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][22]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) + SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14]*(P[0][4]*SKK_LOS[10] + P[1][4]*SKK_LOS[11] + P[2][4]*SKK_LOS[12] - P[3][4]*SKK_LOS[13] - P[9][4]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][4]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][4]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][4]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) - SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]*(P[0][5]*SKK_LOS[10] + P[1][5]*SKK_LOS[11] + P[2][5]*SKK_LOS[12] - P[3][5]*SKK_LOS[13] - P[9][5]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][5]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][5]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][5]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) + SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]*(P[0][6]*SKK_LOS[10] + P[1][6]*SKK_LOS[11] + P[2][6]*SKK_LOS[12] - P[3][6]*SKK_LOS[13] - P[9][6]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][6]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][6]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][6]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14])); +// Kfusion[0] = SK_LOS[1]*(P[0][0]*SKK_LOS[10] - P[0][3]*SKK_LOS[13] + P[0][1]*SKK_LOS[11] + P[0][2]*SKK_LOS[12] - P[0][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[0][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[0][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[0][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[0][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); +// Kfusion[1] = SK_LOS[1]*(P[1][0]*SKK_LOS[10] - P[1][3]*SKK_LOS[13] + P[1][1]*SKK_LOS[11] + P[1][2]*SKK_LOS[12] - P[1][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[1][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[1][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[1][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[1][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); +// Kfusion[2] = SK_LOS[1]*(P[2][0]*SKK_LOS[10] - P[2][3]*SKK_LOS[13] + P[2][1]*SKK_LOS[11] + P[2][2]*SKK_LOS[12] - P[2][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[2][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[2][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[2][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[2][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); +// Kfusion[3] = SK_LOS[1]*(P[3][0]*SKK_LOS[10] - P[3][3]*SKK_LOS[13] + P[3][1]*SKK_LOS[11] + P[3][2]*SKK_LOS[12] - P[3][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[3][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[3][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[3][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[3][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); +// Kfusion[4] = SK_LOS[1]*(P[4][0]*SKK_LOS[10] - P[4][3]*SKK_LOS[13] + P[4][1]*SKK_LOS[11] + P[4][2]*SKK_LOS[12] - P[4][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[4][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[4][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); +// Kfusion[5] = SK_LOS[1]*(P[5][0]*SKK_LOS[10] - P[5][3]*SKK_LOS[13] + P[5][1]*SKK_LOS[11] + P[5][2]*SKK_LOS[12] - P[5][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[5][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[5][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[5][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); +// Kfusion[6] = SK_LOS[1]*(P[6][0]*SKK_LOS[10] - P[6][3]*SKK_LOS[13] + P[6][1]*SKK_LOS[11] + P[6][2]*SKK_LOS[12] - P[6][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[6][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[6][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[6][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[6][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); +// Kfusion[7] = SK_LOS[1]*(P[7][0]*SKK_LOS[10] - P[7][3]*SKK_LOS[13] + P[7][1]*SKK_LOS[11] + P[7][2]*SKK_LOS[12] - P[7][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[7][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[7][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[7][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[7][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); +// Kfusion[8] = SK_LOS[1]*(P[8][0]*SKK_LOS[10] - P[8][3]*SKK_LOS[13] + P[8][1]*SKK_LOS[11] + P[8][2]*SKK_LOS[12] - P[8][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[8][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[8][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[8][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[8][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); +// Kfusion[9] = SK_LOS[1]*(P[9][0]*SKK_LOS[10] - P[9][3]*SKK_LOS[13] + P[9][1]*SKK_LOS[11] + P[9][2]*SKK_LOS[12] - P[9][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[9][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[9][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[9][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[9][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); +// Kfusion[10] = SK_LOS[1]*(P[10][0]*SKK_LOS[10] - P[10][3]*SKK_LOS[13] + P[10][1]*SKK_LOS[11] + P[10][2]*SKK_LOS[12] - P[10][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[10][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[10][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[10][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[10][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); +// Kfusion[11] = SK_LOS[1]*(P[11][0]*SKK_LOS[10] - P[11][3]*SKK_LOS[13] + P[11][1]*SKK_LOS[11] + P[11][2]*SKK_LOS[12] - P[11][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[11][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[11][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[11][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[11][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); +// Kfusion[12] = SK_LOS[1]*(P[12][0]*SKK_LOS[10] - P[12][3]*SKK_LOS[13] + P[12][1]*SKK_LOS[11] + P[12][2]*SKK_LOS[12] - P[12][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[12][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[12][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[12][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[12][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); +// Kfusion[13] = 0.0f;//SK_LOS[1]*(P[13][0]*SKK_LOS[10] - P[13][3]*SKK_LOS[13] + P[13][1]*SKK_LOS[11] + P[13][2]*SKK_LOS[12] - P[13][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[13][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[13][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[13][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[13][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); +// Kfusion[14] = SK_LOS[1]*(P[14][0]*SKK_LOS[10] - P[14][3]*SKK_LOS[13] + P[14][1]*SKK_LOS[11] + P[14][2]*SKK_LOS[12] - P[14][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[14][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[14][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[14][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[14][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); +// Kfusion[15] = SK_LOS[1]*(P[15][0]*SKK_LOS[10] - P[15][3]*SKK_LOS[13] + P[15][1]*SKK_LOS[11] + P[15][2]*SKK_LOS[12] - P[15][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[15][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[15][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[15][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[15][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); +// Kfusion[16] = SK_LOS[1]*(P[16][0]*SKK_LOS[10] - P[16][3]*SKK_LOS[13] + P[16][1]*SKK_LOS[11] + P[16][2]*SKK_LOS[12] - P[16][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[16][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[16][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[16][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[16][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); +// Kfusion[17] = SK_LOS[1]*(P[17][0]*SKK_LOS[10] - P[17][3]*SKK_LOS[13] + P[17][1]*SKK_LOS[11] + P[17][2]*SKK_LOS[12] - P[17][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[17][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[17][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[17][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[17][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); +// Kfusion[18] = SK_LOS[1]*(P[18][0]*SKK_LOS[10] - P[18][3]*SKK_LOS[13] + P[18][1]*SKK_LOS[11] + P[18][2]*SKK_LOS[12] - P[18][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[18][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[18][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[18][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[18][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); +// Kfusion[19] = SK_LOS[1]*(P[19][0]*SKK_LOS[10] - P[19][3]*SKK_LOS[13] + P[19][1]*SKK_LOS[11] + P[19][2]*SKK_LOS[12] - P[19][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[19][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[19][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[19][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[19][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); +// Kfusion[20] = SK_LOS[1]*(P[20][0]*SKK_LOS[10] - P[20][3]*SKK_LOS[13] + P[20][1]*SKK_LOS[11] + P[20][2]*SKK_LOS[12] - P[20][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[20][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[20][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[20][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[20][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); +// Kfusion[21] = SK_LOS[1]*(P[21][0]*SKK_LOS[10] - P[21][3]*SKK_LOS[13] + P[21][1]*SKK_LOS[11] + P[21][2]*SKK_LOS[12] - P[21][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[21][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[21][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[21][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[21][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); +// Kfusion[22] = SK_LOS[1]*(P[22][0]*SKK_LOS[10] - P[22][3]*SKK_LOS[13] + P[22][1]*SKK_LOS[11] + P[22][2]*SKK_LOS[12] - P[22][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[22][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[22][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); +// varInnovOptFlow[1] = 1.0f/SK_LOS[1]; +// innovOptFlow[1] = losPred[1] - losData[1]; + +// // reset the observation index +// obsIndex = 0; +// fuseOptFlowData = false; +// } + +// // Check the innovation for consistency and don't fuse if > 3Sigma +// if ((innovOptFlow[obsIndex]*innovOptFlow[obsIndex]/varInnovOptFlow[obsIndex]) < 9.0f) +// { +// // correct the state vector +// for (uint8_t j = 0; j < n_states; j++) +// { +// states[j] = states[j] - Kfusion[j] * innovOptFlow[obsIndex]; +// } +// // normalise the quaternion states +// float quatMag = sqrt(states[0]*states[0] + states[1]*states[1] + states[2]*states[2] + states[3]*states[3]); +// if (quatMag > 1e-12f) +// { +// for (uint8_t j= 0; j<=3; j++) +// { +// float quatMagInv = 1.0f/quatMag; +// states[j] = states[j] * quatMagInv; +// } +// } +// // correct the covariance P = (I - K*H)*P +// // take advantage of the empty columns in KH to reduce the +// // number of operations +// for (uint8_t i = 0; i < n_states; i++) +// { +// for (uint8_t j = 0; j <= 6; j++) +// { +// KH[i][j] = Kfusion[i] * H_LOS[j]; +// } +// for (uint8_t j = 7; j <= 8; j++) +// { +// KH[i][j] = 0.0f; +// } +// KH[i][9] = Kfusion[i] * H_LOS[9]; +// for (uint8_t j = 10; j <= 21; j++) +// { +// KH[i][j] = 0.0f; +// } +// KH[i][22] = Kfusion[i] * H_LOS[22]; +// } +// for (uint8_t i = 0; i < n_states; i++) +// { +// for (uint8_t j = 0; j < n_states; j++) +// { +// KHP[i][j] = 0.0f; +// for (uint8_t k = 0; k <= 6; k++) +// { +// KHP[i][j] = KHP[i][j] + KH[i][k] * P[k][j]; +// } +// KHP[i][j] = KHP[i][j] + KH[i][9] * P[9][j]; +// KHP[i][j] = KHP[i][j] + KH[i][22] * P[2][j]; +// } +// } +// } +// for (uint8_t i = 0; i < n_states; i++) +// { +// for (uint8_t j = 0; j < n_states; j++) +// { +// P[i][j] = P[i][j] - KHP[i][j]; +// } +// } +// ForceSymmetry(); +// ConstrainVariances(); +// } +} -// Perform sequential fusion of optical flow measurements only when in the air and tilt is less than 30 deg. - if (useOpticalFlow && (fuseOptFlowData || obsIndex == 1) && !onGround && Tbs.z.z > 0.866f && rngMea > 5.0f && rngMea < 39.0f) - { - // Sequential fusion of XY components to spread processing load across - // two prediction time steps. +/* +Estimation of optical flow sensor focal length scale factor and terrain height using a two state EKF +This fiter requires optical flow rates that are not motion compensated +Range to ground measurement is assumed to be via a narrow beam type sensor - eg laser +*/ +void AttPosEKF::GroundEKF() +{ + // propagate ground position state noise each time this is called using the difference in position since the last observations and an RMS gradient assumption + // limit distance to prevent intialisation afer bad gps causing bad numerical conditioning + if (!inhibitGndState) { + float distanceTravelledSq; + distanceTravelledSq = sq(statesAtRngTime[7] - prevPosN) + sq(statesAtRngTime[8] - prevPosE); + prevPosN = statesAtRngTime[7]; + prevPosE = statesAtRngTime[8]; + distanceTravelledSq = min(distanceTravelledSq, 100.0f); + Popt[1][1] += (distanceTravelledSq * sq(gndHgtSigma)); + } + // we aren't using optical flow measurements in this hacked implementation so set the covariances for this state to zero to avoid numerical problems + Popt[0][0] = 0.0f; + Popt[0][1] = 0.0f; + Popt[1][0] = 0.0f; + + // Fuse range finder data + // Need to check that our range finder tilt angle is less than 30 degrees + float cosRngTilt = - Tbn.z.x * sinf(rngFinderPitch) + Tbn.z.z * cosf(rngFinderPitch); + if (useRangeFinder && fuseRngData && cosRngTilt > 0.87f) { + float range; // range from camera to centre of image + float q0; // quaternion at optical flow measurement time + float q1; // quaternion at optical flow measurement time + float q2; // quaternion at optical flow measurement time + float q3; // quaternion at optical flow measurement time + float R_RNG = 0.5; // range measurement variance (m^2) TODO make this a function of range and tilt to allow for sensor, alignment and AHRS errors + + // Copy required states to local variable names + q0 = statesAtRngTime[0]; + q1 = statesAtRngTime[1]; + q2 = statesAtRngTime[2]; + q3 = statesAtRngTime[3]; + + // calculate Kalman gains + float SK_RNG[3]; + SK_RNG[0] = sq(q0) - sq(q1) - sq(q2) + sq(q3); + SK_RNG[1] = 1/(R_RNG + Popt[1][1]/sq(SK_RNG[0])); + SK_RNG[2] = 1/SK_RNG[0]; + float K_RNG[2]; + if (!inhibitScaleState) { + K_RNG[0] = Popt[0][1]*SK_RNG[1]*SK_RNG[2]; + } else { + K_RNG[0] = 0.0f; + } + if (!inhibitGndState) { + K_RNG[1] = Popt[1][1]*SK_RNG[1]*SK_RNG[2]; + } else { + K_RNG[1] = 0.0f; + } - // Calculate observation jacobians and Kalman gains - if (fuseOptFlowData) - { - // Copy required states to local variable names - q0 = statesAtOptFlowTime[0]; - q1 = statesAtOptFlowTime[1]; - q2 = statesAtOptFlowTime[2]; - q3 = statesAtOptFlowTime[3]; - vn = statesAtOptFlowTime[4]; - ve = statesAtOptFlowTime[5]; - vd = statesAtOptFlowTime[6]; - pd = statesAtOptFlowTime[9]; - ptd = statesAtOptFlowTime[22]; - velNED_local.x = vn; - velNED_local.y = ve; - velNED_local.z = vd; - - // calculate rotation from NED to body axes - float q00 = sq(q0); - float q11 = sq(q1); - float q22 = sq(q2); - float q33 = sq(q3); - float q01 = q0 * q1; - float q02 = q0 * q2; - float q03 = q0 * q3; - float q12 = q1 * q2; - float q13 = q1 * q3; - float q23 = q2 * q3; - Tnb_local.x.x = q00 + q11 - q22 - q33; - Tnb_local.y.y = q00 - q11 + q22 - q33; - Tnb_local.z.z = q00 - q11 - q22 + q33; - Tnb_local.y.x = 2*(q12 - q03); - Tnb_local.z.x = 2*(q13 + q02); - Tnb_local.x.y = 2*(q12 + q03); - Tnb_local.z.y = 2*(q23 - q01); - Tnb_local.x.z = 2*(q13 - q02); - Tnb_local.y.z = 2*(q23 + q01); - - // calculate transformation from NED to sensor axes - Tns = Tbs*Tnb_local; - - // calculate range from ground plain to centre of sensor fov assuming flat earth - float range = ConstrainFloat(((ptd - pd)/Tns.z.z),0.5f,100.0f); - - // calculate relative velocity in sensor frame - relVelSensor = Tns*velNED_local; - - // divide velocity by range and include angular rate effects to get predicted angular LOS rates relative to X and Y axes - losPred[0] = relVelSensor.y/range; - losPred[1] = -relVelSensor.x/range; - - //printf("relVelSensor.x=%5.1f, relVelSensor.y=%5.1f\n", relVelSensor.x, relVelSensor.y); - //printf("Xpred=%5.2f, Xmea=%5.2f, Ypred=%5.2f, Ymea=%5.2f, delAng.x=%4.4f, delAng.y=%4.4f\n", losPred[0], losData[0], losPred[1], losData[1], delAng.x, delAng.y); - //printf("omegaX=%5.2f, omegaY=%5.2f, velY=%5.1f velX=%5.1f\n, range=%5.1f\n", delAngRel.x/dt, delAngRel.y/dt, relVelSensor.y, relVelSensor.x, range); + // Calculate the innovation variance for data logging + varInnovRng = 1.0f/SK_RNG[1]; - // Calculate observation jacobians - SH_LOS[0] = a1*(2*q0*q1 + 2*q2*q3) + a2*(2*q0*q2 - 2*q1*q3) - sq(q0) + sq(q1) + sq(q2) - sq(q3); - SH_LOS[1] = vd*(a2*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q2 - 2*q1*q3 - a3*(2*q0*q1 + 2*q2*q3)) - ve*(a3*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + 2*q0*q3 + 2*q1*q2 + a2*(2*q0*q1 - 2*q2*q3)) + vn*(a2*(2*q0*q2 + 2*q1*q3) + a3*(2*q0*q3 - 2*q1*q2) - sq(q0) - sq(q1) + sq(q2) + sq(q3)); - SH_LOS[2] = ve*(a1*(2*q0*q1 - 2*q2*q3) + a3*(2*q0*q3 + 2*q1*q2) - sq(q0) + sq(q1) - sq(q2) + sq(q3)) - vd*(a1*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q1 + 2*q2*q3 + a3*(2*q0*q2 - 2*q1*q3)) + vn*(a3*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) + 2*q0*q3 - 2*q1*q2 - a1*(2*q0*q2 + 2*q1*q3)); - SH_LOS[3] = 1/(pd - ptd); - SH_LOS[4] = 2*q1 - 2*a2*q3 + 2*a3*q2; - SH_LOS[5] = 2*a2*q2 - 2*q0 + 2*a3*q3; - SH_LOS[6] = 2*q2 + 2*a2*q0 - 2*a3*q1; - SH_LOS[7] = 1/sq(pd - ptd); - SH_LOS[8] = 2*q2 + 2*a1*q3 - 2*a3*q1; - SH_LOS[9] = 2*q3 - 2*a1*q2 + 2*a3*q0; - SH_LOS[10] = 2*a1*q1 - 2*q0 + 2*a3*q3; - SH_LOS[11] = 2*q3 + 2*a2*q1 + 2*a3*q0; - SH_LOS[12] = 2*q1 + 2*a1*q0 + 2*a3*q2; - - for (uint8_t i = 0; i < n_states; i++) H_LOS[i] = 0; - H_LOS[0] = - SH_LOS[2]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) - SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[10] - vd*SH_LOS[12] + vn*SH_LOS[9]); - H_LOS[1] = - SH_LOS[2]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[10] + ve*SH_LOS[12] - vn*SH_LOS[8]); - H_LOS[2] = SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[9] + ve*SH_LOS[8] + vn*SH_LOS[12]) - SH_LOS[2]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3); - H_LOS[3] = SH_LOS[2]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[8] - ve*SH_LOS[9] + vn*SH_LOS[10]); - H_LOS[4] = -SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) + 2*q0*q3 - 2*q1*q2 - a1*(2*q0*q2 + 2*q1*q3)); - H_LOS[5] = -SH_LOS[0]*SH_LOS[3]*(a1*(2*q0*q1 - 2*q2*q3) + a3*(2*q0*q3 + 2*q1*q2) - sq(q0) + sq(q1) - sq(q2) + sq(q3)); - H_LOS[6] = SH_LOS[0]*SH_LOS[3]*(a1*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q1 + 2*q2*q3 + a3*(2*q0*q2 - 2*q1*q3)); - H_LOS[9] = SH_LOS[0]*SH_LOS[2]*SH_LOS[7]; - H_LOS[22] = -SH_LOS[0]*SH_LOS[2]*SH_LOS[7]; + // constrain terrain height to be below the vehicle + flowStates[1] = maxf(flowStates[1], statesAtRngTime[9] + minFlowRng); - // Calculate Kalman gain - SKK_LOS[0] = a2*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q2 - 2*q1*q3 - a3*(2*q0*q1 + 2*q2*q3); - SKK_LOS[1] = a3*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + 2*q0*q3 + 2*q1*q2 + a2*(2*q0*q1 - 2*q2*q3); - SKK_LOS[2] = a2*(2*q0*q2 + 2*q1*q3) + a3*(2*q0*q3 - 2*q1*q2) - sq(q0) - sq(q1) + sq(q2) + sq(q3); - SKK_LOS[3] = a1*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q1 + 2*q2*q3 + a3*(2*q0*q2 - 2*q1*q3); - SKK_LOS[4] = a1*(2*q0*q1 - 2*q2*q3) + a3*(2*q0*q3 + 2*q1*q2) - sq(q0) + sq(q1) - sq(q2) + sq(q3); - SKK_LOS[5] = a3*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) + 2*q0*q3 - 2*q1*q2 - a1*(2*q0*q2 + 2*q1*q3); - SKK_LOS[6] = SH_LOS[2]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[8] - ve*SH_LOS[9] + vn*SH_LOS[10]); - SKK_LOS[7] = SH_LOS[2]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[10] + ve*SH_LOS[12] - vn*SH_LOS[8]); - SKK_LOS[8] = SH_LOS[2]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[10] - vd*SH_LOS[12] + vn*SH_LOS[9]); - SKK_LOS[9] = SH_LOS[2]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[9] + ve*SH_LOS[8] + vn*SH_LOS[12]); - SKK_LOS[10] = SH_LOS[1]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[6] - ve*SH_LOS[11] + vn*SH_LOS[5]); - SKK_LOS[11] = SH_LOS[1]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[11] + ve*SH_LOS[6] + vn*SH_LOS[4]); - SKK_LOS[12] = SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6]); - SKK_LOS[13] = SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) - SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11]); - SKK_LOS[14] = SH_LOS[0]; - - SK_LOS[0] = 1/(R_LOS + SKK_LOS[8]*(P[0][0]*SKK_LOS[8] + P[1][0]*SKK_LOS[7] + P[2][0]*SKK_LOS[9] - P[3][0]*SKK_LOS[6] - P[9][0]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][0]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][0]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][0]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][0]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) + SKK_LOS[7]*(P[0][1]*SKK_LOS[8] + P[1][1]*SKK_LOS[7] + P[2][1]*SKK_LOS[9] - P[3][1]*SKK_LOS[6] - P[9][1]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][1]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][1]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][1]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][1]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) + SKK_LOS[9]*(P[0][2]*SKK_LOS[8] + P[1][2]*SKK_LOS[7] + P[2][2]*SKK_LOS[9] - P[3][2]*SKK_LOS[6] - P[9][2]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][2]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][2]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][2]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][2]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) - SKK_LOS[6]*(P[0][3]*SKK_LOS[8] + P[1][3]*SKK_LOS[7] + P[2][3]*SKK_LOS[9] - P[3][3]*SKK_LOS[6] - P[9][3]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][3]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][3]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][3]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][3]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) - SH_LOS[2]*SH_LOS[7]*SKK_LOS[14]*(P[0][9]*SKK_LOS[8] + P[1][9]*SKK_LOS[7] + P[2][9]*SKK_LOS[9] - P[3][9]*SKK_LOS[6] - P[9][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][9]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][9]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][9]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) + SH_LOS[2]*SH_LOS[7]*SKK_LOS[14]*(P[0][22]*SKK_LOS[8] + P[1][22]*SKK_LOS[7] + P[2][22]*SKK_LOS[9] - P[3][22]*SKK_LOS[6] - P[9][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][22]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][22]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][22]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) + SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14]*(P[0][4]*SKK_LOS[8] + P[1][4]*SKK_LOS[7] + P[2][4]*SKK_LOS[9] - P[3][4]*SKK_LOS[6] - P[9][4]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][4]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][4]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][4]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) + SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14]*(P[0][5]*SKK_LOS[8] + P[1][5]*SKK_LOS[7] + P[2][5]*SKK_LOS[9] - P[3][5]*SKK_LOS[6] - P[9][5]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][5]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][5]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][5]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) - SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]*(P[0][6]*SKK_LOS[8] + P[1][6]*SKK_LOS[7] + P[2][6]*SKK_LOS[9] - P[3][6]*SKK_LOS[6] - P[9][6]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][6]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][6]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][6]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14])); - Kfusion[0] = -SK_LOS[0]*(P[0][0]*SKK_LOS[8] + P[0][1]*SKK_LOS[7] - P[0][3]*SKK_LOS[6] + P[0][2]*SKK_LOS[9] - P[0][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[0][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[0][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[0][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[0][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); - Kfusion[1] = -SK_LOS[0]*(P[1][0]*SKK_LOS[8] + P[1][1]*SKK_LOS[7] - P[1][3]*SKK_LOS[6] + P[1][2]*SKK_LOS[9] - P[1][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[1][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[1][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[1][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[1][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); - Kfusion[2] = -SK_LOS[0]*(P[2][0]*SKK_LOS[8] + P[2][1]*SKK_LOS[7] - P[2][3]*SKK_LOS[6] + P[2][2]*SKK_LOS[9] - P[2][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[2][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[2][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[2][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[2][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); - Kfusion[3] = -SK_LOS[0]*(P[3][0]*SKK_LOS[8] + P[3][1]*SKK_LOS[7] - P[3][3]*SKK_LOS[6] + P[3][2]*SKK_LOS[9] - P[3][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[3][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[3][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[3][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[3][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); - Kfusion[4] = -SK_LOS[0]*(P[4][0]*SKK_LOS[8] + P[4][1]*SKK_LOS[7] - P[4][3]*SKK_LOS[6] + P[4][2]*SKK_LOS[9] - P[4][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[4][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[4][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); - Kfusion[5] = -SK_LOS[0]*(P[5][0]*SKK_LOS[8] + P[5][1]*SKK_LOS[7] - P[5][3]*SKK_LOS[6] + P[5][2]*SKK_LOS[9] - P[5][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[5][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[5][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[5][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); - Kfusion[6] = -SK_LOS[0]*(P[6][0]*SKK_LOS[8] + P[6][1]*SKK_LOS[7] - P[6][3]*SKK_LOS[6] + P[6][2]*SKK_LOS[9] - P[6][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[6][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[6][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[6][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); - Kfusion[7] = -SK_LOS[0]*(P[7][0]*SKK_LOS[8] + P[7][1]*SKK_LOS[7] - P[7][3]*SKK_LOS[6] + P[7][2]*SKK_LOS[9] - P[7][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[7][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[7][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[7][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[7][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); - Kfusion[8] = -SK_LOS[0]*(P[8][0]*SKK_LOS[8] + P[8][1]*SKK_LOS[7] - P[8][3]*SKK_LOS[6] + P[8][2]*SKK_LOS[9] - P[8][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[8][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[8][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[8][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[8][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); - Kfusion[9] = -SK_LOS[0]*(P[9][0]*SKK_LOS[8] + P[9][1]*SKK_LOS[7] - P[9][3]*SKK_LOS[6] + P[9][2]*SKK_LOS[9] - P[9][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[9][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[9][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[9][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[9][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); - Kfusion[10] = -SK_LOS[0]*(P[10][0]*SKK_LOS[8] + P[10][1]*SKK_LOS[7] - P[10][3]*SKK_LOS[6] + P[10][2]*SKK_LOS[9] - P[10][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[10][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[10][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[10][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[10][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); - Kfusion[11] = -SK_LOS[0]*(P[11][0]*SKK_LOS[8] + P[11][1]*SKK_LOS[7] - P[11][3]*SKK_LOS[6] + P[11][2]*SKK_LOS[9] - P[11][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[11][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[11][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[11][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[11][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); - Kfusion[12] = -SK_LOS[0]*(P[12][0]*SKK_LOS[8] + P[12][1]*SKK_LOS[7] - P[12][3]*SKK_LOS[6] + P[12][2]*SKK_LOS[9] - P[12][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[12][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[12][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[12][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[12][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); - Kfusion[13] = 0.0f;//-SK_LOS[0]*(P[13][0]*SKK_LOS[8] + P[13][1]*SKK_LOS[7] - P[13][3]*SKK_LOS[6] + P[13][2]*SKK_LOS[9] - P[13][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[13][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[13][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[13][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[13][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); - Kfusion[14] = -SK_LOS[0]*(P[14][0]*SKK_LOS[8] + P[14][1]*SKK_LOS[7] - P[14][3]*SKK_LOS[6] + P[14][2]*SKK_LOS[9] - P[14][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[14][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[14][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[14][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[14][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); - Kfusion[15] = -SK_LOS[0]*(P[15][0]*SKK_LOS[8] + P[15][1]*SKK_LOS[7] - P[15][3]*SKK_LOS[6] + P[15][2]*SKK_LOS[9] - P[15][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[15][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[15][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[15][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[15][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); - Kfusion[16] = -SK_LOS[0]*(P[16][0]*SKK_LOS[8] + P[16][1]*SKK_LOS[7] - P[16][3]*SKK_LOS[6] + P[16][2]*SKK_LOS[9] - P[16][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[16][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[16][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[16][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[16][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); - Kfusion[17] = -SK_LOS[0]*(P[17][0]*SKK_LOS[8] + P[17][1]*SKK_LOS[7] - P[17][3]*SKK_LOS[6] + P[17][2]*SKK_LOS[9] - P[17][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[17][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[17][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[17][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[17][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); - Kfusion[18] = -SK_LOS[0]*(P[18][0]*SKK_LOS[8] + P[18][1]*SKK_LOS[7] - P[18][3]*SKK_LOS[6] + P[18][2]*SKK_LOS[9] - P[18][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[18][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[18][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[18][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[18][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); - Kfusion[19] = -SK_LOS[0]*(P[19][0]*SKK_LOS[8] + P[19][1]*SKK_LOS[7] - P[19][3]*SKK_LOS[6] + P[19][2]*SKK_LOS[9] - P[19][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[19][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[19][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[19][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[19][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); - Kfusion[20] = -SK_LOS[0]*(P[20][0]*SKK_LOS[8] + P[20][1]*SKK_LOS[7] - P[20][3]*SKK_LOS[6] + P[20][2]*SKK_LOS[9] - P[20][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[20][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[20][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[20][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[20][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); - Kfusion[21] = -SK_LOS[0]*(P[21][0]*SKK_LOS[8] + P[21][1]*SKK_LOS[7] - P[21][3]*SKK_LOS[6] + P[21][2]*SKK_LOS[9] - P[21][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[21][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[21][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[21][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[21][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); - Kfusion[22] = -SK_LOS[0]*(P[22][0]*SKK_LOS[8] + P[22][1]*SKK_LOS[7] - P[22][3]*SKK_LOS[6] + P[22][2]*SKK_LOS[9] - P[22][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[22][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[22][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); - varInnovOptFlow[0] = 1.0f/SK_LOS[0]; - innovOptFlow[0] = losPred[0] - losData[0]; - - // set the observation index to 1 to fuse the y component next time round and reset the commence fusion flag - obsIndex = 1; - fuseOptFlowData = false; - } - else if (obsIndex == 1) // we are now fusing the Y measurement - { - // Calculate observation jacobians - for (uint8_t i = 0; i < n_states; i++) H_LOS[i] = 0; - H_LOS[0] = SH_LOS[1]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[6] - ve*SH_LOS[11] + vn*SH_LOS[5]); - H_LOS[1] = SH_LOS[1]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[11] + ve*SH_LOS[6] + vn*SH_LOS[4]); - H_LOS[2] = SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6]); - H_LOS[3] = SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11]) - SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1); - H_LOS[4] = SH_LOS[0]*SH_LOS[3]*(a2*(2*q0*q2 + 2*q1*q3) + a3*(2*q0*q3 - 2*q1*q2) - sq(q0) - sq(q1) + sq(q2) + sq(q3)); - H_LOS[5] = -SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + 2*q0*q3 + 2*q1*q2 + a2*(2*q0*q1 - 2*q2*q3)); - H_LOS[6] = SH_LOS[0]*SH_LOS[3]*(a2*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q2 - 2*q1*q3 - a3*(2*q0*q1 + 2*q2*q3)); - H_LOS[9] = -SH_LOS[0]*SH_LOS[1]*SH_LOS[7]; - H_LOS[22] = SH_LOS[0]*SH_LOS[1]*SH_LOS[7]; - - // Calculate Kalman gains - SK_LOS[1] = 1/(R_LOS + SKK_LOS[12]*(P[0][2]*SKK_LOS[10] + P[1][2]*SKK_LOS[11] + P[2][2]*SKK_LOS[12] - P[3][2]*SKK_LOS[13] - P[9][2]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][2]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][2]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][2]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][2]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) - SKK_LOS[13]*(P[0][3]*SKK_LOS[10] + P[1][3]*SKK_LOS[11] + P[2][3]*SKK_LOS[12] - P[3][3]*SKK_LOS[13] - P[9][3]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][3]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][3]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][3]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][3]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) + SKK_LOS[10]*(P[0][0]*SKK_LOS[10] + P[1][0]*SKK_LOS[11] + P[2][0]*(SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[3]*SKK_LOS[14]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6])) - P[3][0]*(SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) - SH_LOS[3]*SKK_LOS[14]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11])) - P[9][0]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][0]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][0]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][0]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][0]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) + SKK_LOS[11]*(P[0][1]*SKK_LOS[10] + P[1][1]*SKK_LOS[11] + P[2][1]*(SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[3]*SKK_LOS[14]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6])) - P[3][1]*(SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) - SH_LOS[3]*SKK_LOS[14]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11])) - P[9][1]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][1]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][1]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][1]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][1]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) - SH_LOS[1]*SH_LOS[7]*SKK_LOS[14]*(P[0][9]*SKK_LOS[10] + P[1][9]*SKK_LOS[11] + P[2][9]*SKK_LOS[12] - P[3][9]*SKK_LOS[13] - P[9][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][9]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][9]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][9]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) + SH_LOS[1]*SH_LOS[7]*SKK_LOS[14]*(P[0][22]*SKK_LOS[10] + P[1][22]*SKK_LOS[11] + P[2][22]*SKK_LOS[12] - P[3][22]*SKK_LOS[13] - P[9][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][22]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][22]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][22]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) + SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14]*(P[0][4]*SKK_LOS[10] + P[1][4]*SKK_LOS[11] + P[2][4]*SKK_LOS[12] - P[3][4]*SKK_LOS[13] - P[9][4]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][4]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][4]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][4]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) - SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]*(P[0][5]*SKK_LOS[10] + P[1][5]*SKK_LOS[11] + P[2][5]*SKK_LOS[12] - P[3][5]*SKK_LOS[13] - P[9][5]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][5]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][5]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][5]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) + SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]*(P[0][6]*SKK_LOS[10] + P[1][6]*SKK_LOS[11] + P[2][6]*SKK_LOS[12] - P[3][6]*SKK_LOS[13] - P[9][6]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][6]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][6]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][6]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14])); - Kfusion[0] = SK_LOS[1]*(P[0][0]*SKK_LOS[10] - P[0][3]*SKK_LOS[13] + P[0][1]*SKK_LOS[11] + P[0][2]*SKK_LOS[12] - P[0][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[0][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[0][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[0][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[0][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); - Kfusion[1] = SK_LOS[1]*(P[1][0]*SKK_LOS[10] - P[1][3]*SKK_LOS[13] + P[1][1]*SKK_LOS[11] + P[1][2]*SKK_LOS[12] - P[1][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[1][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[1][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[1][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[1][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); - Kfusion[2] = SK_LOS[1]*(P[2][0]*SKK_LOS[10] - P[2][3]*SKK_LOS[13] + P[2][1]*SKK_LOS[11] + P[2][2]*SKK_LOS[12] - P[2][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[2][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[2][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[2][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[2][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); - Kfusion[3] = SK_LOS[1]*(P[3][0]*SKK_LOS[10] - P[3][3]*SKK_LOS[13] + P[3][1]*SKK_LOS[11] + P[3][2]*SKK_LOS[12] - P[3][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[3][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[3][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[3][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[3][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); - Kfusion[4] = SK_LOS[1]*(P[4][0]*SKK_LOS[10] - P[4][3]*SKK_LOS[13] + P[4][1]*SKK_LOS[11] + P[4][2]*SKK_LOS[12] - P[4][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[4][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[4][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); - Kfusion[5] = SK_LOS[1]*(P[5][0]*SKK_LOS[10] - P[5][3]*SKK_LOS[13] + P[5][1]*SKK_LOS[11] + P[5][2]*SKK_LOS[12] - P[5][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[5][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[5][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[5][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); - Kfusion[6] = SK_LOS[1]*(P[6][0]*SKK_LOS[10] - P[6][3]*SKK_LOS[13] + P[6][1]*SKK_LOS[11] + P[6][2]*SKK_LOS[12] - P[6][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[6][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[6][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[6][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[6][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); - Kfusion[7] = SK_LOS[1]*(P[7][0]*SKK_LOS[10] - P[7][3]*SKK_LOS[13] + P[7][1]*SKK_LOS[11] + P[7][2]*SKK_LOS[12] - P[7][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[7][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[7][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[7][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[7][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); - Kfusion[8] = SK_LOS[1]*(P[8][0]*SKK_LOS[10] - P[8][3]*SKK_LOS[13] + P[8][1]*SKK_LOS[11] + P[8][2]*SKK_LOS[12] - P[8][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[8][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[8][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[8][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[8][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); - Kfusion[9] = SK_LOS[1]*(P[9][0]*SKK_LOS[10] - P[9][3]*SKK_LOS[13] + P[9][1]*SKK_LOS[11] + P[9][2]*SKK_LOS[12] - P[9][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[9][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[9][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[9][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[9][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); - Kfusion[10] = SK_LOS[1]*(P[10][0]*SKK_LOS[10] - P[10][3]*SKK_LOS[13] + P[10][1]*SKK_LOS[11] + P[10][2]*SKK_LOS[12] - P[10][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[10][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[10][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[10][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[10][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); - Kfusion[11] = SK_LOS[1]*(P[11][0]*SKK_LOS[10] - P[11][3]*SKK_LOS[13] + P[11][1]*SKK_LOS[11] + P[11][2]*SKK_LOS[12] - P[11][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[11][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[11][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[11][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[11][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); - Kfusion[12] = SK_LOS[1]*(P[12][0]*SKK_LOS[10] - P[12][3]*SKK_LOS[13] + P[12][1]*SKK_LOS[11] + P[12][2]*SKK_LOS[12] - P[12][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[12][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[12][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[12][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[12][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); - Kfusion[13] = 0.0f;//SK_LOS[1]*(P[13][0]*SKK_LOS[10] - P[13][3]*SKK_LOS[13] + P[13][1]*SKK_LOS[11] + P[13][2]*SKK_LOS[12] - P[13][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[13][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[13][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[13][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[13][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); - Kfusion[14] = SK_LOS[1]*(P[14][0]*SKK_LOS[10] - P[14][3]*SKK_LOS[13] + P[14][1]*SKK_LOS[11] + P[14][2]*SKK_LOS[12] - P[14][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[14][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[14][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[14][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[14][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); - Kfusion[15] = SK_LOS[1]*(P[15][0]*SKK_LOS[10] - P[15][3]*SKK_LOS[13] + P[15][1]*SKK_LOS[11] + P[15][2]*SKK_LOS[12] - P[15][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[15][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[15][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[15][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[15][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); - Kfusion[16] = SK_LOS[1]*(P[16][0]*SKK_LOS[10] - P[16][3]*SKK_LOS[13] + P[16][1]*SKK_LOS[11] + P[16][2]*SKK_LOS[12] - P[16][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[16][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[16][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[16][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[16][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); - Kfusion[17] = SK_LOS[1]*(P[17][0]*SKK_LOS[10] - P[17][3]*SKK_LOS[13] + P[17][1]*SKK_LOS[11] + P[17][2]*SKK_LOS[12] - P[17][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[17][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[17][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[17][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[17][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); - Kfusion[18] = SK_LOS[1]*(P[18][0]*SKK_LOS[10] - P[18][3]*SKK_LOS[13] + P[18][1]*SKK_LOS[11] + P[18][2]*SKK_LOS[12] - P[18][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[18][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[18][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[18][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[18][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); - Kfusion[19] = SK_LOS[1]*(P[19][0]*SKK_LOS[10] - P[19][3]*SKK_LOS[13] + P[19][1]*SKK_LOS[11] + P[19][2]*SKK_LOS[12] - P[19][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[19][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[19][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[19][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[19][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); - Kfusion[20] = SK_LOS[1]*(P[20][0]*SKK_LOS[10] - P[20][3]*SKK_LOS[13] + P[20][1]*SKK_LOS[11] + P[20][2]*SKK_LOS[12] - P[20][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[20][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[20][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[20][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[20][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); - Kfusion[21] = SK_LOS[1]*(P[21][0]*SKK_LOS[10] - P[21][3]*SKK_LOS[13] + P[21][1]*SKK_LOS[11] + P[21][2]*SKK_LOS[12] - P[21][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[21][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[21][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[21][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[21][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); - Kfusion[22] = SK_LOS[1]*(P[22][0]*SKK_LOS[10] - P[22][3]*SKK_LOS[13] + P[22][1]*SKK_LOS[11] + P[22][2]*SKK_LOS[12] - P[22][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[22][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[22][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); - varInnovOptFlow[1] = 1.0f/SK_LOS[1]; - innovOptFlow[1] = losPred[1] - losData[1]; - - // reset the observation index - obsIndex = 0; - fuseOptFlowData = false; - } + // estimate range to centre of image + range = (flowStates[1] - statesAtRngTime[9]) * SK_RNG[2]; - // Check the innovation for consistency and don't fuse if > 3Sigma - if ((innovOptFlow[obsIndex]*innovOptFlow[obsIndex]/varInnovOptFlow[obsIndex]) < 9.0f) - { - // correct the state vector - for (uint8_t j = 0; j < n_states; j++) - { - states[j] = states[j] - Kfusion[j] * innovOptFlow[obsIndex]; - } - // normalise the quaternion states - float quatMag = sqrt(states[0]*states[0] + states[1]*states[1] + states[2]*states[2] + states[3]*states[3]); - if (quatMag > 1e-12f) - { - for (uint8_t j= 0; j<=3; j++) - { - float quatMagInv = 1.0f/quatMag; - states[j] = states[j] * quatMagInv; - } - } - // correct the covariance P = (I - K*H)*P - // take advantage of the empty columns in KH to reduce the - // number of operations - for (uint8_t i = 0; i < n_states; i++) - { - for (uint8_t j = 0; j <= 6; j++) - { - KH[i][j] = Kfusion[i] * H_LOS[j]; - } - for (uint8_t j = 7; j <= 8; j++) - { - KH[i][j] = 0.0f; - } - KH[i][9] = Kfusion[i] * H_LOS[9]; - for (uint8_t j = 10; j <= 21; j++) - { - KH[i][j] = 0.0f; - } - KH[i][22] = Kfusion[i] * H_LOS[22]; - } - for (uint8_t i = 0; i < n_states; i++) - { - for (uint8_t j = 0; j < n_states; j++) - { - KHP[i][j] = 0.0f; - for (uint8_t k = 0; k <= 6; k++) - { - KHP[i][j] = KHP[i][j] + KH[i][k] * P[k][j]; - } - KHP[i][j] = KHP[i][j] + KH[i][9] * P[9][j]; - KHP[i][j] = KHP[i][j] + KH[i][22] * P[2][j]; - } - } - } - for (uint8_t i = 0; i < n_states; i++) + // Calculate the measurement innovation + innovRng = range - rngMea; + + // calculate the innovation consistency test ratio + auxRngTestRatio = sq(innovRng) / (sq(rngInnovGate) * varInnovRng); + + // Check the innovation for consistency and don't fuse if out of bounds + if (auxRngTestRatio < 1.0f) { - for (uint8_t j = 0; j < n_states; j++) - { - P[i][j] = P[i][j] - KHP[i][j]; + // correct the state + for (uint8_t i = 0; i < 2 ; i++) { + flowStates[i] -= K_RNG[i] * innovRng; } + // constrain the states + + // constrain focal length to 0.1 to 10 mm + flowStates[0] = ConstrainFloat(flowStates[0], 0.1f, 10.0f); + // constrain altitude + flowStates[1] = maxf(flowStates[1], statesAtRngTime[9] + minFlowRng); + + // correct the covariance matrix + float nextPopt[2][2]; + nextPopt[0][0] = Popt[0][0] - (Popt[0][1]*Popt[1][0]*SK_RNG[1]*SK_RNG[2]) * SK_RNG[2]; + nextPopt[0][1] = Popt[0][1] - (Popt[0][1]*Popt[1][1]*SK_RNG[1]*SK_RNG[2]) * SK_RNG[2]; + nextPopt[1][0] = -Popt[1][0]*((Popt[1][1]*SK_RNG[1]*SK_RNG[2]) * SK_RNG[2] - 1.0f); + nextPopt[1][1] = -Popt[1][1]*((Popt[1][1]*SK_RNG[1]*SK_RNG[2]) * SK_RNG[2] - 1.0f); + // prevent the state variances from becoming negative and maintain symmetry + Popt[0][0] = maxf(nextPopt[0][0],0.0f); + Popt[1][1] = maxf(nextPopt[1][1],0.0f); + Popt[0][1] = 0.5f * (nextPopt[0][1] + nextPopt[1][0]); + Popt[1][0] = Popt[0][1]; } - ForceSymmetry(); - ConstrainVariances(); } } @@ -2199,6 +2304,24 @@ float AttPosEKF::sq(float valIn) return valIn*valIn; } +float AttPosEKF::maxf(float valIn1, float valIn2) +{ + if (valIn1 >= valIn2) { + return valIn1; + } else { + return valIn2; + } +} + +float AttPosEKF::min(float valIn1, float valIn2) +{ + if (valIn1 <= valIn2) { + return valIn1; + } else { + return valIn2; + } +} + // Store states in a history array along with time stamp void AttPosEKF::StoreStates(uint64_t timestamp_ms) { @@ -2395,9 +2518,9 @@ void AttPosEKF::OnGroundCheck() } // don't update terrain offset state if on ground if (onGround) { - inhibitGndHgtState = true; + inhibitGndState = true; } else { - inhibitGndHgtState = false; + inhibitGndState = false; } } diff --git a/src/modules/ekf_att_pos_estimator/estimator_23states.h b/src/modules/ekf_att_pos_estimator/estimator_23states.h index f8bb7a9c4..8ba1c1573 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_23states.h +++ b/src/modules/ekf_att_pos_estimator/estimator_23states.h @@ -80,6 +80,14 @@ public: airspeedMeasurementSigma = 1.4f; gyroProcessNoise = 1.4544411e-2f; accelProcessNoise = 0.5f; + + gndHgtSigma = 0.1f; // terrain gradient 1-sigma + R_LOS = 0.03f; // optical flow measurement noise variance (rad/sec)^2 + flowInnovGate = 3.0f; // number of standard deviations applied to the optical flow innovation consistency check + auxFlowInnovGate = 10.0f; // number of standard deviations applied to the optical flow innovation consistency check used by the auxiliary filter + rngInnovGate = 5.0f; // number of standard deviations applied to the rnage finder innovation consistency check + minFlowRng = 0.01f; //minimum range between ground and flow sensor + moCompR_LOS = 0.2; // scaler from sensor gyro rate to uncertainty in LOS rate } struct mag_state_struct { @@ -125,7 +133,7 @@ public: float statesAtMagMeasTime[n_states]; // filter satates at the effective measurement time float statesAtVtasMeasTime[n_states]; // filter states at the effective measurement time float statesAtRngTime[n_states]; // filter states at the effective measurement time - float statesAtOptFlowTime[n_states]; // States at the effective optical flow measurement time + float statesAtFlowTime[n_states]; // States at the effective optical flow measurement time Vector3f correctedDelAng; // delta angles about the xyz body axes corrected for errors (rad) Vector3f correctedDelVel; // delta velocities along the XYZ body axes corrected for errors (m/s) @@ -204,7 +212,8 @@ public: bool inhibitWindStates; // true when wind states and covariances are to remain constant bool inhibitMagStates; // true when magnetic field states and covariances are to remain constant - bool inhibitGndHgtState; // true when the terrain ground height offset state and covariances are to remain constant + bool inhibitGndState; // true when the terrain ground height offset state and covariances are to remain constant + bool inhibitScaleState; // true when the focal length scale factor state and covariances are to remain constant bool onGround; ///< boolean true when the flight vehicle is on the ground (not flying) bool staticMode; ///< boolean true if no position feedback is fused @@ -223,6 +232,27 @@ public: unsigned storeIndex; + // Optical Flow error estimation + float storedOmega[3][data_buffer_size]; // angular rate vector stored for the last 50 time steps used by optical flow eror estimators + + // Two state EKF used to estimate focal length scale factor and terrain position + float Popt[2][2]; // state covariance matrix + float flowStates[2]; // flow states [scale factor, terrain position] + float prevPosN; // north position at last measurement + float prevPosE; // east position at last measurement + float auxFlowObsInnov[2]; // optical flow observation innovations from focal length scale factor estimator + float auxFlowObsInnovVar[2]; // innovation variance for optical flow observations from focal length scale factor estimator + float fScaleFactorVar; // optical flow sensor focal length scale factor variance + Mat3f Tnb_flow; // Transformation matrix from nav to body at the time fo the optical flow measurement + float R_LOS; // Optical flow observation noise variance (rad/sec)^2 + float auxFlowTestRatio[2]; // ratio of X and Y flow observation innovations to fault threshold + float auxRngTestRatio; // ratio of range observation innovations to fault threshold + float flowInnovGate; // number of standard deviations used for the innovation consistency check + float auxFlowInnovGate; // number of standard deviations applied to the optical flow innovation consistency check + float rngInnovGate; // number of standard deviations used for the innovation consistency check + float minFlowRng; // minimum range over which to fuse optical flow measurements + float moCompR_LOS; // scaler from sensor gyro rate to uncertainty in LOS rate + void updateDtGpsFilt(float dt); void updateDtHgtFilt(float dt); @@ -241,6 +271,8 @@ void FuseRangeFinder(); void FuseOptFlow(); +void GroundEKF(); + void zeroRows(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last); void zeroCols(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last); @@ -283,6 +315,10 @@ static void quat2Tnb(Mat3f &Tnb, const float (&quat)[4]); static float sq(float valIn); +static float maxf(float valIn1, float valIn2); + +static float min(float valIn1, float valIn2); + void OnGroundCheck(); void CovarianceInit(); -- cgit v1.2.3 From 752b89b99811e27082be8147c7ff8426f0199478 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 2 Sep 2014 23:24:54 +0200 Subject: parse hil_optical_flow message publish to flow and range finder topic --- src/modules/mavlink/mavlink_receiver.cpp | 54 +++++++++++++++++++++++++++++++- src/modules/mavlink/mavlink_receiver.h | 2 ++ 2 files changed, 55 insertions(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index bed1bd789..45ea0e168 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -54,6 +54,7 @@ #include #include #include +#include #include #include #include @@ -102,6 +103,7 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) : _battery_pub(-1), _cmd_pub(-1), _flow_pub(-1), + _range_pub(-1), _offboard_control_sp_pub(-1), _local_pos_sp_pub(-1), _global_vel_sp_pub(-1), @@ -200,6 +202,10 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) handle_message_hil_state_quaternion(msg); break; + case MAVLINK_MSG_ID_HIL_OPTICAL_FLOW: + handle_message_hil_optical_flow(msg); + break; + default: break; } @@ -363,6 +369,52 @@ MavlinkReceiver::handle_message_optical_flow(mavlink_message_t *msg) } } +void +MavlinkReceiver::handle_message_hil_optical_flow(mavlink_message_t *msg) +{ + /* optical flow */ + mavlink_hil_optical_flow_t flow; + mavlink_msg_hil_optical_flow_decode(msg, &flow); + + struct optical_flow_s f; + memset(&f, 0, sizeof(f)); + + f.timestamp = hrt_absolute_time(); + f.flow_timestamp = flow.time_usec; + f.flow_raw_x = flow.flow_x; + f.flow_raw_y = flow.flow_y; + f.flow_comp_x_m = flow.flow_comp_m_x; + f.flow_comp_y_m = flow.flow_comp_m_y; + f.ground_distance_m = flow.ground_distance; + f.quality = flow.quality; + f.sensor_id = flow.sensor_id; + + if (_flow_pub < 0) { + _flow_pub = orb_advertise(ORB_ID(optical_flow), &f); + + } else { + orb_publish(ORB_ID(optical_flow), _flow_pub, &f); + } + + struct range_finder_report r; + memset(&r, 0, sizeof(f)); + + r.timestamp = hrt_absolute_time(); + r.error_count = 0; + r.type = RANGE_FINDER_TYPE_LASER; + r.distance = flow.ground_distance; + r.minimum_distance = 0.0f; + r.maximum_distance = 40.0f; // this is set to match the typical range of real sensors, could be made configurable + r.valid = (r.distance > r.minimum_distance) && (r.distance < r.maximum_distance); + + if (_range_pub < 0) { + _range_pub = orb_advertise(ORB_ID(sensor_range_finder), &r); + + } else { + orb_publish(ORB_ID(sensor_range_finder), _range_pub, &r); + } +} + void MavlinkReceiver::handle_message_set_mode(mavlink_message_t *msg) { @@ -444,7 +496,7 @@ MavlinkReceiver::handle_message_vision_position_estimate(mavlink_message_t *msg) vision_position.vx = 0.0f; vision_position.vy = 0.0f; vision_position.vz = 0.0f; - + math::Quaternion q; q.from_euler(pos.roll, pos.pitch, pos.yaw); diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 91125736c..c4e12d8d8 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -112,6 +112,7 @@ private: void handle_message_command_long(mavlink_message_t *msg); void handle_message_command_int(mavlink_message_t *msg); void handle_message_optical_flow(mavlink_message_t *msg); + void handle_message_hil_optical_flow(mavlink_message_t *msg); void handle_message_set_mode(mavlink_message_t *msg); void handle_message_vicon_position_estimate(mavlink_message_t *msg); void handle_message_vision_position_estimate(mavlink_message_t *msg); @@ -142,6 +143,7 @@ private: orb_advert_t _battery_pub; orb_advert_t _cmd_pub; orb_advert_t _flow_pub; + orb_advert_t _range_pub; orb_advert_t _offboard_control_sp_pub; orb_advert_t _local_pos_sp_pub; orb_advert_t _global_vel_sp_pub; -- cgit v1.2.3 From 49f1637d7f6486295c5fc7f541fe06ed934688cf Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 2 Sep 2014 23:32:24 +0200 Subject: comment and whitespace --- src/modules/mavlink/mavlink_receiver.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 45ea0e168..1115304d4 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -396,6 +396,7 @@ MavlinkReceiver::handle_message_hil_optical_flow(mavlink_message_t *msg) orb_publish(ORB_ID(optical_flow), _flow_pub, &f); } + /* Use distance value for range finder report */ struct range_finder_report r; memset(&r, 0, sizeof(f)); @@ -409,7 +410,6 @@ MavlinkReceiver::handle_message_hil_optical_flow(mavlink_message_t *msg) if (_range_pub < 0) { _range_pub = orb_advertise(ORB_ID(sensor_range_finder), &r); - } else { orb_publish(ORB_ID(sensor_range_finder), _range_pub, &r); } -- cgit v1.2.3 From 7db57310d5cdd7295da6c919dafa92d3831296db Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Tue, 2 Sep 2014 15:32:16 -0700 Subject: New mavlink_ftp unit test data --- .../unit_test_data/mavlink_tests/empty_dir/.gitignore | 0 .../unit_test_data/mavlink_tests/test_234.data | Bin 0 -> 234 bytes .../unit_test_data/mavlink_tests/test_235.data | Bin 0 -> 235 bytes .../unit_test_data/mavlink_tests/test_236.data | Bin 0 -> 236 bytes 4 files changed, 0 insertions(+), 0 deletions(-) create mode 100644 ROMFS/px4fmu_test/unit_test_data/mavlink_tests/empty_dir/.gitignore create mode 100644 ROMFS/px4fmu_test/unit_test_data/mavlink_tests/test_234.data create mode 100644 ROMFS/px4fmu_test/unit_test_data/mavlink_tests/test_235.data create mode 100644 ROMFS/px4fmu_test/unit_test_data/mavlink_tests/test_236.data diff --git a/ROMFS/px4fmu_test/unit_test_data/mavlink_tests/empty_dir/.gitignore b/ROMFS/px4fmu_test/unit_test_data/mavlink_tests/empty_dir/.gitignore new file mode 100644 index 000000000..e69de29bb diff --git a/ROMFS/px4fmu_test/unit_test_data/mavlink_tests/test_234.data b/ROMFS/px4fmu_test/unit_test_data/mavlink_tests/test_234.data new file mode 100644 index 000000000..3e075aa4f Binary files /dev/null and b/ROMFS/px4fmu_test/unit_test_data/mavlink_tests/test_234.data differ diff --git a/ROMFS/px4fmu_test/unit_test_data/mavlink_tests/test_235.data b/ROMFS/px4fmu_test/unit_test_data/mavlink_tests/test_235.data new file mode 100644 index 000000000..61372f769 Binary files /dev/null and b/ROMFS/px4fmu_test/unit_test_data/mavlink_tests/test_235.data differ diff --git a/ROMFS/px4fmu_test/unit_test_data/mavlink_tests/test_236.data b/ROMFS/px4fmu_test/unit_test_data/mavlink_tests/test_236.data new file mode 100644 index 000000000..eb7fcb11a Binary files /dev/null and b/ROMFS/px4fmu_test/unit_test_data/mavlink_tests/test_236.data differ -- cgit v1.2.3 From 81275e624e87d5cbb10535ffa2fcc2adbccc793f Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Tue, 2 Sep 2014 15:32:33 -0700 Subject: Added mavlink_ftp unit test --- makefiles/config_px4fmu-v2_test.mk | 3 +++ 1 file changed, 3 insertions(+) diff --git a/makefiles/config_px4fmu-v2_test.mk b/makefiles/config_px4fmu-v2_test.mk index 932f92261..a41670a77 100644 --- a/makefiles/config_px4fmu-v2_test.mk +++ b/makefiles/config_px4fmu-v2_test.mk @@ -54,6 +54,9 @@ MODULES += lib/conversion # LIBRARIES += lib/mathlib/CMSIS +MODULES += modules/unit_test +MODULES += modules/mavlink/mavlink_tests + # # Transitional support - add commands from the NuttX export archive. # -- cgit v1.2.3 From 6f0160bb5db552c921773f278e93d33862811a2a Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Tue, 2 Sep 2014 15:32:50 -0700 Subject: New mavlink_ftp unit test --- .../mavlink/mavlink_tests/mavlink_ftp_test.cpp | 788 +++++++++++++++++++++ .../mavlink/mavlink_tests/mavlink_ftp_test.h | 109 +++ .../mavlink/mavlink_tests/mavlink_tests.cpp | 52 ++ src/modules/mavlink/mavlink_tests/module.mk | 48 ++ 4 files changed, 997 insertions(+) create mode 100644 src/modules/mavlink/mavlink_tests/mavlink_ftp_test.cpp create mode 100644 src/modules/mavlink/mavlink_tests/mavlink_ftp_test.h create mode 100644 src/modules/mavlink/mavlink_tests/mavlink_tests.cpp create mode 100644 src/modules/mavlink/mavlink_tests/module.mk diff --git a/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.cpp b/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.cpp new file mode 100644 index 000000000..ebfce6d74 --- /dev/null +++ b/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.cpp @@ -0,0 +1,788 @@ +/**************************************************************************** + * + * 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 mavlink_ftp_test.cpp +/// @author Don Gagne + +#include +#include +#include +#include + +#include "mavlink_ftp_test.h" +#include "../mavlink_ftp.h" + +/// @brief Test case file name for Read command. File are generated using mavlink_ftp_test_data.py +const MavlinkFtpTest::ReadTestCase MavlinkFtpTest::_rgReadTestCases[] = { + { "/etc/unit_test_data/mavlink_tests/test_234.data", MAVLINK_MSG_FILE_TRANSFER_PROTOCOL_FIELD_PAYLOAD_LEN - sizeof(MavlinkFTP::PayloadHeader) - 1}, // Read takes less than single packet + { "/etc/unit_test_data/mavlink_tests/test_235.data", MAVLINK_MSG_FILE_TRANSFER_PROTOCOL_FIELD_PAYLOAD_LEN - sizeof(MavlinkFTP::PayloadHeader) }, // Read completely fills single packet + { "/etc/unit_test_data/mavlink_tests/test_236.data", MAVLINK_MSG_FILE_TRANSFER_PROTOCOL_FIELD_PAYLOAD_LEN - sizeof(MavlinkFTP::PayloadHeader) + 1 }, // Read take two packets +}; + +const char MavlinkFtpTest::_unittest_microsd_dir[] = "/fs/microsd/ftp_unit_test_dir"; +const char MavlinkFtpTest::_unittest_microsd_file[] = "/fs/microsd/ftp_unit_test_dir/file"; + +MavlinkFtpTest::MavlinkFtpTest() : + _ftp_server{}, + _reply_msg{}, + _lastOutgoingSeqNumber{} +{ +} + +MavlinkFtpTest::~MavlinkFtpTest() +{ + +} + +/// @brief Called before every test to initialize the FTP Server. +void MavlinkFtpTest::init(void) +{ + _ftp_server = new MavlinkFTP;; + _ftp_server->set_unittest_worker(MavlinkFtpTest::receive_message, this); + + _cleanup_microsd(); +} + +/// @brief Called after every test to take down the FTP Server. +void MavlinkFtpTest::cleanup(void) +{ + delete _ftp_server; + + _cleanup_microsd(); +} + +/// @brief Tests for correct behavior of an Ack response. +bool MavlinkFtpTest::_ack_test(void) +{ + MavlinkFTP::PayloadHeader payload; + mavlink_file_transfer_protocol_t ftp_msg; + MavlinkFTP::PayloadHeader *reply; + + payload.opcode = MavlinkFTP::kCmdNone; + + bool success = _send_receive_msg(&payload, // FTP payload header + 0, // size in bytes of data + nullptr, // Data to start into FTP message payload + &ftp_msg, // Response from server + &reply); // Payload inside FTP message response + if (!success) { + return false; + } + + ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck); + ut_compare("Incorrect payload size", reply->size, 0); + + return true; +} + +/// @brief Tests for correct response to a message sent with an invalid CRC. +bool MavlinkFtpTest::_bad_crc_test(void) +{ + mavlink_message_t msg; + MavlinkFTP::PayloadHeader payload; + mavlink_file_transfer_protocol_t ftp_msg; + MavlinkFTP::PayloadHeader *reply; + + payload.opcode = MavlinkFTP::kCmdNone; + + _setup_ftp_msg(&payload, 0, nullptr, &msg); + + ((MavlinkFTP::PayloadHeader*)((mavlink_file_transfer_protocol_t*)msg.payload64)->payload)->crc32++; + + _ftp_server->handle_message(nullptr /* mavlink */, &msg); + + if (!_decode_message(&_reply_msg, &ftp_msg, &reply)) { + return false; + } + + ut_compare("Didn't get Nak back", reply->opcode, MavlinkFTP::kRspNak); + ut_compare("Incorrect payload size", reply->size, 1); + ut_compare("Incorrect error code", reply->data[0], MavlinkFTP::kErrCrc); + + return true; +} + +/// @brief Tests for correct response to an invalid opcpde. +bool MavlinkFtpTest::_bad_opcode_test(void) +{ + MavlinkFTP::PayloadHeader payload; + mavlink_file_transfer_protocol_t ftp_msg; + MavlinkFTP::PayloadHeader *reply; + + payload.opcode = 0xFF; // bogus opcode + + bool success = _send_receive_msg(&payload, // FTP payload header + 0, // size in bytes of data + nullptr, // Data to start into FTP message payload + &ftp_msg, // Response from server + &reply); // Payload inside FTP message response + if (!success) { + return false; + } + + ut_compare("Didn't get Nak back", reply->opcode, MavlinkFTP::kRspNak); + ut_compare("Incorrect payload size", reply->size, 1); + ut_compare("Incorrect error code", reply->data[0], MavlinkFTP::kErrUnknownCommand); + + return true; +} + +/// @brief Tests for correct reponse to a payload which an invalid data size field. +bool MavlinkFtpTest::_bad_datasize_test(void) +{ + mavlink_message_t msg; + MavlinkFTP::PayloadHeader payload; + mavlink_file_transfer_protocol_t ftp_msg; + MavlinkFTP::PayloadHeader *reply; + + payload.opcode = MavlinkFTP::kCmdListDirectory; + + _setup_ftp_msg(&payload, 0, nullptr, &msg); + + // Set the data size to be one larger than is legal + ((MavlinkFTP::PayloadHeader*)((mavlink_file_transfer_protocol_t*)msg.payload64)->payload)->size = MAVLINK_MSG_FILE_TRANSFER_PROTOCOL_FIELD_PAYLOAD_LEN + 1; + + _ftp_server->handle_message(nullptr /* mavlink */, &msg); + + if (!_decode_message(&_reply_msg, &ftp_msg, &reply)) { + return false; + } + + ut_compare("Didn't get Nak back", reply->opcode, MavlinkFTP::kRspNak); + ut_compare("Incorrect payload size", reply->size, 1); + ut_compare("Incorrect error code", reply->data[0], MavlinkFTP::kErrInvalidDataSize); + + return true; +} + +bool MavlinkFtpTest::_list_test(void) +{ + MavlinkFTP::PayloadHeader payload; + mavlink_file_transfer_protocol_t ftp_msg; + MavlinkFTP::PayloadHeader *reply; + + char response1[] = "D.|Dempty_dir|Ftest_234.data\t234|Ftest_235.data\t235|Ftest_236.data\t236"; + char response2[] = "Ddev|Detc|Dfs|Dobj"; + + struct _testCase { + const char *dir; + char *response; + bool success; + }; + struct _testCase rgTestCases[] = { + { "/bogus", nullptr, false }, + { "/etc/unit_test_data/mavlink_tests", response1, true }, + { "/", response2, true }, + }; + + for (size_t i=0; iresponse) + 1; + + char *ptr = strtok (test->response, "|"); + while (ptr != nullptr) + { + ptr = strtok (nullptr, "|"); + } + + payload.opcode = MavlinkFTP::kCmdListDirectory; + payload.offset = 0; + + bool success = _send_receive_msg(&payload, // FTP payload header + strlen(test->dir)+1, // size in bytes of data + (uint8_t*)test->dir, // Data to start into FTP message payload + &ftp_msg, // Response from server + &reply); // Payload inside FTP message response + if (!success) { + return false; + } + + if (test->success) { + ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck); + ut_compare("Incorrect payload size", reply->size, expected_data_size); + ut_compare("Ack payload contents incorrect", memcmp(reply->data, test->response, expected_data_size), 0); + } else { + ut_compare("Didn't get Nak back", reply->opcode, MavlinkFTP::kRspNak); + ut_compare("Incorrect payload size", reply->size, 2); + ut_compare("Incorrect error code", reply->data[0], MavlinkFTP::kErrFailErrno); + } + } + + return true; +} + +/// @brief Tests for correct reponse to a List command on a valid directory, but with an offset that +/// is beyond the last directory entry. +bool MavlinkFtpTest::_list_eof_test(void) +{ + MavlinkFTP::PayloadHeader payload; + mavlink_file_transfer_protocol_t ftp_msg; + MavlinkFTP::PayloadHeader *reply; + const char *dir = "/"; + + payload.opcode = MavlinkFTP::kCmdListDirectory; + payload.offset = 4; // offset past top level dirs + + bool success = _send_receive_msg(&payload, // FTP payload header + strlen(dir)+1, // size in bytes of data + (uint8_t*)dir, // Data to start into FTP message payload + &ftp_msg, // Response from server + &reply); // Payload inside FTP message response + if (!success) { + return false; + } + + ut_compare("Didn't get Nak back", reply->opcode, MavlinkFTP::kRspNak); + ut_compare("Incorrect payload size", reply->size, 1); + ut_compare("Incorrect error code", reply->data[0], MavlinkFTP::kErrEOF); + + return true; +} + +/// @brief Tests for correct reponse to an Open command on a file which does not exist. +bool MavlinkFtpTest::_open_badfile_test(void) +{ + MavlinkFTP::PayloadHeader payload; + mavlink_file_transfer_protocol_t ftp_msg; + MavlinkFTP::PayloadHeader *reply; + const char *dir = "/foo"; // non-existent file + + payload.opcode = MavlinkFTP::kCmdOpenFile; + payload.offset = 0; + + bool success = _send_receive_msg(&payload, // FTP payload header + strlen(dir)+1, // size in bytes of data + (uint8_t*)dir, // Data to start into FTP message payload + &ftp_msg, // Response from server + &reply); // Payload inside FTP message response + if (!success) { + return false; + } + + ut_compare("Didn't get Nak back", reply->opcode, MavlinkFTP::kRspNak); + ut_compare("Incorrect payload size", reply->size, 2); + ut_compare("Incorrect error code", reply->data[0], MavlinkFTP::kErrFailErrno); + + return true; +} + +/// @brief Tests for correct reponse to an Open command on a file, followed by Terminate +bool MavlinkFtpTest::_open_terminate_test(void) +{ + MavlinkFTP::PayloadHeader payload; + mavlink_file_transfer_protocol_t ftp_msg; + MavlinkFTP::PayloadHeader *reply; + + for (size_t i=0; ifile)+1, // size in bytes of data + (uint8_t*)test->file, // Data to start into FTP message payload + &ftp_msg, // Response from server + &reply); // Payload inside FTP message response + if (!success) { + return false; + } + + ut_compare("stat failed", stat(test->file, &st), 0); + + + ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck); + ut_compare("Incorrect payload size", reply->size, sizeof(uint32_t)); + ut_compare("File size incorrect", *((uint32_t*)&reply->data[0]), (uint32_t)st.st_size); + + payload.opcode = MavlinkFTP::kCmdTerminateSession; + payload.session = reply->session; + payload.size = 0; + + success = _send_receive_msg(&payload, // FTP payload header + 0, // size in bytes of data + nullptr, // Data to start into FTP message payload + &ftp_msg, // Response from server + &reply); // Payload inside FTP message response + if (!success) { + return false; + } + + ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck); + ut_compare("Incorrect payload size", reply->size, 0); + } + + return true; +} + +/// @brief Tests for correct reponse to a Terminate command on an invalid session. +bool MavlinkFtpTest::_terminate_badsession_test(void) +{ + MavlinkFTP::PayloadHeader payload; + mavlink_file_transfer_protocol_t ftp_msg; + MavlinkFTP::PayloadHeader *reply; + const char *file = _rgReadTestCases[0].file; + + payload.opcode = MavlinkFTP::kCmdOpenFile; + payload.offset = 0; + + bool success = _send_receive_msg(&payload, // FTP payload header + strlen(file)+1, // size in bytes of data + (uint8_t*)file, // Data to start into FTP message payload + &ftp_msg, // Response from server + &reply); // Payload inside FTP message response + if (!success) { + return false; + } + + ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck); + + payload.opcode = MavlinkFTP::kCmdTerminateSession; + payload.session = reply->session + 1; + payload.size = 0; + + success = _send_receive_msg(&payload, // FTP payload header + 0, // size in bytes of data + nullptr, // Data to start into FTP message payload + &ftp_msg, // Response from server + &reply); // Payload inside FTP message response + if (!success) { + return false; + } + + ut_compare("Didn't get Nak back", reply->opcode, MavlinkFTP::kRspNak); + ut_compare("Incorrect payload size", reply->size, 1); + ut_compare("Incorrect error code", reply->data[0], MavlinkFTP::kErrInvalidSession); + + return true; +} + +/// @brief Tests for correct reponse to a Read command on an open session. +bool MavlinkFtpTest::_read_test(void) +{ + MavlinkFTP::PayloadHeader payload; + mavlink_file_transfer_protocol_t ftp_msg; + MavlinkFTP::PayloadHeader *reply; + + for (size_t i=0; ifile, &st), 0); + uint8_t *bytes = new uint8_t[st.st_size]; + ut_assert("new failed", bytes != nullptr); + int fd = ::open(test->file, O_RDONLY); + ut_assert("open failed", fd != -1); + int bytes_read = ::read(fd, bytes, st.st_size); + ut_compare("read failed", bytes_read, st.st_size); + ::close(fd); + + // Test case data files are created for specific boundary conditions + ut_compare("Test case data files are out of date", test->length, st.st_size); + + payload.opcode = MavlinkFTP::kCmdOpenFile; + payload.offset = 0; + + bool success = _send_receive_msg(&payload, // FTP payload header + strlen(test->file)+1, // size in bytes of data + (uint8_t*)test->file, // Data to start into FTP message payload + &ftp_msg, // Response from server + &reply); // Payload inside FTP message response + if (!success) { + return false; + } + + ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck); + + payload.opcode = MavlinkFTP::kCmdReadFile; + payload.session = reply->session; + payload.offset = 0; + + success = _send_receive_msg(&payload, // FTP payload header + 0, // size in bytes of data + nullptr, // Data to start into FTP message payload + &ftp_msg, // Response from server + &reply); // Payload inside FTP message response + if (!success) { + return false; + } + + ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck); + ut_compare("Offset incorrect", reply->offset, 0); + + if (test->length <= MAVLINK_MSG_FILE_TRANSFER_PROTOCOL_FIELD_PAYLOAD_LEN - sizeof(MavlinkFTP::PayloadHeader)) { + ut_compare("Payload size incorrect", reply->size, (uint32_t)st.st_size); + ut_compare("File contents differ", memcmp(reply->data, bytes, st.st_size), 0); + } + + payload.opcode = MavlinkFTP::kCmdTerminateSession; + payload.session = reply->session; + payload.size = 0; + + success = _send_receive_msg(&payload, // FTP payload header + 0, // size in bytes of data + nullptr, // Data to start into FTP message payload + &ftp_msg, // Response from server + &reply); // Payload inside FTP message response + if (!success) { + return false; + } + + ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck); + ut_compare("Incorrect payload size", reply->size, 0); + } + + return true; +} + +/// @brief Tests for correct reponse to a Read command on an invalid session. +bool MavlinkFtpTest::_read_badsession_test(void) +{ + MavlinkFTP::PayloadHeader payload; + mavlink_file_transfer_protocol_t ftp_msg; + MavlinkFTP::PayloadHeader *reply; + const char *file = _rgReadTestCases[0].file; + + payload.opcode = MavlinkFTP::kCmdOpenFile; + payload.offset = 0; + + bool success = _send_receive_msg(&payload, // FTP payload header + strlen(file)+1, // size in bytes of data + (uint8_t*)file, // Data to start into FTP message payload + &ftp_msg, // Response from server + &reply); // Payload inside FTP message response + if (!success) { + return false; + } + + ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck); + + payload.opcode = MavlinkFTP::kCmdReadFile; + payload.session = reply->session + 1; // Invalid session + payload.offset = 0; + + success = _send_receive_msg(&payload, // FTP payload header + 0, // size in bytes of data + nullptr, // Data to start into FTP message payload + &ftp_msg, // Response from server + &reply); // Payload inside FTP message response + if (!success) { + return false; + } + + ut_compare("Didn't get Nak back", reply->opcode, MavlinkFTP::kRspNak); + ut_compare("Incorrect payload size", reply->size, 1); + ut_compare("Incorrect error code", reply->data[0], MavlinkFTP::kErrInvalidSession); + + return true; +} + +bool MavlinkFtpTest::_removedirectory_test(void) +{ + MavlinkFTP::PayloadHeader payload; + mavlink_file_transfer_protocol_t ftp_msg; + MavlinkFTP::PayloadHeader *reply; + int fd; + + struct _testCase { + const char *dir; + bool success; + bool deleteFile; + }; + static const struct _testCase rgTestCases[] = { + { "/bogus", false, false }, + { "/etc/unit_test_data/mavlink_tests/empty_dir", false, false }, + { _unittest_microsd_dir, false, false }, + { _unittest_microsd_file, false, false }, + { _unittest_microsd_dir, true, true }, + }; + + ut_compare("mkdir failed", ::mkdir(_unittest_microsd_dir, S_IRWXU | S_IRWXG | S_IRWXO), 0); + ut_assert("open failed", (fd = ::open(_unittest_microsd_file, O_CREAT | O_EXCL)) != -1); + ::close(fd); + + for (size_t i=0; ideleteFile) { + ut_compare("unlink failed", ::unlink(_unittest_microsd_file), 0); + } + + payload.opcode = MavlinkFTP::kCmdRemoveDirectory; + payload.offset = 0; + + bool success = _send_receive_msg(&payload, // FTP payload header + strlen(test->dir)+1, // size in bytes of data + (uint8_t*)test->dir, // Data to start into FTP message payload + &ftp_msg, // Response from server + &reply); // Payload inside FTP message response + if (!success) { + return false; + } + + if (test->success) { + ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck); + ut_compare("Incorrect payload size", reply->size, 0); + } else { + ut_compare("Didn't get Nak back", reply->opcode, MavlinkFTP::kRspNak); + ut_compare("Incorrect payload size", reply->size, 2); + ut_compare("Incorrect error code", reply->data[0], MavlinkFTP::kErrFailErrno); + } + } + + return true; +} + +bool MavlinkFtpTest::_createdirectory_test(void) +{ + MavlinkFTP::PayloadHeader payload; + mavlink_file_transfer_protocol_t ftp_msg; + MavlinkFTP::PayloadHeader *reply; + + struct _testCase { + const char *dir; + bool success; + }; + static const struct _testCase rgTestCases[] = { + { "/etc/bogus", false }, + { _unittest_microsd_dir, true }, + { _unittest_microsd_dir, false }, + { "/fs/microsd/bogus/bogus", false }, + }; + + for (size_t i=0; idir)+1, // size in bytes of data + (uint8_t*)test->dir, // Data to start into FTP message payload + &ftp_msg, // Response from server + &reply); // Payload inside FTP message response + if (!success) { + return false; + } + + if (test->success) { + ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck); + ut_compare("Incorrect payload size", reply->size, 0); + } else { + ut_compare("Didn't get Nak back", reply->opcode, MavlinkFTP::kRspNak); + ut_compare("Incorrect payload size", reply->size, 2); + ut_compare("Incorrect error code", reply->data[0], MavlinkFTP::kErrFailErrno); + } + } + + return true; +} + +bool MavlinkFtpTest::_removefile_test(void) +{ + MavlinkFTP::PayloadHeader payload; + mavlink_file_transfer_protocol_t ftp_msg; + MavlinkFTP::PayloadHeader *reply; + int fd; + + struct _testCase { + const char *file; + bool success; + }; + static const struct _testCase rgTestCases[] = { + { "/bogus", false }, + { _rgReadTestCases[0].file, false }, + { _unittest_microsd_dir, false }, + { _unittest_microsd_file, true }, + { _unittest_microsd_file, false }, + }; + + ut_compare("mkdir failed", ::mkdir(_unittest_microsd_dir, S_IRWXU | S_IRWXG | S_IRWXO), 0); + ut_assert("open failed", (fd = ::open(_unittest_microsd_file, O_CREAT | O_EXCL)) != -1); + ::close(fd); + + for (size_t i=0; ifile)+1, // size in bytes of data + (uint8_t*)test->file, // Data to start into FTP message payload + &ftp_msg, // Response from server + &reply); // Payload inside FTP message response + if (!success) { + return false; + } + + if (test->success) { + ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck); + ut_compare("Incorrect payload size", reply->size, 0); + } else { + ut_compare("Didn't get Nak back", reply->opcode, MavlinkFTP::kRspNak); + ut_compare("Incorrect payload size", reply->size, 2); + ut_compare("Incorrect error code", reply->data[0], MavlinkFTP::kErrFailErrno); + } + } + + return true; +} + +/// @brief Static method used as callback from MavlinkFTP. This method will be called by MavlinkFTP when +/// it needs to send a message out on Mavlink. +void MavlinkFtpTest::receive_message(const mavlink_message_t *msg, MavlinkFtpTest *ftp_test) +{ + ftp_test->_receive_message(msg); +} + +/// @brief Non-Static version of receive_message +void MavlinkFtpTest::_receive_message(const mavlink_message_t *msg) +{ + // Move the message into our own member variable + memcpy(&_reply_msg, msg, sizeof(mavlink_message_t)); +} + +/// @brief Decode and validate the incoming message +bool MavlinkFtpTest::_decode_message(const mavlink_message_t *msg, ///< Mavlink message to decode + mavlink_file_transfer_protocol_t *ftp_msg, ///< Decoded FTP message + MavlinkFTP::PayloadHeader **payload) ///< Payload inside FTP message response +{ + mavlink_msg_file_transfer_protocol_decode(msg, ftp_msg); + + // Make sure the targets are correct + ut_compare("Target network non-zero", ftp_msg->target_network, 0); + ut_compare("Target system id mismatch", ftp_msg->target_system, clientSystemId); + ut_compare("Target component id mismatch", ftp_msg->target_component, clientComponentId); + + *payload = reinterpret_cast(ftp_msg->payload); + + // Make sure we have a good CRC + ut_compare("Incoming CRC mismatch", (*payload)->crc32, _payload_crc32((*payload))); + + // Make sure we have a good sequence number + ut_compare("Sequence number mismatch", (*payload)->seqNumber, _lastOutgoingSeqNumber + 1); + + // Bump sequence number for next outgoing message + _lastOutgoingSeqNumber++; + + return true; +} + +/// @brief Returns the 32 bit CRC for the payload, crc32 and padding are set to 0 for calculation. +uint32_t MavlinkFtpTest::_payload_crc32(struct MavlinkFTP::PayloadHeader *payload) +{ + // We calculate CRC with crc and padding set to 0. + uint32_t saveCRC = payload->crc32; + payload->crc32 = 0; + payload->padding[0] = 0; + payload->padding[1] = 0; + payload->padding[2] = 0; + uint32_t retCRC = crc32((const uint8_t*)payload, payload->size + sizeof(MavlinkFTP::PayloadHeader)); + payload->crc32 = saveCRC; + + return retCRC; +} + +/// @brief Initializes an FTP message into a mavlink message +void MavlinkFtpTest::_setup_ftp_msg(MavlinkFTP::PayloadHeader *payload_header, ///< FTP payload header + uint8_t size, ///< size in bytes of data + const uint8_t *data, ///< Data to start into FTP message payload + mavlink_message_t *msg) ///< Returned mavlink message +{ + uint8_t payload_bytes[MAVLINK_MSG_FILE_TRANSFER_PROTOCOL_FIELD_PAYLOAD_LEN]; + MavlinkFTP::PayloadHeader *payload = reinterpret_cast(payload_bytes); + + memcpy(payload, payload_header, sizeof(MavlinkFTP::PayloadHeader)); + + payload->seqNumber = _lastOutgoingSeqNumber; + payload->size = size; + if (size != 0) { + memcpy(payload->data, data, size); + } + + payload->crc32 = _payload_crc32(payload); + + msg->checksum = 0; + mavlink_msg_file_transfer_protocol_pack(clientSystemId, // Sender system id + clientComponentId, // Sender component id + msg, // Message to pack payload into + 0, // Target network + serverSystemId, // Target system id + serverComponentId, // Target component id + payload_bytes); // Payload to pack into message +} + +/// @brief Sends the specified FTP message to the server and returns response +bool MavlinkFtpTest::_send_receive_msg(MavlinkFTP::PayloadHeader *payload_header, ///< FTP payload header + uint8_t size, ///< size in bytes of data + const uint8_t *data, ///< Data to start into FTP message payload + mavlink_file_transfer_protocol_t *ftp_msg_reply, ///< Response from server + MavlinkFTP::PayloadHeader **payload_reply) ///< Payload inside FTP message response +{ + mavlink_message_t msg; + + _setup_ftp_msg(payload_header, size, data, &msg); + _ftp_server->handle_message(nullptr /* mavlink */, &msg); + return _decode_message(&_reply_msg, ftp_msg_reply, payload_reply); +} + +/// @brief Cleans up an files created on microsd during testing +void MavlinkFtpTest::_cleanup_microsd(void) +{ + ::unlink(_unittest_microsd_file); + ::rmdir(_unittest_microsd_dir); +} + +/// @brief Runs all the unit tests +void MavlinkFtpTest::runTests(void) +{ + ut_run_test(_ack_test); + ut_run_test(_bad_crc_test); + ut_run_test(_bad_opcode_test); + ut_run_test(_bad_datasize_test); + ut_run_test(_list_test); + ut_run_test(_list_eof_test); + ut_run_test(_open_badfile_test); + ut_run_test(_open_terminate_test); + ut_run_test(_terminate_badsession_test); + ut_run_test(_read_test); + ut_run_test(_read_badsession_test); + ut_run_test(_removedirectory_test); + ut_run_test(_createdirectory_test); + ut_run_test(_removefile_test); +} + diff --git a/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.h b/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.h new file mode 100644 index 000000000..bbb095a08 --- /dev/null +++ b/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.h @@ -0,0 +1,109 @@ +/**************************************************************************** + * + * 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 mavlink_ftp_test.h +/// @author Don Gagne + +#pragma once + +#include +#include "../mavlink_bridge_header.h" +#include "../mavlink_ftp.h" + +class MavlinkFtpTest : public UnitTest +{ +public: + MavlinkFtpTest(); + virtual ~MavlinkFtpTest(); + + virtual void init(void); + virtual void cleanup(void); + + virtual void runTests(void); + + static void receive_message(const mavlink_message_t *msg, MavlinkFtpTest* ftpTest); + + static const uint8_t serverSystemId = 50; ///< System ID for server + static const uint8_t serverComponentId = 1; ///< Component ID for server + static const uint8_t serverChannel = 0; ///< Channel to send to + + static const uint8_t clientSystemId = 1; ///< System ID for client + static const uint8_t clientComponentId = 0; ///< Component ID for client + + // We don't want any of these + MavlinkFtpTest(const MavlinkFtpTest&); + MavlinkFtpTest& operator=(const MavlinkFtpTest&); + +private: + bool _ack_test(void); + bool _bad_crc_test(void); + bool _bad_opcode_test(void); + bool _bad_datasize_test(void); + bool _list_test(void); + bool _list_eof_test(void); + bool _open_badfile_test(void); + bool _open_terminate_test(void); + bool _terminate_badsession_test(void); + bool _read_test(void); + bool _read_badsession_test(void); + bool _removedirectory_test(void); + bool _createdirectory_test(void); + bool _removefile_test(void); + + void _receive_message(const mavlink_message_t *msg); + uint32_t _payload_crc32(struct MavlinkFTP::PayloadHeader *payload); + void _setup_ftp_msg(MavlinkFTP::PayloadHeader *payload_header, uint8_t size, const uint8_t *data, mavlink_message_t *msg); + bool _decode_message(const mavlink_message_t *msg, mavlink_file_transfer_protocol_t *ftp_msg, MavlinkFTP::PayloadHeader **payload); + bool _send_receive_msg(MavlinkFTP::PayloadHeader *payload_header, + uint8_t size, + const uint8_t *data, + mavlink_file_transfer_protocol_t *ftp_msg_reply, + MavlinkFTP::PayloadHeader **payload_reply); + void _cleanup_microsd(void); + + MavlinkFTP *_ftp_server; + + mavlink_message_t _reply_msg; + + uint16_t _lastOutgoingSeqNumber; + + struct ReadTestCase { + const char *file; + const uint16_t length; + }; + static const ReadTestCase _rgReadTestCases[]; + + static const char _unittest_microsd_dir[]; + static const char _unittest_microsd_file[]; +}; + diff --git a/src/modules/mavlink/mavlink_tests/mavlink_tests.cpp b/src/modules/mavlink/mavlink_tests/mavlink_tests.cpp new file mode 100644 index 000000000..d9c1e413a --- /dev/null +++ b/src/modules/mavlink/mavlink_tests/mavlink_tests.cpp @@ -0,0 +1,52 @@ +/**************************************************************************** + * + * 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 mavlink_ftp_tests.cpp + */ + +#include + +#include "mavlink_ftp_test.h" + +extern "C" __EXPORT int mavlink_tests_main(int argc, char *argv[]); + +int mavlink_tests_main(int argc, char *argv[]) +{ + MavlinkFtpTest* test = new MavlinkFtpTest; + + test->runTests(); + test->printResults(); + + return 0; +} diff --git a/src/modules/mavlink/mavlink_tests/module.mk b/src/modules/mavlink/mavlink_tests/module.mk new file mode 100644 index 000000000..07cfce1dc --- /dev/null +++ b/src/modules/mavlink/mavlink_tests/module.mk @@ -0,0 +1,48 @@ +############################################################################ +# +# 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. +# +############################################################################ + +# +# System state machine tests. +# + +MODULE_COMMAND = mavlink_tests +SRCS = mavlink_tests.cpp \ + mavlink_ftp_test.cpp \ + ../mavlink_ftp.cpp \ + ../mavlink.c + +INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink + +MODULE_STACKSIZE = 5000 + +EXTRACXXFLAGS = -Weffc++ -DMAVLINK_FTP_UNIT_TEST -- cgit v1.2.3 From 1969c9ccf6797c4d86f32b9f57332a9e59071251 Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Tue, 2 Sep 2014 15:33:43 -0700 Subject: Improved unit test framework - Added init/cleanup calls before after test - Added ut_compare macro with better failure reporting --- src/modules/unit_test/unit_test.cpp | 5 +++++ src/modules/unit_test/unit_test.h | 18 ++++++++++++++++++ 2 files changed, 23 insertions(+) diff --git a/src/modules/unit_test/unit_test.cpp b/src/modules/unit_test/unit_test.cpp index 02d1af481..be3b9461a 100644 --- a/src/modules/unit_test/unit_test.cpp +++ b/src/modules/unit_test/unit_test.cpp @@ -56,3 +56,8 @@ void UnitTest::printAssert(const char* msg, const char* test, const char* file, { warnx("Assertion failed: %s - %s (%s:%d)", msg, test, file, line); } + +void UnitTest::printCompare(const char* msg, const char *v1_text, int v1, const char *v2_text, int v2, const char* file, int line) +{ + warnx("Compare failed: %s - (%s:%d) (%s:%d) (%s:%d)", msg, v1_text, v1, v2_text, v2, file, line); +} diff --git a/src/modules/unit_test/unit_test.h b/src/modules/unit_test/unit_test.h index 32eb8c308..1a5489709 100644 --- a/src/modules/unit_test/unit_test.h +++ b/src/modules/unit_test/unit_test.h @@ -52,11 +52,15 @@ INLINE_GLOBAL(const char*, mu_last_test) UnitTest(); virtual ~UnitTest(); + + virtual void init(void) { }; + virtual void cleanup(void) { }; virtual void runTests(void) = 0; void printResults(void); void printAssert(const char* msg, const char* test, const char* file, int line); + void printCompare(const char* msg, const char *v1_text, int v1, const char *v2_text, int v2, const char* file, int line); #define ut_assert(message, test) \ do { \ @@ -68,10 +72,23 @@ INLINE_GLOBAL(const char*, mu_last_test) } \ } while (0) +#define ut_compare(message, v1, v2) \ + do { \ + int _v1 = v1; \ + int _v2 = v2; \ + if (_v1 != _v2) { \ + printCompare(message, #v1, _v1, #v2, _v2, __FILE__, __LINE__); \ + return false; \ + } else { \ + mu_assertion()++; \ + } \ + } while (0) + #define ut_run_test(test) \ do { \ warnx("RUNNING TEST: %s", #test); \ mu_tests_run()++; \ + init(); \ if (!test()) { \ warnx("TEST FAILED: %s", #test); \ mu_tests_failed()++; \ @@ -79,6 +96,7 @@ INLINE_GLOBAL(const char*, mu_last_test) warnx("TEST PASSED: %s", #test); \ mu_tests_passed()++; \ } \ + cleanup(); \ } while (0) }; -- cgit v1.2.3 From e7165d6cbf364cb95e45dad998daa7c494de409e Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Tue, 2 Sep 2014 15:34:02 -0700 Subject: Generator for mavlink_ftp test files --- src/modules/mavlink/mavlink_tests/mavlink_ftp_test_data.py | 7 +++++++ 1 file changed, 7 insertions(+) create mode 100644 src/modules/mavlink/mavlink_tests/mavlink_ftp_test_data.py diff --git a/src/modules/mavlink/mavlink_tests/mavlink_ftp_test_data.py b/src/modules/mavlink/mavlink_tests/mavlink_ftp_test_data.py new file mode 100644 index 000000000..a7e68f2a3 --- /dev/null +++ b/src/modules/mavlink/mavlink_tests/mavlink_ftp_test_data.py @@ -0,0 +1,7 @@ +import sys +print 'Arguments: file - ' + sys.argv[1] + ', length - ' + sys.argv[2] +file = open(sys.argv[1], 'w') +for i in range(int(sys.argv[2])): + b = bytearray([i & 0xFF]) + file.write(b) +file.close() \ No newline at end of file -- cgit v1.2.3 From 37ea85968da111f7dc63cdfa8ca02ad6e601a8c3 Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Tue, 2 Sep 2014 15:34:20 -0700 Subject: Tweak for mavlink_ftp api change --- src/modules/mavlink/mavlink_receiver.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index bed1bd789..823de5172 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -121,7 +121,7 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) : { // make sure the FTP server is started - (void)MavlinkFTP::getServer(); + (void)MavlinkFTP::get_server(); } MavlinkReceiver::~MavlinkReceiver() @@ -173,7 +173,7 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) break; case MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL: - MavlinkFTP::getServer()->handle_message(_mavlink, msg); + MavlinkFTP::get_server()->handle_message(_mavlink, msg); break; default: -- cgit v1.2.3 From 828fb5efd10355d0d33b05fcf597bc4c05cd4db4 Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Tue, 2 Sep 2014 15:36:32 -0700 Subject: Additional FTP command support plus unit test - Restructured to a flatter class implementation - Added support for create/remove directory, remove file - Refactored error codes and command opcodes - Added unit test support - Fixed bugs found by unit test --- src/modules/mavlink/mavlink_ftp.cpp | 445 +++++++++++++++++++++++++----------- src/modules/mavlink/mavlink_ftp.h | 293 ++++++++++-------------- 2 files changed, 427 insertions(+), 311 deletions(-) diff --git a/src/modules/mavlink/mavlink_ftp.cpp b/src/modules/mavlink/mavlink_ftp.cpp index 5b65dc369..72ede8797 100644 --- a/src/modules/mavlink/mavlink_ftp.cpp +++ b/src/modules/mavlink/mavlink_ftp.cpp @@ -31,38 +31,39 @@ * ****************************************************************************/ +/// @file mavlink_ftp.cpp +/// @author px4dev, Don Gagne + #include #include #include #include #include +#include #include "mavlink_ftp.h" +#include "mavlink_tests/mavlink_ftp_test.h" // Uncomment the line below to get better debug output. Never commit with this left on. //#define MAVLINK_FTP_DEBUG -MavlinkFTP *MavlinkFTP::_server; - MavlinkFTP * -MavlinkFTP::getServer() +MavlinkFTP::get_server(void) { - // XXX this really cries out for some locking... - if (_server == nullptr) { - _server = new MavlinkFTP; - } - return _server; + static MavlinkFTP server; + return &server; } MavlinkFTP::MavlinkFTP() : - _session_fds{}, - _workBufs{}, - _workFree{}, - _lock{} + _request_bufs{}, + _request_queue{}, + _request_queue_sem{}, + _utRcvMsgFunc{}, + _ftp_test{} { // initialise the request freelist - dq_init(&_workFree); - sem_init(&_lock, 0, 1); + dq_init(&_request_queue); + sem_init(&_request_queue_sem, 0, 1); // initialize session list for (size_t i=0; idecode(mavlink, msg)) { + if (req == nullptr) { + warnx("Dropping FTP request: queue full\n"); + return; + } - // and queue it for the worker - work_queue(LPWORK, &req->work, &MavlinkFTP::_workerTrampoline, req, 0); - } else { - _qFree(req); + if (msg->msgid == MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL) { + mavlink_msg_file_transfer_protocol_decode(msg, &req->message); + +#ifdef MAVLINK_FTP_UNIT_TEST + if (!_utRcvMsgFunc) { + warnx("Incorrectly written unit test\n"); + return; + } + // We use fake ids when unit testing + req->serverSystemId = MavlinkFtpTest::serverSystemId; + req->serverComponentId = MavlinkFtpTest::serverComponentId; + req->serverChannel = MavlinkFtpTest::serverChannel; +#else + // Not unit testing, use the real thing + req->serverSystemId = mavlink->get_system_id(); + req->serverComponentId = mavlink->get_component_id(); + req->serverChannel = mavlink->get_channel(); +#endif + + // This is the system id we want to target when sending + req->targetSystemId = msg->sysid; + + if (req->message.target_system == req->serverSystemId) { + req->mavlink = mavlink; +#ifdef MAVLINK_FTP_UNIT_TEST + // We are running in Unit Test mode. Don't queue, just call _worket directly. + _process_request(req); +#else + // We are running in normal mode. Queue the request to the worker + work_queue(LPWORK, &req->work, &MavlinkFTP::_worker_trampoline, req, 0); +#endif + return; } } + + _return_request(req); } +/// @brief Queued static work queue routine to handle mavlink messages void -MavlinkFTP::_workerTrampoline(void *arg) +MavlinkFTP::_worker_trampoline(void *arg) { - auto req = reinterpret_cast(arg); - auto server = MavlinkFTP::getServer(); + Request* req = reinterpret_cast(arg); + MavlinkFTP* server = MavlinkFTP::get_server(); // call the server worker with the work item - server->_worker(req); + server->_process_request(req); } +/// @brief Processes an FTP message void -MavlinkFTP::_worker(Request *req) +MavlinkFTP::_process_request(Request *req) { - auto hdr = req->header(); + PayloadHeader *payload = reinterpret_cast(&req->message.payload[0]); + ErrorCode errorCode = kErrNone; - uint32_t messageCRC; // basic sanity checks; must validate length before use - if (hdr->size > kMaxDataLength) { - errorCode = kErrNoRequest; + if (payload->size > kMaxDataLength) { + errorCode = kErrInvalidDataSize; goto out; } // check request CRC to make sure this is one of ours - messageCRC = hdr->crc32; - hdr->crc32 = 0; - hdr->padding[0] = 0; - hdr->padding[1] = 0; - hdr->padding[2] = 0; - if (crc32(req->rawData(), req->dataSize()) != messageCRC) { - errorCode = kErrNoRequest; + if (_payload_crc32(payload) != payload->crc32) { + errorCode = kErrCrc; goto out; warnx("ftp: bad crc"); } #ifdef MAVLINK_FTP_DEBUG - printf("ftp: channel %u opc %u size %u offset %u\n", req->channel(), hdr->opcode, hdr->size, hdr->offset); + printf("ftp: channel %u opc %u size %u offset %u\n", req->serverChannel, payload->opcode, payload->size, payload->offset); #endif - switch (hdr->opcode) { + switch (payload->opcode) { case kCmdNone: break; - case kCmdTerminate: - errorCode = _workTerminate(req); + case kCmdTerminateSession: + errorCode = _workTerminate(payload); break; - case kCmdReset: - errorCode = _workReset(); + case kCmdResetSessions: + errorCode = _workReset(payload); break; - case kCmdList: - errorCode = _workList(req); + case kCmdListDirectory: + errorCode = _workList(payload); break; - case kCmdOpen: - errorCode = _workOpen(req, false); + case kCmdOpenFile: + errorCode = _workOpen(payload, false); break; - case kCmdCreate: - errorCode = _workOpen(req, true); + case kCmdCreateFile: + errorCode = _workOpen(payload, true); break; - case kCmdRead: - errorCode = _workRead(req); + case kCmdReadFile: + errorCode = _workRead(payload); break; - case kCmdWrite: - errorCode = _workWrite(req); + case kCmdWriteFile: + errorCode = _workWrite(payload); break; - case kCmdRemove: - errorCode = _workRemove(req); + case kCmdRemoveFile: + errorCode = _workRemoveFile(payload); break; + case kCmdCreateDirectory: + errorCode = _workCreateDirectory(payload); + break; + + case kCmdRemoveDirectory: + errorCode = _workRemoveDirectory(payload); + break; + default: - errorCode = kErrNoRequest; + errorCode = kErrUnknownCommand; break; } out: // handle success vs. error if (errorCode == kErrNone) { - hdr->opcode = kRspAck; + payload->opcode = kRspAck; #ifdef MAVLINK_FTP_DEBUG warnx("FTP: ack\n"); #endif } else { warnx("FTP: nak %u", errorCode); - hdr->opcode = kRspNak; - hdr->size = 1; - hdr->data[0] = errorCode; + payload->opcode = kRspNak; + payload->size = 1; + payload->data[0] = errorCode; + if (errorCode == kErrFailErrno) { + payload->size = 2; + payload->data[1] = errno; + } } + // respond to the request _reply(req); - // free the request buffer back to the freelist - _qFree(req); + _return_request(req); } +/// @brief Sends the specified FTP reponse message out through mavlink void MavlinkFTP::_reply(Request *req) { - auto hdr = req->header(); + PayloadHeader *payload = reinterpret_cast(&req->message.payload[0]); - hdr->seqNumber = req->header()->seqNumber + 1; + payload->seqNumber = payload->seqNumber + 1; - // message is assumed to be already constructed in the request buffer, so generate the CRC - hdr->crc32 = 0; - hdr->padding[0] = 0; - hdr->padding[1] = 0; - hdr->padding[2] = 0; - hdr->crc32 = crc32(req->rawData(), req->dataSize()); + payload->crc32 = _payload_crc32(payload); - // then pack and send the reply back to the request source - req->reply(); + mavlink_message_t msg; + msg.checksum = 0; +#ifndef MAVLINK_FTP_UNIT_TEST + uint16_t len = +#endif + mavlink_msg_file_transfer_protocol_pack_chan(req->serverSystemId, // Sender system id + req->serverComponentId, // Sender component id + req->serverChannel, // Channel to send on + &msg, // Message to pack payload into + 0, // Target network + req->targetSystemId, // Target system id + 0, // Target component id + (const uint8_t*)payload); // Payload to pack into message + + bool success = true; +#ifdef MAVLINK_FTP_UNIT_TEST + // Unit test hook is set, call that instead + _utRcvMsgFunc(&msg, _ftp_test); +#else + Mavlink *mavlink = req->mavlink; + + mavlink->lockMessageBufferMutex(); + success = mavlink->message_buffer_write(&msg, len); + mavlink->unlockMessageBufferMutex(); + +#endif + + if (!success) { + warnx("FTP TX ERR"); + } +#ifdef MAVLINK_FTP_DEBUG + else { + warnx("wrote: sys: %d, comp: %d, chan: %d, checksum: %d", + req->serverSystemId, + req->serverComponentId, + req->serverChannel, + msg.checksum); + } +#endif } +/// @brief Responds to a List command MavlinkFTP::ErrorCode -MavlinkFTP::_workList(Request *req) +MavlinkFTP::_workList(PayloadHeader* payload) { - auto hdr = req->header(); - char dirPath[kMaxDataLength]; - strncpy(dirPath, req->dataAsCString(), kMaxDataLength); + strncpy(dirPath, _data_as_cstring(payload), kMaxDataLength); DIR *dp = opendir(dirPath); if (dp == nullptr) { warnx("FTP: can't open path '%s'", dirPath); - return kErrNotDir; + return kErrFailErrno; } #ifdef MAVLINK_FTP_DEBUG - warnx("FTP: list %s offset %d", dirPath, hdr->offset); + warnx("FTP: list %s offset %d", dirPath, payload->offset); #endif ErrorCode errorCode = kErrNone; @@ -239,19 +320,19 @@ MavlinkFTP::_workList(Request *req) unsigned offset = 0; // move to the requested offset - seekdir(dp, hdr->offset); + seekdir(dp, payload->offset); for (;;) { // read the directory entry if (readdir_r(dp, &entry, &result)) { warnx("FTP: list %s readdir_r failure\n", dirPath); - errorCode = kErrIO; + errorCode = kErrFailErrno; break; } // no more entries? if (result == nullptr) { - if (hdr->offset != 0 && offset == 0) { + if (payload->offset != 0 && offset == 0) { // User is requesting subsequent dir entries but there were none. This means the user asked // to seek past EOF. errorCode = kErrEOF; @@ -299,94 +380,95 @@ MavlinkFTP::_workList(Request *req) } // Move the data into the buffer - hdr->data[offset++] = direntType; - strcpy((char *)&hdr->data[offset], buf); + payload->data[offset++] = direntType; + strcpy((char *)&payload->data[offset], buf); #ifdef MAVLINK_FTP_DEBUG - printf("FTP: list %s %s\n", dirPath, (char *)&hdr->data[offset-1]); + printf("FTP: list %s %s\n", dirPath, (char *)&payload->data[offset-1]); #endif offset += nameLen + 1; } closedir(dp); - hdr->size = offset; + payload->size = offset; return errorCode; } +/// @brief Responds to an Open command MavlinkFTP::ErrorCode -MavlinkFTP::_workOpen(Request *req, bool create) +MavlinkFTP::_workOpen(PayloadHeader* payload, bool create) { - auto hdr = req->header(); - - int session_index = _findUnusedSession(); + int session_index = _find_unused_session(); if (session_index < 0) { - return kErrNoSession; + warnx("FTP: Open failed - out of sessions\n"); + return kErrNoSessionsAvailable; } - + + char *filename = _data_as_cstring(payload); uint32_t fileSize = 0; if (!create) { struct stat st; - if (stat(req->dataAsCString(), &st) != 0) { - return kErrNotFile; + if (stat(filename, &st) != 0) { + return kErrFailErrno; } fileSize = st.st_size; } int oflag = create ? (O_CREAT | O_EXCL | O_APPEND) : O_RDONLY; - int fd = ::open(req->dataAsCString(), oflag); + int fd = ::open(filename, oflag); if (fd < 0) { - return create ? kErrPerm : kErrNotFile; + return kErrFailErrno; } _session_fds[session_index] = fd; - hdr->session = session_index; + payload->session = session_index; if (create) { - hdr->size = 0; + payload->size = 0; } else { - hdr->size = sizeof(uint32_t); - *((uint32_t*)hdr->data) = fileSize; + payload->size = sizeof(uint32_t); + *((uint32_t*)payload->data) = fileSize; } return kErrNone; } +/// @brief Responds to a Read command MavlinkFTP::ErrorCode -MavlinkFTP::_workRead(Request *req) +MavlinkFTP::_workRead(PayloadHeader* payload) { - auto hdr = req->header(); + int session_index = payload->session; - int session_index = hdr->session; - - if (!_validSession(session_index)) { - return kErrNoSession; + if (!_valid_session(session_index)) { + return kErrInvalidSession; } // Seek to the specified position #ifdef MAVLINK_FTP_DEBUG - warnx("seek %d", hdr->offset); + warnx("seek %d", payload->offset); #endif - if (lseek(_session_fds[session_index], hdr->offset, SEEK_SET) < 0) { + if (lseek(_session_fds[session_index], payload->offset, SEEK_SET) < 0) { // Unable to see to the specified location warnx("seek fail"); return kErrEOF; } - int bytes_read = ::read(_session_fds[session_index], &hdr->data[0], kMaxDataLength); + int bytes_read = ::read(_session_fds[session_index], &payload->data[0], kMaxDataLength); if (bytes_read < 0) { // Negative return indicates error other than eof warnx("read fail %d", bytes_read); - return kErrIO; + return kErrFailErrno; } - hdr->size = bytes_read; + payload->size = bytes_read; return kErrNone; } +/// @brief Responds to a Write command MavlinkFTP::ErrorCode -MavlinkFTP::_workWrite(Request *req) +MavlinkFTP::_workWrite(PayloadHeader* payload) { #if 0 // NYI: Coming soon @@ -409,35 +491,44 @@ MavlinkFTP::_workWrite(Request *req) hdr->size = result; return kErrNone; #else - return kErrPerm; + return kErrUnknownCommand; #endif } +/// @brief Responds to a RemoveFile command MavlinkFTP::ErrorCode -MavlinkFTP::_workRemove(Request *req) +MavlinkFTP::_workRemoveFile(PayloadHeader* payload) { - //auto hdr = req->header(); - - // for now, send error reply - return kErrPerm; + char file[kMaxDataLength]; + strncpy(file, _data_as_cstring(payload), kMaxDataLength); + + if (unlink(file) == 0) { + payload->size = 0; + return kErrNone; + } else { + return kErrFailErrno; + } } +/// @brief Responds to a Terminate command MavlinkFTP::ErrorCode -MavlinkFTP::_workTerminate(Request *req) +MavlinkFTP::_workTerminate(PayloadHeader* payload) { - auto hdr = req->header(); - - if (!_validSession(hdr->session)) { - return kErrNoSession; + if (!_valid_session(payload->session)) { + return kErrInvalidSession; } - ::close(_session_fds[hdr->session]); + ::close(_session_fds[payload->session]); + _session_fds[payload->session] = -1; + + payload->size = 0; return kErrNone; } +/// @brief Responds to a Reset command MavlinkFTP::ErrorCode -MavlinkFTP::_workReset(void) +MavlinkFTP::_workReset(PayloadHeader* payload) { for (size_t i=0; isize = 0; + return kErrNone; } +/// @brief Responds to a RemoveDirectory command +MavlinkFTP::ErrorCode +MavlinkFTP::_workRemoveDirectory(PayloadHeader* payload) +{ + char dir[kMaxDataLength]; + strncpy(dir, _data_as_cstring(payload), kMaxDataLength); + + if (rmdir(dir) == 0) { + payload->size = 0; + return kErrNone; + } else { + return kErrFailErrno; + } +} + +/// @brief Responds to a CreateDirectory command +MavlinkFTP::ErrorCode +MavlinkFTP::_workCreateDirectory(PayloadHeader* payload) +{ + char dir[kMaxDataLength]; + strncpy(dir, _data_as_cstring(payload), kMaxDataLength); + + if (mkdir(dir, S_IRWXU | S_IRWXG | S_IRWXO) == 0) { + payload->size = 0; + return kErrNone; + } else { + return kErrFailErrno; + } +} + +/// @brief Returns true if the specified session is a valid open session bool -MavlinkFTP::_validSession(unsigned index) +MavlinkFTP::_valid_session(unsigned index) { if ((index >= kMaxSession) || (_session_fds[index] < 0)) { return false; @@ -458,8 +582,9 @@ MavlinkFTP::_validSession(unsigned index) return true; } +/// @brief Returns an unused session index int -MavlinkFTP::_findUnusedSession(void) +MavlinkFTP::_find_unused_session(void) { for (size_t i=0; isize < kMaxDataLength) { - requestData()[header()->size] = '\0'; + if (payload->size < kMaxDataLength) { + payload->data[payload->size] = '\0'; } else { - requestData()[kMaxDataLength - 1] = '\0'; + payload->data[kMaxDataLength - 1] = '\0'; } // and return data - return (char *)&(header()->data[0]); + return (char *)&(payload->data[0]); +} + +/// @brief Returns a unused Request entry. NULL if none available. +MavlinkFTP::Request * +MavlinkFTP::_get_request(void) +{ + _lock_request_queue(); + Request* req = reinterpret_cast(dq_remfirst(&_request_queue)); + _unlock_request_queue(); + return req; +} + +/// @brief Locks a semaphore to provide exclusive access to the request queue +void +MavlinkFTP::_lock_request_queue(void) +{ + do {} + while (sem_wait(&_request_queue_sem) != 0); +} + +/// @brief Unlocks the semaphore providing exclusive access to the request queue +void +MavlinkFTP::_unlock_request_queue(void) +{ + sem_post(&_request_queue_sem); +} + +/// @brief Returns a no longer needed request to the queue +void +MavlinkFTP::_return_request(Request *req) +{ + _lock_request_queue(); + dq_addlast(&req->work.dq, &_request_queue); + _unlock_request_queue(); +} + +/// @brief Returns the 32 bit CRC for the payload, crc32 and padding members are set to 0 for calculation. +uint32_t +MavlinkFTP::_payload_crc32(PayloadHeader *payload) +{ + // We calculate CRC with crc and padding set to 0. + uint32_t saveCRC = payload->crc32; + payload->crc32 = 0; + payload->padding[0] = 0; + payload->padding[1] = 0; + payload->padding[2] = 0; + uint32_t retCRC = crc32((const uint8_t*)payload, payload->size + sizeof(PayloadHeader)); + payload->crc32 = saveCRC; + + return retCRC; } diff --git a/src/modules/mavlink/mavlink_ftp.h b/src/modules/mavlink/mavlink_ftp.h index 1dd8f102e..b4cc154d1 100644 --- a/src/modules/mavlink/mavlink_ftp.h +++ b/src/modules/mavlink/mavlink_ftp.h @@ -33,17 +33,8 @@ #pragma once -/** - * @file mavlink_ftp.h - * - * MAVLink remote file server. - * - * A limited number of requests (currently 2) may be outstanding at a time. - * Additional messages will be discarded. - * - * Messages consist of a fixed header, followed by a data area. - * - */ +/// @file mavlink_ftp.h +/// @author px4dev, Don Gagne #include #include @@ -54,183 +45,131 @@ #include "mavlink_messages.h" #include "mavlink_main.h" +class MavlinkFtpTest; + +/// @brief MAVLink remote file server. Support FTP like commands using MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL message. +/// A limited number of requests (kRequestQueueSize) may be outstanding at a time. Additional messages will be discarded. class MavlinkFTP { public: + /// @brief Returns the one Mavlink FTP server in the system. + static MavlinkFTP* get_server(void); + + /// @brief Contructor is only public so unit test code can new objects. MavlinkFTP(); - - static MavlinkFTP *getServer(); - - // static interface - void handle_message(Mavlink* mavlink, - mavlink_message_t *msg); - -private: - - static const unsigned kRequestQueueSize = 2; - - static MavlinkFTP *_server; - - /// @brief Trying to pack structures across differing compilers is questionable for Clients, so we pad the - /// structure ourselves to 32 bit alignment which should get us what we want. - struct RequestHeader + + /// @brief Adds the specified message to the work queue. + void handle_message(Mavlink* mavlink, mavlink_message_t *msg); + + typedef void (*ReceiveMessageFunc_t)(const mavlink_message_t *msg, MavlinkFtpTest* ftpTest); + + /// @brief Sets up the server to run in unit test mode. + /// @param rcvmsgFunc Function which will be called to handle outgoing mavlink messages. + /// @param ftp_test MavlinkFtpTest object which the function is associated with + void set_unittest_worker(ReceiveMessageFunc_t rcvMsgFunc, MavlinkFtpTest *ftp_test); + + /// @brief This is the payload which is in mavlink_file_transfer_protocol_t.payload. We pad the structure ourselves to + /// 32 bit alignment to avoid usage of any pack pragmas. + struct PayloadHeader { - uint16_t seqNumber; ///< sequence number for message - uint8_t session; ///< Session id for read and write commands - uint8_t opcode; ///< Command opcode - uint8_t size; ///< Size of data - uint8_t padding[3]; - uint32_t crc32; ///< CRC for entire Request structure, with crc32 and padding set to 0 - uint32_t offset; ///< Offsets for List and Read commands - uint8_t data[]; + uint16_t seqNumber; ///< sequence number for message + uint8_t session; ///< Session id for read and write commands + uint8_t opcode; ///< Command opcode + uint8_t size; ///< Size of data + uint8_t padding[3]; ///< 32 bit aligment padding + uint32_t crc32; ///< CRC for entire Request structure, with crc32 and padding set to 0 + uint32_t offset; ///< Offsets for List and Read commands + uint8_t data[]; ///< command data, varies by Opcode }; - + + /// @brief Command opcodes enum Opcode : uint8_t { - kCmdNone, // ignored, always acked - kCmdTerminate, // releases sessionID, closes file - kCmdReset, // terminates all sessions - kCmdList, // list files in from - kCmdOpen, // opens for reading, returns - kCmdRead, // reads bytes from in - kCmdCreate, // creates for writing, returns - kCmdWrite, // appends bytes at in - kCmdRemove, // remove file (only if created by server?) - - kRspAck, - kRspNak + kCmdNone, ///< ignored, always acked + kCmdTerminateSession, ///< Terminates open Read session + kCmdResetSessions, ///< Terminates all open Read sessions + kCmdListDirectory, ///< List files in from + kCmdOpenFile, ///< Opens file at for reading, returns + kCmdReadFile, ///< Reads bytes from in + kCmdCreateFile, ///< Creates file at for writing, returns + kCmdWriteFile, ///< Appends bytes to file in + kCmdRemoveFile, ///< Remove file at + kCmdCreateDirectory, ///< Creates directory at + kCmdRemoveDirectory, ///< Removes Directory at , must be empty + + kRspAck, ///< Ack response + kRspNak ///< Nak response }; - + + /// @brief Error codes returned in Nak response PayloadHeader.data[0]. enum ErrorCode : uint8_t - { + { kErrNone, - kErrNoRequest, - kErrNoSession, - kErrSequence, - kErrNotDir, - kErrNotFile, - kErrEOF, - kErrNotAppend, - kErrTooBig, - kErrIO, - kErrPerm - }; - - int _findUnusedSession(void); - bool _validSession(unsigned index); - - static const unsigned kMaxSession = 2; - int _session_fds[kMaxSession]; - - class Request + kErrFail, ///< Unknown failure + kErrFailErrno, ///< Command failed, errno sent back in PayloadHeader.data[1] + kErrInvalidDataSize, ///< PayloadHeader.size is invalid + kErrInvalidSession, ///< Session is not currently open + kErrNoSessionsAvailable, ///< All available Sessions in use + kErrEOF, ///< Offset past end of file for List and Read commands + kErrUnknownCommand, ///< Unknown command opcode + kErrCrc ///< CRC on Payload is incorrect + }; + +private: + /// @brief Unit of work which is queued to work_queue + struct Request { - public: - union { - dq_entry_t entry; - work_s work; - }; - - bool decode(Mavlink *mavlink, mavlink_message_t *fromMessage) { - if (fromMessage->msgid == MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL) { - _systemId = fromMessage->sysid; - _mavlink = mavlink; - mavlink_msg_file_transfer_protocol_decode(fromMessage, &_message); - return _message.target_system == _mavlink->get_system_id(); - } - return false; - } - - void reply() { - - // XXX the proper way would be an IOCTL / uORB call, rather than exploiting the - // flat memory architecture, as we're operating between threads here. - mavlink_message_t msg; - msg.checksum = 0; - unsigned len = mavlink_msg_file_transfer_protocol_pack_chan(_mavlink->get_system_id(), // Sender system id - _mavlink->get_component_id(), // Sender component id - _mavlink->get_channel(), // Channel to send on - &msg, // Message to pack payload into - 0, // Target network - _systemId, // Target system id - 0, // Target component id - rawData()); // Payload to pack into message - - _mavlink->lockMessageBufferMutex(); - bool success = _mavlink->message_buffer_write(&msg, len); - _mavlink->unlockMessageBufferMutex(); - - if (!success) { - warnx("FTP TX ERR"); - } -#ifdef MAVLINK_FTP_DEBUG - else { - warnx("wrote: sys: %d, comp: %d, chan: %d, len: %d, checksum: %d", - _mavlink->get_system_id(), - _mavlink->get_component_id(), - _mavlink->get_channel(), - len, - msg.checksum); - } -#endif - } - - uint8_t *rawData() { return &_message.payload[0]; } - RequestHeader *header() { return reinterpret_cast(&_message.payload[0]); } - uint8_t *requestData() { return &(header()->data[0]); } - unsigned dataSize() { return header()->size + sizeof(RequestHeader); } - mavlink_channel_t channel() { return _mavlink->get_channel(); } - - char *dataAsCString(); - - private: - Mavlink *_mavlink; - mavlink_file_transfer_protocol_t _message; - uint8_t _systemId; - + work_s work; ///< work queue entry + Mavlink *mavlink; ///< Mavlink to reply to + uint8_t serverSystemId; ///< System ID to send from + uint8_t serverComponentId; ///< Component ID to send from + uint8_t serverChannel; ///< Channel to send to + uint8_t targetSystemId; ///< System ID to target reply to + + mavlink_file_transfer_protocol_t message; ///< Protocol message }; - - static const char kDirentFile = 'F'; - static const char kDirentDir = 'D'; - static const char kDirentUnknown = 'U'; - static const uint8_t kMaxDataLength = MAVLINK_MSG_FILE_TRANSFER_PROTOCOL_FIELD_PAYLOAD_LEN - sizeof(RequestHeader); - - /// Request worker; runs on the low-priority work queue to service - /// remote requests. - /// - static void _workerTrampoline(void *arg); - void _worker(Request *req); - - /// Reply to a request (XXX should be a Request method) - /// - void _reply(Request *req); - - ErrorCode _workList(Request *req); - ErrorCode _workOpen(Request *req, bool create); - ErrorCode _workRead(Request *req); - ErrorCode _workWrite(Request *req); - ErrorCode _workRemove(Request *req); - ErrorCode _workTerminate(Request *req); - ErrorCode _workReset(); - - // work freelist - Request _workBufs[kRequestQueueSize]; - dq_queue_t _workFree; - sem_t _lock; - - void _qLock() { do {} while (sem_wait(&_lock) != 0); } - void _qUnlock() { sem_post(&_lock); } - - void _qFree(Request *req) { - _qLock(); - dq_addlast(&req->entry, &_workFree); - _qUnlock(); - } - - Request *_dqFree() { - _qLock(); - auto req = reinterpret_cast(dq_remfirst(&_workFree)); - _qUnlock(); - return req; - } - + + Request *_get_request(void); + void _return_request(Request *req); + void _lock_request_queue(void); + void _unlock_request_queue(void); + + uint32_t _payload_crc32(PayloadHeader *hdr); + + char *_data_as_cstring(PayloadHeader* payload); + + static void _worker_trampoline(void *arg); + void _process_request(Request *req); + void _reply(Request *req); + + ErrorCode _workList(PayloadHeader *payload); + ErrorCode _workOpen(PayloadHeader *payload, bool create); + ErrorCode _workRead(PayloadHeader *payload); + ErrorCode _workWrite(PayloadHeader *payload); + ErrorCode _workTerminate(PayloadHeader *payload); + ErrorCode _workReset(PayloadHeader* payload); + ErrorCode _workRemoveDirectory(PayloadHeader *payload); + ErrorCode _workCreateDirectory(PayloadHeader *payload); + ErrorCode _workRemoveFile(PayloadHeader *payload); + + static const unsigned kRequestQueueSize = 2; ///< Max number of queued requests + Request _request_bufs[kRequestQueueSize]; ///< Request buffers which hold work + dq_queue_t _request_queue; ///< Queue of available Request buffers + sem_t _request_queue_sem; ///< Semaphore for locking access to _request_queue + + int _find_unused_session(void); + bool _valid_session(unsigned index); + + static const char kDirentFile = 'F'; ///< Identifies File returned from List command + static const char kDirentDir = 'D'; ///< Identifies Directory returned from List command + static const char kDirentUnknown = 'U'; ///< Identifies Unknown entry returned from List command + + /// @brief Maximum data size in RequestHeader::data + static const uint8_t kMaxDataLength = MAVLINK_MSG_FILE_TRANSFER_PROTOCOL_FIELD_PAYLOAD_LEN - sizeof(PayloadHeader); + + static const unsigned kMaxSession = 2; ///< Max number of active sessions + int _session_fds[kMaxSession]; ///< Session file descriptors, 0 for empty slot + + ReceiveMessageFunc_t _utRcvMsgFunc; ///< Unit test override for mavlink message sending + MavlinkFtpTest *_ftp_test; ///< Additional parameter to _utRcvMsgFunc; }; -- 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(-) 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 3d01da35d02505e751536e2cc09637797b37bed6 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 3 Sep 2014 10:34:27 +0200 Subject: write sysid & compid to telemetry status --- src/modules/mavlink/mavlink_receiver.cpp | 2 ++ src/modules/uORB/topics/telemetry_status.h | 2 ++ 2 files changed, 4 insertions(+) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 1115304d4..43222880e 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -534,6 +534,8 @@ MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg) tstatus.remote_noise = rstatus.remnoise; tstatus.rxerrors = rstatus.rxerrors; tstatus.fixed = rstatus.fixed; + tstatus.system_id = msg->sysid; + tstatus.component_id = msg->compid; if (_telemetry_status_pub < 0) { _telemetry_status_pub = orb_advertise(telemetry_status_orb_id[_mavlink->get_channel()], &tstatus); diff --git a/src/modules/uORB/topics/telemetry_status.h b/src/modules/uORB/topics/telemetry_status.h index 1297c1a9d..93193f32b 100644 --- a/src/modules/uORB/topics/telemetry_status.h +++ b/src/modules/uORB/topics/telemetry_status.h @@ -67,6 +67,8 @@ struct telemetry_status_s { uint8_t noise; /**< background noise level */ uint8_t remote_noise; /**< remote background noise level */ uint8_t txbuf; /**< how full the tx buffer is as a percentage */ + uint8_t system_id; /**< system id of the remote system */ + uint8_t component_id; /**< component id of the remote system */ }; /** -- cgit v1.2.3 From 3f8793210b47bd8e09ed2adaabc2fab966db5df6 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 3 Sep 2014 10:34:52 +0200 Subject: datalink check: ignore onboard computer --- src/modules/commander/commander.cpp | 20 +++++++++++++++++--- src/modules/commander/commander_params.c | 10 ++++++++++ 2 files changed, 27 insertions(+), 3 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 9885176b7..07fcb5d40 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -685,6 +685,7 @@ int commander_thread_main(int argc, char *argv[]) param_t _param_enable_datalink_loss = param_find("COM_DL_LOSS_EN"); param_t _param_datalink_loss_timeout = param_find("COM_DL_LOSS_T"); param_t _param_datalink_regain_timeout = param_find("COM_DL_REG_T"); + param_t _param_onboard_sysid = param_find("COM_ONBSYSID"); /* welcome user */ warnx("starting"); @@ -879,12 +880,14 @@ int commander_thread_main(int argc, char *argv[]) uint64_t telemetry_last_heartbeat[TELEMETRY_STATUS_ORB_ID_NUM]; uint64_t telemetry_last_dl_loss[TELEMETRY_STATUS_ORB_ID_NUM]; bool telemetry_lost[TELEMETRY_STATUS_ORB_ID_NUM]; + uint8_t telemetry_sysid[TELEMETRY_STATUS_ORB_ID_NUM]; for (int i = 0; i < TELEMETRY_STATUS_ORB_ID_NUM; i++) { telemetry_subs[i] = orb_subscribe(telemetry_status_orb_id[i]); telemetry_last_heartbeat[i] = 0; telemetry_last_dl_loss[i] = 0; telemetry_lost[i] = true; + telemetry_sysid[i] = 0; } /* Subscribe to global position */ @@ -971,6 +974,8 @@ int commander_thread_main(int argc, char *argv[]) int32_t datalink_loss_enabled = false; int32_t datalink_loss_timeout = 10; int32_t datalink_regain_timeout = 0; + uint8_t onboard_sysid = 42; /**< systemid of the onboard computer, telemetry from this sysid + is not validated for the datalink loss check */ /* check which state machines for changes, clear "changed" flag */ bool arming_state_changed = false; @@ -1033,6 +1038,7 @@ int commander_thread_main(int argc, char *argv[]) param_get(_param_enable_datalink_loss, &datalink_loss_enabled); param_get(_param_datalink_loss_timeout, &datalink_loss_timeout); param_get(_param_datalink_regain_timeout, &datalink_regain_timeout); + param_get(_param_onboard_sysid, &onboard_sysid); } orb_check(sp_man_sub, &updated); @@ -1079,6 +1085,7 @@ int commander_thread_main(int argc, char *argv[]) } telemetry_last_heartbeat[i] = telemetry.heartbeat_time; + telemetry_sysid[i] = telemetry.system_id; } } @@ -1562,10 +1569,17 @@ int commander_thread_main(int argc, char *argv[]) mavlink_log_critical(mavlink_fd, "data link %i regained", i); telemetry_lost[i] = false; - have_link = true; - } else if (!telemetry_lost[i]) { + + /* If this is not an onboard link/onboard computer: + * set flag that we have a valid link */ + if (telemetry_sysid[i] != onboard_sysid) { + have_link = true; + } + } else if (!telemetry_lost[i] && telemetry_sysid[i] != onboard_sysid) { /* telemetry was healthy also in last iteration - * we don't have to check a timeout */ + * we don't have to check a timeout, + * telemetry from onboard computers is not accepted as a valid datalink + */ have_link = true; } diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index 30159dad9..98c0982b2 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -128,3 +128,13 @@ PARAM_DEFINE_INT32(COM_DL_LOSS_T, 10); * @max 30 */ PARAM_DEFINE_INT32(COM_DL_REG_T, 0); + +/** Onboard computer system id + * + * The system id of the onboard computer. Heartbeats from this system are ignored during the datalink check + * + * @group commander + * @min 0 + * @max 255 + */ +PARAM_DEFINE_INT32(COM_ONBSYSID, 42); -- cgit v1.2.3 From 8c2a158e121e0a716a4b2d802687b0314c0217ba Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 3 Sep 2014 12:15:27 +0200 Subject: Trust the laser sensor more --- src/modules/ekf_att_pos_estimator/estimator_23states.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/ekf_att_pos_estimator/estimator_23states.h b/src/modules/ekf_att_pos_estimator/estimator_23states.h index 8ba1c1573..a607955a8 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_23states.h +++ b/src/modules/ekf_att_pos_estimator/estimator_23states.h @@ -85,7 +85,7 @@ public: R_LOS = 0.03f; // optical flow measurement noise variance (rad/sec)^2 flowInnovGate = 3.0f; // number of standard deviations applied to the optical flow innovation consistency check auxFlowInnovGate = 10.0f; // number of standard deviations applied to the optical flow innovation consistency check used by the auxiliary filter - rngInnovGate = 5.0f; // number of standard deviations applied to the rnage finder innovation consistency check + rngInnovGate = 10.0f; // number of standard deviations applied to the rnage finder innovation consistency check minFlowRng = 0.01f; //minimum range between ground and flow sensor moCompR_LOS = 0.2; // scaler from sensor gyro rate to uncertainty in LOS rate } -- 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(-) 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 7dfc67d032220c8c679848c684703e13e8d13080 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 3 Sep 2014 17:52:24 +0200 Subject: fix mission item to mavink waypoint conversion --- src/modules/mavlink/mavlink_mission.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp index d436c95e9..a3c127cdc 100644 --- a/src/modules/mavlink/mavlink_mission.cpp +++ b/src/modules/mavlink/mavlink_mission.cpp @@ -798,10 +798,10 @@ int MavlinkMissionManager::format_mavlink_mission_item(const struct mission_item_s *mission_item, mavlink_mission_item_t *mavlink_mission_item) { if (mission_item->altitude_is_relative) { - mavlink_mission_item->frame = MAV_FRAME_GLOBAL; + mavlink_mission_item->frame = MAV_FRAME_GLOBAL_RELATIVE_ALT; } else { - mavlink_mission_item->frame = MAV_FRAME_GLOBAL_RELATIVE_ALT; + mavlink_mission_item->frame = MAV_FRAME_GLOBAL; } switch (mission_item->nav_cmd) { -- 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(+) 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 75e9497ab1fea94985b4e64b02069343e16821b6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 3 Sep 2014 18:15:30 +0200 Subject: Fix FX79 mixer --- ROMFS/px4fmu_common/mixers/FMU_FX79.mix | 13 +++++-------- 1 file changed, 5 insertions(+), 8 deletions(-) diff --git a/ROMFS/px4fmu_common/mixers/FMU_FX79.mix b/ROMFS/px4fmu_common/mixers/FMU_FX79.mix index 112d9b891..b8879af9e 100755 --- a/ROMFS/px4fmu_common/mixers/FMU_FX79.mix +++ b/ROMFS/px4fmu_common/mixers/FMU_FX79.mix @@ -52,21 +52,18 @@ M: 1 O: 10000 10000 0 -10000 10000 S: 0 3 0 20000 -10000 -10000 10000 -Gimbal / flaps / payload mixer for last four channels +Inputs to the mixer come from channel group 2 (payload), channels 0 +(bay servo 1), 1 (bay servo 2) and 3 (drop release). ----------------------------------------------------- M: 1 O: 10000 10000 0 -10000 10000 -S: 0 4 10000 10000 0 -10000 10000 +S: 2 0 10000 10000 0 -10000 10000 M: 1 O: 10000 10000 0 -10000 10000 -S: 0 5 10000 10000 0 -10000 10000 +S: 2 1 10000 10000 0 -10000 10000 M: 1 O: 10000 10000 0 -10000 10000 -S: 0 6 10000 10000 0 -10000 10000 - -M: 1 -O: 10000 10000 0 -10000 10000 -S: 0 7 10000 10000 0 -10000 10000 +S: 2 2 -10000 -10000 0 -10000 10000 -- 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(-) 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 1d038eed047358eee30513673bbcb29bf93dd78e Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Wed, 3 Sep 2014 22:44:16 +0400 Subject: UAVCAN: GNSS fix message update --- src/modules/uavcan/sensors/gnss.cpp | 8 +++----- uavcan | 2 +- 2 files changed, 4 insertions(+), 6 deletions(-) diff --git a/src/modules/uavcan/sensors/gnss.cpp b/src/modules/uavcan/sensors/gnss.cpp index 0d67aad47..24afe6aaf 100644 --- a/src/modules/uavcan/sensors/gnss.cpp +++ b/src/modules/uavcan/sensors/gnss.cpp @@ -43,8 +43,6 @@ #include #include -#define MM_PER_CM 10 // Millimeters per centimeter - const char *const UavcanGnssBridge::NAME = "gnss"; UavcanGnssBridge::UavcanGnssBridge(uavcan::INode &node) : @@ -95,9 +93,9 @@ void UavcanGnssBridge::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure Date: Wed, 3 Sep 2014 21:18:32 +0200 Subject: FOH mode for altitude This introduces a parameter for the navigator. When enabled the navigator publishes a first order old (FOH) type altitude setpoint instead of the default zero order hold. For takeoff and landing the FOH mode is not active. The FOH altitude is calculated such that the sp reaches the altitude of the waypoint when the system is at a horizontal distance equal to the acceptance radius. Also the altitude setpoint will only converge towards the waypoint altitude but never diverge. --- src/modules/navigator/mission.cpp | 86 +++++++++++++++++++++++++++++++++- src/modules/navigator/mission.h | 25 +++++++++- src/modules/navigator/mission_params.c | 13 +++++ 3 files changed, 121 insertions(+), 3 deletions(-) diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index c0e37a3ed..7000dd6c0 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -36,12 +36,14 @@ * Helper class to access missions * * @author Julian Oes + * @author Thomas Gubler */ #include #include #include #include +#include #include @@ -49,6 +51,7 @@ #include #include #include +#include #include #include @@ -62,6 +65,7 @@ Mission::Mission(Navigator *navigator, const char *name) : _param_onboard_enabled(this, "ONBOARD_EN"), _param_takeoff_alt(this, "TAKEOFF_ALT"), _param_dist_1wp(this, "DIST_1WP"), + _param_altmode(this, "ALTMODE"), _onboard_mission({0}), _offboard_mission({0}), _current_onboard_mission_index(-1), @@ -72,7 +76,11 @@ Mission::Mission(Navigator *navigator, const char *name) : _mission_result({0}), _mission_type(MISSION_TYPE_NONE), _inited(false), - _dist_1wp_ok(false) + _dist_1wp_ok(false), + _missionFeasiblityChecker(), + _min_current_sp_distance_xy(FLT_MAX), + _mission_item_previous_alt(NAN), + _distance_current_previous(0.0f) { /* load initial params */ updateParams(); @@ -144,6 +152,8 @@ Mission::on_active() advance_mission(); set_mission_items(); + } else if (_mission_type != MISSION_TYPE_NONE &&_param_altmode.get() == MISSION_ALTMODE_FOH) { + altitude_sp_foh_update(); } else { /* if waypoint position reached allow loiter on the setpoint */ if (_waypoint_position_reached && _mission_item.nav_cmd != NAV_CMD_IDLE) { @@ -202,7 +212,7 @@ Mission::update_offboard_mission() * however warnings are issued to the gcs via mavlink from inside the MissionFeasiblityChecker */ dm_item_t dm_current = DM_KEY_WAYPOINTS_OFFBOARD(_offboard_mission.dataman_id); - missionFeasiblityChecker.checkMissionFeasible(_navigator->get_vstatus()->is_rotary_wing, + _missionFeasiblityChecker.checkMissionFeasible(_navigator->get_vstatus()->is_rotary_wing, dm_current, (size_t) _offboard_mission.count, _navigator->get_geofence(), _navigator->get_home_position()->alt); @@ -313,11 +323,19 @@ Mission::set_mission_items() /* make sure param is up to date */ updateParams(); + /* reset the altitude foh logic, if altitude foh is enabled (param) a new foh element starts now */ + altitude_sp_foh_reset(); + struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); /* set previous position setpoint to current */ set_previous_pos_setpoint(); + /* Copy previous mission item altitude (can be extended to a copy of the full mission item if needed) */ + if (pos_sp_triplet->previous.valid) { + _mission_item_previous_alt = _mission_item.altitude; + } + /* get home distance state */ bool home_dist_ok = check_dist_1wp(); /* the home dist check provides user feedback, so we initialize it to this */ @@ -446,9 +464,73 @@ Mission::set_mission_items() } } + /* Save the distance between the current sp and the previous one */ + if (pos_sp_triplet->current.valid && pos_sp_triplet->previous.valid) { + _distance_current_previous = get_distance_to_next_waypoint(pos_sp_triplet->current.lat, + pos_sp_triplet->current.lon, + pos_sp_triplet->previous.lat, + pos_sp_triplet->previous.lon); + } + + _navigator->set_position_setpoint_triplet_updated(); +} + +void +Mission::altitude_sp_foh_update() +{ + struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); + + /* Don't change setpoint if last and current waypoint are not valid */ + if (!pos_sp_triplet->previous.valid || !pos_sp_triplet->current.valid || + !isfinite(_mission_item_previous_alt)) { + return; + } + + /* Do not try to find a solution if the last waypoint is inside the acceptance radius of the current one */ + if (_distance_current_previous - _mission_item.acceptance_radius < 0.0f) { + return; + } + + /* Don't do FOH for landing and takeoff waypoints, the ground may be near + * and the FW controller has a custom landing logic */ + if (_mission_item.nav_cmd == NAV_CMD_LAND || _mission_item.nav_cmd == NAV_CMD_TAKEOFF) { + return; + } + + + /* Calculate distance to current waypoint */ + float d_current = get_distance_to_next_waypoint(_mission_item.lat, _mission_item.lon, + _navigator->get_global_position()->lat, _navigator->get_global_position()->lon); + + /* Save distance to waypoint if it is the smallest ever achieved */ + _min_current_sp_distance_xy = math::min(d_current, _min_current_sp_distance_xy); + + /* if the minimal distance is smaller then the acceptance radius, we should be at waypoint alt + * navigator will soon switch to the next waypoint item (if there is one) as soon as we reach this altitude */ + if (_min_current_sp_distance_xy < _mission_item.acceptance_radius) { + pos_sp_triplet->current.alt = _mission_item.altitude; + } else { + /* update the altitude sp of the 'current' item in the sp triplet, but do not update the altitude sp + * of the mission item as it is used to check if the mission item is reached + * The setpoint is set linearly and such that the system reaches the current altitude at the acceptance + * radius around the current waypoint + **/ + float delta_alt = (_mission_item.altitude - _mission_item_previous_alt); + float grad = -delta_alt/(_distance_current_previous - _mission_item.acceptance_radius); + float a = _mission_item_previous_alt - grad * _distance_current_previous; + pos_sp_triplet->current.alt = a + grad * _min_current_sp_distance_xy; + + } + _navigator->set_position_setpoint_triplet_updated(); } +void +Mission::altitude_sp_foh_reset() +{ + _min_current_sp_distance_xy = FLT_MAX; +} + bool Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s *mission_item) { diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h index 4da6a1155..f7d12da70 100644 --- a/src/modules/navigator/mission.h +++ b/src/modules/navigator/mission.h @@ -36,6 +36,7 @@ * Navigator mode to access missions * * @author Julian Oes + * @author Thomas Gubler */ #ifndef NAVIGATOR_MISSION_H @@ -75,6 +76,11 @@ public: virtual void on_active(); + enum mission_altitude_mode { + MISSION_ALTMODE_ZOH = 0, + MISSION_ALTMODE_FOH =1 + }; + private: /** * Update onboard mission topic @@ -102,6 +108,16 @@ private: */ void set_mission_items(); + /** + * Updates the altitude sp to follow a foh + */ + void altitude_sp_foh_update(); + + /** + * Resets the altitude sp foh logic + */ + void altitude_sp_foh_reset(); + /** * Read current or next mission item from the dataman and watch out for DO_JUMPS * @return true if successful @@ -136,6 +152,7 @@ private: control::BlockParamInt _param_onboard_enabled; control::BlockParamFloat _param_takeoff_alt; control::BlockParamFloat _param_dist_1wp; + control::BlockParamInt _param_altmode; struct mission_s _onboard_mission; struct mission_s _offboard_mission; @@ -157,7 +174,13 @@ private: bool _inited; bool _dist_1wp_ok; - MissionFeasibilityChecker missionFeasiblityChecker; /**< class that checks if a mission is feasible */ + MissionFeasibilityChecker _missionFeasiblityChecker; /**< class that checks if a mission is feasible */ + + float _min_current_sp_distance_xy; /**< minimum distance which was achieved to the current waypoint */ + float _mission_item_previous_alt; /**< holds the altitude of the previous mission item, + can be replaced by a full copy of the previous mission item if needed*/ + float _distance_current_previous; /**< distance from previous to current sp in pos_sp_triplet, + only use if current and previous are valid */ }; #endif diff --git a/src/modules/navigator/mission_params.c b/src/modules/navigator/mission_params.c index d15e1e771..04c01fe51 100644 --- a/src/modules/navigator/mission_params.c +++ b/src/modules/navigator/mission_params.c @@ -82,3 +82,16 @@ PARAM_DEFINE_INT32(MIS_ONBOARD_EN, 1); * @group Mission */ PARAM_DEFINE_FLOAT(MIS_DIST_1WP, 500); + +/** + * Altitude setpoint mode + * + * 0: the system will follow a zero order hold altitude setpoint + * 1: the system will follow a first order hold altitude setpoint + * values follow the definition in enum mission_altitude_mode + * + * @min 0 + * @max 1 + * @group Mission + */ +PARAM_DEFINE_INT32(MIS_ALTMODE, 0); -- cgit v1.2.3 From 9cf5e6f7b268acd732a14e577752dc32aeaf9430 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 3 Sep 2014 22:08:03 +0200 Subject: whitespace --- src/modules/navigator/mission.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h index f7d12da70..a91c89968 100644 --- a/src/modules/navigator/mission.h +++ b/src/modules/navigator/mission.h @@ -78,7 +78,7 @@ public: enum mission_altitude_mode { MISSION_ALTMODE_ZOH = 0, - MISSION_ALTMODE_FOH =1 + MISSION_ALTMODE_FOH = 1 }; private: -- 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(-) 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(-) 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 67422c9896fb952ccf6db555aeb6f99224f8178b Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 4 Sep 2014 11:41:13 +0200 Subject: foh alt mode: never sink below previous wp alt --- src/modules/navigator/mission.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 7000dd6c0..b33b2049f 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -502,8 +502,10 @@ Mission::altitude_sp_foh_update() float d_current = get_distance_to_next_waypoint(_mission_item.lat, _mission_item.lon, _navigator->get_global_position()->lat, _navigator->get_global_position()->lon); - /* Save distance to waypoint if it is the smallest ever achieved */ - _min_current_sp_distance_xy = math::min(d_current, _min_current_sp_distance_xy); + /* Save distance to waypoint if it is the smallest ever achieved, however make sure that + * _min_current_sp_distance_xy is never larger than the distance between the current and the previous wp */ + _min_current_sp_distance_xy = math::min(math::min(d_current, _min_current_sp_distance_xy), + _distance_current_previous); /* if the minimal distance is smaller then the acceptance radius, we should be at waypoint alt * navigator will soon switch to the next waypoint item (if there is one) as soon as we reach this altitude */ -- 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(-) 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 2d3b6a88dee5220c1b35d6e32f264217da8e36b8 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 4 Sep 2014 17:18:06 +0200 Subject: swissfang: don't build MC apps on FMU1 --- makefiles/config_px4fmu-v1_default.mk | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk index 97eddfdd2..99c0eb8c1 100644 --- a/makefiles/config_px4fmu-v1_default.mk +++ b/makefiles/config_px4fmu-v1_default.mk @@ -67,17 +67,17 @@ MODULES += modules/gpio_led # # Estimation modules (EKF / other filters) # -MODULES += modules/attitude_estimator_ekf +#MODULES += modules/attitude_estimator_ekf MODULES += modules/ekf_att_pos_estimator -MODULES += modules/position_estimator_inav +#MODULES += modules/position_estimator_inav # # Vehicle Control # MODULES += modules/fw_pos_control_l1 MODULES += modules/fw_att_control -MODULES += modules/mc_att_control -MODULES += modules/mc_pos_control +#MODULES += modules/mc_att_control +#MODULES += modules/mc_pos_control # # Logging -- 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(-) 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(-) 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(+) 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 4c7d6707936718760639ee60b3c27698f0ca119c 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(+) 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 81837dcdde4bdef31fc153cd1f893e2fefc74c58 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(-) 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 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(-) 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(-) 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 f315be54162c3284742cd2195d9c78c153289e66 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 6 Sep 2014 10:36:51 +0200 Subject: FW: in seatbelt/althold on ground reset integrators --- src/modules/fw_att_control/fw_att_control_main.cpp | 33 ++++++++++++++++++++++ 1 file changed, 33 insertions(+) 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 ad873203e..c60d8d348 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -63,6 +63,7 @@ #include #include #include +#include #include #include #include @@ -124,6 +125,7 @@ private: int _params_sub; /**< notification of parameter updates */ int _manual_sub; /**< notification of manual control updates */ int _global_pos_sub; /**< global position subscription */ + int _vehicle_status_sub; /**< vehicle status subscription */ orb_advert_t _rate_sp_pub; /**< rate setpoint publication */ orb_advert_t _attitude_sp_pub; /**< attitude setpoint point */ @@ -139,6 +141,7 @@ private: struct actuator_controls_s _actuators; /**< actuator control inputs */ struct actuator_controls_s _actuators_airframe; /**< actuator control inputs */ struct vehicle_global_position_s _global_pos; /**< global position */ + struct vehicle_status_s _vehicle_status; /**< vehicle status */ perf_counter_t _loop_perf; /**< loop performance counter */ perf_counter_t _nonfinite_input_perf; /**< performance counter for non finite input */ @@ -275,6 +278,11 @@ private: */ void global_pos_poll(); + /** + * Check for vehicle status updates. + */ + void vehicle_status_poll(); + /** * Shim for calling task_main from task_create. */ @@ -313,6 +321,7 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : _params_sub(-1), _manual_sub(-1), _global_pos_sub(-1), + _vehicle_status_sub(-1), /* publications */ _rate_sp_pub(-1), @@ -338,6 +347,7 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : _actuators = {}; _actuators_airframe = {}; _global_pos = {}; + _vehicle_status = {}; _parameter_handles.tconst = param_find("FW_ATT_TC"); @@ -560,6 +570,18 @@ FixedwingAttitudeControl::global_pos_poll() } } +void +FixedwingAttitudeControl::vehicle_status_poll() +{ + /* check if there is new status information */ + bool vehicle_status_updated; + orb_check(_vehicle_status_sub, &vehicle_status_updated); + + if (vehicle_status_updated) { + orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &_vehicle_status); + } +} + void FixedwingAttitudeControl::task_main_trampoline(int argc, char *argv[]) { @@ -585,6 +607,7 @@ FixedwingAttitudeControl::task_main() _params_sub = orb_subscribe(ORB_ID(parameter_update)); _manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); _global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); + _vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status)); /* rate limit vehicle status updates to 5Hz */ orb_set_interval(_vcontrol_mode_sub, 200); @@ -599,6 +622,7 @@ FixedwingAttitudeControl::task_main() vehicle_accel_poll(); vehicle_control_mode_poll(); vehicle_manual_poll(); + vehicle_status_poll(); /* wakeup source(s) */ struct pollfd fds[2]; @@ -667,6 +691,8 @@ FixedwingAttitudeControl::task_main() global_pos_poll(); + vehicle_status_poll(); + /* lock integrator until control is started */ bool lock_integrator; @@ -779,6 +805,13 @@ FixedwingAttitudeControl::task_main() } } + /* If the aircraft is on ground reset the integrators */ + if (_vehicle_status.condition_landed) { + _roll_ctrl.reset_integrator(); + _pitch_ctrl.reset_integrator(); + _yaw_ctrl.reset_integrator(); + } + /* Prepare speed_body_u and speed_body_w */ float speed_body_u = 0.0f; float speed_body_v = 0.0f; -- 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(-) 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(-) 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(-) 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(-) 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 e056fbb7038e4274fc54f4e19e66c8174ffff28f 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(-) 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 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(-) 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 cf601c09bf14ac4141bc66f2f71934fcf65da2a6 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 4 Sep 2014 17:18:06 +0200 Subject: swissfang: don't build MC apps on FMU1 --- makefiles/config_px4fmu-v1_default.mk | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk index 97eddfdd2..99c0eb8c1 100644 --- a/makefiles/config_px4fmu-v1_default.mk +++ b/makefiles/config_px4fmu-v1_default.mk @@ -67,17 +67,17 @@ MODULES += modules/gpio_led # # Estimation modules (EKF / other filters) # -MODULES += modules/attitude_estimator_ekf +#MODULES += modules/attitude_estimator_ekf MODULES += modules/ekf_att_pos_estimator -MODULES += modules/position_estimator_inav +#MODULES += modules/position_estimator_inav # # Vehicle Control # MODULES += modules/fw_pos_control_l1 MODULES += modules/fw_att_control -MODULES += modules/mc_att_control -MODULES += modules/mc_pos_control +#MODULES += modules/mc_att_control +#MODULES += modules/mc_pos_control # # Logging -- 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(-) 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(-) 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(-) 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(-) 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(-) 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(-) 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 daf16184202b02119aa1ac83cb82cf85979d43f9 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 8 Sep 2014 12:42:42 +0200 Subject: additional upper pitch limit during launch The pitch limit can be used by the laucnhdetector to limit pitch during critical phases of a launch. For example this can be used to limit pitch while attached to a bungee differently from the standard pitch limit. --- src/lib/launchdetection/CatapultLaunchMethod.cpp | 13 ++++++++++++- src/lib/launchdetection/CatapultLaunchMethod.h | 4 ++++ src/lib/launchdetection/LaunchDetector.cpp | 20 ++++++++++++++++++++ src/lib/launchdetection/LaunchDetector.h | 3 +++ src/lib/launchdetection/LaunchMethod.h | 3 +++ src/lib/launchdetection/launchdetection_params.c | 16 +++++++++++++++- .../fw_pos_control_l1/fw_pos_control_l1_main.cpp | 12 +++++++++--- 7 files changed, 66 insertions(+), 5 deletions(-) diff --git a/src/lib/launchdetection/CatapultLaunchMethod.cpp b/src/lib/launchdetection/CatapultLaunchMethod.cpp index 65ae461db..2ea1c414b 100644 --- a/src/lib/launchdetection/CatapultLaunchMethod.cpp +++ b/src/lib/launchdetection/CatapultLaunchMethod.cpp @@ -52,7 +52,8 @@ CatapultLaunchMethod::CatapultLaunchMethod(SuperBlock *parent) : state(LAUNCHDETECTION_RES_NONE), thresholdAccel(this, "A"), thresholdTime(this, "T"), - motorDelay(this, "MDEL") + motorDelay(this, "MDEL"), + pitchMaxPreThrottle(this, "PMAX") { } @@ -118,4 +119,14 @@ void CatapultLaunchMethod::reset() state = LAUNCHDETECTION_RES_NONE; } +float CatapultLaunchMethod::getPitchMax(float pitchMaxDefault) { + /* If motor is turned on do not impose the extra limit on maximum pitch */ + if (state == LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS) { + return pitchMaxDefault; + } else { + return pitchMaxPreThrottle.get(); + } + +} + } diff --git a/src/lib/launchdetection/CatapultLaunchMethod.h b/src/lib/launchdetection/CatapultLaunchMethod.h index d918c3a76..321dfb1de 100644 --- a/src/lib/launchdetection/CatapultLaunchMethod.h +++ b/src/lib/launchdetection/CatapultLaunchMethod.h @@ -59,6 +59,7 @@ public: void update(float accel_x); LaunchDetectionResult getLaunchDetected() const; void reset(); + float getPitchMax(float pitchMaxDefault); private: hrt_abstime last_timestamp; @@ -70,6 +71,9 @@ private: control::BlockParamFloat thresholdAccel; control::BlockParamFloat thresholdTime; control::BlockParamFloat motorDelay; + control::BlockParamFloat pitchMaxPreThrottle; /**< Upper pitch limit before throttle is turned on. + Can be used to make sure that the AC does not climb + too much while attached to a bungee */ }; diff --git a/src/lib/launchdetection/LaunchDetector.cpp b/src/lib/launchdetection/LaunchDetector.cpp index 2958c0a31..52f5c3257 100644 --- a/src/lib/launchdetection/LaunchDetector.cpp +++ b/src/lib/launchdetection/LaunchDetector.cpp @@ -105,4 +105,24 @@ LaunchDetectionResult LaunchDetector::getLaunchDetected() return LAUNCHDETECTION_RES_NONE; } +float LaunchDetector::getPitchMax(float pitchMaxDefault) { + if (!launchdetection_on.get()) { + return pitchMaxDefault; + } + + /* if a lauchdetectionmethod is active or only one exists return the pitch limit from this method, + * otherwise use the default limit */ + if (activeLaunchDetectionMethodIndex < 0) { + if (sizeof(launchMethods)/sizeof(LaunchMethod) > 1) { + return pitchMaxDefault; + } else { + return launchMethods[0]->getPitchMax(pitchMaxDefault); + } + } else { + return launchMethods[activeLaunchDetectionMethodIndex]->getPitchMax(pitchMaxDefault); + } + +} + + } diff --git a/src/lib/launchdetection/LaunchDetector.h b/src/lib/launchdetection/LaunchDetector.h index b48e724ba..4215b49d2 100644 --- a/src/lib/launchdetection/LaunchDetector.h +++ b/src/lib/launchdetection/LaunchDetector.h @@ -64,6 +64,9 @@ public: float getThrottlePreTakeoff() {return throttlePreTakeoff.get(); } + /* Returns a maximum pitch in deg. Different launch methods may impose upper pitch limits during launch */ + float getPitchMax(float pitchMaxDefault); + // virtual bool getLaunchDetected(); protected: private: diff --git a/src/lib/launchdetection/LaunchMethod.h b/src/lib/launchdetection/LaunchMethod.h index d2f091cea..8b5220cb3 100644 --- a/src/lib/launchdetection/LaunchMethod.h +++ b/src/lib/launchdetection/LaunchMethod.h @@ -62,6 +62,9 @@ public: virtual LaunchDetectionResult getLaunchDetected() const = 0; virtual void reset() = 0; + /* Returns a upper pitch limit if required, otherwise returns pitchMaxDefault */ + virtual float getPitchMax(float pitchMaxDefault) = 0; + protected: private: }; diff --git a/src/lib/launchdetection/launchdetection_params.c b/src/lib/launchdetection/launchdetection_params.c index d35eb11f6..e3aa7ab2d 100644 --- a/src/lib/launchdetection/launchdetection_params.c +++ b/src/lib/launchdetection/launchdetection_params.c @@ -80,7 +80,7 @@ PARAM_DEFINE_FLOAT(LAUN_CAT_T, 0.05f); /** * Motor delay * - * Delay between starting attitude control and powering up the thorttle (giving throttle control to the controller) + * Delay between starting attitude control and powering up the throttle (giving throttle control to the controller) * Before this timespan is up the throttle will be set to LAUN_THR_PRE, set to 0 to deactivate * * @unit seconds @@ -88,6 +88,20 @@ PARAM_DEFINE_FLOAT(LAUN_CAT_T, 0.05f); * @group Launch detection */ PARAM_DEFINE_FLOAT(LAUN_CAT_MDEL, 0.0f); + +/** + * Maximum pitch before the throttle is powered up (during motor delay phase) + * + * This is an extra limit for the maximum pitch which is imposed in the phase before the throttle turns on. + * This allows to limit the maximum pitch angle during a bungee launch (make the launch less steep). + * + * @unit deg + * @min 0 + * @max 45 + * @group Launch detection + */ +PARAM_DEFINE_FLOAT(LAUN_CAT_PMAX, 30.0f); + /** * Throttle setting while detecting launch. * 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..402b171f5 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 @@ -1115,7 +1115,13 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS ? launchDetector.getThrottlePreTakeoff() : _parameters.throttle_max; - /* apply minimum pitch and limit roll if target altitude is not within 10 meters */ + /* select maximum pitch: the launchdetector may impose another limit for the pitch + * depending on the state of the launch */ + float takeoff_pitch_max_rad = math::radians( + launchDetector.getPitchMax(_parameters.pitch_limit_max)); + + /* apply minimum pitch and limit roll if target altitude is not within climbout_diff + * meters */ if (_parameters.climbout_diff > 0.001f && altitude_error > _parameters.climbout_diff) { /* enforce a minimum of 10 degrees pitch up on takeoff, or take parameter */ @@ -1123,7 +1129,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi calculate_target_airspeed(1.3f * _parameters.airspeed_min), eas2tas, math::radians(_parameters.pitch_limit_min), - math::radians(_parameters.pitch_limit_max), + takeoff_pitch_max_rad, _parameters.throttle_min, takeoff_throttle, _parameters.throttle_cruise, true, @@ -1199,7 +1205,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* Copy thrust and pitch values from tecs * making sure again that the correct thrust is used, - * without depending on library calls */ + * without depending on library calls for safety reasons */ if (pos_sp_triplet.current.type == SETPOINT_TYPE_TAKEOFF && launch_detection_state != LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS) { _att_sp.thrust = launchDetector.getThrottlePreTakeoff(); -- cgit v1.2.3 From df181455ebdcfacda73615adba40fa224b4d074c Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 8 Sep 2014 13:00:26 +0200 Subject: launch pitch limit: add mtecs interface --- .../fw_pos_control_l1/fw_pos_control_l1_main.cpp | 20 +++++++++++++++----- 1 file changed, 15 insertions(+), 5 deletions(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 402b171f5..81e25da99 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 @@ -381,7 +381,8 @@ private: bool climbout_mode, float climbout_pitch_min_rad, float altitude, const math::Vector<3> &ground_speed, - tecs_mode mode = TECS_MODE_NORMAL); + tecs_mode mode = TECS_MODE_NORMAL, + bool pitch_max_special = false); }; @@ -1117,8 +1118,8 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* select maximum pitch: the launchdetector may impose another limit for the pitch * depending on the state of the launch */ - float takeoff_pitch_max_rad = math::radians( - launchDetector.getPitchMax(_parameters.pitch_limit_max)); + float takeoff_pitch_max_deg = launchDetector.getPitchMax(_parameters.pitch_limit_max); + float takeoff_pitch_max_rad = math::radians(takeoff_pitch_max_deg); /* apply minimum pitch and limit roll if target altitude is not within climbout_diff * meters */ @@ -1137,7 +1138,8 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi math::radians(10.0f)), _global_pos.alt, ground_speed, - TECS_MODE_TAKEOFF); + TECS_MODE_TAKEOFF, + takeoff_pitch_max_deg != _parameters.pitch_limit_max); /* limit roll motion to ensure enough lift */ _att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-15.0f), @@ -1391,7 +1393,7 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_ bool climbout_mode, float climbout_pitch_min_rad, float altitude, const math::Vector<3> &ground_speed, - tecs_mode mode) + tecs_mode mode, bool pitch_max_special) { if (_mTecs.getEnabled()) { /* Using mtecs library: prepare arguments for mtecs call */ @@ -1406,6 +1408,14 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_ } else { limitOverride.disablePitchMinOverride(); } + + if (pitch_max_special) { + /* Use the maximum pitch from the argument */ + limitOverride.enablePitchMaxOverride(M_RAD_TO_DEG_F * pitch_max_rad); + } else { + /* use pitch max set by MT param */ + limitOverride.disablePitchMaxOverride(); + } _mTecs.updateAltitudeSpeed(flightPathAngle, altitude, alt_sp, _airspeed.true_airspeed_m_s, v_sp, mode, limitOverride); } else { -- cgit v1.2.3 From f4664daa8ed80e369c8600f2f1d1ee8d5be328af Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 9 Sep 2014 09:08:56 +0200 Subject: Add license file --- LICENSE.md | 41 +++++++++++++++++++++++++++++++++++++++++ 1 file changed, 41 insertions(+) create mode 100644 LICENSE.md diff --git a/LICENSE.md b/LICENSE.md new file mode 100644 index 000000000..2ad83eba4 --- /dev/null +++ b/LICENSE.md @@ -0,0 +1,41 @@ +The PX4 firmware is licensed generally under a permissive 3-clause BSD license. Contributions are required +to be made under the same license. Any exception to this general rule is listed below. + + /**************************************************************************** + * + * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + + + - PX4 middleware: BSD 3-clause + - PX4 flight control stack: BSD 3-clause + - NuttX operating system: BSD 3-clause + - Exceptions: Currently only this [400 LOC file](https://github.com/PX4/Firmware/blob/master/src/lib/external_lgpl/tecs/tecs.cpp) remains LGPL, but will be replaced with a BSD implementation. \ No newline at end of file -- cgit v1.2.3 From 35a6074419b3dcf567f23db74b8ea53eff20c9d6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 9 Sep 2014 09:12:38 +0200 Subject: Added readme --- README.md | 10 ++++++++++ 1 file changed, 10 insertions(+) create mode 100644 README.md diff --git a/README.md b/README.md new file mode 100644 index 000000000..84fb02e3b --- /dev/null +++ b/README.md @@ -0,0 +1,10 @@ +## PX4 Aerial Middleware and Flight Control Stack ## + +* Official Website: http://px4.io +* License: BSD 3-clause (see LICENSE.md) +* Supported airframes: + * Multicopters + * Fixed wing +* Binaries (always up-to-date from master): + * [Downloads](https://pixhawk.org/downloads) +* Mailing list: [Google Groups](http://groups.google.com/group/px4users) -- cgit v1.2.3 From 564c9b7b60aaa1187570e188fe7d1e19945d40b5 Mon Sep 17 00:00:00 2001 From: Vladimir Ermakov Date: Tue, 9 Sep 2014 17:02:25 +0400 Subject: FTP: Add req_opcode field for return request opcode in response message. --- src/modules/mavlink/mavlink_ftp.cpp | 3 ++- src/modules/mavlink/mavlink_ftp.h | 3 ++- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/src/modules/mavlink/mavlink_ftp.cpp b/src/modules/mavlink/mavlink_ftp.cpp index 72ede8797..ae9246ece 100644 --- a/src/modules/mavlink/mavlink_ftp.cpp +++ b/src/modules/mavlink/mavlink_ftp.cpp @@ -224,12 +224,14 @@ MavlinkFTP::_process_request(Request *req) out: // handle success vs. error if (errorCode == kErrNone) { + payload->req_opcode = payload->opcode; payload->opcode = kRspAck; #ifdef MAVLINK_FTP_DEBUG warnx("FTP: ack\n"); #endif } else { warnx("FTP: nak %u", errorCode); + payload->req_opcode = payload->opcode; payload->opcode = kRspNak; payload->size = 1; payload->data[0] = errorCode; @@ -654,7 +656,6 @@ MavlinkFTP::_payload_crc32(PayloadHeader *payload) payload->crc32 = 0; payload->padding[0] = 0; payload->padding[1] = 0; - payload->padding[2] = 0; uint32_t retCRC = crc32((const uint8_t*)payload, payload->size + sizeof(PayloadHeader)); payload->crc32 = saveCRC; diff --git a/src/modules/mavlink/mavlink_ftp.h b/src/modules/mavlink/mavlink_ftp.h index b4cc154d1..1fb50a17e 100644 --- a/src/modules/mavlink/mavlink_ftp.h +++ b/src/modules/mavlink/mavlink_ftp.h @@ -76,7 +76,8 @@ public: uint8_t session; ///< Session id for read and write commands uint8_t opcode; ///< Command opcode uint8_t size; ///< Size of data - uint8_t padding[3]; ///< 32 bit aligment padding + uint8_t req_opcode; ///< Request opcode returned in kRspAck, kRspNak message + uint8_t padding[2]; ///< 32 bit aligment padding uint32_t crc32; ///< CRC for entire Request structure, with crc32 and padding set to 0 uint32_t offset; ///< Offsets for List and Read commands uint8_t data[]; ///< command data, varies by Opcode -- cgit v1.2.3 From e7ae13a58e83263973feab3630f90f077786fcc3 Mon Sep 17 00:00:00 2001 From: Vladimir Ermakov Date: Tue, 9 Sep 2014 17:17:11 +0400 Subject: FTP: Make responses start from opcode 128. --- src/modules/mavlink/mavlink_ftp.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_ftp.h b/src/modules/mavlink/mavlink_ftp.h index 1fb50a17e..85e175d44 100644 --- a/src/modules/mavlink/mavlink_ftp.h +++ b/src/modules/mavlink/mavlink_ftp.h @@ -98,7 +98,7 @@ public: kCmdCreateDirectory, ///< Creates directory at kCmdRemoveDirectory, ///< Removes Directory at , must be empty - kRspAck, ///< Ack response + kRspAck = 128, ///< Ack response kRspNak ///< Nak response }; -- cgit v1.2.3 From 0d2e250d119a5a14c8757982c96b2afef09c4d0d Mon Sep 17 00:00:00 2001 From: Vladimir Ermakov Date: Tue, 9 Sep 2014 17:36:41 +0400 Subject: FTP: Remove CRC32 from protocol. Extra crc not needed because mavlink already has crc16. --- src/modules/mavlink/mavlink_ftp.cpp | 27 +++------------------------ src/modules/mavlink/mavlink_ftp.h | 8 ++------ 2 files changed, 5 insertions(+), 30 deletions(-) diff --git a/src/modules/mavlink/mavlink_ftp.cpp b/src/modules/mavlink/mavlink_ftp.cpp index ae9246ece..b847fc625 100644 --- a/src/modules/mavlink/mavlink_ftp.cpp +++ b/src/modules/mavlink/mavlink_ftp.cpp @@ -34,7 +34,6 @@ /// @file mavlink_ftp.cpp /// @author px4dev, Don Gagne -#include #include #include #include @@ -161,13 +160,6 @@ MavlinkFTP::_process_request(Request *req) goto out; } - // check request CRC to make sure this is one of ours - if (_payload_crc32(payload) != payload->crc32) { - errorCode = kErrCrc; - goto out; - warnx("ftp: bad crc"); - } - #ifdef MAVLINK_FTP_DEBUG printf("ftp: channel %u opc %u size %u offset %u\n", req->serverChannel, payload->opcode, payload->size, payload->offset); #endif @@ -255,8 +247,9 @@ MavlinkFTP::_reply(Request *req) PayloadHeader *payload = reinterpret_cast(&req->message.payload[0]); payload->seqNumber = payload->seqNumber + 1; - - payload->crc32 = _payload_crc32(payload); + payload->reserved[0] = 0; + payload->reserved[1] = 0; + payload->reserved[2] = 0; mavlink_message_t msg; msg.checksum = 0; @@ -647,17 +640,3 @@ MavlinkFTP::_return_request(Request *req) _unlock_request_queue(); } -/// @brief Returns the 32 bit CRC for the payload, crc32 and padding members are set to 0 for calculation. -uint32_t -MavlinkFTP::_payload_crc32(PayloadHeader *payload) -{ - // We calculate CRC with crc and padding set to 0. - uint32_t saveCRC = payload->crc32; - payload->crc32 = 0; - payload->padding[0] = 0; - payload->padding[1] = 0; - uint32_t retCRC = crc32((const uint8_t*)payload, payload->size + sizeof(PayloadHeader)); - payload->crc32 = saveCRC; - - return retCRC; -} diff --git a/src/modules/mavlink/mavlink_ftp.h b/src/modules/mavlink/mavlink_ftp.h index 85e175d44..ae7955ab7 100644 --- a/src/modules/mavlink/mavlink_ftp.h +++ b/src/modules/mavlink/mavlink_ftp.h @@ -77,8 +77,7 @@ public: uint8_t opcode; ///< Command opcode uint8_t size; ///< Size of data uint8_t req_opcode; ///< Request opcode returned in kRspAck, kRspNak message - uint8_t padding[2]; ///< 32 bit aligment padding - uint32_t crc32; ///< CRC for entire Request structure, with crc32 and padding set to 0 + uint16_t reserved[3]; ///< reserved area uint32_t offset; ///< Offsets for List and Read commands uint8_t data[]; ///< command data, varies by Opcode }; @@ -112,8 +111,7 @@ public: kErrInvalidSession, ///< Session is not currently open kErrNoSessionsAvailable, ///< All available Sessions in use kErrEOF, ///< Offset past end of file for List and Read commands - kErrUnknownCommand, ///< Unknown command opcode - kErrCrc ///< CRC on Payload is incorrect + kErrUnknownCommand ///< Unknown command opcode }; private: @@ -135,8 +133,6 @@ private: void _lock_request_queue(void); void _unlock_request_queue(void); - uint32_t _payload_crc32(PayloadHeader *hdr); - char *_data_as_cstring(PayloadHeader* payload); static void _worker_trampoline(void *arg); -- cgit v1.2.3 From 745707d19377e2c650258ea77570a5f84a71c1b7 Mon Sep 17 00:00:00 2001 From: Vladimir Ermakov Date: Wed, 10 Sep 2014 11:22:03 +0400 Subject: FTP: remove reserved uint32 from header. --- src/modules/mavlink/mavlink_ftp.cpp | 5 ++--- src/modules/mavlink/mavlink_ftp.h | 2 +- 2 files changed, 3 insertions(+), 4 deletions(-) diff --git a/src/modules/mavlink/mavlink_ftp.cpp b/src/modules/mavlink/mavlink_ftp.cpp index b847fc625..a5c96274b 100644 --- a/src/modules/mavlink/mavlink_ftp.cpp +++ b/src/modules/mavlink/mavlink_ftp.cpp @@ -247,9 +247,8 @@ MavlinkFTP::_reply(Request *req) PayloadHeader *payload = reinterpret_cast(&req->message.payload[0]); payload->seqNumber = payload->seqNumber + 1; - payload->reserved[0] = 0; - payload->reserved[1] = 0; - payload->reserved[2] = 0; + payload->padding[0] = 0; + payload->padding[1] = 0; mavlink_message_t msg; msg.checksum = 0; diff --git a/src/modules/mavlink/mavlink_ftp.h b/src/modules/mavlink/mavlink_ftp.h index ae7955ab7..4892e548c 100644 --- a/src/modules/mavlink/mavlink_ftp.h +++ b/src/modules/mavlink/mavlink_ftp.h @@ -77,7 +77,7 @@ public: uint8_t opcode; ///< Command opcode uint8_t size; ///< Size of data uint8_t req_opcode; ///< Request opcode returned in kRspAck, kRspNak message - uint16_t reserved[3]; ///< reserved area + uint8_t padding[2]; ///< 32 bit aligment padding uint32_t offset; ///< Offsets for List and Read commands uint8_t data[]; ///< command data, varies by Opcode }; -- cgit v1.2.3 From ed66097ebc4808587301bd569b5bef6c312800c4 Mon Sep 17 00:00:00 2001 From: Vladimir Ermakov Date: Wed, 10 Sep 2014 11:54:42 +0400 Subject: FTP: Update unit test for new header size. _list_test failed. --- .../unit_test_data/mavlink_tests/test_234.data | Bin 234 -> 0 bytes .../unit_test_data/mavlink_tests/test_235.data | Bin 235 -> 0 bytes .../unit_test_data/mavlink_tests/test_236.data | Bin 236 -> 0 bytes .../unit_test_data/mavlink_tests/test_238.data | Bin 0 -> 238 bytes .../unit_test_data/mavlink_tests/test_239.data | Bin 0 -> 239 bytes .../unit_test_data/mavlink_tests/test_240.data | Bin 0 -> 240 bytes .../mavlink/mavlink_tests/mavlink_ftp_test.cpp | 57 +++------------------ .../mavlink/mavlink_tests/mavlink_ftp_test.h | 2 - 8 files changed, 6 insertions(+), 53 deletions(-) delete mode 100644 ROMFS/px4fmu_test/unit_test_data/mavlink_tests/test_234.data delete mode 100644 ROMFS/px4fmu_test/unit_test_data/mavlink_tests/test_235.data delete mode 100644 ROMFS/px4fmu_test/unit_test_data/mavlink_tests/test_236.data create mode 100644 ROMFS/px4fmu_test/unit_test_data/mavlink_tests/test_238.data create mode 100644 ROMFS/px4fmu_test/unit_test_data/mavlink_tests/test_239.data create mode 100644 ROMFS/px4fmu_test/unit_test_data/mavlink_tests/test_240.data diff --git a/ROMFS/px4fmu_test/unit_test_data/mavlink_tests/test_234.data b/ROMFS/px4fmu_test/unit_test_data/mavlink_tests/test_234.data deleted file mode 100644 index 3e075aa4f..000000000 Binary files a/ROMFS/px4fmu_test/unit_test_data/mavlink_tests/test_234.data and /dev/null differ diff --git a/ROMFS/px4fmu_test/unit_test_data/mavlink_tests/test_235.data b/ROMFS/px4fmu_test/unit_test_data/mavlink_tests/test_235.data deleted file mode 100644 index 61372f769..000000000 Binary files a/ROMFS/px4fmu_test/unit_test_data/mavlink_tests/test_235.data and /dev/null differ diff --git a/ROMFS/px4fmu_test/unit_test_data/mavlink_tests/test_236.data b/ROMFS/px4fmu_test/unit_test_data/mavlink_tests/test_236.data deleted file mode 100644 index eb7fcb11a..000000000 Binary files a/ROMFS/px4fmu_test/unit_test_data/mavlink_tests/test_236.data and /dev/null differ diff --git a/ROMFS/px4fmu_test/unit_test_data/mavlink_tests/test_238.data b/ROMFS/px4fmu_test/unit_test_data/mavlink_tests/test_238.data new file mode 100644 index 000000000..973de16e5 Binary files /dev/null and b/ROMFS/px4fmu_test/unit_test_data/mavlink_tests/test_238.data differ diff --git a/ROMFS/px4fmu_test/unit_test_data/mavlink_tests/test_239.data b/ROMFS/px4fmu_test/unit_test_data/mavlink_tests/test_239.data new file mode 100644 index 000000000..7e8764c03 Binary files /dev/null and b/ROMFS/px4fmu_test/unit_test_data/mavlink_tests/test_239.data differ diff --git a/ROMFS/px4fmu_test/unit_test_data/mavlink_tests/test_240.data b/ROMFS/px4fmu_test/unit_test_data/mavlink_tests/test_240.data new file mode 100644 index 000000000..f3fe31687 Binary files /dev/null and b/ROMFS/px4fmu_test/unit_test_data/mavlink_tests/test_240.data differ diff --git a/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.cpp b/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.cpp index ebfce6d74..3616db237 100644 --- a/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.cpp +++ b/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.cpp @@ -44,9 +44,9 @@ /// @brief Test case file name for Read command. File are generated using mavlink_ftp_test_data.py const MavlinkFtpTest::ReadTestCase MavlinkFtpTest::_rgReadTestCases[] = { - { "/etc/unit_test_data/mavlink_tests/test_234.data", MAVLINK_MSG_FILE_TRANSFER_PROTOCOL_FIELD_PAYLOAD_LEN - sizeof(MavlinkFTP::PayloadHeader) - 1}, // Read takes less than single packet - { "/etc/unit_test_data/mavlink_tests/test_235.data", MAVLINK_MSG_FILE_TRANSFER_PROTOCOL_FIELD_PAYLOAD_LEN - sizeof(MavlinkFTP::PayloadHeader) }, // Read completely fills single packet - { "/etc/unit_test_data/mavlink_tests/test_236.data", MAVLINK_MSG_FILE_TRANSFER_PROTOCOL_FIELD_PAYLOAD_LEN - sizeof(MavlinkFTP::PayloadHeader) + 1 }, // Read take two packets + { "/etc/unit_test_data/mavlink_tests/test_238.data", MAVLINK_MSG_FILE_TRANSFER_PROTOCOL_FIELD_PAYLOAD_LEN - sizeof(MavlinkFTP::PayloadHeader) - 1}, // Read takes less than single packet + { "/etc/unit_test_data/mavlink_tests/test_239.data", MAVLINK_MSG_FILE_TRANSFER_PROTOCOL_FIELD_PAYLOAD_LEN - sizeof(MavlinkFTP::PayloadHeader) }, // Read completely fills single packet + { "/etc/unit_test_data/mavlink_tests/test_240.data", MAVLINK_MSG_FILE_TRANSFER_PROTOCOL_FIELD_PAYLOAD_LEN - sizeof(MavlinkFTP::PayloadHeader) + 1 }, // Read take two packets }; const char MavlinkFtpTest::_unittest_microsd_dir[] = "/fs/microsd/ftp_unit_test_dir"; @@ -105,33 +105,6 @@ bool MavlinkFtpTest::_ack_test(void) return true; } -/// @brief Tests for correct response to a message sent with an invalid CRC. -bool MavlinkFtpTest::_bad_crc_test(void) -{ - mavlink_message_t msg; - MavlinkFTP::PayloadHeader payload; - mavlink_file_transfer_protocol_t ftp_msg; - MavlinkFTP::PayloadHeader *reply; - - payload.opcode = MavlinkFTP::kCmdNone; - - _setup_ftp_msg(&payload, 0, nullptr, &msg); - - ((MavlinkFTP::PayloadHeader*)((mavlink_file_transfer_protocol_t*)msg.payload64)->payload)->crc32++; - - _ftp_server->handle_message(nullptr /* mavlink */, &msg); - - if (!_decode_message(&_reply_msg, &ftp_msg, &reply)) { - return false; - } - - ut_compare("Didn't get Nak back", reply->opcode, MavlinkFTP::kRspNak); - ut_compare("Incorrect payload size", reply->size, 1); - ut_compare("Incorrect error code", reply->data[0], MavlinkFTP::kErrCrc); - - return true; -} - /// @brief Tests for correct response to an invalid opcpde. bool MavlinkFtpTest::_bad_opcode_test(void) { @@ -191,7 +164,7 @@ bool MavlinkFtpTest::_list_test(void) mavlink_file_transfer_protocol_t ftp_msg; MavlinkFTP::PayloadHeader *reply; - char response1[] = "D.|Dempty_dir|Ftest_234.data\t234|Ftest_235.data\t235|Ftest_236.data\t236"; + char response1[] = "D.|Dempty_dir|Ftest_238.data\t238|Ftest_239.data\t239|Ftest_240.data\t240"; char response2[] = "Ddev|Detc|Dfs|Dobj"; struct _testCase { @@ -690,9 +663,6 @@ bool MavlinkFtpTest::_decode_message(const mavlink_message_t *msg, ///< Mavlin *payload = reinterpret_cast(ftp_msg->payload); - // Make sure we have a good CRC - ut_compare("Incoming CRC mismatch", (*payload)->crc32, _payload_crc32((*payload))); - // Make sure we have a good sequence number ut_compare("Sequence number mismatch", (*payload)->seqNumber, _lastOutgoingSeqNumber + 1); @@ -702,21 +672,6 @@ bool MavlinkFtpTest::_decode_message(const mavlink_message_t *msg, ///< Mavlin return true; } -/// @brief Returns the 32 bit CRC for the payload, crc32 and padding are set to 0 for calculation. -uint32_t MavlinkFtpTest::_payload_crc32(struct MavlinkFTP::PayloadHeader *payload) -{ - // We calculate CRC with crc and padding set to 0. - uint32_t saveCRC = payload->crc32; - payload->crc32 = 0; - payload->padding[0] = 0; - payload->padding[1] = 0; - payload->padding[2] = 0; - uint32_t retCRC = crc32((const uint8_t*)payload, payload->size + sizeof(MavlinkFTP::PayloadHeader)); - payload->crc32 = saveCRC; - - return retCRC; -} - /// @brief Initializes an FTP message into a mavlink message void MavlinkFtpTest::_setup_ftp_msg(MavlinkFTP::PayloadHeader *payload_header, ///< FTP payload header uint8_t size, ///< size in bytes of data @@ -734,7 +689,8 @@ void MavlinkFtpTest::_setup_ftp_msg(MavlinkFTP::PayloadHeader *payload_header, / memcpy(payload->data, data, size); } - payload->crc32 = _payload_crc32(payload); + payload->padding[0] = 0; + payload->padding[1] = 0; msg->checksum = 0; mavlink_msg_file_transfer_protocol_pack(clientSystemId, // Sender system id @@ -771,7 +727,6 @@ void MavlinkFtpTest::_cleanup_microsd(void) void MavlinkFtpTest::runTests(void) { ut_run_test(_ack_test); - ut_run_test(_bad_crc_test); ut_run_test(_bad_opcode_test); ut_run_test(_bad_datasize_test); ut_run_test(_list_test); diff --git a/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.h b/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.h index bbb095a08..babd909da 100644 --- a/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.h +++ b/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.h @@ -66,7 +66,6 @@ public: private: bool _ack_test(void); - bool _bad_crc_test(void); bool _bad_opcode_test(void); bool _bad_datasize_test(void); bool _list_test(void); @@ -81,7 +80,6 @@ private: bool _removefile_test(void); void _receive_message(const mavlink_message_t *msg); - uint32_t _payload_crc32(struct MavlinkFTP::PayloadHeader *payload); void _setup_ftp_msg(MavlinkFTP::PayloadHeader *payload_header, uint8_t size, const uint8_t *data, mavlink_message_t *msg); bool _decode_message(const mavlink_message_t *msg, mavlink_file_transfer_protocol_t *ftp_msg, MavlinkFTP::PayloadHeader **payload); bool _send_receive_msg(MavlinkFTP::PayloadHeader *payload_header, -- cgit v1.2.3 From 0e3c6060b6375fa08b6c0087f7aaf2905ea516d0 Mon Sep 17 00:00:00 2001 From: Vladimir Ermakov Date: Wed, 10 Sep 2014 21:36:08 +0400 Subject: FTP: remove padding zeroing. --- src/modules/mavlink/mavlink_ftp.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/modules/mavlink/mavlink_ftp.cpp b/src/modules/mavlink/mavlink_ftp.cpp index a5c96274b..64206ac54 100644 --- a/src/modules/mavlink/mavlink_ftp.cpp +++ b/src/modules/mavlink/mavlink_ftp.cpp @@ -247,8 +247,6 @@ MavlinkFTP::_reply(Request *req) PayloadHeader *payload = reinterpret_cast(&req->message.payload[0]); payload->seqNumber = payload->seqNumber + 1; - payload->padding[0] = 0; - payload->padding[1] = 0; mavlink_message_t msg; msg.checksum = 0; -- cgit v1.2.3 From 981741f8cea95eebf34577935571260be917abc8 Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Wed, 10 Sep 2014 13:11:36 -0700 Subject: Don't send back U or ./.. entries from List command --- src/modules/mavlink/mavlink_ftp.cpp | 8 ++++++-- src/modules/mavlink/mavlink_ftp.h | 1 - 2 files changed, 6 insertions(+), 3 deletions(-) diff --git a/src/modules/mavlink/mavlink_ftp.cpp b/src/modules/mavlink/mavlink_ftp.cpp index 64206ac54..cf5fae3b3 100644 --- a/src/modules/mavlink/mavlink_ftp.cpp +++ b/src/modules/mavlink/mavlink_ftp.cpp @@ -349,11 +349,15 @@ MavlinkFTP::_workList(PayloadHeader* payload) } break; case DTYPE_DIRECTORY: + if (strcmp(entry.d_name, ".") == 0 || strcmp(entry.d_name, "..") == 0) { + // Don't bother sending these back + continue; + } direntType = kDirentDir; break; default: - direntType = kDirentUnknown; - break; + // We only send back file and diretory entries, skip everything else + continue; } if (entry.d_type == DTYPE_FILE) { diff --git a/src/modules/mavlink/mavlink_ftp.h b/src/modules/mavlink/mavlink_ftp.h index 4892e548c..b52561e8a 100644 --- a/src/modules/mavlink/mavlink_ftp.h +++ b/src/modules/mavlink/mavlink_ftp.h @@ -159,7 +159,6 @@ private: static const char kDirentFile = 'F'; ///< Identifies File returned from List command static const char kDirentDir = 'D'; ///< Identifies Directory returned from List command - static const char kDirentUnknown = 'U'; ///< Identifies Unknown entry returned from List command /// @brief Maximum data size in RequestHeader::data static const uint8_t kMaxDataLength = MAVLINK_MSG_FILE_TRANSFER_PROTOCOL_FIELD_PAYLOAD_LEN - sizeof(PayloadHeader); -- cgit v1.2.3 From dbefa77943fb7f05f391933e82aa4ddf35755851 Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Wed, 10 Sep 2014 13:12:04 -0700 Subject: Update for not getting back "." entries --- src/modules/mavlink/mavlink_tests/mavlink_ftp_test.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.cpp b/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.cpp index 3616db237..c9566f7db 100644 --- a/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.cpp +++ b/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.cpp @@ -164,7 +164,7 @@ bool MavlinkFtpTest::_list_test(void) mavlink_file_transfer_protocol_t ftp_msg; MavlinkFTP::PayloadHeader *reply; - char response1[] = "D.|Dempty_dir|Ftest_238.data\t238|Ftest_239.data\t239|Ftest_240.data\t240"; + char response1[] = "Dempty_dir|Ftest_238.data\t238|Ftest_239.data\t239|Ftest_240.data\t240"; char response2[] = "Ddev|Detc|Dfs|Dobj"; struct _testCase { -- 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(-) 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(-) 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(-) 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 46a9f616eba368bb864f4faf8e920dad7fd4ecc5 Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Fri, 12 Sep 2014 10:54:23 -0700 Subject: Fix List command test Return order from List command is not repeatable --- .../mavlink/mavlink_tests/mavlink_ftp_test.cpp | 46 ++++++++++++++-------- 1 file changed, 30 insertions(+), 16 deletions(-) diff --git a/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.cpp b/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.cpp index c9566f7db..022041c74 100644 --- a/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.cpp +++ b/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.cpp @@ -168,27 +168,20 @@ bool MavlinkFtpTest::_list_test(void) char response2[] = "Ddev|Detc|Dfs|Dobj"; struct _testCase { - const char *dir; - char *response; - bool success; + const char *dir; ///< Directory to run List command on + char *response; ///< Expected response entries from List command + int response_count; ///< Number of directories that should be returned + bool success; ///< true: List command should succeed, false: List command should fail }; struct _testCase rgTestCases[] = { - { "/bogus", nullptr, false }, - { "/etc/unit_test_data/mavlink_tests", response1, true }, - { "/", response2, true }, + { "/bogus", nullptr, 0, false }, + { "/etc/unit_test_data/mavlink_tests", response1, 4, true }, + { "/", response2, 4, true }, }; for (size_t i=0; iresponse) + 1; - - char *ptr = strtok (test->response, "|"); - while (ptr != nullptr) - { - ptr = strtok (nullptr, "|"); - } - payload.opcode = MavlinkFTP::kCmdListDirectory; payload.offset = 0; @@ -203,8 +196,29 @@ bool MavlinkFtpTest::_list_test(void) if (test->success) { ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck); - ut_compare("Incorrect payload size", reply->size, expected_data_size); - ut_compare("Ack payload contents incorrect", memcmp(reply->data, test->response, expected_data_size), 0); + ut_compare("Incorrect payload size", reply->size, strlen(test->response) + 1); + + // The return order of directories from the List command is not repeatable. So we can't do a direct comparison + // to a hardcoded return result string. + + // Convert null terminators to seperator char so we can use strok to parse returned data + for (uint8_t j=0; jsize-1; j++) { + if (reply->data[j] == 0) { + reply->data[j] = '|'; + } + } + + // Loop over returned directory entries trying to find then in the response list + char *dir; + int response_count = 0; + dir = strtok((char *)&reply->data[0], "|"); + while (dir != nullptr) { + ut_assert("Returned directory not found in expected response", strstr(test->response, dir)); + response_count++; + dir = strtok(nullptr, "|"); + } + + ut_compare("Incorrect number of directory entires returned", test->response_count, response_count); } else { ut_compare("Didn't get Nak back", reply->opcode, MavlinkFTP::kRspNak); ut_compare("Incorrect payload size", reply->size, 2); -- 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(-) 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 deda5d0a04300f2ee67194c4fc9a60da37de722a Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Sat, 13 Sep 2014 19:59:44 -0700 Subject: Upgraded unit test framework --- ROMFS/px4fmu_test/init.d/rcS | 30 +++++ makefiles/config_px4fmu-v2_test.mk | 1 + .../commander/commander_tests/commander_tests.cpp | 4 +- .../commander_tests/state_machine_helper_test.cpp | 13 +-- .../commander_tests/state_machine_helper_test.h | 2 +- .../mavlink/mavlink_tests/mavlink_ftp_test.cpp | 10 +- .../mavlink/mavlink_tests/mavlink_ftp_test.h | 9 +- .../mavlink/mavlink_tests/mavlink_tests.cpp | 7 +- src/modules/mavlink/mavlink_tests/module.mk | 2 + src/modules/unit_test/unit_test.cpp | 24 ++-- src/modules/unit_test/unit_test.h | 125 ++++++++++++--------- 11 files changed, 139 insertions(+), 88 deletions(-) diff --git a/ROMFS/px4fmu_test/init.d/rcS b/ROMFS/px4fmu_test/init.d/rcS index 56482d140..bc248ac04 100644 --- a/ROMFS/px4fmu_test/init.d/rcS +++ b/ROMFS/px4fmu_test/init.d/rcS @@ -75,3 +75,33 @@ if [ -f /fs/microsd/mount_test_cmds.txt ] then tests mount fi + +# +# Run unit tests at board boot, reporting failure as needed. +# Add new unit tests using the same pattern as below. +# + +set unit_test_failure 0 + +if mavlink_tests +then +else + set unit_test_failure 1 + set unit_test_failure_list "${unit_test_failure_list} mavlink_tests" +fi + +if commander_tests +then +else + set unit_test_failure 1 + set unit_test_failure_list "${unit_test_failure_list} commander_tests" +fi + +if [ $unit_test_failure == 0 ] +then + echo + echo "All Unit Tests PASSED" +else + echo + echo "Some Unit Tests FAILED:${unit_test_failure_list}" +fi \ No newline at end of file diff --git a/makefiles/config_px4fmu-v2_test.mk b/makefiles/config_px4fmu-v2_test.mk index a41670a77..2f4d9d6a4 100644 --- a/makefiles/config_px4fmu-v2_test.mk +++ b/makefiles/config_px4fmu-v2_test.mk @@ -56,6 +56,7 @@ LIBRARIES += lib/mathlib/CMSIS MODULES += modules/unit_test MODULES += modules/mavlink/mavlink_tests +MODULES += modules/commander/commander_tests # # Transitional support - add commands from the NuttX export archive. diff --git a/src/modules/commander/commander_tests/commander_tests.cpp b/src/modules/commander/commander_tests/commander_tests.cpp index 0abb84a82..2bfa5d0bb 100644 --- a/src/modules/commander/commander_tests/commander_tests.cpp +++ b/src/modules/commander/commander_tests/commander_tests.cpp @@ -48,7 +48,5 @@ extern "C" __EXPORT int commander_tests_main(int argc, char *argv[]); int commander_tests_main(int argc, char *argv[]) { - stateMachineHelperTest(); - - return 0; + return stateMachineHelperTest() ? 0 : -1; } diff --git a/src/modules/commander/commander_tests/state_machine_helper_test.cpp b/src/modules/commander/commander_tests/state_machine_helper_test.cpp index 08dda2fab..874090e93 100644 --- a/src/modules/commander/commander_tests/state_machine_helper_test.cpp +++ b/src/modules/commander/commander_tests/state_machine_helper_test.cpp @@ -49,7 +49,7 @@ public: StateMachineHelperTest(); virtual ~StateMachineHelperTest(); - virtual void runTests(void); + virtual bool run_tests(void); private: bool armingStateTransitionTest(); @@ -488,16 +488,13 @@ bool StateMachineHelperTest::isSafeTest(void) return true; } -void StateMachineHelperTest::runTests(void) +bool StateMachineHelperTest::run_tests(void) { ut_run_test(armingStateTransitionTest); ut_run_test(mainStateTransitionTest); ut_run_test(isSafeTest); + + return (_tests_failed == 0); } -void stateMachineHelperTest(void) -{ - StateMachineHelperTest* test = new StateMachineHelperTest(); - test->runTests(); - test->printResults(); -} +ut_declare_test(stateMachineHelperTest, StateMachineHelperTest) \ No newline at end of file diff --git a/src/modules/commander/commander_tests/state_machine_helper_test.h b/src/modules/commander/commander_tests/state_machine_helper_test.h index bbf66255e..cf6719095 100644 --- a/src/modules/commander/commander_tests/state_machine_helper_test.h +++ b/src/modules/commander/commander_tests/state_machine_helper_test.h @@ -39,6 +39,6 @@ #ifndef STATE_MACHINE_HELPER_TEST_H_ #define STATE_MACHINE_HELPER_TEST_ -void stateMachineHelperTest(void); +bool stateMachineHelperTest(void); #endif /* STATE_MACHINE_HELPER_TEST_H_ */ diff --git a/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.cpp b/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.cpp index 022041c74..a5fb2117e 100644 --- a/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.cpp +++ b/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.cpp @@ -65,7 +65,7 @@ MavlinkFtpTest::~MavlinkFtpTest() } /// @brief Called before every test to initialize the FTP Server. -void MavlinkFtpTest::init(void) +void MavlinkFtpTest::_init(void) { _ftp_server = new MavlinkFTP;; _ftp_server->set_unittest_worker(MavlinkFtpTest::receive_message, this); @@ -74,7 +74,7 @@ void MavlinkFtpTest::init(void) } /// @brief Called after every test to take down the FTP Server. -void MavlinkFtpTest::cleanup(void) +void MavlinkFtpTest::_cleanup(void) { delete _ftp_server; @@ -738,7 +738,7 @@ void MavlinkFtpTest::_cleanup_microsd(void) } /// @brief Runs all the unit tests -void MavlinkFtpTest::runTests(void) +bool MavlinkFtpTest::run_tests(void) { ut_run_test(_ack_test); ut_run_test(_bad_opcode_test); @@ -753,5 +753,9 @@ void MavlinkFtpTest::runTests(void) ut_run_test(_removedirectory_test); ut_run_test(_createdirectory_test); ut_run_test(_removefile_test); + + return (_tests_failed == 0); + } +ut_declare_test(mavlink_ftp_test, MavlinkFtpTest) diff --git a/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.h b/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.h index babd909da..2696192cc 100644 --- a/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.h +++ b/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.h @@ -46,10 +46,7 @@ public: MavlinkFtpTest(); virtual ~MavlinkFtpTest(); - virtual void init(void); - virtual void cleanup(void); - - virtual void runTests(void); + virtual bool run_tests(void); static void receive_message(const mavlink_message_t *msg, MavlinkFtpTest* ftpTest); @@ -65,6 +62,9 @@ public: MavlinkFtpTest& operator=(const MavlinkFtpTest&); private: + virtual void _init(void); + virtual void _cleanup(void); + bool _ack_test(void); bool _bad_opcode_test(void); bool _bad_datasize_test(void); @@ -105,3 +105,4 @@ private: static const char _unittest_microsd_file[]; }; +bool mavlink_ftp_test(void); diff --git a/src/modules/mavlink/mavlink_tests/mavlink_tests.cpp b/src/modules/mavlink/mavlink_tests/mavlink_tests.cpp index d9c1e413a..10cbcb0ec 100644 --- a/src/modules/mavlink/mavlink_tests/mavlink_tests.cpp +++ b/src/modules/mavlink/mavlink_tests/mavlink_tests.cpp @@ -43,10 +43,5 @@ extern "C" __EXPORT int mavlink_tests_main(int argc, char *argv[]); int mavlink_tests_main(int argc, char *argv[]) { - MavlinkFtpTest* test = new MavlinkFtpTest; - - test->runTests(); - test->printResults(); - - return 0; + return mavlink_ftp_test() ? 0 : -1; } diff --git a/src/modules/mavlink/mavlink_tests/module.mk b/src/modules/mavlink/mavlink_tests/module.mk index 07cfce1dc..1cc28cce1 100644 --- a/src/modules/mavlink/mavlink_tests/module.mk +++ b/src/modules/mavlink/mavlink_tests/module.mk @@ -45,4 +45,6 @@ INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink MODULE_STACKSIZE = 5000 +MAXOPTIMIZATION = -Os + EXTRACXXFLAGS = -Weffc++ -DMAVLINK_FTP_UNIT_TEST diff --git a/src/modules/unit_test/unit_test.cpp b/src/modules/unit_test/unit_test.cpp index be3b9461a..b7568ce3a 100644 --- a/src/modules/unit_test/unit_test.cpp +++ b/src/modules/unit_test/unit_test.cpp @@ -36,7 +36,11 @@ #include -UnitTest::UnitTest() +UnitTest::UnitTest() : + _tests_run(0), + _tests_failed(0), + _tests_passed(0), + _assertions(0) { } @@ -44,20 +48,22 @@ UnitTest::~UnitTest() { } -void UnitTest::printResults(void) +void UnitTest::print_results(void) { - warnx(mu_tests_failed() ? "SOME TESTS FAILED" : "ALL TESTS PASSED"); - warnx(" Tests passed : %d", mu_tests_passed()); - warnx(" Tests failed : %d", mu_tests_failed()); - warnx(" Assertions : %d", mu_assertion()); + warnx(_tests_failed ? "SOME TESTS FAILED" : "ALL TESTS PASSED"); + warnx(" Tests passed : %d", _tests_passed); + warnx(" Tests failed : %d", _tests_failed); + warnx(" Assertions : %d", _assertions); } -void UnitTest::printAssert(const char* msg, const char* test, const char* file, int line) +/// @brief Used internally to the ut_assert macro to print assert failures. +void UnitTest::_print_assert(const char* msg, const char* test, const char* file, int line) { - warnx("Assertion failed: %s - %s (%s:%d)", msg, test, file, line); + warnx("Assertion failed: %s - %s (%s:%d)", msg, test, file, line); } -void UnitTest::printCompare(const char* msg, const char *v1_text, int v1, const char *v2_text, int v2, const char* file, int line) +/// @brief Used internally to the ut_compare macro to print assert failures. +void UnitTest::_print_compare(const char* msg, const char *v1_text, int v1, const char *v2_text, int v2, const char* file, int line) { warnx("Compare failed: %s - (%s:%d) (%s:%d) (%s:%d)", msg, v1_text, v1, v2_text, v2, file, line); } diff --git a/src/modules/unit_test/unit_test.h b/src/modules/unit_test/unit_test.h index 1a5489709..8ed4efadf 100644 --- a/src/modules/unit_test/unit_test.h +++ b/src/modules/unit_test/unit_test.h @@ -37,68 +37,85 @@ #include +/// @brief Base class to be used for unit tests. class __EXPORT UnitTest { - public: -#define INLINE_GLOBAL(type,func) inline type& func() { static type x; return x; } - -INLINE_GLOBAL(int, mu_tests_run) -INLINE_GLOBAL(int, mu_tests_failed) -INLINE_GLOBAL(int, mu_tests_passed) -INLINE_GLOBAL(int, mu_assertion) -INLINE_GLOBAL(int, mu_line) -INLINE_GLOBAL(const char*, mu_last_test) UnitTest(); - virtual ~UnitTest(); + virtual ~UnitTest(); - virtual void init(void) { }; - virtual void cleanup(void) { }; - - virtual void runTests(void) = 0; - void printResults(void); - - void printAssert(const char* msg, const char* test, const char* file, int line); - void printCompare(const char* msg, const char *v1_text, int v1, const char *v2_text, int v2, const char* file, int line); - -#define ut_assert(message, test) \ - do { \ - if (!(test)) { \ - printAssert(message, #test, __FILE__, __LINE__); \ - return false; \ - } else { \ - mu_assertion()++; \ - } \ - } while (0) - -#define ut_compare(message, v1, v2) \ - do { \ - int _v1 = v1; \ - int _v2 = v2; \ - if (_v1 != _v2) { \ - printCompare(message, #v1, _v1, #v2, _v2, __FILE__, __LINE__); \ - return false; \ - } else { \ - mu_assertion()++; \ - } \ + /// @brief Override to run your unit tests. Unit tests should be called using ut_run_test macro. + /// @return true: all unit tests succeeded, false: one or more unit tests failed + virtual bool run_tests(void) = 0; + + /// @brief Prints results from running of unit tests. + void print_results(void); + +/// @brief Macro to create a function which will run a unit test class and print results. +#define ut_declare_test(test_function, test_class) \ + bool test_function(void) \ + { \ + test_class* test = new test_class(); \ + bool success = test->run_tests(); \ + test->print_results(); \ + return success; \ + } + +protected: + +/// @brief Runs a single unit test. Unit tests must have the function signature of bool test(void). The unit +/// test should return true if it succeeded, false for fail. +#define ut_run_test(test) \ + do { \ + warnx("RUNNING TEST: %s", #test); \ + _tests_run++; \ + _init(); \ + if (!test()) { \ + warnx("TEST FAILED: %s", #test); \ + _tests_failed++; \ + } else { \ + warnx("TEST PASSED: %s", #test); \ + _tests_passed++; \ + } \ + _cleanup(); \ } while (0) - -#define ut_run_test(test) \ - do { \ - warnx("RUNNING TEST: %s", #test); \ - mu_tests_run()++; \ - init(); \ - if (!test()) { \ - warnx("TEST FAILED: %s", #test); \ - mu_tests_failed()++; \ - } else { \ - warnx("TEST PASSED: %s", #test); \ - mu_tests_passed()++; \ - } \ - cleanup(); \ - } while (0) + +/// @brief Used to assert a value within a unit test. +#define ut_assert(message, test) \ + do { \ + if (!(test)) { \ + _print_assert(message, #test, __FILE__, __LINE__); \ + return false; \ + } else { \ + _assertions++; \ + } \ + } while (0) + +/// @brief Used to compare two integer values within a unit test. If possible use ut_compare instead of ut_assert +/// since it will give you better error reporting of the actual values being compared. +#define ut_compare(message, v1, v2) \ + do { \ + int _v1 = v1; \ + int _v2 = v2; \ + if (_v1 != _v2) { \ + _print_compare(message, #v1, _v1, #v2, _v2, __FILE__, __LINE__); \ + return false; \ + } else { \ + _assertions++; \ + } \ + } while (0) + + virtual void _init(void) { }; ///< Run before each unit test. Override to provide custom behavior. + virtual void _cleanup(void) { }; ///< Run after each unit test. Override to provide custom behavior. + + void _print_assert(const char* msg, const char* test, const char* file, int line); + void _print_compare(const char* msg, const char *v1_text, int v1, const char *v2_text, int v2, const char* file, int line); + int _tests_run; ///< The number of individual unit tests run + int _tests_failed; ///< The number of unit tests which failed + int _tests_passed; ///< The number of unit tests which passed + int _assertions; ///< Total number of assertions tested by all unit tests }; #endif /* UNIT_TEST_H_ */ -- cgit v1.2.3 From 50f7e27d13e3dfc4b94f17a06f29d775e47627f9 Mon Sep 17 00:00:00 2001 From: Anthony Kenga Date: Mon, 15 Sep 2014 12:24:11 +0300 Subject: Fixed parameter storage to support struct parameters. --- src/modules/systemlib/param/param.c | 3 ++- src/modules/systemlib/param/param.h | 2 +- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/src/modules/systemlib/param/param.c b/src/modules/systemlib/param/param.c index e44e6cdb0..6b8d0e634 100644 --- a/src/modules/systemlib/param/param.c +++ b/src/modules/systemlib/param/param.c @@ -322,7 +322,8 @@ param_get_value_ptr(param_t param) v = ¶m_info_base[param].val; } - if (param_type(param) == PARAM_TYPE_STRUCT) { + if (param_type(param) >= PARAM_TYPE_STRUCT + && param_type(param) <= PARAM_TYPE_STRUCT_MAX) { result = v->p; } else { diff --git a/src/modules/systemlib/param/param.h b/src/modules/systemlib/param/param.h index 084cd931a..dc73b37e9 100644 --- a/src/modules/systemlib/param/param.h +++ b/src/modules/systemlib/param/param.h @@ -307,7 +307,7 @@ __EXPORT int param_load_default(void); struct param_info_s __param__##_name = { \ #_name, \ PARAM_TYPE_STRUCT + sizeof(_default), \ - .val.p = &_default; \ + .val.p = &_default \ } /** -- cgit v1.2.3 From 006717734c351926e8daced9805767970c5e65b7 Mon Sep 17 00:00:00 2001 From: Vladimir Ermakov Date: Mon, 15 Sep 2014 21:29:19 +0400 Subject: FTP: Add skip entry information for proper offset calculation. --- src/modules/mavlink/mavlink_ftp.cpp | 12 ++++++++---- src/modules/mavlink/mavlink_ftp.h | 1 + 2 files changed, 9 insertions(+), 4 deletions(-) diff --git a/src/modules/mavlink/mavlink_ftp.cpp b/src/modules/mavlink/mavlink_ftp.cpp index cf5fae3b3..2a85c3702 100644 --- a/src/modules/mavlink/mavlink_ftp.cpp +++ b/src/modules/mavlink/mavlink_ftp.cpp @@ -351,16 +351,20 @@ MavlinkFTP::_workList(PayloadHeader* payload) case DTYPE_DIRECTORY: if (strcmp(entry.d_name, ".") == 0 || strcmp(entry.d_name, "..") == 0) { // Don't bother sending these back - continue; + direntType = kDirentSkip; + } else { + direntType = kDirentDir; } - direntType = kDirentDir; break; default: // We only send back file and diretory entries, skip everything else - continue; + direntType = kDirentSkip; } - if (entry.d_type == DTYPE_FILE) { + if (direntType == kDirentSkip) { + // Skip send only dirent identifier + buf[0] = '\0'; + } else if (direntType == kDirentFile) { // Files send filename and file length snprintf(buf, sizeof(buf), "%s\t%d", entry.d_name, fileSize); } else { diff --git a/src/modules/mavlink/mavlink_ftp.h b/src/modules/mavlink/mavlink_ftp.h index b52561e8a..657e2f855 100644 --- a/src/modules/mavlink/mavlink_ftp.h +++ b/src/modules/mavlink/mavlink_ftp.h @@ -159,6 +159,7 @@ private: static const char kDirentFile = 'F'; ///< Identifies File returned from List command static const char kDirentDir = 'D'; ///< Identifies Directory returned from List command + static const char kDirentSkip = 'S'; ///< Identifies Skipped entry from List command /// @brief Maximum data size in RequestHeader::data static const uint8_t kMaxDataLength = MAVLINK_MSG_FILE_TRANSFER_PROTOCOL_FIELD_PAYLOAD_LEN - sizeof(PayloadHeader); -- cgit v1.2.3 From 60c63f48729f028ec9f5b6de93da91873d152bfd Mon Sep 17 00:00:00 2001 From: Vladimir Ermakov Date: Wed, 17 Sep 2014 10:07:29 +0400 Subject: FTP: Add new open command for write. All open commands now return file size. --- src/modules/mavlink/mavlink_ftp.cpp | 38 ++++++++++++++++++------------------- src/modules/mavlink/mavlink_ftp.h | 7 ++++--- 2 files changed, 23 insertions(+), 22 deletions(-) diff --git a/src/modules/mavlink/mavlink_ftp.cpp b/src/modules/mavlink/mavlink_ftp.cpp index 2a85c3702..b17036c8e 100644 --- a/src/modules/mavlink/mavlink_ftp.cpp +++ b/src/modules/mavlink/mavlink_ftp.cpp @@ -180,12 +180,16 @@ MavlinkFTP::_process_request(Request *req) errorCode = _workList(payload); break; - case kCmdOpenFile: - errorCode = _workOpen(payload, false); + case kCmdOpenFileRO: + errorCode = _workOpen(payload, O_RDONLY); break; case kCmdCreateFile: - errorCode = _workOpen(payload, true); + errorCode = _workOpen(payload, O_CREAT | O_EXCL | O_WRONLY); + break; + + case kCmdOpenFileWO: + errorCode = _workOpen(payload, O_CREAT | O_WRONLY); break; case kCmdReadFile: @@ -396,27 +400,27 @@ MavlinkFTP::_workList(PayloadHeader* payload) /// @brief Responds to an Open command MavlinkFTP::ErrorCode -MavlinkFTP::_workOpen(PayloadHeader* payload, bool create) +MavlinkFTP::_workOpen(PayloadHeader* payload, int oflag) { int session_index = _find_unused_session(); if (session_index < 0) { warnx("FTP: Open failed - out of sessions\n"); return kErrNoSessionsAvailable; } - + char *filename = _data_as_cstring(payload); - + uint32_t fileSize = 0; - if (!create) { - struct stat st; - if (stat(filename, &st) != 0) { + struct stat st; + if (stat(filename, &st) != 0) { + // fail only if requested open for read + if (oflag & O_RDONLY) return kErrFailErrno; - } - fileSize = st.st_size; + else + st.st_size = 0; } + fileSize = st.st_size; - int oflag = create ? (O_CREAT | O_EXCL | O_APPEND) : O_RDONLY; - int fd = ::open(filename, oflag); if (fd < 0) { return kErrFailErrno; @@ -424,12 +428,8 @@ MavlinkFTP::_workOpen(PayloadHeader* payload, bool create) _session_fds[session_index] = fd; payload->session = session_index; - if (create) { - payload->size = 0; - } else { - payload->size = sizeof(uint32_t); - *((uint32_t*)payload->data) = fileSize; - } + payload->size = sizeof(uint32_t); + *((uint32_t*)payload->data) = fileSize; return kErrNone; } diff --git a/src/modules/mavlink/mavlink_ftp.h b/src/modules/mavlink/mavlink_ftp.h index 657e2f855..1c873fe9f 100644 --- a/src/modules/mavlink/mavlink_ftp.h +++ b/src/modules/mavlink/mavlink_ftp.h @@ -89,13 +89,14 @@ public: kCmdTerminateSession, ///< Terminates open Read session kCmdResetSessions, ///< Terminates all open Read sessions kCmdListDirectory, ///< List files in from - kCmdOpenFile, ///< Opens file at for reading, returns + kCmdOpenFileRO, ///< Opens file at for reading, returns kCmdReadFile, ///< Reads bytes from in kCmdCreateFile, ///< Creates file at for writing, returns - kCmdWriteFile, ///< Appends bytes to file in + kCmdWriteFile, ///< Writes bytes to in kCmdRemoveFile, ///< Remove file at kCmdCreateDirectory, ///< Creates directory at kCmdRemoveDirectory, ///< Removes Directory at , must be empty + kCmdOpenFileWO, ///< Opens file at for writing, returns kRspAck = 128, ///< Ack response kRspNak ///< Nak response @@ -140,7 +141,7 @@ private: void _reply(Request *req); ErrorCode _workList(PayloadHeader *payload); - ErrorCode _workOpen(PayloadHeader *payload, bool create); + ErrorCode _workOpen(PayloadHeader *payload, int oflag); ErrorCode _workRead(PayloadHeader *payload); ErrorCode _workWrite(PayloadHeader *payload); ErrorCode _workTerminate(PayloadHeader *payload); -- cgit v1.2.3 From 72887e14d9964cf12f8ca0a48d0d185f8d469ecb Mon Sep 17 00:00:00 2001 From: Vladimir Ermakov Date: Wed, 17 Sep 2014 13:04:21 +0400 Subject: FTP: Implement write command. --- src/modules/mavlink/mavlink_ftp.cpp | 41 ++++++++++++++++++++++--------------- 1 file changed, 24 insertions(+), 17 deletions(-) diff --git a/src/modules/mavlink/mavlink_ftp.cpp b/src/modules/mavlink/mavlink_ftp.cpp index b17036c8e..695dcb0dc 100644 --- a/src/modules/mavlink/mavlink_ftp.cpp +++ b/src/modules/mavlink/mavlink_ftp.cpp @@ -470,29 +470,36 @@ MavlinkFTP::_workRead(PayloadHeader* payload) MavlinkFTP::ErrorCode MavlinkFTP::_workWrite(PayloadHeader* payload) { -#if 0 - // NYI: Coming soon - auto hdr = req->header(); - - // look up session - auto session = getSession(hdr->session); - if (session == nullptr) { - return kErrNoSession; + int session_index = payload->session; + + if (!_valid_session(session_index)) { + return kErrInvalidSession; } - // append to file - int result = session->append(hdr->offset, &hdr->data[0], hdr->size); + // Seek to the specified position +#ifdef MAVLINK_FTP_DEBUG + warnx("seek %d", payload->offset); +#endif + if (lseek(_session_fds[session_index], payload->offset, SEEK_SET) < 0) { + // Unable to see to the specified location + warnx("seek fail"); + return kErrFailErrno; + } - if (result < 0) { - // XXX might also be no space, I/O, etc. - return kErrNotAppend; + int bytes_written = ::write(_session_fds[session_index], &payload->data[0], payload->size); + if (bytes_written < 0) { + // Negative return indicates error other than eof + warnx("write fail %d", bytes_written); + return kErrFailErrno; } - hdr->size = result; + // need to decide how to indicate how much data was written + // current: old code set hdr->size + payload->size = bytes_written; + //payload->size = sizeof(uint32_t); + //*((uint32_t*)payload->data) = bytes_written; + return kErrNone; -#else - return kErrUnknownCommand; -#endif } /// @brief Responds to a RemoveFile command -- cgit v1.2.3 From f55d20a133397b2529826f015bec3452294ad4c2 Mon Sep 17 00:00:00 2001 From: Vladimir Ermakov Date: Wed, 17 Sep 2014 18:42:00 +0400 Subject: FTP: Add truncate command. Unfortunately NuttX not provides truncate(), had to be emulated by copying with O_TRUNC flag. --- src/modules/mavlink/mavlink_ftp.cpp | 126 ++++++++++++++++++++++++++++++++++++ src/modules/mavlink/mavlink_ftp.h | 3 + 2 files changed, 129 insertions(+) diff --git a/src/modules/mavlink/mavlink_ftp.cpp b/src/modules/mavlink/mavlink_ftp.cpp index 695dcb0dc..6865655da 100644 --- a/src/modules/mavlink/mavlink_ftp.cpp +++ b/src/modules/mavlink/mavlink_ftp.cpp @@ -204,6 +204,10 @@ MavlinkFTP::_process_request(Request *req) errorCode = _workRemoveFile(payload); break; + case kCmdTruncateFile: + errorCode = _workTruncateFile(payload); + break; + case kCmdCreateDirectory: errorCode = _workCreateDirectory(payload); break; @@ -517,6 +521,82 @@ MavlinkFTP::_workRemoveFile(PayloadHeader* payload) } } +/// @brief Responds to a TruncateFile command +MavlinkFTP::ErrorCode +MavlinkFTP::_workTruncateFile(PayloadHeader* payload) +{ + char file[kMaxDataLength]; + const char temp_file[] = "/fs/microsd/.trunc.tmp"; + strncpy(file, _data_as_cstring(payload), kMaxDataLength); + payload->size = 0; + + // emulate truncate(file, payload->offset) by + // copying to temp and overwrite with O_TRUNC flag. + + struct stat st; + if (stat(file, &st) != 0) { + return kErrFailErrno; + } + + if (!S_ISREG(st.st_mode)) { + errno = EISDIR; + return kErrFailErrno; + } + + // check perms allow us to write (not romfs) + int mode = st.st_mode & (S_IRWXU | S_IRWXG | S_IRWXO); + if (!(mode & ~(S_IRUSR | S_IXUSR | S_IRGRP | S_IXGRP | S_IROTH | S_IXOTH))) { + errno = EROFS; + return kErrFailErrno; + } + + if (payload->offset == st.st_size) { + // nothing to do + return kErrNone; + } + else if (payload->offset == 0) { + // 1: truncate all data + int fd = ::open(file, O_TRUNC | O_WRONLY); + if (fd < 0) { + return kErrFailErrno; + } + + ::close(fd); + return kErrNone; + } + else if (payload->offset > st.st_size) { + // 2: extend file + int fd = ::open(file, O_WRONLY); + if (fd < 0) { + return kErrFailErrno; + } + + if (lseek(fd, payload->offset - 1, SEEK_SET) != 0) { + ::close(fd); + return kErrFailErrno; + } + + bool ok = 1 == ::write(fd, "", 1); + ::close(fd); + + return (ok)? kErrNone : kErrFailErrno; + } + else { + // 3: truncate + if (_copy_file(file, temp_file, payload->offset) != 0) { + return kErrFailErrno; + } + if (_copy_file(temp_file, file, payload->offset) != 0) { + return kErrFailErrno; + } + if (::unlink(temp_file) != 0) { + return kErrFailErrno; + } + + return kErrNone; + } +} + /// @brief Responds to a Terminate command MavlinkFTP::ErrorCode MavlinkFTP::_workTerminate(PayloadHeader* payload) @@ -652,3 +732,49 @@ MavlinkFTP::_return_request(Request *req) _unlock_request_queue(); } +/// @brief Copy file (with limited space) +int +MavlinkFTP::_copy_file(const char *src_path, const char *dst_path, ssize_t length) +{ + char buff[512]; + int src_fd = -1, dst_fd = -1; + + src_fd = ::open(src_path, O_RDONLY); + if (src_fd < 0) { + return -1; + } + + dst_fd = ::open(dst_path, O_CREAT | O_TRUNC | O_WRONLY); + if (dst_fd < 0) { + ::close(src_fd); + return -1; + } + + while (length > 0) { + ssize_t bytes_read, bytes_written; + size_t blen = (length > sizeof(buff))? sizeof(buff) : length; + + bytes_read = ::read(src_fd, buff, blen); + if (bytes_read == 0) { + // EOF + break; + } + else if (bytes_read < 0) { + warnx("cp: read"); + break; + } + + bytes_written = ::write(dst_fd, buff, bytes_read); + if (bytes_written != bytes_read) { + warnx("cp: short write"); + break; + } + + length -= bytes_written; + } + + ::close(src_fd); + ::close(dst_fd); + + return (length > 0)? -1 : 0; +} diff --git a/src/modules/mavlink/mavlink_ftp.h b/src/modules/mavlink/mavlink_ftp.h index 1c873fe9f..bde8d7ec5 100644 --- a/src/modules/mavlink/mavlink_ftp.h +++ b/src/modules/mavlink/mavlink_ftp.h @@ -97,6 +97,7 @@ public: kCmdCreateDirectory, ///< Creates directory at kCmdRemoveDirectory, ///< Removes Directory at , must be empty kCmdOpenFileWO, ///< Opens file at for writing, returns + kCmdTruncateFile, ///< Truncate file at to length kRspAck = 128, ///< Ack response kRspNak ///< Nak response @@ -139,6 +140,7 @@ private: static void _worker_trampoline(void *arg); void _process_request(Request *req); void _reply(Request *req); + int _copy_file(const char *src_path, const char *dst_path, ssize_t length); ErrorCode _workList(PayloadHeader *payload); ErrorCode _workOpen(PayloadHeader *payload, int oflag); @@ -149,6 +151,7 @@ private: ErrorCode _workRemoveDirectory(PayloadHeader *payload); ErrorCode _workCreateDirectory(PayloadHeader *payload); ErrorCode _workRemoveFile(PayloadHeader *payload); + ErrorCode _workTruncateFile(PayloadHeader *payload); static const unsigned kRequestQueueSize = 2; ///< Max number of queued requests Request _request_bufs[kRequestQueueSize]; ///< Request buffers which hold work -- cgit v1.2.3 From 407a4e0f0675f0b283eea0632208c8c3f8b9ca20 Mon Sep 17 00:00:00 2001 From: Vladimir Ermakov Date: Wed, 17 Sep 2014 20:02:32 +0400 Subject: FTP: fix truncate errors. Also correct errno reporting. Seems that warnx() everytime changes originak errno to EINVAL. --- src/modules/mavlink/mavlink_ftp.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/modules/mavlink/mavlink_ftp.cpp b/src/modules/mavlink/mavlink_ftp.cpp index 6865655da..14a42ed72 100644 --- a/src/modules/mavlink/mavlink_ftp.cpp +++ b/src/modules/mavlink/mavlink_ftp.cpp @@ -230,6 +230,7 @@ out: warnx("FTP: ack\n"); #endif } else { + int r_errno = errno; warnx("FTP: nak %u", errorCode); payload->req_opcode = payload->opcode; payload->opcode = kRspNak; @@ -237,7 +238,7 @@ out: payload->data[0] = errorCode; if (errorCode == kErrFailErrno) { payload->size = 2; - payload->data[1] = errno; + payload->data[1] = r_errno; } } @@ -544,8 +545,7 @@ MavlinkFTP::_workTruncateFile(PayloadHeader* payload) } // check perms allow us to write (not romfs) - int mode = st.st_mode & (S_IRWXU | S_IRWXG | S_IRWXO); - if (!(mode & ~(S_IRUSR | S_IXUSR | S_IRGRP | S_IXGRP | S_IROTH | S_IXOTH))) { + if (!(st.st_mode & (S_IWUSR | S_IWGRP | S_IWOTH))) { errno = EROFS; return kErrFailErrno; } @@ -571,7 +571,7 @@ MavlinkFTP::_workTruncateFile(PayloadHeader* payload) return kErrFailErrno; } - if (lseek(fd, payload->offset - 1, SEEK_SET) != 0) { + if (lseek(fd, payload->offset - 1, SEEK_SET) < 0) { ::close(fd); return kErrFailErrno; } -- cgit v1.2.3 From 772c5f745cdb2213e9d89d2ab3f373ac5ea2009a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 17 Sep 2014 16:37:10 -0500 Subject: Make space on FMUv1 --- makefiles/config_px4fmu-v1_default.mk | 3 --- 1 file changed, 3 deletions(-) diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk index 97eddfdd2..6f39b56a1 100644 --- a/makefiles/config_px4fmu-v1_default.mk +++ b/makefiles/config_px4fmu-v1_default.mk @@ -27,8 +27,6 @@ MODULES += drivers/ms5611 MODULES += drivers/mb12xx MODULES += drivers/gps MODULES += drivers/hil -MODULES += drivers/hott/hott_telemetry -MODULES += drivers/hott/hott_sensors MODULES += drivers/blinkm MODULES += drivers/rgbled MODULES += drivers/mkblctrl @@ -42,7 +40,6 @@ MODULES += modules/sensors # System commands # MODULES += systemcmds/mtd -MODULES += systemcmds/bl_update MODULES += systemcmds/mixer MODULES += systemcmds/param MODULES += systemcmds/perf -- cgit v1.2.3 From ec39427d79249575b1bd8ca7af0929d08c084060 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 19 Sep 2014 09:51:56 +0200 Subject: geofence: lat/lon is double This is the same change as 5832948371866aec8f0c7f16b13869f270d36aad but against the master branch. Some time ago the type of lat and lon in the global_pos uorb topic was changed but this use here was missed. --- src/modules/navigator/geofence.cpp | 6 +----- 1 file changed, 1 insertion(+), 5 deletions(-) diff --git a/src/modules/navigator/geofence.cpp b/src/modules/navigator/geofence.cpp index 266215308..49aec21e0 100644 --- a/src/modules/navigator/geofence.cpp +++ b/src/modules/navigator/geofence.cpp @@ -76,11 +76,7 @@ Geofence::~Geofence() bool Geofence::inside(const struct vehicle_global_position_s *vehicle) { - double lat = vehicle->lat / 1e7d; - double lon = vehicle->lon / 1e7d; - //float alt = vehicle->alt; - - return inside(lat, lon, vehicle->alt); + return inside(vehicle->lat, vehicle->lon, vehicle->alt); } bool Geofence::inside(double lat, double lon, float altitude) -- 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(-) 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(-) 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(-) 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 bdddcf338de5db7b1a7aef6ce124e4c180a27254 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sat, 20 Sep 2014 22:53:56 +1000 Subject: viper: changed some values for doors and bottle drop servos --- ROMFS/px4fmu_common/init.d/3035_viper | 3 ++- ROMFS/px4fmu_common/mixers/Viper.mix | 2 +- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/3035_viper b/ROMFS/px4fmu_common/init.d/3035_viper index 3714b612f..ac4d919ce 100644 --- a/ROMFS/px4fmu_common/init.d/3035_viper +++ b/ROMFS/px4fmu_common/init.d/3035_viper @@ -9,4 +9,5 @@ sh /etc/init.d/rc.fw_defaults set MIXER Viper -set FAILSAFE "-c567 -p 1000" +set FAILSAFE "-c56 -p 1000" +set FAILSAFE "-c7 -p 1200" diff --git a/ROMFS/px4fmu_common/mixers/Viper.mix b/ROMFS/px4fmu_common/mixers/Viper.mix index 5a0381bd8..5aa3828f2 100755 --- a/ROMFS/px4fmu_common/mixers/Viper.mix +++ b/ROMFS/px4fmu_common/mixers/Viper.mix @@ -66,6 +66,6 @@ S: 2 1 10000 10000 0 -10000 10000 M: 1 O: 10000 10000 0 -10000 10000 -S: 2 2 -10000 -10000 0 -10000 10000 +S: 2 2 -8000 -8000 0 -10000 10000 -- 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(-) 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(-) 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(-) 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(+) 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(-) 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(-) 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 d6b669f4971d8d7329f0bfd5ade64bed2bd3120f 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(+) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 07fcb5d40..0ada8e978 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1442,8 +1442,12 @@ int commander_thread_main(int argc, char *argv[]) static bool flight_termination_printed = false; if (!flight_termination_printed) { warnx("Flight termination because of navigator request or geofence"); + mavlink_log_critical(mavlink_fd, "GF violation: flight termination"); flight_termination_printed = true; } + if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0 ) { + mavlink_log_critical(mavlink_fd, "GF violation: flight termination"); + } } // no reset is done here on purpose, on geofence violation we want to stay in flighttermination } -- cgit v1.2.3 From 31a17ce29a76a2f58e6feb92abe1de01eed5a8eb 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(-) 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 e3cac1999a0b4398e669f90cd28279ec2a0784a7 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(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 9a8c54e7e..fc9120f13 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -386,9 +386,13 @@ Navigator::task_main() /* global position updated */ if (fds[0].revents & POLLIN) { global_position_update(); - if (_geofence.getSource() == Geofence::GF_SOURCE_GLOBALPOS) { + static int gposcounter = 0; + if (_geofence.getSource() == Geofence::GF_SOURCE_GLOBALPOS && + gposcounter % 10 == 0) { + /* Geofence is checked only every 10th gpos update */ have_geofence_position_data = true; } + gposcounter++; } /* Check geofence violation */ -- 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(-) 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(-) 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(-) 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 973c034d6ec502009a8fd92c14ef02c4d1769a64 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(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 0ada8e978..b9a0bd2cb 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -686,6 +686,9 @@ int commander_thread_main(int argc, char *argv[]) param_t _param_datalink_loss_timeout = param_find("COM_DL_LOSS_T"); param_t _param_datalink_regain_timeout = param_find("COM_DL_REG_T"); param_t _param_onboard_sysid = param_find("COM_ONBSYSID"); + param_t _param_ef_throttle_thres = param_find("COM_EF_THROT"); + param_t _param_ef_current2throttle_thres = param_find("COM_EF_C2T"); + param_t _param_ef_time_thres = param_find("COM_EF_TIME"); /* welcome user */ warnx("starting"); @@ -974,8 +977,16 @@ int commander_thread_main(int argc, char *argv[]) int32_t datalink_loss_enabled = false; int32_t datalink_loss_timeout = 10; int32_t datalink_regain_timeout = 0; - uint8_t onboard_sysid = 42; /**< systemid of the onboard computer, telemetry from this sysid - is not validated for the datalink loss check */ + int32_t onboard_sysid = 42; /**< systemid of the onboard computer, + telemetry from this sysid is not + validated for the datalink loss check */ + + /* Thresholds for engine failure detection */ + int32_t ef_throttle_thres = 1.0f; + int32_t ef_current2throttle_thres = 0.0f; + int32_t ef_time_thres = 1000.0f; + uint64_t timestamp_engine_healthy = 0; /**< absolute time when engine + was healty*/ /* check which state machines for changes, clear "changed" flag */ bool arming_state_changed = false; @@ -1039,6 +1050,9 @@ int commander_thread_main(int argc, char *argv[]) param_get(_param_datalink_loss_timeout, &datalink_loss_timeout); param_get(_param_datalink_regain_timeout, &datalink_regain_timeout); param_get(_param_onboard_sysid, &onboard_sysid); + param_get(_param_ef_throttle_thres, &ef_throttle_thres); + param_get(_param_ef_current2throttle_thres, &ef_current2throttle_thres); + param_get(_param_ef_time_thres, &ef_time_thres); } orb_check(sp_man_sub, &updated); @@ -1576,10 +1590,11 @@ int commander_thread_main(int argc, char *argv[]) /* If this is not an onboard link/onboard computer: * set flag that we have a valid link */ - if (telemetry_sysid[i] != onboard_sysid) { + if (telemetry_sysid[i] != (uint8_t)onboard_sysid) { have_link = true; } - } else if (!telemetry_lost[i] && telemetry_sysid[i] != onboard_sysid) { + } else if (!telemetry_lost[i] && telemetry_sysid[i] != + (uint8_t)onboard_sysid) { /* telemetry was healthy also in last iteration * we don't have to check a timeout, * telemetry from onboard computers is not accepted as a valid datalink @@ -1612,6 +1627,35 @@ int commander_thread_main(int argc, char *argv[]) } } + /* Check engine failure + * only for fixed wing for now + */ + if (!circuit_breaker_enabled("CBRK_ENGINEFAIL", CBRK_ENGINEFAIL_KEY) && + status.is_rotary_wing == false && + armed.armed && + ((actuator_controls.control[3] > ef_throttle_thres && + battery.current_a/actuator_controls.control[3] < + ef_current2throttle_thres) || + (status.engine_failure))) { + /* potential failure, measure time */ + if (timestamp_engine_healthy > 0 && + hrt_elapsed_time(×tamp_engine_healthy) > + ef_time_thres * 1e6 && + !status.engine_failure) { + status.engine_failure = true; + status_changed = true; + mavlink_log_critical(mavlink_fd, "Engine Failure"); + } + } else { + /* no failure reset flag */ + timestamp_engine_healthy = hrt_absolute_time(); + if (status.engine_failure) { + status.engine_failure = false; + status_changed = true; + } + } + + /* handle commands last, as the system needs to be updated to handle them */ orb_check(cmd_sub, &updated); diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index 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 d18f3ee70d5fbeb150c6b37ccafa4f622494ec19 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(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index b9a0bd2cb..b7a16e4e6 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -128,7 +128,6 @@ extern struct system_load_s system_load; #define POSITION_TIMEOUT (2 * 1000 * 1000) /**< consider the local or global position estimate invalid after 600ms */ #define FAILSAFE_DEFAULT_TIMEOUT (3 * 1000 * 1000) /**< hysteresis time - the failsafe will trigger after 3 seconds in this state */ -#define RC_TIMEOUT 500000 #define OFFBOARD_TIMEOUT 500000 #define DIFFPRESS_TIMEOUT 2000000 @@ -684,6 +683,7 @@ int commander_thread_main(int argc, char *argv[]) param_t _param_enable_parachute = param_find("NAV_PARACHUTE_EN"); param_t _param_enable_datalink_loss = param_find("COM_DL_LOSS_EN"); param_t _param_datalink_loss_timeout = param_find("COM_DL_LOSS_T"); + param_t _param_rc_loss_timeout = param_find("COM_RC_LOSS_T"); param_t _param_datalink_regain_timeout = param_find("COM_DL_REG_T"); param_t _param_onboard_sysid = param_find("COM_ONBSYSID"); param_t _param_ef_throttle_thres = param_find("COM_EF_THROT"); @@ -976,6 +976,7 @@ int commander_thread_main(int argc, char *argv[]) int32_t datalink_loss_enabled = false; int32_t datalink_loss_timeout = 10; + float rc_loss_timeout = 0.5; int32_t datalink_regain_timeout = 0; int32_t onboard_sysid = 42; /**< systemid of the onboard computer, telemetry from this sysid is not @@ -1048,6 +1049,7 @@ int commander_thread_main(int argc, char *argv[]) param_get(_param_enable_parachute, ¶chute_enabled); param_get(_param_enable_datalink_loss, &datalink_loss_enabled); param_get(_param_datalink_loss_timeout, &datalink_loss_timeout); + param_get(_param_rc_loss_timeout, &rc_loss_timeout); param_get(_param_datalink_regain_timeout, &datalink_regain_timeout); param_get(_param_onboard_sysid, &onboard_sysid); param_get(_param_ef_throttle_thres, &ef_throttle_thres); @@ -1466,7 +1468,8 @@ int commander_thread_main(int argc, char *argv[]) } /* RC input check */ - if (!status.rc_input_blocked && sp_man.timestamp != 0 && hrt_absolute_time() < sp_man.timestamp + RC_TIMEOUT) { + if (!status.rc_input_blocked && sp_man.timestamp != 0 && + hrt_absolute_time() < sp_man.timestamp + (uint64_t)(rc_loss_timeout * 1e6f)) { /* handle the case where RC signal was regained */ if (!status.rc_signal_found_once) { status.rc_signal_found_once = true; diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index 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 21009e89a4c748d8a61174058bb378c1d6306b8d 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(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index b7a16e4e6..134f23c0b 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1688,9 +1688,12 @@ int commander_thread_main(int argc, char *argv[]) static bool flight_termination_printed = false; if (!flight_termination_printed) { warnx("Flight termination because of data link loss && gps failure"); + mavlink_log_critical(mavlink_fd, "DL and GPS lost: flight termination"); flight_termination_printed = true; } - mavlink_log_critical(mavlink_fd, "DL and GPS lost: flight termination"); + if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0 ) { + mavlink_log_critical(mavlink_fd, "DL and GPS lost: flight termination"); + } } /* At this point the rc signal and the gps system have been checked @@ -1709,7 +1712,9 @@ int commander_thread_main(int argc, char *argv[]) warnx("Flight termination because of RC signal loss && gps failure"); flight_termination_printed = true; } - mavlink_log_critical(mavlink_fd, "RC and GPS lost: flight termination"); + if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0 ) { + mavlink_log_critical(mavlink_fd, "RC and GPS lost: flight termination"); + } } } -- cgit v1.2.3 From a8239b2c4516c36d30767c0ae61b30f1e2dde096 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(-) 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 1fb8e76f0a4508cadd63b47e6f94a6638b699bcc 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(-) 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 ee08f3e98..f44985d50 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -1218,7 +1218,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } if (_vehicle_status.engine_failure || _vehicle_status.engine_failure_cmd) { - /* Set thrrust to 0 to minimize damage */ + /* Set thrust to 0 to minimize damage */ _att_sp.thrust = 0.0f; } else if (pos_sp_triplet.current.type == SETPOINT_TYPE_TAKEOFF && launch_detection_state != LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS) { -- cgit v1.2.3 From e17411769847f8681dc05da72ed4a06ff27a7a32 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(-) 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 517333c80..7cf721a35 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -860,8 +860,12 @@ FixedwingAttitudeControl::task_main() } } - /* throttle passed through */ - _actuators.control[3] = (isfinite(throttle_sp)) ? throttle_sp : 0.0f; + /* throttle passed through if it is finite and if no engine failure was + * detected */ + _actuators.control[3] = (isfinite(throttle_sp) && + !(_vehicle_status.engine_failure || + _vehicle_status.engine_failure_cmd)) ? + throttle_sp : 0.0f; if (!isfinite(throttle_sp)) { if (_debug && loop_counter % 10 == 0) { warnx("throttle_sp %.4f", (double)throttle_sp); -- cgit v1.2.3 From b5ffcfe3d12a0e2e98cb5319e0286c8215150672 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(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 134f23c0b..b43f08f67 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -685,7 +685,6 @@ int commander_thread_main(int argc, char *argv[]) param_t _param_datalink_loss_timeout = param_find("COM_DL_LOSS_T"); param_t _param_rc_loss_timeout = param_find("COM_RC_LOSS_T"); param_t _param_datalink_regain_timeout = param_find("COM_DL_REG_T"); - param_t _param_onboard_sysid = param_find("COM_ONBSYSID"); param_t _param_ef_throttle_thres = param_find("COM_EF_THROT"); param_t _param_ef_current2throttle_thres = param_find("COM_EF_C2T"); param_t _param_ef_time_thres = param_find("COM_EF_TIME"); @@ -883,14 +882,12 @@ int commander_thread_main(int argc, char *argv[]) uint64_t telemetry_last_heartbeat[TELEMETRY_STATUS_ORB_ID_NUM]; uint64_t telemetry_last_dl_loss[TELEMETRY_STATUS_ORB_ID_NUM]; bool telemetry_lost[TELEMETRY_STATUS_ORB_ID_NUM]; - uint8_t telemetry_sysid[TELEMETRY_STATUS_ORB_ID_NUM]; for (int i = 0; i < TELEMETRY_STATUS_ORB_ID_NUM; i++) { telemetry_subs[i] = orb_subscribe(telemetry_status_orb_id[i]); telemetry_last_heartbeat[i] = 0; telemetry_last_dl_loss[i] = 0; telemetry_lost[i] = true; - telemetry_sysid[i] = 0; } /* Subscribe to global position */ @@ -978,9 +975,6 @@ int commander_thread_main(int argc, char *argv[]) int32_t datalink_loss_timeout = 10; float rc_loss_timeout = 0.5; int32_t datalink_regain_timeout = 0; - int32_t onboard_sysid = 42; /**< systemid of the onboard computer, - telemetry from this sysid is not - validated for the datalink loss check */ /* Thresholds for engine failure detection */ int32_t ef_throttle_thres = 1.0f; @@ -988,7 +982,6 @@ int commander_thread_main(int argc, char *argv[]) int32_t ef_time_thres = 1000.0f; uint64_t timestamp_engine_healthy = 0; /**< absolute time when engine was healty*/ - /* check which state machines for changes, clear "changed" flag */ bool arming_state_changed = false; bool main_state_changed = false; @@ -1051,7 +1044,6 @@ int commander_thread_main(int argc, char *argv[]) param_get(_param_datalink_loss_timeout, &datalink_loss_timeout); param_get(_param_rc_loss_timeout, &rc_loss_timeout); param_get(_param_datalink_regain_timeout, &datalink_regain_timeout); - param_get(_param_onboard_sysid, &onboard_sysid); param_get(_param_ef_throttle_thres, &ef_throttle_thres); param_get(_param_ef_current2throttle_thres, &ef_current2throttle_thres); param_get(_param_ef_time_thres, &ef_time_thres); @@ -1101,7 +1093,6 @@ int commander_thread_main(int argc, char *argv[]) } telemetry_last_heartbeat[i] = telemetry.heartbeat_time; - telemetry_sysid[i] = telemetry.system_id; } } @@ -1590,18 +1581,10 @@ int commander_thread_main(int argc, char *argv[]) mavlink_log_critical(mavlink_fd, "data link %i regained", i); telemetry_lost[i] = false; - - /* If this is not an onboard link/onboard computer: - * set flag that we have a valid link */ - if (telemetry_sysid[i] != (uint8_t)onboard_sysid) { - have_link = true; - } - } else if (!telemetry_lost[i] && telemetry_sysid[i] != - (uint8_t)onboard_sysid) { + have_link = true; + } else if (!telemetry_lost[i]) { /* telemetry was healthy also in last iteration - * we don't have to check a timeout, - * telemetry from onboard computers is not accepted as a valid datalink - */ + * we don't have to check a timeout */ have_link = true; } diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index 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 c7966d56f5457be2eab4d6ee9cfadb7c3f22674b 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(-) 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 fc9120f13..4f92954fd 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 5a0e0d041229700d2ae9c14e4ab2798d34fff14c 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(-) 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 4f92954fd..b63394544 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -127,7 +127,6 @@ Navigator::Navigator() : _loop_perf(perf_alloc(PC_ELAPSED, "navigator")), _geofence{}, _geofence_violation_warning_sent(false), - _fence_valid(false), _inside_fence(true), _navigation_mode(nullptr), _mission(this, "MIS"), @@ -536,7 +535,7 @@ Navigator::status() // warnx("Compass heading in degrees %5.5f", (double)(_global_pos.yaw * M_RAD_TO_DEG_F)); // } - if (_fence_valid) { + if (_geofence.valid()) { warnx("Geofence is valid"); /* TODO: needed? */ // warnx("Vertex longitude latitude"); @@ -544,7 +543,7 @@ Navigator::status() // warnx("%6u %9.5f %8.5f", i, (double)_fence.vertices[i].lon, (double)_fence.vertices[i].lat); } else { - warnx("Geofence not set"); + warnx("Geofence not set (no /etc/geofence.txt on microsd) or not valid"); } } -- cgit v1.2.3 From d113fcfc54c246f3d5ac22ad5485f7103aecab41 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(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index b43f08f67..bf15bbeb6 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 8eb310061693c86a9c8b3c7f6e2bab1ec680c46a Mon Sep 17 00:00:00 2001 From: Vladimir Ermakov Date: Mon, 22 Sep 2014 13:48:43 +0400 Subject: FTP: Rename command. Payload: `\0` See: man 2 rename --- src/modules/mavlink/mavlink_ftp.cpp | 31 +++++++++++++++++++++++++++++++ src/modules/mavlink/mavlink_ftp.h | 2 ++ 2 files changed, 33 insertions(+) diff --git a/src/modules/mavlink/mavlink_ftp.cpp b/src/modules/mavlink/mavlink_ftp.cpp index 14a42ed72..117cc4ec1 100644 --- a/src/modules/mavlink/mavlink_ftp.cpp +++ b/src/modules/mavlink/mavlink_ftp.cpp @@ -204,6 +204,10 @@ MavlinkFTP::_process_request(Request *req) errorCode = _workRemoveFile(payload); break; + case kCmdRename: + errorCode = _workRename(payload); + break; + case kCmdTruncateFile: errorCode = _workTruncateFile(payload); break; @@ -629,6 +633,33 @@ MavlinkFTP::_workReset(PayloadHeader* payload) return kErrNone; } +/// @brief Responds to a Rename command +MavlinkFTP::ErrorCode +MavlinkFTP::_workRename(PayloadHeader* payload) +{ + char oldpath[kMaxDataLength]; + char newpath[kMaxDataLength]; + + char *ptr = _data_as_cstring(payload); + size_t oldpath_sz = strlen(ptr); + + if (oldpath_sz == payload->size) { + // no newpath + errno = EINVAL; + return kErrFailErrno; + } + + strncpy(oldpath, ptr, kMaxDataLength); + strncpy(newpath, ptr + oldpath_sz + 1, kMaxDataLength); + + if (rename(oldpath, newpath) == 0) { + payload->size = 0; + return kErrNone; + } else { + return kErrFailErrno; + } +} + /// @brief Responds to a RemoveDirectory command MavlinkFTP::ErrorCode MavlinkFTP::_workRemoveDirectory(PayloadHeader* payload) diff --git a/src/modules/mavlink/mavlink_ftp.h b/src/modules/mavlink/mavlink_ftp.h index bde8d7ec5..0fbd010be 100644 --- a/src/modules/mavlink/mavlink_ftp.h +++ b/src/modules/mavlink/mavlink_ftp.h @@ -98,6 +98,7 @@ public: kCmdRemoveDirectory, ///< Removes Directory at , must be empty kCmdOpenFileWO, ///< Opens file at for writing, returns kCmdTruncateFile, ///< Truncate file at to length + kCmdRename, ///< Rename to kRspAck = 128, ///< Ack response kRspNak ///< Nak response @@ -152,6 +153,7 @@ private: ErrorCode _workCreateDirectory(PayloadHeader *payload); ErrorCode _workRemoveFile(PayloadHeader *payload); ErrorCode _workTruncateFile(PayloadHeader *payload); + ErrorCode _workRename(PayloadHeader *payload); static const unsigned kRequestQueueSize = 2; ///< Max number of queued requests Request _request_bufs[kRequestQueueSize]; ///< Request buffers which hold work -- cgit v1.2.3 From ef5a93c09ce4bd08729cf3fbf87a6c1d453c646f Mon Sep 17 00:00:00 2001 From: Vladimir Ermakov Date: Mon, 22 Sep 2014 15:19:38 +0400 Subject: FTP: Add file checksum calculation command. --- src/modules/mavlink/mavlink_ftp.cpp | 39 +++++++++++++++++++++++++++++++++++++ src/modules/mavlink/mavlink_ftp.h | 2 ++ 2 files changed, 41 insertions(+) diff --git a/src/modules/mavlink/mavlink_ftp.cpp b/src/modules/mavlink/mavlink_ftp.cpp index 117cc4ec1..c26fe6b4b 100644 --- a/src/modules/mavlink/mavlink_ftp.cpp +++ b/src/modules/mavlink/mavlink_ftp.cpp @@ -34,6 +34,7 @@ /// @file mavlink_ftp.cpp /// @author px4dev, Don Gagne +#include #include #include #include @@ -220,6 +221,11 @@ MavlinkFTP::_process_request(Request *req) errorCode = _workRemoveDirectory(payload); break; + + case kCmdCalcFileCRC32: + errorCode = _workCalcFileCRC32(payload); + break; + default: errorCode = kErrUnknownCommand; break; @@ -690,6 +696,39 @@ MavlinkFTP::_workCreateDirectory(PayloadHeader* payload) } } +/// @brief Responds to a CalcFileCRC32 command +MavlinkFTP::ErrorCode +MavlinkFTP::_workCalcFileCRC32(PayloadHeader* payload) +{ + char file_buf[256]; + uint32_t checksum = 0; + ssize_t bytes_read; + strncpy(file_buf, _data_as_cstring(payload), kMaxDataLength); + + int fd = ::open(file_buf, O_RDONLY); + if (fd < 0) { + return kErrFailErrno; + } + + do { + bytes_read = ::read(fd, file_buf, sizeof(file_buf)); + if (bytes_read < 0) { + int r_errno = errno; + ::close(fd); + errno = r_errno; + return kErrFailErrno; + } + + checksum = crc32part((uint8_t*)file_buf, bytes_read, checksum); + } while (bytes_read == sizeof(file_buf)); + + ::close(fd); + + payload->size = sizeof(uint32_t); + *((uint32_t*)payload->data) = checksum; + return kErrNone; +} + /// @brief Returns true if the specified session is a valid open session bool MavlinkFTP::_valid_session(unsigned index) diff --git a/src/modules/mavlink/mavlink_ftp.h b/src/modules/mavlink/mavlink_ftp.h index 0fbd010be..bef6775a9 100644 --- a/src/modules/mavlink/mavlink_ftp.h +++ b/src/modules/mavlink/mavlink_ftp.h @@ -99,6 +99,7 @@ public: kCmdOpenFileWO, ///< Opens file at for writing, returns kCmdTruncateFile, ///< Truncate file at to length kCmdRename, ///< Rename to + kCmdCalcFileCRC32, ///< Calculate CRC32 for file at kRspAck = 128, ///< Ack response kRspNak ///< Nak response @@ -154,6 +155,7 @@ private: ErrorCode _workRemoveFile(PayloadHeader *payload); ErrorCode _workTruncateFile(PayloadHeader *payload); ErrorCode _workRename(PayloadHeader *payload); + ErrorCode _workCalcFileCRC32(PayloadHeader *payload); static const unsigned kRequestQueueSize = 2; ///< Max number of queued requests Request _request_bufs[kRequestQueueSize]; ///< Request buffers which hold work -- 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(-) 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 ec09f080083fc297228857397eef0651085887d9 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(-) diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp index 606521f20..7ae7f60cb 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(+) 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 d9bb3a51124cb3006f7548ac6ee629b48a939336 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 23 Sep 2014 06:08:24 +1000 Subject: viper: removed wrong failsafe values --- ROMFS/px4fmu_common/init.d/3035_viper | 2 -- 1 file changed, 2 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/3035_viper b/ROMFS/px4fmu_common/init.d/3035_viper index ac4d919ce..f3b0e8418 100644 --- a/ROMFS/px4fmu_common/init.d/3035_viper +++ b/ROMFS/px4fmu_common/init.d/3035_viper @@ -9,5 +9,3 @@ sh /etc/init.d/rc.fw_defaults set MIXER Viper -set FAILSAFE "-c56 -p 1000" -set FAILSAFE "-c7 -p 1200" -- 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(-) 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(-) 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 7dd81c8cb2a05321bcb28819d6eb3aec67052517 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 27 Sep 2014 16:21:25 +0200 Subject: WIP on laser driver unit test --- Tools/tests-host/Makefile | 11 ++- Tools/tests-host/sf0x_data.txt | 33 +++++++++ Tools/tests-host/sf0x_test | Bin 0 -> 9228 bytes Tools/tests-host/sf0x_test.cpp | 157 +++++++++++++++++++++++++++++++++++++++++ 4 files changed, 199 insertions(+), 2 deletions(-) create mode 100644 Tools/tests-host/sf0x_data.txt create mode 100755 Tools/tests-host/sf0x_test create mode 100644 Tools/tests-host/sf0x_test.cpp diff --git a/Tools/tests-host/Makefile b/Tools/tests-host/Makefile index f0737ef88..b1547c23f 100644 --- a/Tools/tests-host/Makefile +++ b/Tools/tests-host/Makefile @@ -3,7 +3,7 @@ CC=g++ CFLAGS=-I. -I../../src/modules -I ../../src/include -I../../src/drivers \ -I../../src -I../../src/lib -D__EXPORT="" -Dnullptr="0" -lm -all: mixer_test sbus2_test autodeclination_test +all: mixer_test sbus2_test autodeclination_test sf0x_test MIXER_FILES=../../src/systemcmds/tests/test_mixer.cpp \ ../../src/systemcmds/tests/test_conv.cpp \ @@ -20,6 +20,10 @@ SBUS2_FILES=../../src/modules/px4iofirmware/sbus.c \ hrt.cpp \ sbus2_test.cpp +SF0X_FILES= \ + hrt.cpp \ + sf0x_test.cpp + AUTODECLINATION_FILES= ../../src/lib/geo/geo_mag_declination.c \ hrt.cpp \ autodeclination_test.cpp @@ -30,10 +34,13 @@ mixer_test: $(MIXER_FILES) sbus2_test: $(SBUS2_FILES) $(CC) -o sbus2_test $(SBUS2_FILES) $(CFLAGS) +sf0x_test: $(SF0X_FILES) + $(CC) -o sf0x_test $(SF0X_FILES) $(CFLAGS) + autodeclination_test: $(SBUS2_FILES) $(CC) -o autodeclination_test $(AUTODECLINATION_FILES) $(CFLAGS) .PHONY: clean clean: - rm -f $(ODIR)/*.o *~ core $(INCDIR)/*~ mixer_test sbus2_test autodeclination_test \ No newline at end of file + rm -f $(ODIR)/*.o *~ core $(INCDIR)/*~ mixer_test sbus2_test autodeclination_test sf0x_test \ No newline at end of file diff --git a/Tools/tests-host/sf0x_data.txt b/Tools/tests-host/sf0x_data.txt new file mode 100644 index 000000000..f61f59523 --- /dev/null +++ b/Tools/tests-host/sf0x_data.txt @@ -0,0 +1,33 @@ +0.02 +0.25 +0.03 +0.02 +0.25 +0.03 +0.02 +0.25 +0.03 +0.02 +0.25 +0.03 +0.25 +0.03 +0.02 +0.25 +0.03 +0.08 +0.40 +0.44 + + +0 +. +0 +1 + +0 +0. +.0 +.01 +01 +1 \ No newline at end of file diff --git a/Tools/tests-host/sf0x_test b/Tools/tests-host/sf0x_test new file mode 100755 index 000000000..e56c35465 Binary files /dev/null and b/Tools/tests-host/sf0x_test differ diff --git a/Tools/tests-host/sf0x_test.cpp b/Tools/tests-host/sf0x_test.cpp new file mode 100644 index 000000000..d41f7e54a --- /dev/null +++ b/Tools/tests-host/sf0x_test.cpp @@ -0,0 +1,157 @@ + +#include +#include +#include +#include +#include +#include + +enum SF0X_PARSE_STATE { + SF0X_PARSE_STATE0_UNSYNC = 0, + SF0X_PARSE_STATE1_SYNC, + SF0X_PARSE_STATE2_GOT_DIGIT0, + SF0X_PARSE_STATE3_GOT_DOT, + SF0X_PARSE_STATE4_GOT_DIGIT1, + SF0X_PARSE_STATE5_GOT_DIGIT2, + SF0X_PARSE_STATE6_GOT_CARRIAGE_RETURN +}; + +int sf0x_parser(char c, char *parserbuf, unsigned *parserbuf_index, enum *SF0X_PARSE_STATE state, float *dist) +{ + int ret = -1; + + unsigned len = strlen(parserbuf); + switch (state) { + case SF0X_PARSE_STATE0_UNSYNC: + if (c == '\n') { + *state = SF0X_PARSE_STATE1_SYNC; + *parserbuf_index = 0; + } + break; + + case SF0X_PARSE_STATE1_SYNC: + if (c >= '0' && c <= '9') { + *state = SF0X_PARSE_STATE2_GOT_DIGIT0; + parserbuf[parserbuf_index] = c; + parserbuf_index++; + } + break; + + case SF0X_PARSE_STATE2_GOT_DIGIT0: + if (c >= '0' && c <= '9') { + *state = SF0X_PARSE_STATE2_GOT_DIGIT0; + parserbuf[parserbuf_index] = c; + parserbuf_index++; + } else if (c == '.') { + *state = SF0X_PARSE_STATE3_GOT_DOT; + parserbuf[parserbuf_index] = c; + parserbuf_index++; + } else { + *state = SF0X_PARSE_STATE0_UNSYNC; + } + break; + + case SF0X_PARSE_STATE3_GOT_DOT: + if (c >= '0' && c <= '9') { + *state = SF0X_PARSE_STATE4_GOT_DIGIT1; + parserbuf[parserbuf_index] = c; + parserbuf_index++; + } else { + *state = SF0X_PARSE_STATE0_UNSYNC; + } + break; + + case SF0X_PARSE_STATE4_GOT_DIGIT1: + if (c >= '0' && c <= '9') { + *state = SF0X_PARSE_STATE4_GOT_DIGIT2; + parserbuf[parserbuf_index] = c; + parserbuf_index++; + } else { + *state = SF0X_PARSE_STATE0_UNSYNC; + } + break; + + case SF0X_PARSE_STATE5_GOT_DIGIT2: + if (c == '\r') { + *state = SF0X_PARSE_STATE6_GOT_CARRIAGE_RETURN; + } else { + *state = SF0X_PARSE_STATE0_UNSYNC; + } + break; + + case SF0X_PARSE_STATE6_GOT_CARRIAGE_RETURN: + if (c == '\n') { + parserbuf[parserbuf_index] = '\0'; + *dist = strtod(linebuf, &end); + *state = SF0X_PARSE_STATE0_SYNC; + *parserbuf_index = 0; + ret = 0; + } else { + *state = SF0X_PARSE_STATE0_UNSYNC; + } + break; + } + + + + return ret; +} + +int main(int argc, char *argv[]) { + warnx("SF0X test started"); + + if (argc < 2) + errx(1, "Need a filename for the input file"); + + warnx("loading data from: %s", argv[1]); + + FILE *fp; + + fp = fopen(argv[1],"rt"); + + if (!fp) + errx(1, "failed opening file"); + + int ret = 0; + + const char LINE_MAX = 20; + char _linebuf[LINE_MAX]; + _linebuf[0] = '\0'; + + char *end; + + enum SF0X_PARSE_STATE state = SF0X_PARSE_STATE0_UNSYNC; + float dist_m; + char _parserbuf[LINE_MAX]; + unsigned _parsebuf_index = 0; + + while (fgets(_linebuf, LINE_MAX, fp) != NULL) { + + printf("\n%s", _linebuf); + + int parse_ret; + + for (int i = 0; i < strlen(_linebuf); i++) + { + printf("%0x ", _linebuf[i]); + parse_ret = sf0x_parser(_linebuf[i], _parserbuf, &_parsebuf_index, &state, &dist_m); + + if (parse_ret == 0) { + printf("PARSED!"); + } + } + + printf("\nparsed: %f %s\n", dist_m, (parse_ret == 0) ? "OK" : ""); + } + + // Init the parser + + + if (ret == EOF) { + warnx("Test finished, reached end of file"); + } else { + warnx("Test aborted, errno: %d", ret); + } + + return ret; +} -- cgit v1.2.3 From 8a18cfa3869555389e7e9ff8f104d83f9c54cb43 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(-) 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 cf4604f5c3614bc5cd1b82dfee693cbac36181ab Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 28 Sep 2014 13:34:44 +0400 Subject: navigator/mission.cpp: indentation fixed --- src/modules/navigator/mission.cpp | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 128dab7d7..719301a0a 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -417,13 +417,13 @@ Mission::set_mission_items() if (_navigator->get_global_position()->alt < takeoff_alt - _navigator->get_acceptance_radius()) { mavlink_log_critical(_navigator->get_mavlink_fd(), "takeoff to %.1f meters above home", (double)(takeoff_alt - _navigator->get_home_position()->alt)); - _mission_item.nav_cmd = NAV_CMD_TAKEOFF; - _mission_item.lat = _navigator->get_global_position()->lat; - _mission_item.lon = _navigator->get_global_position()->lon; - _mission_item.altitude = takeoff_alt; - _mission_item.altitude_is_relative = false; - _mission_item.autocontinue = true; - _mission_item.time_inside = 0; + _mission_item.nav_cmd = NAV_CMD_TAKEOFF; + _mission_item.lat = _navigator->get_global_position()->lat; + _mission_item.lon = _navigator->get_global_position()->lon; + _mission_item.altitude = takeoff_alt; + _mission_item.altitude_is_relative = false; + _mission_item.autocontinue = true; + _mission_item.time_inside = 0; mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current); -- cgit v1.2.3 From 2d81c2cf466596b7c4f787a52836fd200af2a097 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 28 Sep 2014 13:43:42 +0400 Subject: mc_pos_control: commented code block removed --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 10 ---------- 1 file changed, 10 deletions(-) diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index ce26f7dc2..99deb0d29 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -1022,16 +1022,6 @@ MulticopterPositionControl::task_main() _vel_sp(2) = _params.land_speed; } - -// if (!_control_mode.flag_control_manual_enabled) { -// /* limit 3D speed only in non-manual modes */ -// float vel_sp_norm = _vel_sp.edivide(_params.vel_max).length(); -// -// if (vel_sp_norm > 1.0f) { -// _vel_sp /= vel_sp_norm; -// } -// } - _global_vel_sp.vx = _vel_sp(0); _global_vel_sp.vy = _vel_sp(1); _global_vel_sp.vz = _vel_sp(2); -- cgit v1.2.3 From 964fddb387b8e635d2cd13fb2248643791304489 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 28 Sep 2014 16:08:25 +0200 Subject: make geofence update rate independent from positon update rate --- src/modules/navigator/navigator_main.cpp | 12 +++++++----- 1 file changed, 7 insertions(+), 5 deletions(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index b63394544..fad46998e 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -87,6 +87,7 @@ */ extern "C" __EXPORT int navigator_main(int argc, char *argv[]); +#define GEOFENCE_CHECK_INTERVAL 200000 namespace navigator { @@ -343,7 +344,7 @@ Navigator::task_main() _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); } - bool have_geofence_position_data = false; + static bool have_geofence_position_data = false; /* gps updated */ if (fds[7].revents & POLLIN) { @@ -388,17 +389,18 @@ Navigator::task_main() if (fds[0].revents & POLLIN) { global_position_update(); static int gposcounter = 0; - if (_geofence.getSource() == Geofence::GF_SOURCE_GLOBALPOS && - gposcounter % 10 == 0) { - /* Geofence is checked only every 10th gpos update */ + if (_geofence.getSource() == Geofence::GF_SOURCE_GLOBALPOS) { have_geofence_position_data = true; } gposcounter++; } /* Check geofence violation */ - if (have_geofence_position_data) { + static hrt_abstime last_geofence_check = 0; + if (have_geofence_position_data && hrt_elapsed_time(&last_geofence_check) > GEOFENCE_CHECK_INTERVAL) { bool inside = _geofence.inside(_global_pos, _gps_pos, _sensor_combined.baro_alt_meter); + last_geofence_check = hrt_absolute_time(); + have_geofence_position_data = false; if (!inside) { /* inform other apps via the mission result */ _mission_result.geofence_violated = true; -- cgit v1.2.3 From 038e1cac03198259d6f7630c6bb7c65c35f44fae Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 28 Sep 2014 16:17:17 +0200 Subject: increase default engine failure threshold --- src/modules/commander/commander_params.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index f4bc7fd7f..1b0c4258b 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -159,7 +159,7 @@ PARAM_DEFINE_FLOAT(COM_EF_C2T, 5.0f); * @min 0.0f * @max 7.0f */ -PARAM_DEFINE_FLOAT(COM_EF_TIME, 5.0f); +PARAM_DEFINE_FLOAT(COM_EF_TIME, 10.0f); /** RC loss time threshold * -- cgit v1.2.3 From ab400089bc2a42f1f0ace569d8f0ee58f4338e1d Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 28 Sep 2014 16:17:40 +0200 Subject: disable flight termination as default for now --- src/modules/systemlib/circuit_breaker.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/systemlib/circuit_breaker.c b/src/modules/systemlib/circuit_breaker.c index 9e5429988..64420be37 100644 --- a/src/modules/systemlib/circuit_breaker.c +++ b/src/modules/systemlib/circuit_breaker.c @@ -106,7 +106,7 @@ PARAM_DEFINE_INT32(CBRK_AIRSPD_CHK, 0); * @max 121212 * @group Circuit Breaker */ -PARAM_DEFINE_INT32(CBRK_FLIGHTTERM, 0); +PARAM_DEFINE_INT32(CBRK_FLIGHTTERM, 121212); /** * Circuit breaker for engine failure detection -- cgit v1.2.3 From 70e5d4027a3b1465d5128dbf9a04cbb6545e043d Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 30 Sep 2014 09:08:31 +0400 Subject: navigator: autocontinue fix --- src/modules/navigator/mission.cpp | 26 +++++++++++++++++--------- 1 file changed, 17 insertions(+), 9 deletions(-) diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 104065132..8bacaa425 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -150,8 +150,19 @@ Mission::on_active() /* lets check if we reached the current mission item */ if (_mission_type != MISSION_TYPE_NONE && is_mission_item_reached()) { - advance_mission(); - set_mission_items(); + if (_mission_item.autocontinue) { + /* switch to next waypoint if 'autocontinue' flag set */ + advance_mission(); + set_mission_items(); + + } else { + /* else just report that item reached */ + if (_mission_type == MISSION_TYPE_OFFBOARD) { + if (!(_mission_result.seq_reached == _current_offboard_mission_index && _mission_result.reached)) { + report_mission_item_reached(); + } + } + } } else if (_mission_type != MISSION_TYPE_NONE &&_param_altmode.get() == MISSION_ALTMODE_FOH) { altitude_sp_foh_update(); @@ -694,10 +705,8 @@ Mission::save_offboard_mission_state() void Mission::report_mission_item_reached() { - if (_mission_type == MISSION_TYPE_OFFBOARD) { - _mission_result.reached = true; - _mission_result.seq_reached = _current_offboard_mission_index; - } + _mission_result.reached = true; + _mission_result.seq_reached = _current_offboard_mission_index; publish_mission_result(); } @@ -705,6 +714,8 @@ void Mission::report_current_offboard_mission_item() { warnx("current offboard mission index: %d", _current_offboard_mission_index); + _mission_result.reached = false; + _mission_result.finished = false; _mission_result.seq_current = _current_offboard_mission_index; publish_mission_result(); @@ -730,7 +741,4 @@ Mission::publish_mission_result() /* advertise and publish */ _mission_result_pub = orb_advertise(ORB_ID(mission_result), &_mission_result); } - /* reset reached bool */ - _mission_result.reached = false; - _mission_result.finished = false; } -- cgit v1.2.3 From 3e1eec5906ea8df955bf6d0ac7ec182979f47c90 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Wed, 10 Sep 2014 14:42:32 +0900 Subject: mpu6k: set hardware filter during ACCELIOCLOWPASS also set from GYROIOCLOWPASS Conflicts: mavlink/include/mavlink/v1.0 --- src/drivers/mpu6000/mpu6000.cpp | 15 +++++---------- 1 file changed, 5 insertions(+), 10 deletions(-) diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index 6f5dae7ad..fb38c7e67 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -968,11 +968,9 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg) return _accel_filter_x.get_cutoff_freq(); case ACCELIOCSLOWPASS: - if (arg == 0) { - // allow disabling of on-chip filter using - // zero as desired filter frequency - _set_dlpf_filter(0); - } + // set hardware filtering + _set_dlpf_filter(arg); + // set software filtering _accel_filter_x.set_cutoff_frequency(1.0e6f / _call_interval, arg); _accel_filter_y.set_cutoff_frequency(1.0e6f / _call_interval, arg); _accel_filter_z.set_cutoff_frequency(1.0e6f / _call_interval, arg); @@ -1056,11 +1054,8 @@ MPU6000::gyro_ioctl(struct file *filp, int cmd, unsigned long arg) _gyro_filter_x.set_cutoff_frequency(1.0e6f / _call_interval, arg); _gyro_filter_y.set_cutoff_frequency(1.0e6f / _call_interval, arg); _gyro_filter_z.set_cutoff_frequency(1.0e6f / _call_interval, arg); - if (arg == 0) { - // allow disabling of on-chip filter using 0 - // as desired frequency - _set_dlpf_filter(0); - } + // set hardware filtering + _set_dlpf_filter(arg); return OK; case GYROIOCSSCALE: -- cgit v1.2.3 From 8ced6bb49bf32128d2673e565cb112ca6d2f21eb Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 30 Sep 2014 10:05:39 +0200 Subject: Set filter frequency for hardware and software in parallel, always do so in the same order --- src/drivers/mpu6000/mpu6000.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index fb38c7e67..b22bb2e07 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -910,12 +910,14 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg) // adjust filters float cutoff_freq_hz = _accel_filter_x.get_cutoff_freq(); float sample_rate = 1.0e6f/ticks; + _set_dlpf_filter(cutoff_freq_hz); _accel_filter_x.set_cutoff_frequency(sample_rate, cutoff_freq_hz); _accel_filter_y.set_cutoff_frequency(sample_rate, cutoff_freq_hz); _accel_filter_z.set_cutoff_frequency(sample_rate, cutoff_freq_hz); float cutoff_freq_hz_gyro = _gyro_filter_x.get_cutoff_freq(); + _set_dlpf_filter(cutoff_freq_hz_gyro); _gyro_filter_x.set_cutoff_frequency(sample_rate, cutoff_freq_hz_gyro); _gyro_filter_y.set_cutoff_frequency(sample_rate, cutoff_freq_hz_gyro); _gyro_filter_z.set_cutoff_frequency(sample_rate, cutoff_freq_hz_gyro); @@ -1051,11 +1053,11 @@ MPU6000::gyro_ioctl(struct file *filp, int cmd, unsigned long arg) case GYROIOCGLOWPASS: return _gyro_filter_x.get_cutoff_freq(); case GYROIOCSLOWPASS: + // set hardware filtering + _set_dlpf_filter(arg); _gyro_filter_x.set_cutoff_frequency(1.0e6f / _call_interval, arg); _gyro_filter_y.set_cutoff_frequency(1.0e6f / _call_interval, arg); _gyro_filter_z.set_cutoff_frequency(1.0e6f / _call_interval, arg); - // set hardware filtering - _set_dlpf_filter(arg); return OK; case GYROIOCSSCALE: -- cgit v1.2.3 From 70606d400bdceaa86331670938c1e0ae988ed97c Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 30 Sep 2014 11:11:30 +0200 Subject: remove wrong comments --- src/modules/navigator/navigator_main.cpp | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index fad46998e..bf5e36d39 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -432,9 +432,6 @@ Navigator::task_main() _can_loiter_at_sp = false; break; case NAVIGATION_STATE_AUTO_MISSION: - /* Some failsafe modes prohibit the fallback to mission - * usually this is done after some time to make sure - * that the full failsafe operation is performed */ _navigation_mode = &_mission; break; case NAVIGATION_STATE_AUTO_LOITER: @@ -450,7 +447,7 @@ Navigator::task_main() case NAVIGATION_STATE_AUTO_RTL: _navigation_mode = &_rtl; break; - case NAVIGATION_STATE_AUTO_RTGS: //XXX OBC: differentiate between rc loss and dl loss here + case NAVIGATION_STATE_AUTO_RTGS: /* Use complex data link loss mode only when enabled via param * otherwise use rtl */ if (_param_datalinkloss_obc.get() != 0) { -- cgit v1.2.3 From 1072a3380c2d6bdea010bb5091c6d0b23fe6f224 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 30 Sep 2014 11:11:46 +0200 Subject: enable engine failure circuit breaker --- src/modules/systemlib/circuit_breaker.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/systemlib/circuit_breaker.c b/src/modules/systemlib/circuit_breaker.c index 64420be37..83c8344d5 100644 --- a/src/modules/systemlib/circuit_breaker.c +++ b/src/modules/systemlib/circuit_breaker.c @@ -120,7 +120,7 @@ PARAM_DEFINE_INT32(CBRK_FLIGHTTERM, 121212); * @max 284953 * @group Circuit Breaker */ -PARAM_DEFINE_INT32(CBRK_ENGINEFAIL, 0); +PARAM_DEFINE_INT32(CBRK_ENGINEFAIL, 284953); bool circuit_breaker_enabled(const char* breaker, int32_t magic) { -- cgit v1.2.3 From d4c0dc2ba0271f4d9c8044fd2a3a178cbb9987e3 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 30 Sep 2014 11:20:30 +0200 Subject: add and activate circuit breaker for gps failure detection --- src/modules/commander/commander.cpp | 5 +++-- src/modules/systemlib/circuit_breaker.c | 14 ++++++++++++++ src/modules/systemlib/circuit_breaker.h | 1 + 3 files changed, 18 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index bf15bbeb6..b86f3678b 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1417,8 +1417,9 @@ int commander_thread_main(int argc, char *argv[]) } /* check if GPS fix is ok */ - if (gps_position.fix_type >= 3 && //XXX check eph and epv ? - hrt_elapsed_time(&gps_position.timestamp_position) < FAILSAFE_DEFAULT_TIMEOUT) { + if (circuit_breaker_enabled("CBRK_GPSFAIL", CBRK_GPSFAIL_KEY) || + (gps_position.fix_type >= 3 && + hrt_elapsed_time(&gps_position.timestamp_position) < FAILSAFE_DEFAULT_TIMEOUT)) { /* handle the case where gps was regained */ if (status.gps_failure) { status.gps_failure = false; diff --git a/src/modules/systemlib/circuit_breaker.c b/src/modules/systemlib/circuit_breaker.c index 83c8344d5..12187d70e 100644 --- a/src/modules/systemlib/circuit_breaker.c +++ b/src/modules/systemlib/circuit_breaker.c @@ -122,6 +122,20 @@ PARAM_DEFINE_INT32(CBRK_FLIGHTTERM, 121212); */ PARAM_DEFINE_INT32(CBRK_ENGINEFAIL, 284953); +/** + * Circuit breaker for gps failure detection + * + * Setting this parameter to 240024 will disable the gps failure detection. + * If the aircraft is in gps failure mode the gps failure flag will be + * set to healthy + * WARNING: ENABLING THIS CIRCUIT BREAKER IS AT OWN RISK + * + * @min 0 + * @max 240024 + * @group Circuit Breaker + */ +PARAM_DEFINE_INT32(CBRK_GPSFAIL, 240024); + bool circuit_breaker_enabled(const char* breaker, int32_t magic) { int32_t val; diff --git a/src/modules/systemlib/circuit_breaker.h b/src/modules/systemlib/circuit_breaker.h index 6a55e4948..b3431722f 100644 --- a/src/modules/systemlib/circuit_breaker.h +++ b/src/modules/systemlib/circuit_breaker.h @@ -55,6 +55,7 @@ #define CBRK_AIRSPD_CHK_KEY 162128 #define CBRK_FLIGHTTERM_KEY 121212 #define CBRK_ENGINEFAIL_KEY 284953 +#define CBRK_GPSFAIL_KEY 240024 #include -- cgit v1.2.3 From 8b7c57a0d050d51f45f8c66536e5450e7a494d73 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 30 Sep 2014 13:39:40 +0200 Subject: px4io driver: update cb only when changed --- src/drivers/px4io/px4io.cpp | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 6f8d6f5f6..fbb5d4f2e 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -295,6 +295,7 @@ private: float _battery_amp_bias; ///< current sensor bias float _battery_mamphour_total;///< amp hours consumed so far uint64_t _battery_last_timestamp;///< last amp hour calculation timestamp + bool _cb_flighttermination; ///< true if the flight termination circuit breaker is enabled #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 bool _dsm_vcc_ctl; ///< true if relay 1 controls DSM satellite RX power @@ -515,7 +516,8 @@ PX4IO::PX4IO(device::Device *interface) : _battery_amp_per_volt(90.0f / 5.0f), // this matches the 3DR current sensor _battery_amp_bias(0), _battery_mamphour_total(0), - _battery_last_timestamp(0) + _battery_last_timestamp(0), + _cb_flighttermination(true) #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 , _dsm_vcc_ctl(false) #endif @@ -1051,6 +1053,9 @@ PX4IO::task_main() } } + /* Update Circuit breakers */ + _cb_flighttermination = circuit_breaker_enabled("CBRK_FLIGHTTERM", CBRK_FLIGHTTERM_KEY); + } } @@ -1170,7 +1175,7 @@ PX4IO::io_set_arming_state() } /* Do not set failsafe if circuit breaker is enabled */ - if (armed.force_failsafe && !circuit_breaker_enabled("CBRK_FLIGHTTERM", CBRK_FLIGHTTERM_KEY)) { + if (armed.force_failsafe && !_cb_flighttermination) { set |= PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE; } else { clear |= PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE; -- cgit v1.2.3 From d11f05b12c0b0c56703ab14303f91efd123b9dc6 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 30 Sep 2014 13:40:03 +0200 Subject: commander: update gps and engine cb only when changed --- src/modules/commander/commander.cpp | 20 ++++++++++++++------ src/modules/uORB/topics/vehicle_status.h | 2 ++ 2 files changed, 16 insertions(+), 6 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index b86f3678b..9ebe006f0 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -775,6 +775,8 @@ int commander_thread_main(int argc, char *argv[]) // CIRCUIT BREAKERS status.circuit_breaker_engaged_power_check = false; status.circuit_breaker_engaged_airspd_check = false; + status.circuit_breaker_engaged_enginefailure_check = false; + status.circuit_breaker_engaged_gpsfailure_check = false; /* publish initial state */ status_pub = orb_advertise(ORB_ID(vehicle_status), &status); @@ -980,8 +982,8 @@ int commander_thread_main(int argc, char *argv[]) int32_t ef_throttle_thres = 1.0f; int32_t ef_current2throttle_thres = 0.0f; int32_t ef_time_thres = 1000.0f; - uint64_t timestamp_engine_healthy = 0; /**< absolute time when engine - was healty*/ + uint64_t timestamp_engine_healthy = 0; /**< absolute time when engine was healty */ + /* check which state machines for changes, clear "changed" flag */ bool arming_state_changed = false; bool main_state_changed = false; @@ -1028,8 +1030,14 @@ int commander_thread_main(int argc, char *argv[]) param_get(_param_system_id, &(status.system_id)); param_get(_param_component_id, &(status.component_id)); - status.circuit_breaker_engaged_power_check = circuit_breaker_enabled("CBRK_SUPPLY_CHK", CBRK_SUPPLY_CHK_KEY); - status.circuit_breaker_engaged_airspd_check = circuit_breaker_enabled("CBRK_AIRSPD_CHK", CBRK_AIRSPD_CHK_KEY); + status.circuit_breaker_engaged_power_check = + circuit_breaker_enabled("CBRK_SUPPLY_CHK", CBRK_SUPPLY_CHK_KEY); + status.circuit_breaker_engaged_airspd_check = + circuit_breaker_enabled("CBRK_AIRSPD_CHK", CBRK_AIRSPD_CHK_KEY); + status.circuit_breaker_engaged_enginefailure_check = + circuit_breaker_enabled("CBRK_ENGINEFAIL", CBRK_ENGINEFAIL_KEY); + status.circuit_breaker_engaged_gpsfailure_check = + circuit_breaker_enabled("CBRK_GPSFAIL", CBRK_GPSFAIL_KEY); status_changed = true; @@ -1417,7 +1425,7 @@ int commander_thread_main(int argc, char *argv[]) } /* check if GPS fix is ok */ - if (circuit_breaker_enabled("CBRK_GPSFAIL", CBRK_GPSFAIL_KEY) || + if (status.circuit_breaker_engaged_gpsfailure_check || (gps_position.fix_type >= 3 && hrt_elapsed_time(&gps_position.timestamp_position) < FAILSAFE_DEFAULT_TIMEOUT)) { /* handle the case where gps was regained */ @@ -1616,7 +1624,7 @@ int commander_thread_main(int argc, char *argv[]) /* Check engine failure * only for fixed wing for now */ - if (!circuit_breaker_enabled("CBRK_ENGINEFAIL", CBRK_ENGINEFAIL_KEY) && + if (!status.circuit_breaker_engaged_enginefailure_check && status.is_rotary_wing == false && armed.armed && ((actuator_controls.control[3] > ef_throttle_thres && diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index 9dccb2309..505039d90 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -239,6 +239,8 @@ struct vehicle_status_s { bool circuit_breaker_engaged_power_check; bool circuit_breaker_engaged_airspd_check; + bool circuit_breaker_engaged_enginefailure_check; + bool circuit_breaker_engaged_gpsfailure_check; }; /** -- cgit v1.2.3 From 213f4aadbdd089c3f793ba7ac13e0bc3b9b46fee Mon Sep 17 00:00:00 2001 From: Denis Yeldandi Date: Tue, 30 Sep 2014 15:44:47 +0400 Subject: Ashtech GPS driver --- src/drivers/drv_gps.h | 3 +- src/drivers/gps/ashtech.cpp | 616 +++++++++++++++++++++++++ src/drivers/gps/ashtech.h | 96 ++++ src/drivers/gps/gps.cpp | 18 + src/drivers/gps/module.mk | 1 + src/modules/uORB/topics/vehicle_gps_position.h | 2 +- 6 files changed, 734 insertions(+), 2 deletions(-) create mode 100644 src/drivers/gps/ashtech.cpp create mode 100644 src/drivers/gps/ashtech.h diff --git a/src/drivers/drv_gps.h b/src/drivers/drv_gps.h index e14f4e00d..76a211000 100644 --- a/src/drivers/drv_gps.h +++ b/src/drivers/drv_gps.h @@ -57,7 +57,8 @@ typedef enum { GPS_DRIVER_MODE_NONE = 0, GPS_DRIVER_MODE_UBX, - GPS_DRIVER_MODE_MTK + GPS_DRIVER_MODE_MTK, + GPS_DRIVER_MODE_ASHTECH } gps_driver_mode_t; diff --git a/src/drivers/gps/ashtech.cpp b/src/drivers/gps/ashtech.cpp new file mode 100644 index 000000000..d338ff8e5 --- /dev/null +++ b/src/drivers/gps/ashtech.cpp @@ -0,0 +1,616 @@ +#include "ashtech.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +typedef double float64_t; +typedef float float32_t; + +char* str_scanDec(const char *pos, int8_t sign, int8_t n_max_digit, int32_t *result) +{ + int8_t n=0; + int32_t d=0; + int8_t neg = 0; + + if(*pos == '-') { neg = 1; pos++; } + else if(*pos == '+') { pos++; } + else if(sign) return(NULL); + while(*pos >= '0' && *pos <= '9') + { + d = d*10 + (*(pos++) - '0'); + n++; + if(n_max_digit>0 && n==n_max_digit) break; + } + if(n==0 || n>10) return(NULL); + if(neg) *result = -d; + else *result = d; + return((char *)pos); +} + +char* scanFloat64(const char *pos, int8_t sign, int8_t n_max_int, int8_t n_max_frac, + float64_t *result) +{ + float64_t f=0.0, div=1.0; + int32_t d_int; + int8_t n=0, isneg= 0; + + if( *pos == '-' ) isneg= 1; + if((pos = str_scanDec(pos, sign, n_max_int, &d_int)) == NULL) return(NULL); + if(*(pos) == '.') + { + pos++; + while(*pos >= '0' && *pos <= '9') + { + f = f*(10.0) + (float64_t)(*(pos++) - '0'); + div *= (0.1); + n++; + if(n_max_frac>0 && n==n_max_frac) break; + } + } + else if(n_max_frac > 0) return(NULL); + if( isneg ) *result = (float64_t)d_int - f*div; + else *result = (float64_t)d_int + f*div; + return((char *)pos); +} + + + +ASHTECH::ASHTECH(const int &fd, struct vehicle_gps_position_s *gps_position, struct satellite_info_s *satellite_info): +_fd(fd), +_satellite_info(satellite_info), +_gps_position(gps_position) +{ + decode_init(); + _decode_state = NME_DECODE_UNINIT; + _rx_buffer_bytes = 0; +} + +ASHTECH::~ASHTECH(){ +} +//All NMEA descriptions are taken from +//http://www.trimble.com/OEM_ReceiverHelp/V4.44/en/NMEA-0183messages_MessageOverview.html +int ASHTECH::handle_message(int len){ + if(len < 7) return 0; + int uiCalcComma = 0; + for(int i = 0 ; i < len; i++){ + if(_rx_buffer[i] == ',')uiCalcComma++; + } + char* bufptr = (char*)( _rx_buffer+6); + + if((memcmp(_rx_buffer+3, "ZDA,",3) == 0)&&(uiCalcComma == 6)){ + /* + UTC day, month, and year, and local time zone offset + An example of the ZDA message string is: + + $GPZDA,172809.456,12,07,1996,00,00*45 + + ZDA message fields + Field Meaning + 0 Message ID $GPZDA + 1 UTC + 2 Day, ranging between 01 and 31 + 3 Month, ranging between 01 and 12 + 4 Year + 5 Local time zone offset from GMT, ranging from 00 through �13 hours + 6 Local time zone offset from GMT, ranging from 00 through 59 minutes + 7 The checksum data, always begins with * + Fields 5 and 6 together yield the total offset. For example, if field 5 is -5 and field 6 is +15, local time is 5 hours and 15 minutes earlier than GMT. + */ + float64_t ashtech_time = 0.0; + int day = 0, month = 0,year = 0,local_time_off_hour = 0,local_time_off_min = 0; + if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&ashtech_time); + if(bufptr && *(++bufptr) != ',') bufptr = str_scanDec(bufptr, 0, 9, &day); + if(bufptr && *(++bufptr) != ',') bufptr = str_scanDec(bufptr, 0, 9, &month); + if(bufptr && *(++bufptr) != ',') bufptr = str_scanDec(bufptr, 0, 9, &year); + if(bufptr && *(++bufptr) != ',') bufptr = str_scanDec(bufptr, 0, 9, &local_time_off_hour); + if(bufptr && *(++bufptr) != ',') bufptr = str_scanDec(bufptr, 0, 9, &local_time_off_min); + + + int ashtech_hour = ashtech_time/10000; + int ashtech_minute = (ashtech_time - ashtech_hour*10000)/100; + float64_t ashtech_sec = ashtech_time - ashtech_hour*10000 - ashtech_minute*100; + //convert to unix timestamp + struct tm timeinfo; + timeinfo.tm_year = year - 1900; + timeinfo.tm_mon = month - 1; + timeinfo.tm_mday = day; + timeinfo.tm_hour = ashtech_hour; + timeinfo.tm_min = ashtech_minute; + timeinfo.tm_sec = int(ashtech_sec); + time_t epoch = mktime(&timeinfo); + + _gps_position->time_gps_usec = (uint64_t)epoch * 1000000; //TODO: test this + _gps_position->time_gps_usec += (uint64_t)((ashtech_sec - int(ashtech_sec)) * 1e6); + _gps_position->timestamp_time = hrt_absolute_time(); + } + + else if((memcmp(_rx_buffer+3, "GGA,",3) == 0)&&(uiCalcComma == 14)){ + /* + Time, position, and fix related data + An example of the GBS message string is: + + $GPGGA,172814.0,3723.46587704,N,12202.26957864,W,2,6,1.2,18.893,M,-25.669,M,2.0,0031*4F + + Note - The data string exceeds the ASHTECH standard length. + GGA message fields + Field Meaning + 0 Message ID $GPGGA + 1 UTC of position fix + 2 Latitude + 3 Direction of latitude: + N: North + S: South + 4 Longitude + 5 Direction of longitude: + E: East + W: West + 6 GPS Quality indicator: + 0: Fix not valid + 1: GPS fix + 2: Differential GPS fix, OmniSTAR VBS + 4: Real-Time Kinematic, fixed integers + 5: Real-Time Kinematic, float integers, OmniSTAR XP/HP or Location RTK + 7 Number of SVs in use, range from 00 through to 24+ + 8 HDOP + 9 Orthometric height (MSL reference) + 10 M: unit of measure for orthometric height is meters + 11 Geoid separation + 12 M: geoid separation measured in meters + 13 Age of differential GPS data record, Type 1 or Type 9. Null field when DGPS is not used. + 14 Reference station ID, range 0000-4095. A null field when any reference station ID is selected and no corrections are received1. + 15 + The checksum data, always begins with * + Note - If a user-defined geoid model, or an inclined + */ + float64_t ashtech_time = 0.0, lat = 0.0, lon = 0.0, alt = 0.0; + int num_of_sv = 0, fix_quality=0; + float64_t hdop = 99.9; + char ns = '?', ew = '?'; + + if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&ashtech_time); + if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&lat); + if(bufptr && *(++bufptr) != ',') ns = *(bufptr++); + if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&lon); + if(bufptr && *(++bufptr) != ',') ew = *(bufptr++); + if(bufptr && *(++bufptr) != ',') bufptr = str_scanDec(bufptr, 0, 9, &fix_quality); + if(bufptr && *(++bufptr) != ',') bufptr = str_scanDec(bufptr, 0, 9, &num_of_sv); + if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&hdop); + if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&alt); + + if(ns == 'S') + lat = -lat; + if(ew == 'W') + lon = -lon; + + _gps_position->lat = (int(lat*0.01) + (lat*0.01 - int(lat*0.01))*100.0/60.0)*10000000; + _gps_position->lon = (int(lon*0.01) + (lon*0.01 - int(lon*0.01))*100.0/60.0)*10000000; + _gps_position->alt = alt*1000; + _rate_count_lat_lon++; + + if((lat == 0.0) && (lon == 0.0) && (alt == 0.0)){ + _gps_position->fix_type = 0; + } + else + _gps_position->fix_type = 3 + fix_quality; + + _gps_position->timestamp_position = hrt_absolute_time(); + + _gps_position->vel_m_s = 0; /**< GPS ground speed (m/s) */ + _gps_position->vel_n_m_s = 0; /**< GPS ground speed in m/s */ + _gps_position->vel_e_m_s = 0; /**< GPS ground speed in m/s */ + _gps_position->vel_d_m_s = 0; /**< GPS ground speed in m/s */ + _gps_position->cog_rad = 0; /**< Course over ground (NOT heading, but direction of movement) in rad, -PI..PI */ + _gps_position->vel_ned_valid = true; /**< Flag to indicate if NED speed is valid */ + _gps_position->c_variance_rad = 0.1; + _gps_position->timestamp_velocity = hrt_absolute_time(); + return 1; + } + else if ((memcmp(_rx_buffer, "$PASHR,POS,", 11) == 0) && (uiCalcComma == 18)) { + /* + Example $PASHR,POS,2,10,125410.00,5525.8138702,N,03833.9587380,E,131.555,1.0,0.0,0.007,-0.001,2.0,1.0,1.7,1.0,*34 + + $PASHR,POS,d1,d2,m3,m4,c5,m6,c7,f8,f9,f10,f11,f12,f13,f14,f15,f16,s17*cc + Parameter Description Range + d1 Position mode 0: standalone + 1: differential + 2: RTK float + 3: RTK fixed + 5: Dead reckoning + 9: SBAS (see NPT setting) + d2 Number of satellite used in position fix 0-99 + m3 Current UTC time of position fix (hhmmss.ss) 000000.00-235959.99 + m4 Latitude of position (ddmm.mmmmmm) 0-90 degrees 00-59.9999999 minutes + c5 Latitude sector N, S + m6 Longitude of position (dddmm.mmmmmm) 0-180 degrees 00-59.9999999 minutes + c7 Longitude sector E,W + f8 Altitude above ellipsoid +9999.000 + f9 Differential age (data link age), seconds 0.0-600.0 + f10 True track/course over ground in degrees 0.0-359.9 + f11 Speed over ground in knots 0.0-999.9 + f12 Vertical velocity in decimeters per second +999.9 + f13 PDOP 0-99.9 + f14 HDOP 0-99.9 + f15 VDOP 0-99.9 + f16 TDOP 0-99.9 + s17 Reserved no data + *cc Checksum + */ + bufptr = (char*)( _rx_buffer+10); + float64_t ashtech_time = 0.0, lat = 0.0, lon = 0.0, alt = 0.0; + int num_of_sv = 0, fix_quality=0; + float64_t track_true = 0.0, ground_speed = 0.0 , age_of_corr = 0.0; + float64_t hdop = 99.9, vdop = 99.9, pdop = 99.9, tdop = 99.9,vertic_vel = 0.0; + char ns = '?', ew = '?'; + + if(bufptr && *(++bufptr) != ',') bufptr = str_scanDec(bufptr, 0, 9, &fix_quality); + if(bufptr && *(++bufptr) != ',') bufptr = str_scanDec(bufptr, 0, 9, &num_of_sv); + if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&ashtech_time); + if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&lat); + if(bufptr && *(++bufptr) != ',') ns = *(bufptr++); + if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&lon); + if(bufptr && *(++bufptr) != ',') ew = *(bufptr++); + if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&alt); + if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&age_of_corr); + if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&track_true); + if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&ground_speed); + if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&vertic_vel); + if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&pdop); + if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&hdop); + if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&vdop); + if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&tdop); + + if(ns == 'S') + lat = -lat; + if(ew == 'W') + lon = -lon; + + _gps_position->lat = (int(lat*0.01) + (lat*0.01 - int(lat*0.01))*100.0/60.0)*10000000; + _gps_position->lon = (int(lon*0.01) + (lon*0.01 - int(lon*0.01))*100.0/60.0)*10000000; + _gps_position->alt = alt*1000; + _rate_count_lat_lon++; + + if((lat == 0.0) && (lon == 0.0) && (alt == 0.0)){ + _gps_position->fix_type = 0; + } + else + _gps_position->fix_type = 3 + fix_quality; + + _gps_position->timestamp_position = hrt_absolute_time(); + + const float64_t m_pi = 3.14159265; + + double track_rad = track_true * m_pi / 180.0; + + double velocity_ms = ground_speed / 3.6; + double velocity_north = velocity_ms * cos(track_rad); + double velocity_east = velocity_ms * sin(track_rad); + + _gps_position->vel_m_s = velocity_ms; /**< GPS ground speed (m/s) */ + _gps_position->vel_n_m_s = velocity_north; /**< GPS ground speed in m/s */ + _gps_position->vel_e_m_s = velocity_east; /**< GPS ground speed in m/s */ + _gps_position->vel_d_m_s = -vertic_vel; /**< GPS ground speed in m/s */ + _gps_position->cog_rad = track_rad; /**< Course over ground (NOT heading, but direction of movement) in rad, -PI..PI */ + _gps_position->vel_ned_valid = true; /**< Flag to indicate if NED speed is valid */ + _gps_position->c_variance_rad = 0.1; + _gps_position->timestamp_velocity = hrt_absolute_time(); + return 1; + + } + else if((memcmp(_rx_buffer+3, "GST,",3) == 0)&&(uiCalcComma == 8)){ + /* + Position error statistics + An example of the GST message string is: + + $GPGST,172814.0,0.006,0.023,0.020,273.6,0.023,0.020,0.031*6A + + The Talker ID ($--) will vary depending on the satellite system used for the position solution: + + $GP - GPS only + $GL - GLONASS only + $GN - Combined + GST message fields + Field Meaning + 0 Message ID $GPGST + 1 UTC of position fix + 2 RMS value of the pseudorange residuals; includes carrier phase residuals during periods of RTK (float) and RTK (fixed) processing + 3 Error ellipse semi-major axis 1 sigma error, in meters + 4 Error ellipse semi-minor axis 1 sigma error, in meters + 5 Error ellipse orientation, degrees from true north + 6 Latitude 1 sigma error, in meters + 7 Longitude 1 sigma error, in meters + 8 Height 1 sigma error, in meters + 9 The checksum data, always begins with * + */ + float64_t ashtech_time = 0.0, lat_err = 0.0, lon_err = 0.0, alt_err = 0.0; + float64_t min_err = 0.0, maj_err = 0.0, deg_from_north = 0.0, rms_err = 0.0; + + if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&ashtech_time); + if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&rms_err); + if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&maj_err); + if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&min_err); + if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,°_from_north); + if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&lat_err); + if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&lon_err); + if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&alt_err); + + _gps_position->eph = sqrt(lat_err*lat_err +lon_err*lon_err); + _gps_position->epv = alt_err; + + _gps_position->s_variance_m_s = 0; + _gps_position->timestamp_variance = hrt_absolute_time(); + } + else if((memcmp(_rx_buffer+3, "VTG,",3) == 0)&&(uiCalcComma == 9)){ + /* + Track made good and speed over ground + An example of the VTG message string is: + + $GPVTG,054.7,T,034.4,M,005.5,N,010.2,K*48 + + VTG message fields + Field Meaning + 0 Message ID $GPVTG + 1 Track made good (degrees true) + 2 T: track made good is relative to true north + 3 Track made good (degrees magnetic) + 4 M: track made good is relative to magnetic north + 5 Speed, in knots + 6 N: speed is measured in knots + 7 Speed over ground in kilometers/hour (kph) + 8 K: speed over ground is measured in kph + 9 The checksum data, always begins with * + */ + /*float64_t track_true = 0.0, speed = 0.0, track_magnetic = 0.0, ground_speed = 0.0; + char true_north = '?', magnetic_north = '?', speed_in_knots = '?', speed_in_kph = '?'; + + if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&track_true); + if(bufptr && *(++bufptr) != ',') true_north = *(bufptr++); + if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&track_magnetic); + if(bufptr && *(++bufptr) != ',') magnetic_north = *(bufptr++); + if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&speed); + if(bufptr && *(++bufptr) != ',') speed_in_knots = *(bufptr++); + if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&ground_speed); + if(bufptr && *(++bufptr) != ',') speed_in_kph = *(bufptr++); + + const float64_t dPI = 3.14159265; + float64_t tan_C = tan(track_true * dPI / 180.0); + float64_t lat_ = sqrt(ground_speed*ground_speed/ (1+tan_C)); //km/hour + float64_t lon_ = lat_*tan_C; // //km/hour*/ + } + else if((memcmp(_rx_buffer+3, "GSV,",3) == 0)){ + /* + The GSV message string identifies the number of SVs in view, the PRN numbers, elevations, azimuths, and SNR values. An example of the GSV message string is: + + $GPGSV,4,1,13,02,02,213,,03,-3,000,,11,00,121,,14,13,172,05*67 + + GSV message fields + Field Meaning + 0 Message ID $GPGSV + 1 Total number of messages of this type in this cycle + 2 Message number + 3 Total number of SVs visible + 4 SV PRN number + 5 Elevation, in degrees, 90� maximum + 6 Azimuth, degrees from True North, 000� through 359� + 7 SNR, 00 through 99 dB (null when not tracking) + 8-11 Information about second SV, same format as fields 4 through 7 + 12-15 Information about third SV, same format as fields 4 through 7 + 16-19 Information about fourth SV, same format as fields 4 through 7 + 20 The checksum data, always begins with * + */ +// currently process only gps, because do not know what +// Global satellite ID I should use for non GPS sats + bool bGPS = false; + if(memcmp(_rx_buffer, "$GP",3) != 0) + return 0; + else + bGPS = true; + + int all_msg_num, this_msg_num, tot_sv_visible; + struct gsv_sat{ + int svid; + int elevation; + int azimuth; + int snr; + } sat[4]; + memset(sat, 0, sizeof(sat)); + + if(bufptr && *(++bufptr) != ',') bufptr = str_scanDec(bufptr, 0, 9, &all_msg_num); + if(bufptr && *(++bufptr) != ',') bufptr = str_scanDec(bufptr, 0, 9, &this_msg_num); + if(bufptr && *(++bufptr) != ',') bufptr = str_scanDec(bufptr, 0, 9, &tot_sv_visible); + if((this_msg_num<1) || (this_msg_num>all_msg_num)){ + return 0; + } + if((this_msg_num == 0)&&(bGPS == true)){ + memset(_satellite_info->svid, 0,sizeof(_satellite_info->svid)); + memset(_satellite_info->used, 0,sizeof(_satellite_info->used)); + memset(_satellite_info->snr, 0,sizeof(_satellite_info->snr)); + memset(_satellite_info->elevation,0,sizeof(_satellite_info->elevation)); + memset(_satellite_info->azimuth, 0,sizeof(_satellite_info->azimuth)); + } + + int end = 4; + if(this_msg_num == all_msg_num){ + end = tot_sv_visible - (this_msg_num-1)*4; + _gps_position->satellites_used = tot_sv_visible; + _satellite_info->count = SAT_INFO_MAX_SATELLITES; + _satellite_info->timestamp = hrt_absolute_time(); + } + for(int y = 0 ; y < end ;y++){ + if(bufptr && *(++bufptr) != ',') bufptr = str_scanDec(bufptr, 0, 9, &sat[y].svid); + if(bufptr && *(++bufptr) != ',') bufptr = str_scanDec(bufptr, 0, 9, &sat[y].elevation); + if(bufptr && *(++bufptr) != ',') bufptr = str_scanDec(bufptr, 0, 9, &sat[y].azimuth); + if(bufptr && *(++bufptr) != ',') bufptr = str_scanDec(bufptr, 0, 9, &sat[y].snr); + + _satellite_info->svid[y+(this_msg_num-1)*4] = sat[y].svid; + _satellite_info->used[y+(this_msg_num-1)*4] = ((sat[y].snr>0)? true: false); + _satellite_info->snr[y+(this_msg_num-1)*4] = sat[y].snr; + _satellite_info->elevation[y+(this_msg_num-1)*4] = sat[y].elevation; + _satellite_info->azimuth[y+(this_msg_num-1)*4] = sat[y].azimuth; + } + } + + return 0; +} + + +int ASHTECH::receive(unsigned timeout){ +{ + /* poll descriptor */ + pollfd fds[1]; + fds[0].fd = _fd; + fds[0].events = POLLIN; + + uint8_t buf[32]; + + /* timeout additional to poll */ + uint64_t time_started = hrt_absolute_time(); + + int j = 0; + ssize_t count = 0; + + while (true) { + + /* pass received bytes to the packet decoder */ + while (j < count) { + int l=0; + if ((l = parse_char(buf[j])) > 0) { + /* return to configure during configuration or to the gps driver during normal work + * if a packet has arrived */ + if (handle_message(l) > 0) + return 1; + } + /* in case we keep trying but only get crap from GPS */ + if (time_started + timeout*1000*2 < hrt_absolute_time() ) { + return -1; + } + j++; + } + + /* everything is read */ + j = count = 0; + + /* then poll for new data */ + int ret = ::poll(fds, sizeof(fds) / sizeof(fds[0]), timeout*2); + + if (ret < 0) { + /* something went wrong when polling */ + return -1; + + } else if (ret == 0) { + /* Timeout */ + return -1; + + } else if (ret > 0) { + /* if we have new data from GPS, go handle it */ + if (fds[0].revents & POLLIN) { + /* + * We are here because poll says there is some data, so this + * won't block even on a blocking device. If more bytes are + * available, we'll go back to poll() again... + */ + count = ::read(_fd, buf, sizeof(buf)); + } + } + } +} + +} +#define HEXDIGIT_CHAR(d) ((char)((d) + (((d) < 0xA) ? '0' : 'A'-0xA))) + +int ASHTECH::parse_char(uint8_t b) +{ + int iRet = 0; + + switch (_decode_state) { + /* First, look for sync1 */ + case NME_DECODE_UNINIT: + if (b == '$') { + _decode_state = NME_DECODE_GOT_SYNC1; + _rx_buffer_bytes = 0; + _rx_buffer[_rx_buffer_bytes++] = b; + } + + break; + + case NME_DECODE_GOT_SYNC1: + if (b == '$') { + _decode_state = NME_DECODE_GOT_SYNC1; + _rx_buffer_bytes = 0; + + } else if (b == '*') { + _decode_state = NME_DECODE_GOT_ASTERIKS; + } + + if (_rx_buffer_bytes >= (sizeof(_rx_buffer) - 5)) { + _decode_state = NME_DECODE_UNINIT; + _rx_buffer_bytes = 0; + + } else + _rx_buffer[_rx_buffer_bytes++] = b; + + break; + + case NME_DECODE_GOT_ASTERIKS: + _rx_buffer[_rx_buffer_bytes++] = b; + _decode_state = NME_DECODE_GOT_FIRST_CS_BYTE; + break; + + case NME_DECODE_GOT_FIRST_CS_BYTE: + _rx_buffer[_rx_buffer_bytes++] = b; + uint8_t checksum = 0; + uint8_t *buffer = _rx_buffer + 1; + uint8_t *bufend = _rx_buffer + _rx_buffer_bytes - 3; + + for (; buffer < bufend; buffer++) checksum ^= *buffer; + + if ((HEXDIGIT_CHAR(checksum >> 4) == *(_rx_buffer + _rx_buffer_bytes - 2)) && + (HEXDIGIT_CHAR(checksum & 0x0F) == *(_rx_buffer + _rx_buffer_bytes - 1))) { + iRet = _rx_buffer_bytes; + } + + _decode_state = NME_DECODE_UNINIT; + _rx_buffer_bytes = 0; + break; + } + + return iRet; +} + +void ASHTECH::decode_init(void){ + +} + +//ashtech boad configuration script +//char comm[] = "$PASHS,NME,GGA,A,ON,0.1\r\n" +char comm[] = "$PASHS,NME,ZDA,B,ON,3\r\n"\ + "$PASHS,NME,GGA,B,OFF\r\n"\ + "$PASHS,NME,GST,B,ON,3\r\n"\ + "$PASHS,NME,POS,B,ON,0.1\r\n"\ + "$PASHS,NME,GSV,B,ON,3\r\n"\ + "$PASHS,SPD,A,8\r\n"\ + "$PASHS,SPD,B,7\r\n"; // default baud is 7 + +int ASHTECH::configure(unsigned &baudrate){ + /* try different baudrates */ + const unsigned baudrates_to_try[] = {9600, 38400, 19200, 57600, 115200}; + + for (int baud_i = 0; baud_i < sizeof(baudrates_to_try)/sizeof(baudrates_to_try[0]); baud_i++) { + baudrate = baudrates_to_try[baud_i]; + set_baudrate(_fd, baudrate); + write(_fd, (uint8_t*)comm, sizeof(comm)); + } + set_baudrate(_fd, 38400); + return 0; +} diff --git a/src/drivers/gps/ashtech.h b/src/drivers/gps/ashtech.h new file mode 100644 index 000000000..e8c5ffb4c --- /dev/null +++ b/src/drivers/gps/ashtech.h @@ -0,0 +1,96 @@ +/**************************************************************************** + * + * Copyright (C) 2013. All rights reserved. + * Author: Boriskin Aleksey + * Kistanov Alexander + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/* @file ASHTECH protocol definitions */ + +#ifndef ASHTECH_H_ +#define ASHTECH_H_ + +#include "gps_helper.h" + +#ifndef RECV_BUFFER_SIZE +#define RECV_BUFFER_SIZE 512 + +#define SAT_INFO_MAX_SATELLITES 20 +#endif + + +class ASHTECH : public GPS_Helper +{ + enum ashtech_decode_state_t { + NME_DECODE_UNINIT, + NME_DECODE_GOT_SYNC1, + NME_DECODE_GOT_ASTERIKS, + NME_DECODE_GOT_FIRST_CS_BYTE + }; + + int _fd; + struct satellite_info_s *_satellite_info; + struct vehicle_gps_position_s *_gps_position; + int ashtechlog_fd;//miklm + + ashtech_decode_state_t _decode_state; + uint8_t _rx_buffer[RECV_BUFFER_SIZE]; + uint16_t _rx_buffer_bytes; + bool _parse_error; // parse error flag + char *_parse_pos; // parse position + + bool _gsv_in_progress; // Indicates that gsv data parsing is in progress + //int _satellites_count; // Number of satellites info parsed. + uint8_t count; /**< Number of satellites in satellite info */ + uint8_t svid[SAT_INFO_MAX_SATELLITES]; /**< Space vehicle ID [1..255], see scheme below */ + uint8_t used[SAT_INFO_MAX_SATELLITES]; /**< 0: Satellite not used, 1: used for navigation */ + uint8_t elevation[SAT_INFO_MAX_SATELLITES]; /**< Elevation (0: right on top of receiver, 90: on the horizon) of satellite */ + uint8_t azimuth[SAT_INFO_MAX_SATELLITES]; /**< Direction of satellite, 0: 0 deg, 255: 360 deg. */ + uint8_t snr[SAT_INFO_MAX_SATELLITES]; /**< dBHz, Signal to noise ratio of satellite C/N0, range 0..99, zero when not tracking this satellite. */ + +public: + ASHTECH(const int &fd, struct vehicle_gps_position_s *gps_position, struct satellite_info_s *satellite_info); + ~ASHTECH(); + int receive(unsigned timeout); + int configure(unsigned &baudrate); + void decode_init(void); + int handle_message(int len); + int parse_char(uint8_t b); + /** Read int ASHTECH parameter */ + int32_t read_int(); + /** Read float ASHTECH parameter */ + double read_float(); + /** Read char ASHTECH parameter */ + char read_char(); + +}; + +#endif /* ASHTECH_H_ */ diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index 34dd63086..2547cda97 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -69,6 +69,7 @@ #include "ubx.h" #include "mtk.h" +#include "ashtech.h" #define TIMEOUT_5HZ 500 @@ -341,6 +342,11 @@ GPS::task_main() _Helper = new MTK(_serial_fd, &_report_gps_pos); break; + case GPS_DRIVER_MODE_ASHTECH: + + _Helper = new ASHTECH(_serial_fd, &_report_gps_pos, _p_report_sat_info); + break; + default: break; } @@ -402,6 +408,10 @@ GPS::task_main() mode_str = "MTK"; break; + case GPS_DRIVER_MODE_ASHTECH: + mode_str = "ASHTECH"; + break; + default: break; } @@ -429,6 +439,10 @@ GPS::task_main() break; case GPS_DRIVER_MODE_MTK: + _mode = GPS_DRIVER_MODE_ASHTECH; + break; + + case GPS_DRIVER_MODE_ASHTECH: _mode = GPS_DRIVER_MODE_UBX; break; @@ -475,6 +489,10 @@ GPS::print_info() warnx("protocol: MTK"); break; + case GPS_DRIVER_MODE_ASHTECH: + warnx("protocol: ASHTECH"); + break; + default: break; } diff --git a/src/drivers/gps/module.mk b/src/drivers/gps/module.mk index b00818424..4f99b0d3b 100644 --- a/src/drivers/gps/module.mk +++ b/src/drivers/gps/module.mk @@ -40,6 +40,7 @@ MODULE_COMMAND = gps SRCS = gps.cpp \ gps_helper.cpp \ mtk.cpp \ + ashtech.cpp \ ubx.cpp MODULE_STACKSIZE = 1200 diff --git a/src/modules/uORB/topics/vehicle_gps_position.h b/src/modules/uORB/topics/vehicle_gps_position.h index 80d65cd69..43909e2fa 100644 --- a/src/modules/uORB/topics/vehicle_gps_position.h +++ b/src/modules/uORB/topics/vehicle_gps_position.h @@ -62,7 +62,7 @@ struct vehicle_gps_position_s { uint64_t timestamp_variance; float s_variance_m_s; /**< speed accuracy estimate m/s */ float c_variance_rad; /**< course accuracy estimate rad */ - uint8_t fix_type; /**< 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. */ + uint8_t fix_type; /**< 0-1: no fix, 2: 2D fix, 3: 3D fix, 4: Real-Time Kinematic, fixed integers, 5: Real-Time Kinematic, float integers, OmniSTAR XP/HP or Location RTK. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. */ float eph; /**< GPS HDOP horizontal dilution of position in m */ float epv; /**< GPS VDOP horizontal dilution of position in m */ -- cgit v1.2.3 From 47dcf88271b4b05f2007383e62a6dd239b7eb859 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 30 Sep 2014 15:18:30 +0200 Subject: Flash optimization --- makefiles/config_px4fmu-v1_default.mk | 6 +++--- src/lib/conversion/module.mk | 2 ++ 2 files changed, 5 insertions(+), 3 deletions(-) diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk index 6f39b56a1..cf631c2be 100644 --- a/makefiles/config_px4fmu-v1_default.mk +++ b/makefiles/config_px4fmu-v1_default.mk @@ -24,14 +24,14 @@ MODULES += drivers/l3gd20 MODULES += drivers/mpu6000 MODULES += drivers/hmc5883 MODULES += drivers/ms5611 -MODULES += drivers/mb12xx +#MODULES += drivers/mb12xx MODULES += drivers/gps MODULES += drivers/hil -MODULES += drivers/blinkm +#MODULES += drivers/blinkm MODULES += drivers/rgbled MODULES += drivers/mkblctrl MODULES += drivers/airspeed -MODULES += drivers/ets_airspeed +#MODULES += drivers/ets_airspeed MODULES += drivers/meas_airspeed MODULES += drivers/frsky_telemetry MODULES += modules/sensors diff --git a/src/lib/conversion/module.mk b/src/lib/conversion/module.mk index f5f59a2dc..4593c4887 100644 --- a/src/lib/conversion/module.mk +++ b/src/lib/conversion/module.mk @@ -36,3 +36,5 @@ # SRCS = rotation.cpp + +MAXOPTIMIZATION = -Os -- cgit v1.2.3 From 47f151d4cefa5d7a9da34d6139aa493259f8b2fd Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 30 Sep 2014 15:19:06 +0200 Subject: Deactivate FrSky telem by default --- makefiles/config_px4fmu-v1_default.mk | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk index cf631c2be..9fe16fbb6 100644 --- a/makefiles/config_px4fmu-v1_default.mk +++ b/makefiles/config_px4fmu-v1_default.mk @@ -33,7 +33,7 @@ MODULES += drivers/mkblctrl MODULES += drivers/airspeed #MODULES += drivers/ets_airspeed MODULES += drivers/meas_airspeed -MODULES += drivers/frsky_telemetry +#MODULES += drivers/frsky_telemetry MODULES += modules/sensors # -- 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(-) 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 c5a1ddd8f2dd32005c2581886e96b3419009fef4 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 30 Sep 2014 15:42:56 +0200 Subject: re-enable MC apps on FMUV1 --- makefiles/config_px4fmu-v1_default.mk | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk index 9aa8320ee..9fe16fbb6 100644 --- a/makefiles/config_px4fmu-v1_default.mk +++ b/makefiles/config_px4fmu-v1_default.mk @@ -64,17 +64,17 @@ MODULES += modules/gpio_led # # Estimation modules (EKF / other filters) # -#MODULES += modules/attitude_estimator_ekf +MODULES += modules/attitude_estimator_ekf MODULES += modules/ekf_att_pos_estimator -#MODULES += modules/position_estimator_inav +MODULES += modules/position_estimator_inav # # Vehicle Control # MODULES += modules/fw_pos_control_l1 MODULES += modules/fw_att_control -#MODULES += modules/mc_att_control -#MODULES += modules/mc_pos_control +MODULES += modules/mc_att_control +MODULES += modules/mc_pos_control # # Logging -- 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(-) 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(+) 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(-) 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(-) 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 a960fcbdef475e5ab264760568b15867d55b7774 Mon Sep 17 00:00:00 2001 From: Denis Yeldandi Date: Thu, 2 Oct 2014 14:34:20 +0400 Subject: Increased ashtech POS frequency and increased baudrate --- src/drivers/gps/ashtech.cpp | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/src/drivers/gps/ashtech.cpp b/src/drivers/gps/ashtech.cpp index d338ff8e5..614aade00 100644 --- a/src/drivers/gps/ashtech.cpp +++ b/src/drivers/gps/ashtech.cpp @@ -597,20 +597,22 @@ void ASHTECH::decode_init(void){ char comm[] = "$PASHS,NME,ZDA,B,ON,3\r\n"\ "$PASHS,NME,GGA,B,OFF\r\n"\ "$PASHS,NME,GST,B,ON,3\r\n"\ - "$PASHS,NME,POS,B,ON,0.1\r\n"\ + "$PASHS,NME,POS,B,ON,0.05\r\n"\ "$PASHS,NME,GSV,B,ON,3\r\n"\ "$PASHS,SPD,A,8\r\n"\ - "$PASHS,SPD,B,7\r\n"; // default baud is 7 + "$PASHS,SPD,B,9\r\n"; // default baud is 7 int ASHTECH::configure(unsigned &baudrate){ /* try different baudrates */ const unsigned baudrates_to_try[] = {9600, 38400, 19200, 57600, 115200}; + for (int baud_i = 0; baud_i < sizeof(baudrates_to_try)/sizeof(baudrates_to_try[0]); baud_i++) { baudrate = baudrates_to_try[baud_i]; set_baudrate(_fd, baudrate); write(_fd, (uint8_t*)comm, sizeof(comm)); } - set_baudrate(_fd, 38400); + + set_baudrate(_fd, 115200); return 0; } -- cgit v1.2.3 From 84908f8f3db5179ffff0d96d15756ab112535482 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 2 Oct 2014 15:45:02 +0400 Subject: mc_pos_control: AUTO speed limiting bug fixed --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 99deb0d29..ec7b2a78f 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -829,10 +829,12 @@ MulticopterPositionControl::control_auto(float dt) /* move setpoint not faster than max allowed speed */ math::Vector<3> pos_sp_old_s = _pos_sp.emult(scale); - math::Vector<3> d_pos_s = pos_sp_s - pos_sp_old_s; - float d_pos_s_len = d_pos_s.length(); - if (d_pos_s_len > dt) { - pos_sp_s = pos_sp_old_s + d_pos_s / d_pos_s_len * dt; + + /* difference between current and desired position setpoints, 1 = max speed */ + math::Vector<3> d_pos_m = (pos_sp_s - pos_sp_old_s).edivide(_params.pos_p); + float d_pos_m_len = d_pos_m.length(); + if (d_pos_m_len > dt) { + pos_sp_s = pos_sp_old_s + (d_pos_m / d_pos_m_len * dt).emult(_params.pos_p); } /* scale result back to normal space */ -- cgit v1.2.3 From 5846f217d0e09ee4c1c64c0ebf0f2e621a14fa69 Mon Sep 17 00:00:00 2001 From: tstellanova Date: Fri, 3 Oct 2014 17:18:44 -0700 Subject: Use global position altitude to report HUD altitude, for consistency. Otherwise the HUD altitude jumps between two very different values. --- src/modules/mavlink/mavlink_messages.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index a2f3828ff..941b45d88 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -820,7 +820,11 @@ protected: msg.groundspeed = sqrtf(pos.vel_n * pos.vel_n + pos.vel_e * pos.vel_e); msg.heading = _wrap_2pi(att.yaw) * M_RAD_TO_DEG_F; msg.throttle = armed.armed ? act.control[3] * 100.0f : 0.0f; - msg.alt = sensor_combined.baro_alt_meter; + /* + Do not use sensor_combined.baro_alt_meter here because + it is mismatched with WSG84 AMSL used elsewhere for reporting altitude. + */ + msg.alt = pos.alt; msg.climb = -pos.vel_d; _mavlink->send_message(MAVLINK_MSG_ID_VFR_HUD, &msg); -- cgit v1.2.3 From 04ceb3c95d6c713dfb4f673d3a4cafa7c27c13ca Mon Sep 17 00:00:00 2001 From: Denis Yeldandi Date: Sat, 4 Oct 2014 09:21:47 +0400 Subject: Fixed km/h -> knots in POS --- src/drivers/gps/ashtech.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/gps/ashtech.cpp b/src/drivers/gps/ashtech.cpp index d338ff8e5..69c22ab28 100644 --- a/src/drivers/gps/ashtech.cpp +++ b/src/drivers/gps/ashtech.cpp @@ -293,7 +293,7 @@ int ASHTECH::handle_message(int len){ double track_rad = track_true * m_pi / 180.0; - double velocity_ms = ground_speed / 3.6; + double velocity_ms = ground_speed / 1.9438445; /** knots to m/s */ double velocity_north = velocity_ms * cos(track_rad); double velocity_east = velocity_ms * sin(track_rad); -- cgit v1.2.3 From 7bde4fa6342230ab37644f94d364cd6e9541773e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 4 Oct 2014 12:31:16 +0200 Subject: Revert "Use global position altitude to report HUD altitude, for consistency." --- src/modules/mavlink/mavlink_messages.cpp | 6 +----- 1 file changed, 1 insertion(+), 5 deletions(-) diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 941b45d88..a2f3828ff 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -820,11 +820,7 @@ protected: msg.groundspeed = sqrtf(pos.vel_n * pos.vel_n + pos.vel_e * pos.vel_e); msg.heading = _wrap_2pi(att.yaw) * M_RAD_TO_DEG_F; msg.throttle = armed.armed ? act.control[3] * 100.0f : 0.0f; - /* - Do not use sensor_combined.baro_alt_meter here because - it is mismatched with WSG84 AMSL used elsewhere for reporting altitude. - */ - msg.alt = pos.alt; + msg.alt = sensor_combined.baro_alt_meter; msg.climb = -pos.vel_d; _mavlink->send_message(MAVLINK_MSG_ID_VFR_HUD, &msg); -- cgit v1.2.3 From 56a9d16fc476e99106bda0627cc20f69e987fc6a Mon Sep 17 00:00:00 2001 From: Vladimir Ermakov Date: Sun, 5 Oct 2014 13:19:13 +0400 Subject: FTP: Save errno in _copy_file(). --- src/modules/mavlink/mavlink_ftp.cpp | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/src/modules/mavlink/mavlink_ftp.cpp b/src/modules/mavlink/mavlink_ftp.cpp index c26fe6b4b..4ed06561d 100644 --- a/src/modules/mavlink/mavlink_ftp.cpp +++ b/src/modules/mavlink/mavlink_ftp.cpp @@ -560,7 +560,7 @@ MavlinkFTP::_workTruncateFile(PayloadHeader* payload) return kErrFailErrno; } - if (payload->offset == st.st_size) { + if (payload->offset == (unsigned)st.st_size) { // nothing to do return kErrNone; } @@ -574,7 +574,7 @@ MavlinkFTP::_workTruncateFile(PayloadHeader* payload) ::close(fd); return kErrNone; } - else if (payload->offset > st.st_size) { + else if (payload->offset > (unsigned)st.st_size) { // 2: extend file int fd = ::open(file, O_WRONLY); if (fd < 0) { @@ -808,6 +808,7 @@ MavlinkFTP::_copy_file(const char *src_path, const char *dst_path, ssize_t lengt { char buff[512]; int src_fd = -1, dst_fd = -1; + int op_errno = 0; src_fd = ::open(src_path, O_RDONLY); if (src_fd < 0) { @@ -816,7 +817,9 @@ MavlinkFTP::_copy_file(const char *src_path, const char *dst_path, ssize_t lengt dst_fd = ::open(dst_path, O_CREAT | O_TRUNC | O_WRONLY); if (dst_fd < 0) { + op_errno = errno; ::close(src_fd); + errno = op_errno; return -1; } @@ -831,12 +834,14 @@ MavlinkFTP::_copy_file(const char *src_path, const char *dst_path, ssize_t lengt } else if (bytes_read < 0) { warnx("cp: read"); + op_errno = errno; break; } bytes_written = ::write(dst_fd, buff, bytes_read); if (bytes_written != bytes_read) { warnx("cp: short write"); + op_errno = errno; break; } @@ -846,5 +851,6 @@ MavlinkFTP::_copy_file(const char *src_path, const char *dst_path, ssize_t lengt ::close(src_fd); ::close(dst_fd); + errno = op_errno; return (length > 0)? -1 : 0; } -- cgit v1.2.3 From 0fbdb8d326e3a764fde840e426e093bae32bcba6 Mon Sep 17 00:00:00 2001 From: Vladimir Ermakov Date: Sun, 5 Oct 2014 13:23:57 +0400 Subject: FTP: Return bytes written in payload. --- src/modules/mavlink/mavlink_ftp.cpp | 7 ++----- 1 file changed, 2 insertions(+), 5 deletions(-) diff --git a/src/modules/mavlink/mavlink_ftp.cpp b/src/modules/mavlink/mavlink_ftp.cpp index 4ed06561d..f17497aa8 100644 --- a/src/modules/mavlink/mavlink_ftp.cpp +++ b/src/modules/mavlink/mavlink_ftp.cpp @@ -508,11 +508,8 @@ MavlinkFTP::_workWrite(PayloadHeader* payload) return kErrFailErrno; } - // need to decide how to indicate how much data was written - // current: old code set hdr->size - payload->size = bytes_written; - //payload->size = sizeof(uint32_t); - //*((uint32_t*)payload->data) = bytes_written; + payload->size = sizeof(uint32_t); + *((uint32_t*)payload->data) = bytes_written; return kErrNone; } -- cgit v1.2.3 From 2722921b30a48fd3fb67ba25efe8c4e3ca1338c8 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 5 Oct 2014 12:30:29 +0200 Subject: Fixed function name of mission modification logic, attributed @DrTon --- src/modules/navigator/mission.cpp | 9 +++++---- src/modules/navigator/mission.h | 13 +++++++------ 2 files changed, 12 insertions(+), 10 deletions(-) diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 87d16f1e6..b5926df81 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -37,6 +37,7 @@ * * @author Julian Oes * @author Thomas Gubler + * @author Anton Babushkin */ #include @@ -157,7 +158,7 @@ Mission::on_active() /* else just report that item reached */ if (_mission_type == MISSION_TYPE_OFFBOARD) { if (!(_navigator->get_mission_result()->seq_reached == _current_offboard_mission_index && _navigator->get_mission_result()->reached)) { - report_mission_item_reached(); + set_mission_item_reached(); } } } @@ -701,7 +702,7 @@ Mission::save_offboard_mission_state() } void -Mission::report_mission_item_reached() +Mission::set_mission_item_reached() { _navigator->get_mission_result()->reached = true; _navigator->get_mission_result()->seq_reached = _current_offboard_mission_index; @@ -709,7 +710,7 @@ Mission::report_mission_item_reached() } void -Mission::report_current_offboard_mission_item() +Mission::set_current_offboard_mission_item() { warnx("current offboard mission index: %d", _current_offboard_mission_index); _navigator->get_mission_result()->reached = false; @@ -721,7 +722,7 @@ Mission::report_current_offboard_mission_item() } void -Mission::report_mission_finished() +Mission::set_mission_finished() { _navigator->get_mission_result()->finished = true; _navigator->publish_mission_result(); diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h index 5164737f3..ea7cc0927 100644 --- a/src/modules/navigator/mission.h +++ b/src/modules/navigator/mission.h @@ -37,6 +37,7 @@ * * @author Julian Oes * @author Thomas Gubler + * @author Anton Babushkin */ #ifndef NAVIGATOR_MISSION_H @@ -130,19 +131,19 @@ private: void save_offboard_mission_state(); /** - * Report that a mission item has been reached + * Set a mission item as reached */ - void report_mission_item_reached(); + void set_mission_item_reached(); /** - * Rport the current mission item + * Set the current offboard mission item */ - void report_current_offboard_mission_item(); + void set_current_offboard_mission_item(); /** - * Report that the mission is finished if one exists or that none exists + * Set that the mission is finished if one exists or that none exists */ - void report_mission_finished(); + void set_mission_finished(); control::BlockParamInt _param_onboard_enabled; control::BlockParamFloat _param_takeoff_alt; -- 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(-) 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(-) 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 2587074a07f67ed37c8d4f0b4d7f1b31bb2f6525 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 5 Oct 2014 13:07:56 +0200 Subject: Compile fixes in navigator --- src/modules/navigator/mission.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index b5926df81..7fac69a61 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -234,7 +234,7 @@ Mission::update_offboard_mission() _current_offboard_mission_index = 0; } - report_current_offboard_mission_item(); + set_current_offboard_mission_item(); } @@ -396,7 +396,7 @@ Mission::set_mission_items() _navigator->set_can_loiter_at_sp(pos_sp_triplet->current.type == SETPOINT_TYPE_LOITER); reset_mission_item_reached(); - report_mission_finished(); + set_mission_finished(); _navigator->set_position_setpoint_triplet_updated(); return; @@ -476,7 +476,7 @@ Mission::set_mission_items() reset_mission_item_reached(); if (_mission_type == MISSION_TYPE_OFFBOARD) { - report_current_offboard_mission_item(); + set_current_offboard_mission_item(); } // TODO: report onboard mission item somehow -- cgit v1.2.3 From ff17f31ccedc99bb9151e2f905d336345f2ed7d0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 5 Oct 2014 13:09:02 +0200 Subject: Dataman: Optimize for space --- src/modules/dataman/module.mk | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/dataman/module.mk b/src/modules/dataman/module.mk index 234607b3d..7ebe09fb7 100644 --- a/src/modules/dataman/module.mk +++ b/src/modules/dataman/module.mk @@ -40,3 +40,5 @@ MODULE_COMMAND = dataman SRCS = dataman.c MODULE_STACKSIZE = 1200 + +MAXOPTIMIZATION = -Os -- cgit v1.2.3 From 850c630e11dca24bc160d00b580c7ff7d8b6ff6d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 5 Oct 2014 13:09:19 +0200 Subject: Navigator: Optimize for space --- src/modules/navigator/module.mk | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/navigator/module.mk b/src/modules/navigator/module.mk index c075903b7..fb8d07901 100644 --- a/src/modules/navigator/module.mk +++ b/src/modules/navigator/module.mk @@ -63,3 +63,5 @@ INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink MODULE_STACKSIZE = 1200 EXTRACXXFLAGS = -Weffc++ + +MAXOPTIMIZATION = -Os -- cgit v1.2.3 From c0eefc983a5bbe2faf05eb21daaf3514b767daa6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 5 Oct 2014 13:13:22 +0200 Subject: File test compile fixes --- src/modules/mavlink/mavlink_tests/mavlink_ftp_test.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.cpp b/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.cpp index a5fb2117e..4caa820b6 100644 --- a/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.cpp +++ b/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.cpp @@ -265,7 +265,7 @@ bool MavlinkFtpTest::_open_badfile_test(void) MavlinkFTP::PayloadHeader *reply; const char *dir = "/foo"; // non-existent file - payload.opcode = MavlinkFTP::kCmdOpenFile; + payload.opcode = MavlinkFTP::kCmdOpenFileRO; payload.offset = 0; bool success = _send_receive_msg(&payload, // FTP payload header @@ -295,7 +295,7 @@ bool MavlinkFtpTest::_open_terminate_test(void) struct stat st; const ReadTestCase *test = &_rgReadTestCases[i]; - payload.opcode = MavlinkFTP::kCmdOpenFile; + payload.opcode = MavlinkFTP::kCmdOpenFileRO; payload.offset = 0; bool success = _send_receive_msg(&payload, // FTP payload header @@ -342,7 +342,7 @@ bool MavlinkFtpTest::_terminate_badsession_test(void) MavlinkFTP::PayloadHeader *reply; const char *file = _rgReadTestCases[0].file; - payload.opcode = MavlinkFTP::kCmdOpenFile; + payload.opcode = MavlinkFTP::kCmdOpenFileRO; payload.offset = 0; bool success = _send_receive_msg(&payload, // FTP payload header @@ -400,7 +400,7 @@ bool MavlinkFtpTest::_read_test(void) // Test case data files are created for specific boundary conditions ut_compare("Test case data files are out of date", test->length, st.st_size); - payload.opcode = MavlinkFTP::kCmdOpenFile; + payload.opcode = MavlinkFTP::kCmdOpenFileRO; payload.offset = 0; bool success = _send_receive_msg(&payload, // FTP payload header @@ -463,7 +463,7 @@ bool MavlinkFtpTest::_read_badsession_test(void) MavlinkFTP::PayloadHeader *reply; const char *file = _rgReadTestCases[0].file; - payload.opcode = MavlinkFTP::kCmdOpenFile; + payload.opcode = MavlinkFTP::kCmdOpenFileRO; payload.offset = 0; bool success = _send_receive_msg(&payload, // FTP payload header -- cgit v1.2.3 From 72f6aaca96f7c7936f33cc3e7d7073ddb92d9bfa Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 6 Oct 2014 07:53:18 +0200 Subject: Add ST24 test harness --- Tools/tests-host/.gitignore | 1 + Tools/tests-host/Makefile | 11 ++++++-- Tools/tests-host/st24_test.cpp | 58 ++++++++++++++++++++++++++++++++++++++++++ 3 files changed, 68 insertions(+), 2 deletions(-) create mode 100644 Tools/tests-host/st24_test.cpp diff --git a/Tools/tests-host/.gitignore b/Tools/tests-host/.gitignore index 87b314c61..b06f99815 100644 --- a/Tools/tests-host/.gitignore +++ b/Tools/tests-host/.gitignore @@ -2,3 +2,4 @@ mixer_test sbus2_test autodeclination_test +st24_test diff --git a/Tools/tests-host/Makefile b/Tools/tests-host/Makefile index f0737ef88..5ee0c9432 100644 --- a/Tools/tests-host/Makefile +++ b/Tools/tests-host/Makefile @@ -3,7 +3,7 @@ CC=g++ CFLAGS=-I. -I../../src/modules -I ../../src/include -I../../src/drivers \ -I../../src -I../../src/lib -D__EXPORT="" -Dnullptr="0" -lm -all: mixer_test sbus2_test autodeclination_test +all: mixer_test sbus2_test autodeclination_test st24_test MIXER_FILES=../../src/systemcmds/tests/test_mixer.cpp \ ../../src/systemcmds/tests/test_conv.cpp \ @@ -20,6 +20,10 @@ SBUS2_FILES=../../src/modules/px4iofirmware/sbus.c \ hrt.cpp \ sbus2_test.cpp +ST24_FILES=../../src/lib/rc/st24.c \ + hrt.cpp \ + st24_test.cpp + AUTODECLINATION_FILES= ../../src/lib/geo/geo_mag_declination.c \ hrt.cpp \ autodeclination_test.cpp @@ -33,7 +37,10 @@ sbus2_test: $(SBUS2_FILES) autodeclination_test: $(SBUS2_FILES) $(CC) -o autodeclination_test $(AUTODECLINATION_FILES) $(CFLAGS) +st24_test: $(ST24_FILES) + $(CC) -o st24_test $(ST24_FILES) $(CFLAGS) + .PHONY: clean clean: - rm -f $(ODIR)/*.o *~ core $(INCDIR)/*~ mixer_test sbus2_test autodeclination_test \ No newline at end of file + rm -f $(ODIR)/*.o *~ core $(INCDIR)/*~ mixer_test sbus2_test autodeclination_test st24_test \ No newline at end of file diff --git a/Tools/tests-host/st24_test.cpp b/Tools/tests-host/st24_test.cpp new file mode 100644 index 000000000..680791ca8 --- /dev/null +++ b/Tools/tests-host/st24_test.cpp @@ -0,0 +1,58 @@ + +#include +#include +#include +#include +#include +#include +#include "../../src/systemcmds/tests/tests.h" + +int main(int argc, char *argv[]) { + warnx("ST24 test started"); + + if (argc < 2) + errx(1, "Need a filename for the input file"); + + warnx("loading data from: %s", argv[1]); + + FILE *fp; + + fp = fopen(argv[1],"rt"); + + if (!fp) + errx(1, "failed opening file"); + + float f; + unsigned x; + int ret; + + // Trash the first 20 lines + for (unsigned i = 0; i < 20; i++) { + (void)fscanf(fp, "%f,%x,,", &f, &x); + } + + float last_time = 0; + + while (EOF != (ret = fscanf(fp, "%f,%x,,", &f, &x))) { + if (((f - last_time) * 1000 * 1000) > 3000) { + warnx("FRAME RESET\n\n"); + } + + warnx("%f: 0x%02x", (double)f, x); + + last_time = f; + + // Pipe the data into the parser + hrt_abstime now = hrt_absolute_time(); + + //if (partial_frame_count % 25 == 0) + //sbus_parse(now, frame, &partial_frame_count, rc_values, &num_values, &sbus_failsafe, &sbus_frame_drop, max_channels); + } + + if (ret == EOF) { + warnx("Test finished, reached end of file"); + } else { + warnx("Test aborted, errno: %d", ret); + } + +} -- cgit v1.2.3 From 7b0ac1db8583f3cd553e8dbcb4b338180ea5fb8b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 6 Oct 2014 07:53:32 +0200 Subject: Fix compile error in parser --- src/lib/rc/st24.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/rc/st24.c b/src/lib/rc/st24.c index 956a72bf3..2ae576532 100644 --- a/src/lib/rc/st24.c +++ b/src/lib/rc/st24.c @@ -156,7 +156,7 @@ uint8_t st24_decode(uint8_t byte, uint8_t *rssi, uint8_t* rx_count, uint16_t *ch case ST24_PACKET_TYPE_CHANNELDATA24: { - ChannelData24* d = (ChannelData12*)&_rxpacket; + ChannelData24* d = (ChannelData24*)&_rxpacket; *rssi = d->rssi; *rx_count = d->packet_count; -- cgit v1.2.3 From b4c188cf19097a26c49c4b0235b11ae939554aa0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 6 Oct 2014 13:41:29 +0200 Subject: Fix file location for st24 test --- Tools/tests-host/Makefile | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Tools/tests-host/Makefile b/Tools/tests-host/Makefile index 5ee0c9432..f44f18be6 100644 --- a/Tools/tests-host/Makefile +++ b/Tools/tests-host/Makefile @@ -24,7 +24,7 @@ ST24_FILES=../../src/lib/rc/st24.c \ hrt.cpp \ st24_test.cpp -AUTODECLINATION_FILES= ../../src/lib/geo/geo_mag_declination.c \ +AUTODECLINATION_FILES= ../../src/lib/geo_lookup/geo_mag_declination.c \ hrt.cpp \ autodeclination_test.cpp -- cgit v1.2.3 From 6bf005e9019939d266da80f60aeeb126b66935de Mon Sep 17 00:00:00 2001 From: Denis Yeldandi Date: Mon, 6 Oct 2014 17:56:56 +0400 Subject: PASHS,POP,20 is needed for 20Hz configuration --- src/drivers/gps/ashtech.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/drivers/gps/ashtech.cpp b/src/drivers/gps/ashtech.cpp index b37f70ad1..d40b12721 100644 --- a/src/drivers/gps/ashtech.cpp +++ b/src/drivers/gps/ashtech.cpp @@ -594,7 +594,8 @@ void ASHTECH::decode_init(void){ //ashtech boad configuration script //char comm[] = "$PASHS,NME,GGA,A,ON,0.1\r\n" -char comm[] = "$PASHS,NME,ZDA,B,ON,3\r\n"\ +char comm[] = "$PASHS,POP,20\r\n"\ + "$PASHS,NME,ZDA,B,ON,3\r\n"\ "$PASHS,NME,GGA,B,OFF\r\n"\ "$PASHS,NME,GST,B,ON,3\r\n"\ "$PASHS,NME,POS,B,ON,0.05\r\n"\ -- cgit v1.2.3 From 34e75672bbf9a0e0e12d6a27abac5e1fcb3a0339 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 6 Oct 2014 19:20:02 +0200 Subject: SBUS2 fix --- Tools/tests-host/sbus2_test.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/Tools/tests-host/sbus2_test.cpp b/Tools/tests-host/sbus2_test.cpp index d8fcb695d..e2c18369c 100644 --- a/Tools/tests-host/sbus2_test.cpp +++ b/Tools/tests-host/sbus2_test.cpp @@ -29,7 +29,8 @@ int main(int argc, char *argv[]) { // Trash the first 20 lines for (unsigned i = 0; i < 20; i++) { - (void)fscanf(fp, "%f,%x,,", &f, &x); + char buf[200]; + (void)fgets(buf, sizeof(buf), fp); } // Init the parser -- cgit v1.2.3 From 966688d0921e397f7a19c8e9f3c9fd40b9561bba Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 6 Oct 2014 19:20:17 +0200 Subject: Fixed ST24 test --- Tools/tests-host/st24_test.cpp | 24 +++++++++++++++++++----- 1 file changed, 19 insertions(+), 5 deletions(-) diff --git a/Tools/tests-host/st24_test.cpp b/Tools/tests-host/st24_test.cpp index 680791ca8..e1134bba6 100644 --- a/Tools/tests-host/st24_test.cpp +++ b/Tools/tests-host/st24_test.cpp @@ -4,7 +4,7 @@ #include #include #include -#include +#include #include "../../src/systemcmds/tests/tests.h" int main(int argc, char *argv[]) { @@ -28,7 +28,8 @@ int main(int argc, char *argv[]) { // Trash the first 20 lines for (unsigned i = 0; i < 20; i++) { - (void)fscanf(fp, "%f,%x,,", &f, &x); + char buf[200]; + (void)fgets(buf, sizeof(buf), fp); } float last_time = 0; @@ -38,15 +39,28 @@ int main(int argc, char *argv[]) { warnx("FRAME RESET\n\n"); } - warnx("%f: 0x%02x", (double)f, x); + uint8_t b = static_cast(x); last_time = f; // Pipe the data into the parser hrt_abstime now = hrt_absolute_time(); - //if (partial_frame_count % 25 == 0) - //sbus_parse(now, frame, &partial_frame_count, rc_values, &num_values, &sbus_failsafe, &sbus_frame_drop, max_channels); + uint8_t rssi; + uint8_t rx_count; + uint16_t channel_count; + int16_t channels[20]; + + + if (!st24_decode(b, &rssi, &rx_count, &channel_count, channels, sizeof(channels) / sizeof(channels[0]))) + { + //warnx("decoded: %u channels", (unsigned)channel_count); + for (unsigned i = 0; i < channel_count; i++) { + warnx("channel %u: %d", i, static_cast(channels[i])); + } + } + + //warnx("%f: 0x%02x >> RSSI: %u #: %u", (double)f, x, static_cast(rssi), static_cast(rx_count)); } if (ret == EOF) { -- cgit v1.2.3 From debff9e1796fe035975307dfc9d3e4ed081e3ded Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 6 Oct 2014 19:20:39 +0200 Subject: Updates to ST24 decoding library --- src/lib/rc/st24.c | 86 ++++++++++++++++++++++++++++++++++++++++--------------- src/lib/rc/st24.h | 2 +- 2 files changed, 64 insertions(+), 24 deletions(-) diff --git a/src/lib/rc/st24.c b/src/lib/rc/st24.c index 2ae576532..f43f2de7b 100644 --- a/src/lib/rc/st24.c +++ b/src/lib/rc/st24.c @@ -40,10 +40,11 @@ */ #include +#include #include "st24.h" enum ST24_DECODE_STATE { - ST24_DECODE_STATE_UNSYNCED, + ST24_DECODE_STATE_UNSYNCED = 0, ST24_DECODE_STATE_GOT_STX1, ST24_DECODE_STATE_GOT_STX2, ST24_DECODE_STATE_GOT_LEN, @@ -51,6 +52,24 @@ enum ST24_DECODE_STATE { ST24_DECODE_STATE_GOT_DATA }; +const char* decode_states[] = {"UNSYNCED", + "GOT_STX1", + "GOT_STX2", + "GOT_LEN", + "GOT_TYPE", + "GOT_DATA"}; + +/* define range mapping here, -+100% -> 1000..2000 */ +#define ST24_RANGE_MIN 0.0f +#define ST24_RANGE_MAX 8000.0f + +#define ST24_TARGET_MIN 1000.0f +#define ST24_TARGET_MAX 2000.0f + +/* pre-calculate the floating point stuff as far as possible at compile time */ +#define ST24_SCALE_FACTOR ((ST24_TARGET_MAX - ST24_TARGET_MIN) / (ST24_RANGE_MAX - ST24_RANGE_MIN)) +#define ST24_SCALE_OFFSET (int)(ST24_TARGET_MIN - (ST24_SCALE_FACTOR * ST24_RANGE_MIN + 0.5f)) + static enum ST24_DECODE_STATE _decode_state = ST24_DECODE_STATE_UNSYNCED; static unsigned _rxlen; @@ -83,7 +102,7 @@ uint8_t st24_common_crc8(uint8_t *ptr, uint8_t len) } -uint8_t st24_decode(uint8_t byte, uint8_t *rssi, uint8_t* rx_count, uint16_t *channels, uint16_t max_chan_count) +uint8_t st24_decode(uint8_t byte, uint8_t *rssi, uint8_t* rx_count, uint16_t *channel_count, int16_t *channels, uint16_t max_chan_count) { bool ret = false; @@ -104,9 +123,14 @@ uint8_t st24_decode(uint8_t byte, uint8_t *rssi, uint8_t* rx_count, uint16_t *ch break; case ST24_DECODE_STATE_GOT_STX2: - _rxpacket.length = byte; - _rxlen = 0; - _decode_state = ST24_DECODE_STATE_GOT_LEN; + /* ensure no data overflow failure or hack is possible */ + if ((unsigned)byte <= sizeof(_rxpacket.length) + sizeof(_rxpacket.type) + sizeof(_rxpacket.st24_data)) { + _rxpacket.length = byte; + _rxlen = 0; + _decode_state = ST24_DECODE_STATE_GOT_LEN; + } else { + _decode_state = ST24_DECODE_STATE_UNSYNCED; + } break; case ST24_DECODE_STATE_GOT_LEN: @@ -116,10 +140,9 @@ uint8_t st24_decode(uint8_t byte, uint8_t *rssi, uint8_t* rx_count, uint16_t *ch break; case ST24_DECODE_STATE_GOT_TYPE: - if (_rxlen < (_rxpacket.length - 1)) { - _rxpacket.st24_data[_rxlen] = byte; - _rxlen++; - } else { + _rxpacket.st24_data[_rxlen - 1] = byte; + _rxlen++; + if (_rxlen == (_rxpacket.length - 1)) { _decode_state = ST24_DECODE_STATE_GOT_DATA; } break; @@ -128,8 +151,7 @@ uint8_t st24_decode(uint8_t byte, uint8_t *rssi, uint8_t* rx_count, uint16_t *ch _rxpacket.crc8 = byte; _rxlen++; - if (st24_common_crc8((uint8_t*)&(_rxpacket.length), sizeof(_rxpacket.length) + - sizeof(_rxpacket.st24_data) + sizeof(_rxpacket.type)) == _rxpacket.crc8) { + if (st24_common_crc8((uint8_t*)&(_rxpacket.length), _rxlen) == _rxpacket.crc8) { ret = true; @@ -143,13 +165,18 @@ uint8_t st24_decode(uint8_t byte, uint8_t *rssi, uint8_t* rx_count, uint16_t *ch *rssi = d->rssi; *rx_count = d->packet_count; + *channel_count = 12; - for (unsigned i = 0; i < 1; i += 2) { - channels[i] = ((uint16_t)d->channel[i]) << 8; - channels[i] |= (0xF & d->channel[i+1]); + for (unsigned i = 0; i < *channel_count; i += 2) { + channels[i] = ((uint16_t)d->channel[i]) << 4; + channels[i] |= ((uint16_t)(0xF0 & d->channel[i+1]) >> 4); - channels[i+1] = ((uint16_t)(0xF & d->channel[i+1])) << 4; - channels[i+1] |= d->channel[i+2]; + channels[i+1] = ((uint16_t)d->channel[i+2] << 4); + channels[i+1] |= ((uint16_t)(0x0F & d->channel[i+1])); + + /* convert values to 1000-2000 ppm encoding in a not too sloppy fashion */ + // channels[i] = (uint16_t)(channels[i] * ST24_SCALE_FACTOR +.5f) + ST24_SCALE_OFFSET; + // channels[i+1] = (uint16_t)(channels[i+1] * ST24_SCALE_FACTOR +.5f) + ST24_SCALE_OFFSET; } } break; @@ -160,17 +187,29 @@ uint8_t st24_decode(uint8_t byte, uint8_t *rssi, uint8_t* rx_count, uint16_t *ch *rssi = d->rssi; *rx_count = d->packet_count; + *channel_count = 24; + + for (unsigned i = 0; i < *channel_count; i += 2) { + channels[i] = ((uint16_t)d->channel[i]) << 4; + channels[i] |= ((uint16_t)(0xF0 & d->channel[i+1]) >> 4); - for (unsigned i = 0; i < 1; i += 2) { - channels[i] = ((uint16_t)d->channel[i]) << 8; - channels[i] |= (0xF & d->channel[i+1]); + channels[i+1] = ((uint16_t)d->channel[i+2] << 4); + channels[i+1] |= ((uint16_t)(0x0F & d->channel[i+1])); - channels[i+1] = ((uint16_t)(0xF & d->channel[i+1])) << 4; - channels[i+1] |= d->channel[i+2]; + // XXX apply scaling } } break; + case ST24_PACKET_TYPE_TRANSMITTERGPSDATA: + { + + ReceiverFcPacket* d = (ReceiverFcPacket*)&_rxpacket; + /* we silently ignore this data for now, as its not classic TX data */ + ret = false; + } + break; + default: ret = false; break; @@ -178,10 +217,11 @@ uint8_t st24_decode(uint8_t byte, uint8_t *rssi, uint8_t* rx_count, uint16_t *ch } else { /* decoding failed */ - _decode_state = ST24_DECODE_STATE_UNSYNCED; + } + _decode_state = ST24_DECODE_STATE_UNSYNCED; break; } - return ret; + return !ret; } diff --git a/src/lib/rc/st24.h b/src/lib/rc/st24.h index 24dbf8e51..45a835f77 100644 --- a/src/lib/rc/st24.h +++ b/src/lib/rc/st24.h @@ -157,6 +157,6 @@ uint8_t st24_common_crc8(uint8_t *ptr, uint8_t len); * @param max_chan_count maximum channels to decode - if more channels are decoded, the last n are skipped and success (0) is returned * @return 0 for success (a decoded packet), 1 for no packet yet (accumulating), 3 for out of sync, 4 for checksum error */ -__EXPORT uint8_t st24_decode(uint8_t byte, uint8_t *rssi, uint8_t* rx_count, uint16_t *channels, uint16_t max_chan_count); +__EXPORT uint8_t st24_decode(uint8_t byte, uint8_t *rssi, uint8_t* rx_count, uint16_t* channel_count, int16_t *channels, uint16_t max_chan_count); __END_DECLS -- cgit v1.2.3 From 9c89499696baa15f709d125e46866e9f11ad5432 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 7 Oct 2014 07:14:28 +0200 Subject: Fix up ST24 lib --- Tools/tests-host/st24_test.cpp | 9 +++++++-- src/lib/rc/st24.c | 44 +++++++++++++++++++++++++++--------------- 2 files changed, 35 insertions(+), 18 deletions(-) diff --git a/Tools/tests-host/st24_test.cpp b/Tools/tests-host/st24_test.cpp index e1134bba6..f143542bd 100644 --- a/Tools/tests-host/st24_test.cpp +++ b/Tools/tests-host/st24_test.cpp @@ -36,7 +36,7 @@ int main(int argc, char *argv[]) { while (EOF != (ret = fscanf(fp, "%f,%x,,", &f, &x))) { if (((f - last_time) * 1000 * 1000) > 3000) { - warnx("FRAME RESET\n\n"); + //warnx("FRAME RESET\n\n"); } uint8_t b = static_cast(x); @@ -56,8 +56,13 @@ int main(int argc, char *argv[]) { { //warnx("decoded: %u channels", (unsigned)channel_count); for (unsigned i = 0; i < channel_count; i++) { - warnx("channel %u: %d", i, static_cast(channels[i])); + + int16_t val = channels[i]; + // if (i == 6) + warnx("channel %u: %d 0x%03X", i, static_cast(val), static_cast(val)); } + // unsigned chan = 1; + // warnx("channel %u: %d", chan, static_cast(channels[chan])); } //warnx("%f: 0x%02x >> RSSI: %u #: %u", (double)f, x, static_cast(rssi), static_cast(rx_count)); diff --git a/src/lib/rc/st24.c b/src/lib/rc/st24.c index f43f2de7b..5a13649b5 100644 --- a/src/lib/rc/st24.c +++ b/src/lib/rc/st24.c @@ -61,7 +61,7 @@ const char* decode_states[] = {"UNSYNCED", /* define range mapping here, -+100% -> 1000..2000 */ #define ST24_RANGE_MIN 0.0f -#define ST24_RANGE_MAX 8000.0f +#define ST24_RANGE_MAX 4096.0f #define ST24_TARGET_MIN 1000.0f #define ST24_TARGET_MAX 2000.0f @@ -161,42 +161,54 @@ uint8_t st24_decode(uint8_t byte, uint8_t *rssi, uint8_t* rx_count, uint16_t *ch case ST24_PACKET_TYPE_CHANNELDATA12: { - ChannelData12* d = (ChannelData12*)&_rxpacket; + ChannelData12* d = (ChannelData12*)_rxpacket.st24_data; *rssi = d->rssi; *rx_count = d->packet_count; *channel_count = 12; - for (unsigned i = 0; i < *channel_count; i += 2) { - channels[i] = ((uint16_t)d->channel[i]) << 4; - channels[i] |= ((uint16_t)(0xF0 & d->channel[i+1]) >> 4); + unsigned stride_count = (*channel_count * 3) / 2; + unsigned chan_index = 0; - channels[i+1] = ((uint16_t)d->channel[i+2] << 4); - channels[i+1] |= ((uint16_t)(0x0F & d->channel[i+1])); + for (unsigned i = 0; i < stride_count; i += 3) { + channels[chan_index] = ((uint16_t)d->channel[i] << 4); + channels[chan_index] |= ((uint16_t)(0xF0 & d->channel[i+1]) >> 4); + /* convert values to 1000-2000 ppm encoding in a not too sloppy fashion */ + channels[chan_index] = (uint16_t)(channels[chan_index] * ST24_SCALE_FACTOR +.5f) + ST24_SCALE_OFFSET; + chan_index++; + channels[chan_index] = ((uint16_t)d->channel[i+2]); + channels[chan_index] |= (((uint16_t)(0x0F & d->channel[i+1])) << 8); /* convert values to 1000-2000 ppm encoding in a not too sloppy fashion */ - // channels[i] = (uint16_t)(channels[i] * ST24_SCALE_FACTOR +.5f) + ST24_SCALE_OFFSET; - // channels[i+1] = (uint16_t)(channels[i+1] * ST24_SCALE_FACTOR +.5f) + ST24_SCALE_OFFSET; + channels[chan_index] = (uint16_t)(channels[chan_index] * ST24_SCALE_FACTOR +.5f) + ST24_SCALE_OFFSET; + chan_index++; } } break; case ST24_PACKET_TYPE_CHANNELDATA24: { - ChannelData24* d = (ChannelData24*)&_rxpacket; + ChannelData24* d = (ChannelData24*)&_rxpacket.st24_data; *rssi = d->rssi; *rx_count = d->packet_count; *channel_count = 24; - for (unsigned i = 0; i < *channel_count; i += 2) { - channels[i] = ((uint16_t)d->channel[i]) << 4; - channels[i] |= ((uint16_t)(0xF0 & d->channel[i+1]) >> 4); + unsigned stride_count = (*channel_count * 3) / 2; + unsigned chan_index = 0; - channels[i+1] = ((uint16_t)d->channel[i+2] << 4); - channels[i+1] |= ((uint16_t)(0x0F & d->channel[i+1])); + for (unsigned i = 0; i < stride_count; i += 3) { + channels[chan_index] = ((uint16_t)d->channel[i] << 4); + channels[chan_index] |= ((uint16_t)(0xF0 & d->channel[i+1]) >> 4); + /* convert values to 1000-2000 ppm encoding in a not too sloppy fashion */ + channels[chan_index] = (uint16_t)(channels[chan_index] * ST24_SCALE_FACTOR +.5f) + ST24_SCALE_OFFSET; + chan_index++; - // XXX apply scaling + channels[chan_index] = ((uint16_t)d->channel[i+2]); + channels[chan_index] |= (((uint16_t)(0x0F & d->channel[i+1])) << 8); + /* convert values to 1000-2000 ppm encoding in a not too sloppy fashion */ + channels[chan_index] = (uint16_t)(channels[chan_index] * ST24_SCALE_FACTOR +.5f) + ST24_SCALE_OFFSET; + chan_index++; } } break; -- cgit v1.2.3 From cb0cbe479ae4bdaab6b6570857d240940fea713d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 7 Oct 2014 08:05:32 +0200 Subject: Finalizing ST24 lib --- src/lib/rc/st24.c | 2 +- src/lib/rc/st24.h | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/lib/rc/st24.c b/src/lib/rc/st24.c index 5a13649b5..31a1e3fd3 100644 --- a/src/lib/rc/st24.c +++ b/src/lib/rc/st24.c @@ -102,7 +102,7 @@ uint8_t st24_common_crc8(uint8_t *ptr, uint8_t len) } -uint8_t st24_decode(uint8_t byte, uint8_t *rssi, uint8_t* rx_count, uint16_t *channel_count, int16_t *channels, uint16_t max_chan_count) +uint8_t st24_decode(uint8_t byte, uint8_t *rssi, uint8_t* rx_count, uint16_t *channel_count, uint16_t *channels, uint16_t max_chan_count) { bool ret = false; diff --git a/src/lib/rc/st24.h b/src/lib/rc/st24.h index 45a835f77..6023a7507 100644 --- a/src/lib/rc/st24.h +++ b/src/lib/rc/st24.h @@ -157,6 +157,6 @@ uint8_t st24_common_crc8(uint8_t *ptr, uint8_t len); * @param max_chan_count maximum channels to decode - if more channels are decoded, the last n are skipped and success (0) is returned * @return 0 for success (a decoded packet), 1 for no packet yet (accumulating), 3 for out of sync, 4 for checksum error */ -__EXPORT uint8_t st24_decode(uint8_t byte, uint8_t *rssi, uint8_t* rx_count, uint16_t* channel_count, int16_t *channels, uint16_t max_chan_count); +__EXPORT uint8_t st24_decode(uint8_t byte, uint8_t *rssi, uint8_t* rx_count, uint16_t* channel_count, uint16_t *channels, uint16_t max_chan_count); __END_DECLS -- cgit v1.2.3 From c2687a777444f68e7a7685ec628848b950f02571 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 7 Oct 2014 08:05:45 +0200 Subject: ST24 integration in IO firmware --- src/modules/px4iofirmware/controls.c | 73 ++++++++++++++++++++++-------------- src/modules/px4iofirmware/dsm.c | 10 ++++- src/modules/px4iofirmware/protocol.h | 1 + src/modules/px4iofirmware/px4io.h | 2 +- 4 files changed, 54 insertions(+), 32 deletions(-) diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c index 3aa3d28ff..1b7be692a 100644 --- a/src/modules/px4iofirmware/controls.c +++ b/src/modules/px4iofirmware/controls.c @@ -52,7 +52,7 @@ #define RC_CHANNEL_LOW_THRESH -8000 /* 10% threshold */ static bool ppm_input(uint16_t *values, uint16_t *num_values, uint16_t *frame_len); -static bool dsm_port_input(void); +static bool dsm_port_input(uint8_t *rssi); static perf_counter_t c_gather_dsm; static perf_counter_t c_gather_sbus; @@ -60,9 +60,49 @@ static perf_counter_t c_gather_ppm; static int _dsm_fd; -bool dsm_port_input() +bool dsm_port_input(uint8_t *rssi) { + perf_begin(c_gather_dsm); + uint16_t temp_count = r_raw_rc_count; + uint8_t n_bytes = 0; + uint8_t *bytes; + bool dsm_updated = dsm_input(r_raw_rc_values, &temp_count, &n_bytes, &bytes); + if (dsm_updated) { + r_raw_rc_flags |= PX4IO_P_STATUS_FLAGS_RC_DSM; + r_raw_rc_count = temp_count & 0x7fff; + if (temp_count & 0x8000) + r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_RC_DSM11; + else + r_raw_rc_flags &= ~PX4IO_P_RAW_RC_FLAGS_RC_DSM11; + + r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP); + r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE); + + } + perf_end(c_gather_dsm); + /* get data from FD and attempt to parse with DSM and ST24 libs */ + uint8_t st24_rssi, rx_count; + uint16_t st24_channel_count; + uint8_t st24_maxchans = 18; + + bool st24_updated = false; + + for (unsigned i = 0; i < n_bytes; i++) { + /* set updated flag if one complete packet was parsed */ + st24_updated |= st24_decode(bytes[i], &st24_rssi, &rx_count, + &st24_channel_count, r_raw_rc_values, st24_maxchans); + } + + if (st24_updated) { + + *rssi = st24_rssi; + r_raw_rc_count = st24_channel_count; + + r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_ST24; + r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP); + r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE); + } return false; } @@ -127,20 +167,7 @@ controls_tick() { #endif perf_begin(c_gather_dsm); - uint16_t temp_count = r_raw_rc_count; - bool dsm_updated = dsm_input(r_raw_rc_values, &temp_count); - if (dsm_updated) { - r_raw_rc_flags |= PX4IO_P_STATUS_FLAGS_RC_DSM; - r_raw_rc_count = temp_count & 0x7fff; - if (temp_count & 0x8000) - r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_RC_DSM11; - else - r_raw_rc_flags &= ~PX4IO_P_RAW_RC_FLAGS_RC_DSM11; - - r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP); - r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE); - - } + (void)dsm_port_input(&rssi); perf_end(c_gather_dsm); perf_begin(c_gather_sbus); @@ -186,18 +213,6 @@ controls_tick() { } perf_end(c_gather_ppm); - uint8_t st24_rssi, rx_count; - uint8_t st24_maxchans = 18; - bool st24_updated = st24_decode(0, &st24_rssi, &rx_count, r_raw_rc_values, st24_maxchans); - if (st24_updated) { - - rssi = st24_rssi; - - r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_PPM; - r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP); - r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE); - } - /* limit number of channels to allowable data size */ if (r_raw_rc_count > PX4IO_RC_INPUT_CHANNELS) r_raw_rc_count = PX4IO_RC_INPUT_CHANNELS; @@ -402,7 +417,7 @@ controls_tick() { r_status_flags |= PX4IO_P_STATUS_FLAGS_OVERRIDE; /* mix new RC input control values to servos */ - if (dsm_updated || sbus_updated || ppm_updated) + if (dsm_updated || sbus_updated || ppm_updated || st24_updated) mixer_tick(); } else { diff --git a/src/modules/px4iofirmware/dsm.c b/src/modules/px4iofirmware/dsm.c index d3f365822..6e57c9ec7 100644 --- a/src/modules/px4iofirmware/dsm.c +++ b/src/modules/px4iofirmware/dsm.c @@ -452,10 +452,12 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values) * * @param[out] values pointer to per channel array of decoded values * @param[out] num_values pointer to number of raw channel values returned, high order bit 0:10 bit data, 1:11 bit data + * @param[out] n_butes number of bytes read + * @param[out] bytes pointer to the buffer of read bytes * @return true=decoded raw channel values updated, false=no update */ bool -dsm_input(uint16_t *values, uint16_t *num_values) +dsm_input(uint16_t *values, uint16_t *num_values, uint8_t *n_bytes, uint8_t **bytes) { ssize_t ret; hrt_abstime now; @@ -478,8 +480,12 @@ dsm_input(uint16_t *values, uint16_t *num_values) ret = read(dsm_fd, &dsm_frame[dsm_partial_frame_count], DSM_FRAME_SIZE - dsm_partial_frame_count); /* if the read failed for any reason, just give up here */ - if (ret < 1) + if (ret < 1) { return false; + } else { + *n_bytes = ret; + *bytes = &dsm_frame[dsm_partial_frame_count]; + } dsm_last_rx_time = now; diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h index 4739f6e40..e907d2959 100644 --- a/src/modules/px4iofirmware/protocol.h +++ b/src/modules/px4iofirmware/protocol.h @@ -112,6 +112,7 @@ #define PX4IO_P_STATUS_FLAGS_FAILSAFE (1 << 11) /* failsafe is active */ #define PX4IO_P_STATUS_FLAGS_SAFETY_OFF (1 << 12) /* safety is off */ #define PX4IO_P_STATUS_FLAGS_FMU_INITIALIZED (1 << 13) /* FMU was initialized and OK once */ +#define PX4IO_P_STATUS_FLAGS_RC_ST24 (1 << 14) /* SBUS input is valid */ #define PX4IO_P_STATUS_ALARMS 3 /* alarm flags - alarms latch, write 1 to a bit to clear it */ #define PX4IO_P_STATUS_ALARMS_VBATT_LOW (1 << 0) /* [1] VBatt is very close to regulator dropout */ diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h index b00a96717..e32fcc72b 100644 --- a/src/modules/px4iofirmware/px4io.h +++ b/src/modules/px4iofirmware/px4io.h @@ -215,7 +215,7 @@ extern uint16_t adc_measure(unsigned channel); extern void controls_init(void); extern void controls_tick(void); extern int dsm_init(const char *device); -extern bool dsm_input(uint16_t *values, uint16_t *num_values); +extern bool dsm_input(uint16_t *values, uint16_t *num_values, uint8_t *n_bytes, uint8_t **bytes); extern void dsm_bind(uint16_t cmd, int pulses); extern int sbus_init(const char *device); extern bool sbus_input(uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_channels); -- cgit v1.2.3 From 3fc064882f37919b21e0176a071a7a9430688987 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 7 Oct 2014 09:28:14 +0200 Subject: ST24 lib: formatting --- src/lib/rc/st24.c | 264 ++++++++++++++++++++++++++++-------------------------- src/lib/rc/st24.h | 3 +- 2 files changed, 138 insertions(+), 129 deletions(-) diff --git a/src/lib/rc/st24.c b/src/lib/rc/st24.c index 31a1e3fd3..bb42830db 100644 --- a/src/lib/rc/st24.c +++ b/src/lib/rc/st24.c @@ -52,12 +52,13 @@ enum ST24_DECODE_STATE { ST24_DECODE_STATE_GOT_DATA }; -const char* decode_states[] = {"UNSYNCED", - "GOT_STX1", - "GOT_STX2", - "GOT_LEN", - "GOT_TYPE", - "GOT_DATA"}; +const char *decode_states[] = {"UNSYNCED", + "GOT_STX1", + "GOT_STX2", + "GOT_LEN", + "GOT_TYPE", + "GOT_DATA" + }; /* define range mapping here, -+100% -> 1000..2000 */ #define ST24_RANGE_MIN 0.0f @@ -102,137 +103,144 @@ uint8_t st24_common_crc8(uint8_t *ptr, uint8_t len) } -uint8_t st24_decode(uint8_t byte, uint8_t *rssi, uint8_t* rx_count, uint16_t *channel_count, uint16_t *channels, uint16_t max_chan_count) +uint8_t st24_decode(uint8_t byte, uint8_t *rssi, uint8_t *rx_count, uint16_t *channel_count, uint16_t *channels, + uint16_t max_chan_count) { bool ret = false; switch (_decode_state) { - case ST24_DECODE_STATE_UNSYNCED: - if (byte == ST24_STX1) { - _decode_state = ST24_DECODE_STATE_GOT_STX1; - } - break; + case ST24_DECODE_STATE_UNSYNCED: + if (byte == ST24_STX1) { + _decode_state = ST24_DECODE_STATE_GOT_STX1; + } - case ST24_DECODE_STATE_GOT_STX1: - if (byte == ST24_STX2) { - _decode_state = ST24_DECODE_STATE_GOT_STX2; - } else { - _decode_state = ST24_DECODE_STATE_UNSYNCED; - } - break; - - case ST24_DECODE_STATE_GOT_STX2: - /* ensure no data overflow failure or hack is possible */ - if ((unsigned)byte <= sizeof(_rxpacket.length) + sizeof(_rxpacket.type) + sizeof(_rxpacket.st24_data)) { - _rxpacket.length = byte; - _rxlen = 0; - _decode_state = ST24_DECODE_STATE_GOT_LEN; - } else { - _decode_state = ST24_DECODE_STATE_UNSYNCED; - } - break; - - case ST24_DECODE_STATE_GOT_LEN: - _rxpacket.type = byte; - _rxlen++; - _decode_state = ST24_DECODE_STATE_GOT_TYPE; - break; - - case ST24_DECODE_STATE_GOT_TYPE: - _rxpacket.st24_data[_rxlen - 1] = byte; - _rxlen++; - if (_rxlen == (_rxpacket.length - 1)) { - _decode_state = ST24_DECODE_STATE_GOT_DATA; - } - break; - - case ST24_DECODE_STATE_GOT_DATA: - _rxpacket.crc8 = byte; - _rxlen++; - - if (st24_common_crc8((uint8_t*)&(_rxpacket.length), _rxlen) == _rxpacket.crc8) { - - ret = true; - - /* decode the actual packet */ - - switch (_rxpacket.type) { - - case ST24_PACKET_TYPE_CHANNELDATA12: - { - ChannelData12* d = (ChannelData12*)_rxpacket.st24_data; - - *rssi = d->rssi; - *rx_count = d->packet_count; - *channel_count = 12; - - unsigned stride_count = (*channel_count * 3) / 2; - unsigned chan_index = 0; - - for (unsigned i = 0; i < stride_count; i += 3) { - channels[chan_index] = ((uint16_t)d->channel[i] << 4); - channels[chan_index] |= ((uint16_t)(0xF0 & d->channel[i+1]) >> 4); - /* convert values to 1000-2000 ppm encoding in a not too sloppy fashion */ - channels[chan_index] = (uint16_t)(channels[chan_index] * ST24_SCALE_FACTOR +.5f) + ST24_SCALE_OFFSET; - chan_index++; - - channels[chan_index] = ((uint16_t)d->channel[i+2]); - channels[chan_index] |= (((uint16_t)(0x0F & d->channel[i+1])) << 8); - /* convert values to 1000-2000 ppm encoding in a not too sloppy fashion */ - channels[chan_index] = (uint16_t)(channels[chan_index] * ST24_SCALE_FACTOR +.5f) + ST24_SCALE_OFFSET; - chan_index++; - } - } - break; - - case ST24_PACKET_TYPE_CHANNELDATA24: - { - ChannelData24* d = (ChannelData24*)&_rxpacket.st24_data; - - *rssi = d->rssi; - *rx_count = d->packet_count; - *channel_count = 24; - - unsigned stride_count = (*channel_count * 3) / 2; - unsigned chan_index = 0; - - for (unsigned i = 0; i < stride_count; i += 3) { - channels[chan_index] = ((uint16_t)d->channel[i] << 4); - channels[chan_index] |= ((uint16_t)(0xF0 & d->channel[i+1]) >> 4); - /* convert values to 1000-2000 ppm encoding in a not too sloppy fashion */ - channels[chan_index] = (uint16_t)(channels[chan_index] * ST24_SCALE_FACTOR +.5f) + ST24_SCALE_OFFSET; - chan_index++; - - channels[chan_index] = ((uint16_t)d->channel[i+2]); - channels[chan_index] |= (((uint16_t)(0x0F & d->channel[i+1])) << 8); - /* convert values to 1000-2000 ppm encoding in a not too sloppy fashion */ - channels[chan_index] = (uint16_t)(channels[chan_index] * ST24_SCALE_FACTOR +.5f) + ST24_SCALE_OFFSET; - chan_index++; - } - } - break; - - case ST24_PACKET_TYPE_TRANSMITTERGPSDATA: - { - - ReceiverFcPacket* d = (ReceiverFcPacket*)&_rxpacket; - /* we silently ignore this data for now, as its not classic TX data */ - ret = false; - } - break; - - default: - ret = false; - break; + break; + + case ST24_DECODE_STATE_GOT_STX1: + if (byte == ST24_STX2) { + _decode_state = ST24_DECODE_STATE_GOT_STX2; + + } else { + _decode_state = ST24_DECODE_STATE_UNSYNCED; + } + + break; + + case ST24_DECODE_STATE_GOT_STX2: + + /* ensure no data overflow failure or hack is possible */ + if ((unsigned)byte <= sizeof(_rxpacket.length) + sizeof(_rxpacket.type) + sizeof(_rxpacket.st24_data)) { + _rxpacket.length = byte; + _rxlen = 0; + _decode_state = ST24_DECODE_STATE_GOT_LEN; + + } else { + _decode_state = ST24_DECODE_STATE_UNSYNCED; + } + + break; + + case ST24_DECODE_STATE_GOT_LEN: + _rxpacket.type = byte; + _rxlen++; + _decode_state = ST24_DECODE_STATE_GOT_TYPE; + break; + + case ST24_DECODE_STATE_GOT_TYPE: + _rxpacket.st24_data[_rxlen - 1] = byte; + _rxlen++; + + if (_rxlen == (_rxpacket.length - 1)) { + _decode_state = ST24_DECODE_STATE_GOT_DATA; + } + + break; + + case ST24_DECODE_STATE_GOT_DATA: + _rxpacket.crc8 = byte; + _rxlen++; + + if (st24_common_crc8((uint8_t *) & (_rxpacket.length), _rxlen) == _rxpacket.crc8) { + + ret = true; + + /* decode the actual packet */ + + switch (_rxpacket.type) { + + case ST24_PACKET_TYPE_CHANNELDATA12: { + ChannelData12 *d = (ChannelData12 *)_rxpacket.st24_data; + + *rssi = d->rssi; + *rx_count = d->packet_count; + *channel_count = 12; + + unsigned stride_count = (*channel_count * 3) / 2; + unsigned chan_index = 0; + + for (unsigned i = 0; i < stride_count; i += 3) { + channels[chan_index] = ((uint16_t)d->channel[i] << 4); + channels[chan_index] |= ((uint16_t)(0xF0 & d->channel[i + 1]) >> 4); + /* convert values to 1000-2000 ppm encoding in a not too sloppy fashion */ + channels[chan_index] = (uint16_t)(channels[chan_index] * ST24_SCALE_FACTOR + .5f) + ST24_SCALE_OFFSET; + chan_index++; + + channels[chan_index] = ((uint16_t)d->channel[i + 2]); + channels[chan_index] |= (((uint16_t)(0x0F & d->channel[i + 1])) << 8); + /* convert values to 1000-2000 ppm encoding in a not too sloppy fashion */ + channels[chan_index] = (uint16_t)(channels[chan_index] * ST24_SCALE_FACTOR + .5f) + ST24_SCALE_OFFSET; + chan_index++; + } } + break; + + case ST24_PACKET_TYPE_CHANNELDATA24: { + ChannelData24 *d = (ChannelData24 *)&_rxpacket.st24_data; + + *rssi = d->rssi; + *rx_count = d->packet_count; + *channel_count = 24; + + unsigned stride_count = (*channel_count * 3) / 2; + unsigned chan_index = 0; + + for (unsigned i = 0; i < stride_count; i += 3) { + channels[chan_index] = ((uint16_t)d->channel[i] << 4); + channels[chan_index] |= ((uint16_t)(0xF0 & d->channel[i + 1]) >> 4); + /* convert values to 1000-2000 ppm encoding in a not too sloppy fashion */ + channels[chan_index] = (uint16_t)(channels[chan_index] * ST24_SCALE_FACTOR + .5f) + ST24_SCALE_OFFSET; + chan_index++; + + channels[chan_index] = ((uint16_t)d->channel[i + 2]); + channels[chan_index] |= (((uint16_t)(0x0F & d->channel[i + 1])) << 8); + /* convert values to 1000-2000 ppm encoding in a not too sloppy fashion */ + channels[chan_index] = (uint16_t)(channels[chan_index] * ST24_SCALE_FACTOR + .5f) + ST24_SCALE_OFFSET; + chan_index++; + } + } + break; - } else { - /* decoding failed */ - + case ST24_PACKET_TYPE_TRANSMITTERGPSDATA: { + + // ReceiverFcPacket* d = (ReceiverFcPacket*)&_rxpacket.st24_data; + /* we silently ignore this data for now, as it is unused */ + ret = false; + } + break; + + default: + ret = false; + break; } - _decode_state = ST24_DECODE_STATE_UNSYNCED; - break; + + } else { + /* decoding failed */ + + } + + _decode_state = ST24_DECODE_STATE_UNSYNCED; + break; } return !ret; diff --git a/src/lib/rc/st24.h b/src/lib/rc/st24.h index 6023a7507..1876eabc5 100644 --- a/src/lib/rc/st24.h +++ b/src/lib/rc/st24.h @@ -157,6 +157,7 @@ uint8_t st24_common_crc8(uint8_t *ptr, uint8_t len); * @param max_chan_count maximum channels to decode - if more channels are decoded, the last n are skipped and success (0) is returned * @return 0 for success (a decoded packet), 1 for no packet yet (accumulating), 3 for out of sync, 4 for checksum error */ -__EXPORT uint8_t st24_decode(uint8_t byte, uint8_t *rssi, uint8_t* rx_count, uint16_t* channel_count, uint16_t *channels, uint16_t max_chan_count); +__EXPORT uint8_t st24_decode(uint8_t byte, uint8_t *rssi, uint8_t *rx_count, uint16_t *channel_count, + uint16_t *channels, uint16_t max_chan_count); __END_DECLS -- cgit v1.2.3 From 35caa8bd996392924ab1d92eb46c3d40e0a91b28 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 7 Oct 2014 09:28:36 +0200 Subject: PX4IO Controls: compile fixes --- src/modules/px4iofirmware/controls.c | 19 ++++++++++--------- 1 file changed, 10 insertions(+), 9 deletions(-) diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c index 1b7be692a..8670e30c2 100644 --- a/src/modules/px4iofirmware/controls.c +++ b/src/modules/px4iofirmware/controls.c @@ -52,7 +52,7 @@ #define RC_CHANNEL_LOW_THRESH -8000 /* 10% threshold */ static bool ppm_input(uint16_t *values, uint16_t *num_values, uint16_t *frame_len); -static bool dsm_port_input(uint8_t *rssi); +static bool dsm_port_input(uint8_t *rssi, bool *dsm_updated, bool *st24_updated); static perf_counter_t c_gather_dsm; static perf_counter_t c_gather_sbus; @@ -60,14 +60,14 @@ static perf_counter_t c_gather_ppm; static int _dsm_fd; -bool dsm_port_input(uint8_t *rssi) +bool dsm_port_input(uint8_t *rssi, bool *dsm_updated, bool *st24_updated) { perf_begin(c_gather_dsm); uint16_t temp_count = r_raw_rc_count; uint8_t n_bytes = 0; uint8_t *bytes; - bool dsm_updated = dsm_input(r_raw_rc_values, &temp_count, &n_bytes, &bytes); - if (dsm_updated) { + *dsm_updated = dsm_input(r_raw_rc_values, &temp_count, &n_bytes, &bytes); + if (*dsm_updated) { r_raw_rc_flags |= PX4IO_P_STATUS_FLAGS_RC_DSM; r_raw_rc_count = temp_count & 0x7fff; if (temp_count & 0x8000) @@ -86,15 +86,15 @@ bool dsm_port_input(uint8_t *rssi) uint16_t st24_channel_count; uint8_t st24_maxchans = 18; - bool st24_updated = false; + *st24_updated = false; for (unsigned i = 0; i < n_bytes; i++) { /* set updated flag if one complete packet was parsed */ - st24_updated |= st24_decode(bytes[i], &st24_rssi, &rx_count, + *st24_updated |= st24_decode(bytes[i], &st24_rssi, &rx_count, &st24_channel_count, r_raw_rc_values, st24_maxchans); } - if (st24_updated) { + if (*st24_updated) { *rssi = st24_rssi; r_raw_rc_count = st24_channel_count; @@ -104,7 +104,7 @@ bool dsm_port_input(uint8_t *rssi) r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE); } - return false; + return (*dsm_updated | *st24_updated); } void @@ -167,7 +167,8 @@ controls_tick() { #endif perf_begin(c_gather_dsm); - (void)dsm_port_input(&rssi); + bool dsm_updated, st24_updated; + (void)dsm_port_input(&rssi, &dsm_updated, &st24_updated); perf_end(c_gather_dsm); perf_begin(c_gather_sbus); -- cgit v1.2.3 From c7e8570f8355cb96b4daf4f1d3283807ae0cbdc5 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 7 Oct 2014 09:36:24 +0200 Subject: PX4IO firmware: Fix comment --- src/modules/px4iofirmware/protocol.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h index e907d2959..89a470b44 100644 --- a/src/modules/px4iofirmware/protocol.h +++ b/src/modules/px4iofirmware/protocol.h @@ -112,7 +112,7 @@ #define PX4IO_P_STATUS_FLAGS_FAILSAFE (1 << 11) /* failsafe is active */ #define PX4IO_P_STATUS_FLAGS_SAFETY_OFF (1 << 12) /* safety is off */ #define PX4IO_P_STATUS_FLAGS_FMU_INITIALIZED (1 << 13) /* FMU was initialized and OK once */ -#define PX4IO_P_STATUS_FLAGS_RC_ST24 (1 << 14) /* SBUS input is valid */ +#define PX4IO_P_STATUS_FLAGS_RC_ST24 (1 << 14) /* ST24 input is valid */ #define PX4IO_P_STATUS_ALARMS 3 /* alarm flags - alarms latch, write 1 to a bit to clear it */ #define PX4IO_P_STATUS_ALARMS_VBATT_LOW (1 << 0) /* [1] VBatt is very close to regulator dropout */ -- cgit v1.2.3 From 7921184e4081de18f92874df03a082c04510fa24 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 7 Oct 2014 09:36:42 +0200 Subject: IO input driver: Output ST24 as receiver type / status --- src/drivers/drv_rc_input.h | 3 ++- src/drivers/px4io/px4io.cpp | 10 +++++++++- 2 files changed, 11 insertions(+), 2 deletions(-) diff --git a/src/drivers/drv_rc_input.h b/src/drivers/drv_rc_input.h index 47fa8fa59..b249c2a09 100644 --- a/src/drivers/drv_rc_input.h +++ b/src/drivers/drv_rc_input.h @@ -83,7 +83,8 @@ enum RC_INPUT_SOURCE { RC_INPUT_SOURCE_PX4FMU_PPM, RC_INPUT_SOURCE_PX4IO_PPM, RC_INPUT_SOURCE_PX4IO_SPEKTRUM, - RC_INPUT_SOURCE_PX4IO_SBUS + RC_INPUT_SOURCE_PX4IO_SBUS, + RC_INPUT_SOURCE_PX4IO_ST24 }; /** diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index fbb5d4f2e..d212be766 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1617,6 +1617,9 @@ PX4IO::io_publish_raw_rc() } else if (_status & PX4IO_P_STATUS_FLAGS_RC_SBUS) { rc_val.input_source = RC_INPUT_SOURCE_PX4IO_SBUS; + } else if (_status & PX4IO_P_STATUS_FLAGS_RC_ST24) { + rc_val.input_source = RC_INPUT_SOURCE_PX4IO_ST24; + } else { rc_val.input_source = RC_INPUT_SOURCE_UNKNOWN; @@ -1934,13 +1937,15 @@ PX4IO::print_status(bool extended_status) io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FREEMEM)); uint16_t flags = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS); uint16_t io_status_flags = flags; - printf("status 0x%04x%s%s%s%s%s%s%s%s%s%s%s%s\n", + printf("status 0x%04x%s%s%s%s%s%s%s%s%s%s%s%s%s%s\n", flags, ((flags & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED) ? " OUTPUTS_ARMED" : ""), ((flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) ? " SAFETY_OFF" : " SAFETY_SAFE"), ((flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) ? " OVERRIDE" : ""), ((flags & PX4IO_P_STATUS_FLAGS_RC_OK) ? " RC_OK" : " RC_FAIL"), ((flags & PX4IO_P_STATUS_FLAGS_RC_PPM) ? " PPM" : ""), + ((flags & PX4IO_P_STATUS_FLAGS_RC_DSM) ? " DSM" : ""), + ((flags & PX4IO_P_STATUS_FLAGS_RC_ST24) ? " ST24" : ""), ((flags & PX4IO_P_STATUS_FLAGS_RC_SBUS) ? " SBUS" : ""), ((flags & PX4IO_P_STATUS_FLAGS_FMU_OK) ? " FMU_OK" : " FMU_FAIL"), ((flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) ? " RAW_PWM_PASSTHROUGH" : ""), @@ -2465,6 +2470,9 @@ PX4IO::ioctl(file * filep, int cmd, unsigned long arg) } else if (status & PX4IO_P_STATUS_FLAGS_RC_SBUS) { rc_val->input_source = RC_INPUT_SOURCE_PX4IO_SBUS; + } else if (status & PX4IO_P_STATUS_FLAGS_RC_ST24) { + rc_val->input_source = RC_INPUT_SOURCE_PX4IO_ST24; + } else { rc_val->input_source = RC_INPUT_SOURCE_UNKNOWN; } -- cgit v1.2.3 From 7e5910bdbf10c5c00f438d17d1b0bcd00d6c020d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 7 Oct 2014 09:54:20 +0200 Subject: Formatted ST24 test code --- Tools/tests-host/st24_test.cpp | 31 +++++++++++++++---------------- 1 file changed, 15 insertions(+), 16 deletions(-) diff --git a/Tools/tests-host/st24_test.cpp b/Tools/tests-host/st24_test.cpp index f143542bd..25a9355e2 100644 --- a/Tools/tests-host/st24_test.cpp +++ b/Tools/tests-host/st24_test.cpp @@ -7,20 +7,23 @@ #include #include "../../src/systemcmds/tests/tests.h" -int main(int argc, char *argv[]) { +int main(int argc, char *argv[]) +{ warnx("ST24 test started"); - if (argc < 2) + if (argc < 2) { errx(1, "Need a filename for the input file"); + } warnx("loading data from: %s", argv[1]); FILE *fp; - - fp = fopen(argv[1],"rt"); - if (!fp) + fp = fopen(argv[1], "rt"); + + if (!fp) { errx(1, "failed opening file"); + } float f; unsigned x; @@ -36,7 +39,7 @@ int main(int argc, char *argv[]) { while (EOF != (ret = fscanf(fp, "%f,%x,,", &f, &x))) { if (((f - last_time) * 1000 * 1000) > 3000) { - //warnx("FRAME RESET\n\n"); + // warnx("FRAME RESET\n\n"); } uint8_t b = static_cast(x); @@ -49,27 +52,23 @@ int main(int argc, char *argv[]) { uint8_t rssi; uint8_t rx_count; uint16_t channel_count; - int16_t channels[20]; + uint16_t channels[20]; - if (!st24_decode(b, &rssi, &rx_count, &channel_count, channels, sizeof(channels) / sizeof(channels[0]))) - { - //warnx("decoded: %u channels", (unsigned)channel_count); + if (!st24_decode(b, &rssi, &rx_count, &channel_count, channels, sizeof(channels) / sizeof(channels[0]))) { + warnx("decoded: %u channels (converted to PPM range)", (unsigned)channel_count); + for (unsigned i = 0; i < channel_count; i++) { int16_t val = channels[i]; - // if (i == 6) - warnx("channel %u: %d 0x%03X", i, static_cast(val), static_cast(val)); + warnx("channel %u: %d 0x%03X", i, static_cast(val), static_cast(val)); } - // unsigned chan = 1; - // warnx("channel %u: %d", chan, static_cast(channels[chan])); } - - //warnx("%f: 0x%02x >> RSSI: %u #: %u", (double)f, x, static_cast(rssi), static_cast(rx_count)); } if (ret == EOF) { warnx("Test finished, reached end of file"); + } else { warnx("Test aborted, errno: %d", ret); } -- 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(-) 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(-) 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 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 0078ba2a3bc224a30908fbcdf9c138101241dda5 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 7 Oct 2014 12:47:25 +0200 Subject: Removed bogus warnignn from test --- Tools/tests-host/sf0x_test.cpp | 6 +----- 1 file changed, 1 insertion(+), 5 deletions(-) diff --git a/Tools/tests-host/sf0x_test.cpp b/Tools/tests-host/sf0x_test.cpp index ebc893b60..6b98c2427 100644 --- a/Tools/tests-host/sf0x_test.cpp +++ b/Tools/tests-host/sf0x_test.cpp @@ -58,11 +58,7 @@ int main(int argc, char *argv[]) { } - if (ret == EOF) { - warnx("Test finished, reached end of file"); - } else { - warnx("Test aborted, errno: %d", ret); - } + warnx("test finished"); return ret; } -- 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(-) 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 4d186e56eab98d79ca981a44ca099e172162d745 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 7 Oct 2014 12:51:19 +0200 Subject: Remove unused test data --- Tools/tests-host/sf0x_data.txt | 33 --------------------------------- 1 file changed, 33 deletions(-) delete mode 100644 Tools/tests-host/sf0x_data.txt diff --git a/Tools/tests-host/sf0x_data.txt b/Tools/tests-host/sf0x_data.txt deleted file mode 100644 index f61f59523..000000000 --- a/Tools/tests-host/sf0x_data.txt +++ /dev/null @@ -1,33 +0,0 @@ -0.02 -0.25 -0.03 -0.02 -0.25 -0.03 -0.02 -0.25 -0.03 -0.02 -0.25 -0.03 -0.25 -0.03 -0.02 -0.25 -0.03 -0.08 -0.40 -0.44 - - -0 -. -0 -1 - -0 -0. -.0 -.01 -01 -1 \ No newline at end of file -- cgit v1.2.3 From cebdae438d3f24075aab09275a537f02c5113b36 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 7 Oct 2014 12:51:50 +0200 Subject: Add missing newline --- Tools/tests-host/Makefile | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Tools/tests-host/Makefile b/Tools/tests-host/Makefile index 903c3a4ca..5647de16d 100644 --- a/Tools/tests-host/Makefile +++ b/Tools/tests-host/Makefile @@ -44,4 +44,4 @@ autodeclination_test: $(SBUS2_FILES) .PHONY: clean clean: - rm -f $(ODIR)/*.o *~ core $(INCDIR)/*~ mixer_test sbus2_test autodeclination_test sf0x_test \ No newline at end of file + rm -f $(ODIR)/*.o *~ core $(INCDIR)/*~ mixer_test sbus2_test autodeclination_test sf0x_test -- 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(-) 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(-) 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(+) 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(-) 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 From 6436db1a99888aff3802008c91f8a6c151f7da9c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 7 Oct 2014 22:24:32 +0200 Subject: Fix parser return type handling --- src/modules/px4iofirmware/controls.c | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c index 8670e30c2..622f20c7d 100644 --- a/src/modules/px4iofirmware/controls.c +++ b/src/modules/px4iofirmware/controls.c @@ -83,15 +83,14 @@ bool dsm_port_input(uint8_t *rssi, bool *dsm_updated, bool *st24_updated) /* get data from FD and attempt to parse with DSM and ST24 libs */ uint8_t st24_rssi, rx_count; - uint16_t st24_channel_count; - uint8_t st24_maxchans = 18; + uint16_t st24_channel_count = 0; *st24_updated = false; for (unsigned i = 0; i < n_bytes; i++) { /* set updated flag if one complete packet was parsed */ - *st24_updated |= st24_decode(bytes[i], &st24_rssi, &rx_count, - &st24_channel_count, r_raw_rc_values, st24_maxchans); + *st24_updated |= (OK == st24_decode(bytes[i], &st24_rssi, &rx_count, + &st24_channel_count, r_raw_rc_values, PX4IO_RC_INPUT_CHANNELS)); } if (*st24_updated) { -- cgit v1.2.3 From 726b10651aa38f1b542e6f3845da9aaf58af72a2 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 7 Oct 2014 22:25:03 +0200 Subject: ST24: Fix parser return values, update docs --- src/lib/rc/st24.c | 16 +++++++++------- src/lib/rc/st24.h | 4 ++-- 2 files changed, 11 insertions(+), 9 deletions(-) diff --git a/src/lib/rc/st24.c b/src/lib/rc/st24.c index bb42830db..2b2178f97 100644 --- a/src/lib/rc/st24.c +++ b/src/lib/rc/st24.c @@ -103,16 +103,18 @@ uint8_t st24_common_crc8(uint8_t *ptr, uint8_t len) } -uint8_t st24_decode(uint8_t byte, uint8_t *rssi, uint8_t *rx_count, uint16_t *channel_count, uint16_t *channels, +int st24_decode(uint8_t byte, uint8_t *rssi, uint8_t *rx_count, uint16_t *channel_count, uint16_t *channels, uint16_t max_chan_count) { - bool ret = false; + int ret = 1; switch (_decode_state) { case ST24_DECODE_STATE_UNSYNCED: if (byte == ST24_STX1) { _decode_state = ST24_DECODE_STATE_GOT_STX1; + } else { + ret = 3; } break; @@ -163,7 +165,7 @@ uint8_t st24_decode(uint8_t byte, uint8_t *rssi, uint8_t *rx_count, uint16_t *ch if (st24_common_crc8((uint8_t *) & (_rxpacket.length), _rxlen) == _rxpacket.crc8) { - ret = true; + ret = 0; /* decode the actual packet */ @@ -225,23 +227,23 @@ uint8_t st24_decode(uint8_t byte, uint8_t *rssi, uint8_t *rx_count, uint16_t *ch // ReceiverFcPacket* d = (ReceiverFcPacket*)&_rxpacket.st24_data; /* we silently ignore this data for now, as it is unused */ - ret = false; + ret = 2; } break; default: - ret = false; + ret = 2; break; } } else { /* decoding failed */ - + ret = 4; } _decode_state = ST24_DECODE_STATE_UNSYNCED; break; } - return !ret; + return ret; } diff --git a/src/lib/rc/st24.h b/src/lib/rc/st24.h index 1876eabc5..454234601 100644 --- a/src/lib/rc/st24.h +++ b/src/lib/rc/st24.h @@ -155,9 +155,9 @@ uint8_t st24_common_crc8(uint8_t *ptr, uint8_t len); * @param rx_count pointer to a byte where the receive count of packets signce last wireless frame is written back to * @param channels pointer to a datastructure of size max_chan_count where channel values (12 bit) are written back to * @param max_chan_count maximum channels to decode - if more channels are decoded, the last n are skipped and success (0) is returned - * @return 0 for success (a decoded packet), 1 for no packet yet (accumulating), 3 for out of sync, 4 for checksum error + * @return 0 for success (a decoded packet), 1 for no packet yet (accumulating), 2 for unknown packet, 3 for out of sync, 4 for checksum error */ -__EXPORT uint8_t st24_decode(uint8_t byte, uint8_t *rssi, uint8_t *rx_count, uint16_t *channel_count, +__EXPORT int st24_decode(uint8_t byte, uint8_t *rssi, uint8_t *rx_count, uint16_t *channel_count, uint16_t *channels, uint16_t max_chan_count); __END_DECLS -- cgit v1.2.3 From 8c6c08dcb5ce87e613cbf867571f219a60e1b813 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 7 Oct 2014 22:46:07 +0200 Subject: Limit channel count effectively --- src/lib/rc/st24.c | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/src/lib/rc/st24.c b/src/lib/rc/st24.c index 2b2178f97..e8a791b8f 100644 --- a/src/lib/rc/st24.c +++ b/src/lib/rc/st24.c @@ -176,7 +176,9 @@ int st24_decode(uint8_t byte, uint8_t *rssi, uint8_t *rx_count, uint16_t *channe *rssi = d->rssi; *rx_count = d->packet_count; - *channel_count = 12; + + /* this can lead to rounding of the strides */ + *channel_count = (max_chan_count < 12) ? max_chan_count : 12; unsigned stride_count = (*channel_count * 3) / 2; unsigned chan_index = 0; @@ -202,7 +204,9 @@ int st24_decode(uint8_t byte, uint8_t *rssi, uint8_t *rx_count, uint16_t *channe *rssi = d->rssi; *rx_count = d->packet_count; - *channel_count = 24; + + /* this can lead to rounding of the strides */ + *channel_count = (max_chan_count < 24) ? max_chan_count : 24; unsigned stride_count = (*channel_count * 3) / 2; unsigned chan_index = 0; -- cgit v1.2.3 From 9290e7b7f2e9b7e2f75b9d52c15a0c7f2064c146 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 8 Oct 2014 09:57:50 +0200 Subject: Added VTOL types --- mavlink/include/mavlink/v1.0 | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mavlink/include/mavlink/v1.0 b/mavlink/include/mavlink/v1.0 index 2423e47b4..8587a007c 160000 --- a/mavlink/include/mavlink/v1.0 +++ b/mavlink/include/mavlink/v1.0 @@ -1 +1 @@ -Subproject commit 2423e47b4f9169e77f7194b36fa2118e018c94e2 +Subproject commit 8587a007cf89cb13f146ac46be94b61276343ce1 -- cgit v1.2.3 From 94574f1a58735547a72c65556a683335fdf7ffbd Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 31 Jul 2014 10:40:50 +1000 Subject: lsm303d: don't use DRDY when not on internal SPI bus external SPI bus does not have accel DRDY connected Conflicts: mavlink/include/mavlink/v1.0 --- src/drivers/lsm303d/lsm303d.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 6880cf0f8..01c89192a 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -1519,8 +1519,10 @@ LSM303D::measure() { // if the accel doesn't have any data ready then re-schedule // for 100 microseconds later. This ensures we don't double - // read a value and then miss the next value - if (stm32_gpioread(GPIO_EXTI_ACCEL_DRDY) == 0) { + // read a value and then miss the next value. + // Note that DRDY is not available when the lsm303d is + // connected on the external bus + if (_bus == PX4_SPI_BUS_SENSORS && stm32_gpioread(GPIO_EXTI_ACCEL_DRDY) == 0) { perf_count(_accel_reschedules); hrt_call_delay(&_accel_call, 100); return; -- cgit v1.2.3 From 0e51b99915ab5f69e723576d562a8b1c015bf14d Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Wed, 27 Aug 2014 21:34:48 +0900 Subject: device: initialise device id Conflicts: mavlink/include/mavlink/v1.0 --- src/drivers/device/device.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/drivers/device/device.cpp b/src/drivers/device/device.cpp index f1f777dce..d46901683 100644 --- a/src/drivers/device/device.cpp +++ b/src/drivers/device/device.cpp @@ -97,6 +97,7 @@ Device::Device(const char *name, /* setup a default device ID. When bus_type is UNKNOWN the other fields are invalid */ + _device_id.devid = 0; _device_id.devid_s.bus_type = DeviceBusType_UNKNOWN; _device_id.devid_s.bus = 0; _device_id.devid_s.address = 0; -- cgit v1.2.3 From b1887eee7910b2e85077fd99a9142fb9b1dfefcf Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 8 Oct 2014 12:28:03 +0200 Subject: Fix comment --- src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index 2c50e5c75..27c1f981e 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -782,7 +782,7 @@ FixedwingEstimator::task_main() while (!_task_should_exit) { - /* wait for up to 500ms for data */ + /* wait for up to 100ms for data */ int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100); /* timed out - periodic check for _task_should_exit, etc. */ -- cgit v1.2.3 From af783d4d98c596a4288ec9da7f3facfd6f281933 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Wed, 8 Oct 2014 20:52:38 +0400 Subject: UAVCAN update --- uavcan | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/uavcan b/uavcan index 286adbcc5..01d5bb242 160000 --- a/uavcan +++ b/uavcan @@ -1 +1 @@ -Subproject commit 286adbcc56c4b093143b647ec7546abb149cd53b +Subproject commit 01d5bb242a6197e17e0ed466ed86e7ba42bd7d01 -- cgit v1.2.3 From 1bf1cec5672a7f30656e3fe44535ab0390bbd2d0 Mon Sep 17 00:00:00 2001 From: Denis Yeldandi Date: Wed, 8 Oct 2014 23:37:41 +0400 Subject: Corrected gps_fix values description --- src/modules/uORB/topics/vehicle_gps_position.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/uORB/topics/vehicle_gps_position.h b/src/modules/uORB/topics/vehicle_gps_position.h index 43909e2fa..31e616f4f 100644 --- a/src/modules/uORB/topics/vehicle_gps_position.h +++ b/src/modules/uORB/topics/vehicle_gps_position.h @@ -62,7 +62,7 @@ struct vehicle_gps_position_s { uint64_t timestamp_variance; float s_variance_m_s; /**< speed accuracy estimate m/s */ float c_variance_rad; /**< course accuracy estimate rad */ - uint8_t fix_type; /**< 0-1: no fix, 2: 2D fix, 3: 3D fix, 4: Real-Time Kinematic, fixed integers, 5: Real-Time Kinematic, float integers, OmniSTAR XP/HP or Location RTK. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. */ + uint8_t fix_type; /**< 0-1: no fix, 2: 2D fix, 3: 3D fix, 4: RTCM code differential, 5: Real-Time Kinematic, float, 6: Real-Time Kinematic, fixed, 8: Extrapolated. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. */ float eph; /**< GPS HDOP horizontal dilution of position in m */ float epv; /**< GPS VDOP horizontal dilution of position in m */ -- cgit v1.2.3 From 09b5206404116b263feb50de18c54d53544c9114 Mon Sep 17 00:00:00 2001 From: Denis Yeldandi Date: Thu, 9 Oct 2014 11:15:11 +0400 Subject: Code style fix --- src/drivers/gps/ashtech.cpp | 1103 +++++++++++++++++++++++-------------------- src/drivers/gps/ashtech.h | 4 +- src/drivers/gps/gps.cpp | 13 +- 3 files changed, 605 insertions(+), 515 deletions(-) diff --git a/src/drivers/gps/ashtech.cpp b/src/drivers/gps/ashtech.cpp index d40b12721..7342d01d1 100644 --- a/src/drivers/gps/ashtech.cpp +++ b/src/drivers/gps/ashtech.cpp @@ -18,282 +18,343 @@ typedef double float64_t; typedef float float32_t; -char* str_scanDec(const char *pos, int8_t sign, int8_t n_max_digit, int32_t *result) +char *str_scanDec(const char *pos, int8_t sign, int8_t n_max_digit, int32_t *result) { - int8_t n=0; - int32_t d=0; - int8_t neg = 0; - - if(*pos == '-') { neg = 1; pos++; } - else if(*pos == '+') { pos++; } - else if(sign) return(NULL); - while(*pos >= '0' && *pos <= '9') - { - d = d*10 + (*(pos++) - '0'); - n++; - if(n_max_digit>0 && n==n_max_digit) break; - } - if(n==0 || n>10) return(NULL); - if(neg) *result = -d; - else *result = d; - return((char *)pos); + int8_t n = 0; + int32_t d = 0; + int8_t neg = 0; + + if (*pos == '-') { neg = 1; pos++; } + + else if (*pos == '+') { pos++; } + + else if (sign) { return (NULL); } + + while (*pos >= '0' && *pos <= '9') { + d = d * 10 + (*(pos++) - '0'); + n++; + + if (n_max_digit > 0 && n == n_max_digit) { break; } + } + + if (n == 0 || n > 10) { return (NULL); } + + if (neg) { *result = -d; } + + else { *result = d; } + + return ((char *)pos); } -char* scanFloat64(const char *pos, int8_t sign, int8_t n_max_int, int8_t n_max_frac, - float64_t *result) +char *scanFloat64(const char *pos, int8_t sign, int8_t n_max_int, int8_t n_max_frac, + float64_t *result) { - float64_t f=0.0, div=1.0; - int32_t d_int; - int8_t n=0, isneg= 0; - - if( *pos == '-' ) isneg= 1; - if((pos = str_scanDec(pos, sign, n_max_int, &d_int)) == NULL) return(NULL); - if(*(pos) == '.') - { - pos++; - while(*pos >= '0' && *pos <= '9') - { - f = f*(10.0) + (float64_t)(*(pos++) - '0'); - div *= (0.1); - n++; - if(n_max_frac>0 && n==n_max_frac) break; - } - } - else if(n_max_frac > 0) return(NULL); - if( isneg ) *result = (float64_t)d_int - f*div; - else *result = (float64_t)d_int + f*div; - return((char *)pos); + float64_t f = 0.0, div = 1.0; + int32_t d_int; + int8_t n = 0, isneg = 0; + + if (*pos == '-') { isneg = 1; } + + if ((pos = str_scanDec(pos, sign, n_max_int, &d_int)) == NULL) { return (NULL); } + + if (*(pos) == '.') { + pos++; + + while (*pos >= '0' && *pos <= '9') { + f = f * (10.0) + (float64_t)(*(pos++) - '0'); + div *= (0.1); + n++; + + if (n_max_frac > 0 && n == n_max_frac) { break; } + } + + } else if (n_max_frac > 0) { return (NULL); } + + if (isneg) { *result = (float64_t)d_int - f * div; } + + else { *result = (float64_t)d_int + f * div; } + + return ((char *)pos); } ASHTECH::ASHTECH(const int &fd, struct vehicle_gps_position_s *gps_position, struct satellite_info_s *satellite_info): -_fd(fd), -_satellite_info(satellite_info), -_gps_position(gps_position) + _fd(fd), + _satellite_info(satellite_info), + _gps_position(gps_position) { - decode_init(); - _decode_state = NME_DECODE_UNINIT; - _rx_buffer_bytes = 0; + decode_init(); + _decode_state = NME_DECODE_UNINIT; + _rx_buffer_bytes = 0; } -ASHTECH::~ASHTECH(){ +ASHTECH::~ASHTECH() +{ } -//All NMEA descriptions are taken from -//http://www.trimble.com/OEM_ReceiverHelp/V4.44/en/NMEA-0183messages_MessageOverview.html -int ASHTECH::handle_message(int len){ - if(len < 7) return 0; - int uiCalcComma = 0; - for(int i = 0 ; i < len; i++){ - if(_rx_buffer[i] == ',')uiCalcComma++; - } - char* bufptr = (char*)( _rx_buffer+6); - - if((memcmp(_rx_buffer+3, "ZDA,",3) == 0)&&(uiCalcComma == 6)){ - /* - UTC day, month, and year, and local time zone offset - An example of the ZDA message string is: - - $GPZDA,172809.456,12,07,1996,00,00*45 - - ZDA message fields - Field Meaning - 0 Message ID $GPZDA - 1 UTC - 2 Day, ranging between 01 and 31 - 3 Month, ranging between 01 and 12 - 4 Year - 5 Local time zone offset from GMT, ranging from 00 through �13 hours - 6 Local time zone offset from GMT, ranging from 00 through 59 minutes - 7 The checksum data, always begins with * - Fields 5 and 6 together yield the total offset. For example, if field 5 is -5 and field 6 is +15, local time is 5 hours and 15 minutes earlier than GMT. - */ - float64_t ashtech_time = 0.0; - int day = 0, month = 0,year = 0,local_time_off_hour = 0,local_time_off_min = 0; - if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&ashtech_time); - if(bufptr && *(++bufptr) != ',') bufptr = str_scanDec(bufptr, 0, 9, &day); - if(bufptr && *(++bufptr) != ',') bufptr = str_scanDec(bufptr, 0, 9, &month); - if(bufptr && *(++bufptr) != ',') bufptr = str_scanDec(bufptr, 0, 9, &year); - if(bufptr && *(++bufptr) != ',') bufptr = str_scanDec(bufptr, 0, 9, &local_time_off_hour); - if(bufptr && *(++bufptr) != ',') bufptr = str_scanDec(bufptr, 0, 9, &local_time_off_min); - - - int ashtech_hour = ashtech_time/10000; - int ashtech_minute = (ashtech_time - ashtech_hour*10000)/100; - float64_t ashtech_sec = ashtech_time - ashtech_hour*10000 - ashtech_minute*100; - //convert to unix timestamp - struct tm timeinfo; - timeinfo.tm_year = year - 1900; - timeinfo.tm_mon = month - 1; - timeinfo.tm_mday = day; - timeinfo.tm_hour = ashtech_hour; - timeinfo.tm_min = ashtech_minute; - timeinfo.tm_sec = int(ashtech_sec); - time_t epoch = mktime(&timeinfo); - - _gps_position->time_gps_usec = (uint64_t)epoch * 1000000; //TODO: test this - _gps_position->time_gps_usec += (uint64_t)((ashtech_sec - int(ashtech_sec)) * 1e6); - _gps_position->timestamp_time = hrt_absolute_time(); - } - - else if((memcmp(_rx_buffer+3, "GGA,",3) == 0)&&(uiCalcComma == 14)){ - /* - Time, position, and fix related data - An example of the GBS message string is: - - $GPGGA,172814.0,3723.46587704,N,12202.26957864,W,2,6,1.2,18.893,M,-25.669,M,2.0,0031*4F - - Note - The data string exceeds the ASHTECH standard length. - GGA message fields - Field Meaning - 0 Message ID $GPGGA - 1 UTC of position fix - 2 Latitude - 3 Direction of latitude: - N: North - S: South - 4 Longitude - 5 Direction of longitude: - E: East - W: West - 6 GPS Quality indicator: - 0: Fix not valid - 1: GPS fix - 2: Differential GPS fix, OmniSTAR VBS - 4: Real-Time Kinematic, fixed integers - 5: Real-Time Kinematic, float integers, OmniSTAR XP/HP or Location RTK - 7 Number of SVs in use, range from 00 through to 24+ - 8 HDOP - 9 Orthometric height (MSL reference) - 10 M: unit of measure for orthometric height is meters - 11 Geoid separation - 12 M: geoid separation measured in meters - 13 Age of differential GPS data record, Type 1 or Type 9. Null field when DGPS is not used. - 14 Reference station ID, range 0000-4095. A null field when any reference station ID is selected and no corrections are received1. - 15 - The checksum data, always begins with * - Note - If a user-defined geoid model, or an inclined - */ - float64_t ashtech_time = 0.0, lat = 0.0, lon = 0.0, alt = 0.0; - int num_of_sv = 0, fix_quality=0; - float64_t hdop = 99.9; - char ns = '?', ew = '?'; - - if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&ashtech_time); - if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&lat); - if(bufptr && *(++bufptr) != ',') ns = *(bufptr++); - if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&lon); - if(bufptr && *(++bufptr) != ',') ew = *(bufptr++); - if(bufptr && *(++bufptr) != ',') bufptr = str_scanDec(bufptr, 0, 9, &fix_quality); - if(bufptr && *(++bufptr) != ',') bufptr = str_scanDec(bufptr, 0, 9, &num_of_sv); - if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&hdop); - if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&alt); - - if(ns == 'S') - lat = -lat; - if(ew == 'W') - lon = -lon; - - _gps_position->lat = (int(lat*0.01) + (lat*0.01 - int(lat*0.01))*100.0/60.0)*10000000; - _gps_position->lon = (int(lon*0.01) + (lon*0.01 - int(lon*0.01))*100.0/60.0)*10000000; - _gps_position->alt = alt*1000; - _rate_count_lat_lon++; - - if((lat == 0.0) && (lon == 0.0) && (alt == 0.0)){ - _gps_position->fix_type = 0; - } - else - _gps_position->fix_type = 3 + fix_quality; - - _gps_position->timestamp_position = hrt_absolute_time(); - - _gps_position->vel_m_s = 0; /**< GPS ground speed (m/s) */ - _gps_position->vel_n_m_s = 0; /**< GPS ground speed in m/s */ - _gps_position->vel_e_m_s = 0; /**< GPS ground speed in m/s */ - _gps_position->vel_d_m_s = 0; /**< GPS ground speed in m/s */ - _gps_position->cog_rad = 0; /**< Course over ground (NOT heading, but direction of movement) in rad, -PI..PI */ - _gps_position->vel_ned_valid = true; /**< Flag to indicate if NED speed is valid */ - _gps_position->c_variance_rad = 0.1; - _gps_position->timestamp_velocity = hrt_absolute_time(); - return 1; - } - else if ((memcmp(_rx_buffer, "$PASHR,POS,", 11) == 0) && (uiCalcComma == 18)) { - /* - Example $PASHR,POS,2,10,125410.00,5525.8138702,N,03833.9587380,E,131.555,1.0,0.0,0.007,-0.001,2.0,1.0,1.7,1.0,*34 - - $PASHR,POS,d1,d2,m3,m4,c5,m6,c7,f8,f9,f10,f11,f12,f13,f14,f15,f16,s17*cc - Parameter Description Range - d1 Position mode 0: standalone - 1: differential - 2: RTK float - 3: RTK fixed - 5: Dead reckoning - 9: SBAS (see NPT setting) - d2 Number of satellite used in position fix 0-99 - m3 Current UTC time of position fix (hhmmss.ss) 000000.00-235959.99 - m4 Latitude of position (ddmm.mmmmmm) 0-90 degrees 00-59.9999999 minutes - c5 Latitude sector N, S - m6 Longitude of position (dddmm.mmmmmm) 0-180 degrees 00-59.9999999 minutes - c7 Longitude sector E,W - f8 Altitude above ellipsoid +9999.000 - f9 Differential age (data link age), seconds 0.0-600.0 - f10 True track/course over ground in degrees 0.0-359.9 - f11 Speed over ground in knots 0.0-999.9 - f12 Vertical velocity in decimeters per second +999.9 - f13 PDOP 0-99.9 - f14 HDOP 0-99.9 - f15 VDOP 0-99.9 - f16 TDOP 0-99.9 - s17 Reserved no data - *cc Checksum - */ - bufptr = (char*)( _rx_buffer+10); - float64_t ashtech_time = 0.0, lat = 0.0, lon = 0.0, alt = 0.0; - int num_of_sv = 0, fix_quality=0; - float64_t track_true = 0.0, ground_speed = 0.0 , age_of_corr = 0.0; - float64_t hdop = 99.9, vdop = 99.9, pdop = 99.9, tdop = 99.9,vertic_vel = 0.0; - char ns = '?', ew = '?'; - - if(bufptr && *(++bufptr) != ',') bufptr = str_scanDec(bufptr, 0, 9, &fix_quality); - if(bufptr && *(++bufptr) != ',') bufptr = str_scanDec(bufptr, 0, 9, &num_of_sv); - if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&ashtech_time); - if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&lat); - if(bufptr && *(++bufptr) != ',') ns = *(bufptr++); - if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&lon); - if(bufptr && *(++bufptr) != ',') ew = *(bufptr++); - if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&alt); - if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&age_of_corr); - if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&track_true); - if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&ground_speed); - if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&vertic_vel); - if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&pdop); - if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&hdop); - if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&vdop); - if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&tdop); - - if(ns == 'S') - lat = -lat; - if(ew == 'W') - lon = -lon; - - _gps_position->lat = (int(lat*0.01) + (lat*0.01 - int(lat*0.01))*100.0/60.0)*10000000; - _gps_position->lon = (int(lon*0.01) + (lon*0.01 - int(lon*0.01))*100.0/60.0)*10000000; - _gps_position->alt = alt*1000; - _rate_count_lat_lon++; - - if((lat == 0.0) && (lon == 0.0) && (alt == 0.0)){ - _gps_position->fix_type = 0; - } - else - _gps_position->fix_type = 3 + fix_quality; - - _gps_position->timestamp_position = hrt_absolute_time(); - - const float64_t m_pi = 3.14159265; - - double track_rad = track_true * m_pi / 180.0; - - double velocity_ms = ground_speed / 1.9438445; /** knots to m/s */ + +/* + * All NMEA descriptions are taken from + * http://www.trimble.com/OEM_ReceiverHelp/V4.44/en/NMEA-0183messages_MessageOverview.html + */ + +int ASHTECH::handle_message(int len) +{ + if (len < 7) { return 0; } + + int uiCalcComma = 0; + + for (int i = 0 ; i < len; i++) { + if (_rx_buffer[i] == ',') { uiCalcComma++; } + } + + char *bufptr = (char *)(_rx_buffer + 6); + + if ((memcmp(_rx_buffer + 3, "ZDA,", 3) == 0) && (uiCalcComma == 6)) { + /* + UTC day, month, and year, and local time zone offset + An example of the ZDA message string is: + + $GPZDA,172809.456,12,07,1996,00,00*45 + + ZDA message fields + Field Meaning + 0 Message ID $GPZDA + 1 UTC + 2 Day, ranging between 01 and 31 + 3 Month, ranging between 01 and 12 + 4 Year + 5 Local time zone offset from GMT, ranging from 00 through �13 hours + 6 Local time zone offset from GMT, ranging from 00 through 59 minutes + 7 The checksum data, always begins with * + Fields 5 and 6 together yield the total offset. For example, if field 5 is -5 and field 6 is +15, local time is 5 hours and 15 minutes earlier than GMT. + */ + float64_t ashtech_time = 0.0; + int day = 0, month = 0, year = 0, local_time_off_hour = 0, local_time_off_min = 0; + + if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &ashtech_time); } + + if (bufptr && *(++bufptr) != ',') { bufptr = str_scanDec(bufptr, 0, 9, &day); } + + if (bufptr && *(++bufptr) != ',') { bufptr = str_scanDec(bufptr, 0, 9, &month); } + + if (bufptr && *(++bufptr) != ',') { bufptr = str_scanDec(bufptr, 0, 9, &year); } + + if (bufptr && *(++bufptr) != ',') { bufptr = str_scanDec(bufptr, 0, 9, &local_time_off_hour); } + + if (bufptr && *(++bufptr) != ',') { bufptr = str_scanDec(bufptr, 0, 9, &local_time_off_min); } + + + int ashtech_hour = ashtech_time / 10000; + int ashtech_minute = (ashtech_time - ashtech_hour * 10000) / 100; + float64_t ashtech_sec = ashtech_time - ashtech_hour * 10000 - ashtech_minute * 100; + /* + * convert to unix timestamp + */ + struct tm timeinfo; + timeinfo.tm_year = year - 1900; + timeinfo.tm_mon = month - 1; + timeinfo.tm_mday = day; + timeinfo.tm_hour = ashtech_hour; + timeinfo.tm_min = ashtech_minute; + timeinfo.tm_sec = int(ashtech_sec); + time_t epoch = mktime(&timeinfo); + + _gps_position->time_gps_usec = (uint64_t)epoch * 1000000; //TODO: test this + _gps_position->time_gps_usec += (uint64_t)((ashtech_sec - int(ashtech_sec)) * 1e6); + _gps_position->timestamp_time = hrt_absolute_time(); + } + + else if ((memcmp(_rx_buffer + 3, "GGA,", 3) == 0) && (uiCalcComma == 14)) { + /* + Time, position, and fix related data + An example of the GBS message string is: + + $GPGGA,172814.0,3723.46587704,N,12202.26957864,W,2,6,1.2,18.893,M,-25.669,M,2.0,0031*4F + + Note - The data string exceeds the ASHTECH standard length. + GGA message fields + Field Meaning + 0 Message ID $GPGGA + 1 UTC of position fix + 2 Latitude + 3 Direction of latitude: + N: North + S: South + 4 Longitude + 5 Direction of longitude: + E: East + W: West + 6 GPS Quality indicator: + 0: Fix not valid + 1: GPS fix + 2: Differential GPS fix, OmniSTAR VBS + 4: Real-Time Kinematic, fixed integers + 5: Real-Time Kinematic, float integers, OmniSTAR XP/HP or Location RTK + 7 Number of SVs in use, range from 00 through to 24+ + 8 HDOP + 9 Orthometric height (MSL reference) + 10 M: unit of measure for orthometric height is meters + 11 Geoid separation + 12 M: geoid separation measured in meters + 13 Age of differential GPS data record, Type 1 or Type 9. Null field when DGPS is not used. + 14 Reference station ID, range 0000-4095. A null field when any reference station ID is selected and no corrections are received1. + 15 + The checksum data, always begins with * + Note - If a user-defined geoid model, or an inclined + */ + float64_t ashtech_time = 0.0, lat = 0.0, lon = 0.0, alt = 0.0; + int num_of_sv = 0, fix_quality = 0; + float64_t hdop = 99.9; + char ns = '?', ew = '?'; + + if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &ashtech_time); } + + if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &lat); } + + if (bufptr && *(++bufptr) != ',') { ns = *(bufptr++); } + + if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &lon); } + + if (bufptr && *(++bufptr) != ',') { ew = *(bufptr++); } + + if (bufptr && *(++bufptr) != ',') { bufptr = str_scanDec(bufptr, 0, 9, &fix_quality); } + + if (bufptr && *(++bufptr) != ',') { bufptr = str_scanDec(bufptr, 0, 9, &num_of_sv); } + + if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &hdop); } + + if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &alt); } + + if (ns == 'S') { + lat = -lat; + } + + if (ew == 'W') { + lon = -lon; + } + + _gps_position->lat = (int(lat * 0.01) + (lat * 0.01 - int(lat * 0.01)) * 100.0 / 60.0) * 10000000; + _gps_position->lon = (int(lon * 0.01) + (lon * 0.01 - int(lon * 0.01)) * 100.0 / 60.0) * 10000000; + _gps_position->alt = alt * 1000; + _rate_count_lat_lon++; + + if ((lat == 0.0) && (lon == 0.0) && (alt == 0.0)) { + _gps_position->fix_type = 0; + + } else { + _gps_position->fix_type = 3 + fix_quality; + } + + _gps_position->timestamp_position = hrt_absolute_time(); + + _gps_position->vel_m_s = 0; /**< GPS ground speed (m/s) */ + _gps_position->vel_n_m_s = 0; /**< GPS ground speed in m/s */ + _gps_position->vel_e_m_s = 0; /**< GPS ground speed in m/s */ + _gps_position->vel_d_m_s = 0; /**< GPS ground speed in m/s */ + _gps_position->cog_rad = + 0; /**< Course over ground (NOT heading, but direction of movement) in rad, -PI..PI */ + _gps_position->vel_ned_valid = true; /**< Flag to indicate if NED speed is valid */ + _gps_position->c_variance_rad = 0.1; + _gps_position->timestamp_velocity = hrt_absolute_time(); + return 1; + + } else if ((memcmp(_rx_buffer, "$PASHR,POS,", 11) == 0) && (uiCalcComma == 18)) { + /* + Example $PASHR,POS,2,10,125410.00,5525.8138702,N,03833.9587380,E,131.555,1.0,0.0,0.007,-0.001,2.0,1.0,1.7,1.0,*34 + + $PASHR,POS,d1,d2,m3,m4,c5,m6,c7,f8,f9,f10,f11,f12,f13,f14,f15,f16,s17*cc + Parameter Description Range + d1 Position mode 0: standalone + 1: differential + 2: RTK float + 3: RTK fixed + 5: Dead reckoning + 9: SBAS (see NPT setting) + d2 Number of satellite used in position fix 0-99 + m3 Current UTC time of position fix (hhmmss.ss) 000000.00-235959.99 + m4 Latitude of position (ddmm.mmmmmm) 0-90 degrees 00-59.9999999 minutes + c5 Latitude sector N, S + m6 Longitude of position (dddmm.mmmmmm) 0-180 degrees 00-59.9999999 minutes + c7 Longitude sector E,W + f8 Altitude above ellipsoid +9999.000 + f9 Differential age (data link age), seconds 0.0-600.0 + f10 True track/course over ground in degrees 0.0-359.9 + f11 Speed over ground in knots 0.0-999.9 + f12 Vertical velocity in decimeters per second +999.9 + f13 PDOP 0-99.9 + f14 HDOP 0-99.9 + f15 VDOP 0-99.9 + f16 TDOP 0-99.9 + s17 Reserved no data + *cc Checksum + */ + bufptr = (char *)(_rx_buffer + 10); + float64_t ashtech_time = 0.0, lat = 0.0, lon = 0.0, alt = 0.0; + int num_of_sv = 0, fix_quality = 0; + float64_t track_true = 0.0, ground_speed = 0.0 , age_of_corr = 0.0; + float64_t hdop = 99.9, vdop = 99.9, pdop = 99.9, tdop = 99.9, vertic_vel = 0.0; + char ns = '?', ew = '?'; + + if (bufptr && *(++bufptr) != ',') { bufptr = str_scanDec(bufptr, 0, 9, &fix_quality); } + + if (bufptr && *(++bufptr) != ',') { bufptr = str_scanDec(bufptr, 0, 9, &num_of_sv); } + + if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &ashtech_time); } + + if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &lat); } + + if (bufptr && *(++bufptr) != ',') { ns = *(bufptr++); } + + if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &lon); } + + if (bufptr && *(++bufptr) != ',') { ew = *(bufptr++); } + + if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &alt); } + + if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &age_of_corr); } + + if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &track_true); } + + if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &ground_speed); } + + if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &vertic_vel); } + + if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &pdop); } + + if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &hdop); } + + if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &vdop); } + + if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &tdop); } + + if (ns == 'S') { + lat = -lat; + } + + if (ew == 'W') { + lon = -lon; + } + + _gps_position->lat = (int(lat * 0.01) + (lat * 0.01 - int(lat * 0.01)) * 100.0 / 60.0) * 10000000; + _gps_position->lon = (int(lon * 0.01) + (lon * 0.01 - int(lon * 0.01)) * 100.0 / 60.0) * 10000000; + _gps_position->alt = alt * 1000; + _rate_count_lat_lon++; + + if ((lat == 0.0) && (lon == 0.0) && (alt == 0.0)) { + _gps_position->fix_type = 0; + + } else { + _gps_position->fix_type = 3 + fix_quality; + } + + _gps_position->timestamp_position = hrt_absolute_time(); + + const float64_t m_pi = 3.14159265; + + double track_rad = track_true * m_pi / 180.0; + + double velocity_ms = ground_speed / 1.9438445; /** knots to m/s */ double velocity_north = velocity_ms * cos(track_rad); double velocity_east = velocity_ms * sin(track_rad); @@ -301,230 +362,255 @@ int ASHTECH::handle_message(int len){ _gps_position->vel_n_m_s = velocity_north; /**< GPS ground speed in m/s */ _gps_position->vel_e_m_s = velocity_east; /**< GPS ground speed in m/s */ _gps_position->vel_d_m_s = -vertic_vel; /**< GPS ground speed in m/s */ - _gps_position->cog_rad = track_rad; /**< Course over ground (NOT heading, but direction of movement) in rad, -PI..PI */ + _gps_position->cog_rad = + track_rad; /**< Course over ground (NOT heading, but direction of movement) in rad, -PI..PI */ _gps_position->vel_ned_valid = true; /**< Flag to indicate if NED speed is valid */ _gps_position->c_variance_rad = 0.1; _gps_position->timestamp_velocity = hrt_absolute_time(); - return 1; - - } - else if((memcmp(_rx_buffer+3, "GST,",3) == 0)&&(uiCalcComma == 8)){ - /* - Position error statistics - An example of the GST message string is: - - $GPGST,172814.0,0.006,0.023,0.020,273.6,0.023,0.020,0.031*6A - - The Talker ID ($--) will vary depending on the satellite system used for the position solution: - - $GP - GPS only - $GL - GLONASS only - $GN - Combined - GST message fields - Field Meaning - 0 Message ID $GPGST - 1 UTC of position fix - 2 RMS value of the pseudorange residuals; includes carrier phase residuals during periods of RTK (float) and RTK (fixed) processing - 3 Error ellipse semi-major axis 1 sigma error, in meters - 4 Error ellipse semi-minor axis 1 sigma error, in meters - 5 Error ellipse orientation, degrees from true north - 6 Latitude 1 sigma error, in meters - 7 Longitude 1 sigma error, in meters - 8 Height 1 sigma error, in meters - 9 The checksum data, always begins with * - */ - float64_t ashtech_time = 0.0, lat_err = 0.0, lon_err = 0.0, alt_err = 0.0; - float64_t min_err = 0.0, maj_err = 0.0, deg_from_north = 0.0, rms_err = 0.0; - - if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&ashtech_time); - if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&rms_err); - if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&maj_err); - if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&min_err); - if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,°_from_north); - if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&lat_err); - if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&lon_err); - if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&alt_err); - - _gps_position->eph = sqrt(lat_err*lat_err +lon_err*lon_err); - _gps_position->epv = alt_err; - - _gps_position->s_variance_m_s = 0; - _gps_position->timestamp_variance = hrt_absolute_time(); - } - else if((memcmp(_rx_buffer+3, "VTG,",3) == 0)&&(uiCalcComma == 9)){ - /* - Track made good and speed over ground - An example of the VTG message string is: - - $GPVTG,054.7,T,034.4,M,005.5,N,010.2,K*48 - - VTG message fields - Field Meaning - 0 Message ID $GPVTG - 1 Track made good (degrees true) - 2 T: track made good is relative to true north - 3 Track made good (degrees magnetic) - 4 M: track made good is relative to magnetic north - 5 Speed, in knots - 6 N: speed is measured in knots - 7 Speed over ground in kilometers/hour (kph) - 8 K: speed over ground is measured in kph - 9 The checksum data, always begins with * - */ - /*float64_t track_true = 0.0, speed = 0.0, track_magnetic = 0.0, ground_speed = 0.0; - char true_north = '?', magnetic_north = '?', speed_in_knots = '?', speed_in_kph = '?'; - - if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&track_true); - if(bufptr && *(++bufptr) != ',') true_north = *(bufptr++); - if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&track_magnetic); - if(bufptr && *(++bufptr) != ',') magnetic_north = *(bufptr++); - if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&speed); - if(bufptr && *(++bufptr) != ',') speed_in_knots = *(bufptr++); - if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&ground_speed); - if(bufptr && *(++bufptr) != ',') speed_in_kph = *(bufptr++); - - const float64_t dPI = 3.14159265; - float64_t tan_C = tan(track_true * dPI / 180.0); - float64_t lat_ = sqrt(ground_speed*ground_speed/ (1+tan_C)); //km/hour - float64_t lon_ = lat_*tan_C; // //km/hour*/ - } - else if((memcmp(_rx_buffer+3, "GSV,",3) == 0)){ - /* - The GSV message string identifies the number of SVs in view, the PRN numbers, elevations, azimuths, and SNR values. An example of the GSV message string is: - - $GPGSV,4,1,13,02,02,213,,03,-3,000,,11,00,121,,14,13,172,05*67 - - GSV message fields - Field Meaning - 0 Message ID $GPGSV - 1 Total number of messages of this type in this cycle - 2 Message number - 3 Total number of SVs visible - 4 SV PRN number - 5 Elevation, in degrees, 90� maximum - 6 Azimuth, degrees from True North, 000� through 359� - 7 SNR, 00 through 99 dB (null when not tracking) - 8-11 Information about second SV, same format as fields 4 through 7 - 12-15 Information about third SV, same format as fields 4 through 7 - 16-19 Information about fourth SV, same format as fields 4 through 7 - 20 The checksum data, always begins with * - */ -// currently process only gps, because do not know what -// Global satellite ID I should use for non GPS sats - bool bGPS = false; - if(memcmp(_rx_buffer, "$GP",3) != 0) - return 0; - else - bGPS = true; - - int all_msg_num, this_msg_num, tot_sv_visible; - struct gsv_sat{ - int svid; - int elevation; - int azimuth; - int snr; - } sat[4]; - memset(sat, 0, sizeof(sat)); - - if(bufptr && *(++bufptr) != ',') bufptr = str_scanDec(bufptr, 0, 9, &all_msg_num); - if(bufptr && *(++bufptr) != ',') bufptr = str_scanDec(bufptr, 0, 9, &this_msg_num); - if(bufptr && *(++bufptr) != ',') bufptr = str_scanDec(bufptr, 0, 9, &tot_sv_visible); - if((this_msg_num<1) || (this_msg_num>all_msg_num)){ - return 0; - } - if((this_msg_num == 0)&&(bGPS == true)){ - memset(_satellite_info->svid, 0,sizeof(_satellite_info->svid)); - memset(_satellite_info->used, 0,sizeof(_satellite_info->used)); - memset(_satellite_info->snr, 0,sizeof(_satellite_info->snr)); - memset(_satellite_info->elevation,0,sizeof(_satellite_info->elevation)); - memset(_satellite_info->azimuth, 0,sizeof(_satellite_info->azimuth)); - } - - int end = 4; - if(this_msg_num == all_msg_num){ - end = tot_sv_visible - (this_msg_num-1)*4; - _gps_position->satellites_used = tot_sv_visible; - _satellite_info->count = SAT_INFO_MAX_SATELLITES; - _satellite_info->timestamp = hrt_absolute_time(); - } - for(int y = 0 ; y < end ;y++){ - if(bufptr && *(++bufptr) != ',') bufptr = str_scanDec(bufptr, 0, 9, &sat[y].svid); - if(bufptr && *(++bufptr) != ',') bufptr = str_scanDec(bufptr, 0, 9, &sat[y].elevation); - if(bufptr && *(++bufptr) != ',') bufptr = str_scanDec(bufptr, 0, 9, &sat[y].azimuth); - if(bufptr && *(++bufptr) != ',') bufptr = str_scanDec(bufptr, 0, 9, &sat[y].snr); - - _satellite_info->svid[y+(this_msg_num-1)*4] = sat[y].svid; - _satellite_info->used[y+(this_msg_num-1)*4] = ((sat[y].snr>0)? true: false); - _satellite_info->snr[y+(this_msg_num-1)*4] = sat[y].snr; - _satellite_info->elevation[y+(this_msg_num-1)*4] = sat[y].elevation; - _satellite_info->azimuth[y+(this_msg_num-1)*4] = sat[y].azimuth; - } - } - - return 0; + return 1; + + } else if ((memcmp(_rx_buffer + 3, "GST,", 3) == 0) && (uiCalcComma == 8)) { + /* + Position error statistics + An example of the GST message string is: + + $GPGST,172814.0,0.006,0.023,0.020,273.6,0.023,0.020,0.031*6A + + The Talker ID ($--) will vary depending on the satellite system used for the position solution: + + $GP - GPS only + $GL - GLONASS only + $GN - Combined + GST message fields + Field Meaning + 0 Message ID $GPGST + 1 UTC of position fix + 2 RMS value of the pseudorange residuals; includes carrier phase residuals during periods of RTK (float) and RTK (fixed) processing + 3 Error ellipse semi-major axis 1 sigma error, in meters + 4 Error ellipse semi-minor axis 1 sigma error, in meters + 5 Error ellipse orientation, degrees from true north + 6 Latitude 1 sigma error, in meters + 7 Longitude 1 sigma error, in meters + 8 Height 1 sigma error, in meters + 9 The checksum data, always begins with * + */ + float64_t ashtech_time = 0.0, lat_err = 0.0, lon_err = 0.0, alt_err = 0.0; + float64_t min_err = 0.0, maj_err = 0.0, deg_from_north = 0.0, rms_err = 0.0; + + if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &ashtech_time); } + + if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &rms_err); } + + if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &maj_err); } + + if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &min_err); } + + if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, °_from_north); } + + if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &lat_err); } + + if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &lon_err); } + + if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &alt_err); } + + _gps_position->eph = sqrt(lat_err * lat_err + lon_err * lon_err); + _gps_position->epv = alt_err; + + _gps_position->s_variance_m_s = 0; + _gps_position->timestamp_variance = hrt_absolute_time(); + + } else if ((memcmp(_rx_buffer + 3, "VTG,", 3) == 0) && (uiCalcComma == 9)) { + /* + Track made good and speed over ground + An example of the VTG message string is: + + $GPVTG,054.7,T,034.4,M,005.5,N,010.2,K*48 + + VTG message fields + Field Meaning + 0 Message ID $GPVTG + 1 Track made good (degrees true) + 2 T: track made good is relative to true north + 3 Track made good (degrees magnetic) + 4 M: track made good is relative to magnetic north + 5 Speed, in knots + 6 N: speed is measured in knots + 7 Speed over ground in kilometers/hour (kph) + 8 K: speed over ground is measured in kph + 9 The checksum data, always begins with * + */ + /*float64_t track_true = 0.0, speed = 0.0, track_magnetic = 0.0, ground_speed = 0.0; + char true_north = '?', magnetic_north = '?', speed_in_knots = '?', speed_in_kph = '?'; + + if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&track_true); + if(bufptr && *(++bufptr) != ',') true_north = *(bufptr++); + if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&track_magnetic); + if(bufptr && *(++bufptr) != ',') magnetic_north = *(bufptr++); + if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&speed); + if(bufptr && *(++bufptr) != ',') speed_in_knots = *(bufptr++); + if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&ground_speed); + if(bufptr && *(++bufptr) != ',') speed_in_kph = *(bufptr++); + + const float64_t dPI = 3.14159265; + float64_t tan_C = tan(track_true * dPI / 180.0); + float64_t lat_ = sqrt(ground_speed*ground_speed/ (1+tan_C)); //km/hour + float64_t lon_ = lat_*tan_C; // //km/hour*/ + } else if ((memcmp(_rx_buffer + 3, "GSV,", 3) == 0)) { + /* + The GSV message string identifies the number of SVs in view, the PRN numbers, elevations, azimuths, and SNR values. An example of the GSV message string is: + + $GPGSV,4,1,13,02,02,213,,03,-3,000,,11,00,121,,14,13,172,05*67 + + GSV message fields + Field Meaning + 0 Message ID $GPGSV + 1 Total number of messages of this type in this cycle + 2 Message number + 3 Total number of SVs visible + 4 SV PRN number + 5 Elevation, in degrees, 90� maximum + 6 Azimuth, degrees from True North, 000� through 359� + 7 SNR, 00 through 99 dB (null when not tracking) + 8-11 Information about second SV, same format as fields 4 through 7 + 12-15 Information about third SV, same format as fields 4 through 7 + 16-19 Information about fourth SV, same format as fields 4 through 7 + 20 The checksum data, always begins with * + */ + /* + * currently process only gps, because do not know what + * Global satellite ID I should use for non GPS sats + */ + bool bGPS = false; + + if (memcmp(_rx_buffer, "$GP", 3) != 0) { + return 0; + + } else { + bGPS = true; + } + + int all_msg_num, this_msg_num, tot_sv_visible; + struct gsv_sat { + int svid; + int elevation; + int azimuth; + int snr; + } sat[4]; + memset(sat, 0, sizeof(sat)); + + if (bufptr && *(++bufptr) != ',') { bufptr = str_scanDec(bufptr, 0, 9, &all_msg_num); } + + if (bufptr && *(++bufptr) != ',') { bufptr = str_scanDec(bufptr, 0, 9, &this_msg_num); } + + if (bufptr && *(++bufptr) != ',') { bufptr = str_scanDec(bufptr, 0, 9, &tot_sv_visible); } + + if ((this_msg_num < 1) || (this_msg_num > all_msg_num)) { + return 0; + } + + if ((this_msg_num == 0) && (bGPS == true)) { + memset(_satellite_info->svid, 0, sizeof(_satellite_info->svid)); + memset(_satellite_info->used, 0, sizeof(_satellite_info->used)); + memset(_satellite_info->snr, 0, sizeof(_satellite_info->snr)); + memset(_satellite_info->elevation, 0, sizeof(_satellite_info->elevation)); + memset(_satellite_info->azimuth, 0, sizeof(_satellite_info->azimuth)); + } + + int end = 4; + + if (this_msg_num == all_msg_num) { + end = tot_sv_visible - (this_msg_num - 1) * 4; + _gps_position->satellites_used = tot_sv_visible; + _satellite_info->count = SAT_INFO_MAX_SATELLITES; + _satellite_info->timestamp = hrt_absolute_time(); + } + + for (int y = 0 ; y < end ; y++) { + if (bufptr && *(++bufptr) != ',') { bufptr = str_scanDec(bufptr, 0, 9, &sat[y].svid); } + + if (bufptr && *(++bufptr) != ',') { bufptr = str_scanDec(bufptr, 0, 9, &sat[y].elevation); } + + if (bufptr && *(++bufptr) != ',') { bufptr = str_scanDec(bufptr, 0, 9, &sat[y].azimuth); } + + if (bufptr && *(++bufptr) != ',') { bufptr = str_scanDec(bufptr, 0, 9, &sat[y].snr); } + + _satellite_info->svid[y + (this_msg_num - 1) * 4] = sat[y].svid; + _satellite_info->used[y + (this_msg_num - 1) * 4] = ((sat[y].snr > 0) ? true : false); + _satellite_info->snr[y + (this_msg_num - 1) * 4] = sat[y].snr; + _satellite_info->elevation[y + (this_msg_num - 1) * 4] = sat[y].elevation; + _satellite_info->azimuth[y + (this_msg_num - 1) * 4] = sat[y].azimuth; + } + } + + return 0; } -int ASHTECH::receive(unsigned timeout){ +int ASHTECH::receive(unsigned timeout) { - /* poll descriptor */ - pollfd fds[1]; - fds[0].fd = _fd; - fds[0].events = POLLIN; - - uint8_t buf[32]; - - /* timeout additional to poll */ - uint64_t time_started = hrt_absolute_time(); - - int j = 0; - ssize_t count = 0; - - while (true) { - - /* pass received bytes to the packet decoder */ - while (j < count) { - int l=0; - if ((l = parse_char(buf[j])) > 0) { - /* return to configure during configuration or to the gps driver during normal work - * if a packet has arrived */ - if (handle_message(l) > 0) - return 1; - } - /* in case we keep trying but only get crap from GPS */ - if (time_started + timeout*1000*2 < hrt_absolute_time() ) { - return -1; - } - j++; - } - - /* everything is read */ - j = count = 0; - - /* then poll for new data */ - int ret = ::poll(fds, sizeof(fds) / sizeof(fds[0]), timeout*2); - - if (ret < 0) { - /* something went wrong when polling */ - return -1; - - } else if (ret == 0) { - /* Timeout */ - return -1; - - } else if (ret > 0) { - /* if we have new data from GPS, go handle it */ - if (fds[0].revents & POLLIN) { - /* - * We are here because poll says there is some data, so this - * won't block even on a blocking device. If more bytes are - * available, we'll go back to poll() again... - */ - count = ::read(_fd, buf, sizeof(buf)); - } - } - } -} + { + /* poll descriptor */ + pollfd fds[1]; + fds[0].fd = _fd; + fds[0].events = POLLIN; + + uint8_t buf[32]; + + /* timeout additional to poll */ + uint64_t time_started = hrt_absolute_time(); + + int j = 0; + ssize_t count = 0; + + while (true) { + + /* pass received bytes to the packet decoder */ + while (j < count) { + int l = 0; + + if ((l = parse_char(buf[j])) > 0) { + /* return to configure during configuration or to the gps driver during normal work + * if a packet has arrived */ + if (handle_message(l) > 0) { + return 1; + } + } + + /* in case we keep trying but only get crap from GPS */ + if (time_started + timeout * 1000 * 2 < hrt_absolute_time()) { + return -1; + } + + j++; + } + + /* everything is read */ + j = count = 0; + + /* then poll for new data */ + int ret = ::poll(fds, sizeof(fds) / sizeof(fds[0]), timeout * 2); + + if (ret < 0) { + /* something went wrong when polling */ + return -1; + + } else if (ret == 0) { + /* Timeout */ + return -1; + + } else if (ret > 0) { + /* if we have new data from GPS, go handle it */ + if (fds[0].revents & POLLIN) { + /* + * We are here because poll says there is some data, so this + * won't block even on a blocking device. If more bytes are + * available, we'll go back to poll() again... + */ + count = ::read(_fd, buf, sizeof(buf)); + } + } + } + } } #define HEXDIGIT_CHAR(d) ((char)((d) + (((d) < 0xA) ? '0' : 'A'-0xA))) @@ -557,8 +643,9 @@ int ASHTECH::parse_char(uint8_t b) _decode_state = NME_DECODE_UNINIT; _rx_buffer_bytes = 0; - } else + } else { _rx_buffer[_rx_buffer_bytes++] = b; + } break; @@ -573,7 +660,7 @@ int ASHTECH::parse_char(uint8_t b) uint8_t *buffer = _rx_buffer + 1; uint8_t *bufend = _rx_buffer + _rx_buffer_bytes - 3; - for (; buffer < bufend; buffer++) checksum ^= *buffer; + for (; buffer < bufend; buffer++) { checksum ^= *buffer; } if ((HEXDIGIT_CHAR(checksum >> 4) == *(_rx_buffer + _rx_buffer_bytes - 2)) && (HEXDIGIT_CHAR(checksum & 0x0F) == *(_rx_buffer + _rx_buffer_bytes - 1))) { @@ -588,32 +675,36 @@ int ASHTECH::parse_char(uint8_t b) return iRet; } -void ASHTECH::decode_init(void){ +void ASHTECH::decode_init(void) +{ } -//ashtech boad configuration script -//char comm[] = "$PASHS,NME,GGA,A,ON,0.1\r\n" +/* + * ashtech boad configuration script + */ + char comm[] = "$PASHS,POP,20\r\n"\ - "$PASHS,NME,ZDA,B,ON,3\r\n"\ - "$PASHS,NME,GGA,B,OFF\r\n"\ - "$PASHS,NME,GST,B,ON,3\r\n"\ - "$PASHS,NME,POS,B,ON,0.05\r\n"\ - "$PASHS,NME,GSV,B,ON,3\r\n"\ - "$PASHS,SPD,A,8\r\n"\ - "$PASHS,SPD,B,9\r\n"; // default baud is 7 - -int ASHTECH::configure(unsigned &baudrate){ - /* try different baudrates */ - const unsigned baudrates_to_try[] = {9600, 38400, 19200, 57600, 115200}; - - - for (int baud_i = 0; baud_i < sizeof(baudrates_to_try)/sizeof(baudrates_to_try[0]); baud_i++) { - baudrate = baudrates_to_try[baud_i]; - set_baudrate(_fd, baudrate); - write(_fd, (uint8_t*)comm, sizeof(comm)); - } - - set_baudrate(_fd, 115200); - return 0; + "$PASHS,NME,ZDA,B,ON,3\r\n"\ + "$PASHS,NME,GGA,B,OFF\r\n"\ + "$PASHS,NME,GST,B,ON,3\r\n"\ + "$PASHS,NME,POS,B,ON,0.05\r\n"\ + "$PASHS,NME,GSV,B,ON,3\r\n"\ + "$PASHS,SPD,A,8\r\n"\ + "$PASHS,SPD,B,9\r\n"; + +int ASHTECH::configure(unsigned &baudrate) +{ + /* try different baudrates */ + const unsigned baudrates_to_try[] = {9600, 38400, 19200, 57600, 115200}; + + + for (int baud_i = 0; baud_i < sizeof(baudrates_to_try) / sizeof(baudrates_to_try[0]); baud_i++) { + baudrate = baudrates_to_try[baud_i]; + set_baudrate(_fd, baudrate); + write(_fd, (uint8_t *)comm, sizeof(comm)); + } + + set_baudrate(_fd, 115200); + return 0; } diff --git a/src/drivers/gps/ashtech.h b/src/drivers/gps/ashtech.h index e8c5ffb4c..56f8571a4 100644 --- a/src/drivers/gps/ashtech.h +++ b/src/drivers/gps/ashtech.h @@ -67,8 +67,8 @@ class ASHTECH : public GPS_Helper bool _parse_error; // parse error flag char *_parse_pos; // parse position - bool _gsv_in_progress; // Indicates that gsv data parsing is in progress - //int _satellites_count; // Number of satellites info parsed. + bool _gsv_in_progress; /**< Indicates that gsv data parsing is in progress */ + //int _satellites_count; /**< Number of satellites info parsed. */ uint8_t count; /**< Number of satellites in satellite info */ uint8_t svid[SAT_INFO_MAX_SATELLITES]; /**< Space vehicle ID [1..255], see scheme below */ uint8_t used[SAT_INFO_MAX_SATELLITES]; /**< 0: Satellite not used, 1: used for navigation */ diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index 2547cda97..5fb4b9ff8 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -343,7 +343,6 @@ GPS::task_main() break; case GPS_DRIVER_MODE_ASHTECH: - _Helper = new ASHTECH(_serial_fd, &_report_gps_pos, _p_report_sat_info); break; @@ -408,9 +407,9 @@ GPS::task_main() mode_str = "MTK"; break; - case GPS_DRIVER_MODE_ASHTECH: - mode_str = "ASHTECH"; - break; + case GPS_DRIVER_MODE_ASHTECH: + mode_str = "ASHTECH"; + break; default: break; @@ -439,10 +438,10 @@ GPS::task_main() break; case GPS_DRIVER_MODE_MTK: - _mode = GPS_DRIVER_MODE_ASHTECH; - break; + _mode = GPS_DRIVER_MODE_ASHTECH; + break; - case GPS_DRIVER_MODE_ASHTECH: + case GPS_DRIVER_MODE_ASHTECH: _mode = GPS_DRIVER_MODE_UBX; break; -- cgit v1.2.3 From cfc6c234b83a457e4288c39f778341f6c8489e85 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 9 Oct 2014 09:22:34 +0200 Subject: att pos estimator: Initialize distance --- src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index 27c1f981e..03ae1d847 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -382,6 +382,7 @@ FixedwingEstimator::FixedwingEstimator() : _local_pos({}), _gps({}), _wind({}), + _distance{}, _gyro_offsets({}), _accel_offsets({}), -- cgit v1.2.3 From aca9b138b7e92bd2a32e8673c81f3fe2d088a427 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 9 Oct 2014 09:23:00 +0200 Subject: att pos estimator: Use float constant where it should be float. --- src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index 03ae1d847..a61346c2e 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -1085,7 +1085,7 @@ FixedwingEstimator::task_main() float baro_elapsed = (_baro.timestamp - baro_last) / 1e6f; - _ekf->updateDtHgtFilt(math::constrain(baro_elapsed, 0.001f, 0.1)); + _ekf->updateDtHgtFilt(math::constrain(baro_elapsed, 0.001f, 0.1f)); _ekf->baroHgt = _baro.altitude; -- cgit v1.2.3 From e0719f21417d3ba572df18460073faa923d5ff29 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 9 Oct 2014 09:23:28 +0200 Subject: Initialize inhibition variable. --- src/modules/ekf_att_pos_estimator/estimator_23states.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp index 94a6d165f..e629d3345 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp @@ -119,6 +119,7 @@ AttPosEKF::AttPosEKF() : inhibitWindStates(true), inhibitMagStates(true), inhibitGndState(true), + inhibitScaleState(true), onGround(true), staticMode(true), -- cgit v1.2.3 From d524b17164125415c831e38ec1e7db11a6ea97e1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 9 Oct 2014 09:24:08 +0200 Subject: Disambiguate local variable names. --- src/modules/ekf_att_pos_estimator/estimator_23states.cpp | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp index e629d3345..c17e034ad 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp @@ -2561,15 +2561,15 @@ void AttPosEKF::CovarianceInit() P[22][22] = sq(0.5f); } -float AttPosEKF::ConstrainFloat(float val, float min, float max) +float AttPosEKF::ConstrainFloat(float val, float min_val, float max_val) { float ret; - if (val > max) { - ret = max; - ekf_debug("> max: %8.4f, val: %8.4f", (double)max, (double)val); - } else if (val < min) { - ret = min; - ekf_debug("< min: %8.4f, val: %8.4f", (double)min, (double)val); + if (val > max_val) { + ret = max_val; + ekf_debug("> max: %8.4f, val: %8.4f", (double)max_val, (double)val); + } else if (val < min_val) { + ret = min_val; + ekf_debug("< min: %8.4f, val: %8.4f", (double)min_val, (double)val); } else { ret = val; } -- cgit v1.2.3 From fa194832ce136116b37ba8ce787b40efce53bc3d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 9 Oct 2014 09:24:47 +0200 Subject: mavlink: Handle new auto sub states. --- src/modules/mavlink/mavlink_messages.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index a2f3828ff..c182cfdb9 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -162,6 +162,8 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set break; case NAVIGATION_STATE_AUTO_RTL: + /* fallthrough */ + case NAVIGATION_STATE_AUTO_RCRECOVER: *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; @@ -170,6 +172,8 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set break; case NAVIGATION_STATE_LAND: + case NAVIGATION_STATE_AUTO_LANDENGFAIL: + case NAVIGATION_STATE_AUTO_LANDGPSFAIL: /* fallthrough */ case NAVIGATION_STATE_DESCEND: *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED -- cgit v1.2.3 From 5ec6cb0789917aa49c629b8d9cb354df1e3c0f0f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 9 Oct 2014 09:26:46 +0200 Subject: navigator: Remove excessive C++ check flags, which enable debatable warnings. --- src/modules/navigator/module.mk | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/modules/navigator/module.mk b/src/modules/navigator/module.mk index 72dc2815c..c44d4c35e 100644 --- a/src/modules/navigator/module.mk +++ b/src/modules/navigator/module.mk @@ -61,6 +61,4 @@ INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink MODULE_STACKSIZE = 1200 -EXTRACXXFLAGS = -Weffc++ - MAXOPTIMIZATION = -Os -- cgit v1.2.3 From 7bb9b3efa71107c8988f6933720365b42c8a4046 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 9 Oct 2014 09:27:25 +0200 Subject: IO firmware hot fix: Use right pointer type for RSSI value. --- src/modules/px4iofirmware/controls.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c index 622f20c7d..7b127759a 100644 --- a/src/modules/px4iofirmware/controls.c +++ b/src/modules/px4iofirmware/controls.c @@ -52,7 +52,7 @@ #define RC_CHANNEL_LOW_THRESH -8000 /* 10% threshold */ static bool ppm_input(uint16_t *values, uint16_t *num_values, uint16_t *frame_len); -static bool dsm_port_input(uint8_t *rssi, bool *dsm_updated, bool *st24_updated); +static bool dsm_port_input(uint16_t *rssi, bool *dsm_updated, bool *st24_updated); static perf_counter_t c_gather_dsm; static perf_counter_t c_gather_sbus; @@ -60,7 +60,7 @@ static perf_counter_t c_gather_ppm; static int _dsm_fd; -bool dsm_port_input(uint8_t *rssi, bool *dsm_updated, bool *st24_updated) +bool dsm_port_input(uint16_t *rssi, bool *dsm_updated, bool *st24_updated) { perf_begin(c_gather_dsm); uint16_t temp_count = r_raw_rc_count; -- cgit v1.2.3 From 1d50af272f5f3727faad911a69e1b3807f28ca2d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 9 Oct 2014 09:29:05 +0200 Subject: EEPROM driver: Do not issue warnings that result from our special case handling - this driver is only used for this one particular eeprom and out of the NuttX tree. --- src/systemcmds/mtd/24xxxx_mtd.c | 15 ++++++++++++--- 1 file changed, 12 insertions(+), 3 deletions(-) diff --git a/src/systemcmds/mtd/24xxxx_mtd.c b/src/systemcmds/mtd/24xxxx_mtd.c index 991363797..f85ed8e2d 100644 --- a/src/systemcmds/mtd/24xxxx_mtd.c +++ b/src/systemcmds/mtd/24xxxx_mtd.c @@ -80,11 +80,17 @@ /* As a minimum, the size of the AT24 part and its 7-bit I2C address are required. */ #ifndef CONFIG_AT24XX_SIZE -# warning "Assuming AT24 size 64" +/* XXX this is a well vetted special case, + * do not issue a warning any more + * # warning "Assuming AT24 size 64" + */ # define CONFIG_AT24XX_SIZE 64 #endif #ifndef CONFIG_AT24XX_ADDR -# warning "Assuming AT24 address of 0x50" +/* XXX this is a well vetted special case, + * do not issue a warning any more + * # warning "Assuming AT24 address of 0x50" + */ # define CONFIG_AT24XX_ADDR 0x50 #endif @@ -115,7 +121,10 @@ */ #ifndef CONFIG_AT24XX_MTD_BLOCKSIZE -# warning "Assuming driver block size is the same as the FLASH page size" +/* XXX this is a well vetted special case, + * do not issue a warning any more + * # warning "Assuming driver block size is the same as the FLASH page size" + */ # define CONFIG_AT24XX_MTD_BLOCKSIZE AT24XX_PAGESIZE #endif -- cgit v1.2.3 From 9f33555f53d253a04de1ca97d1306b763c9d2b6e Mon Sep 17 00:00:00 2001 From: Denis Yeldandi Date: Thu, 9 Oct 2014 11:36:04 +0400 Subject: More code style fixes --- src/drivers/gps/ashtech.h | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/drivers/gps/ashtech.h b/src/drivers/gps/ashtech.h index 56f8571a4..6e65d0b2b 100644 --- a/src/drivers/gps/ashtech.h +++ b/src/drivers/gps/ashtech.h @@ -59,16 +59,16 @@ class ASHTECH : public GPS_Helper int _fd; struct satellite_info_s *_satellite_info; struct vehicle_gps_position_s *_gps_position; - int ashtechlog_fd;//miklm + int ashtechlog_fd; ashtech_decode_state_t _decode_state; uint8_t _rx_buffer[RECV_BUFFER_SIZE]; uint16_t _rx_buffer_bytes; - bool _parse_error; // parse error flag - char *_parse_pos; // parse position + bool _parse_error; /** parse error flag */ + char *_parse_pos; /** parse position */ bool _gsv_in_progress; /**< Indicates that gsv data parsing is in progress */ - //int _satellites_count; /**< Number of satellites info parsed. */ + /* int _satellites_count; /**< Number of satellites info parsed. */ uint8_t count; /**< Number of satellites in satellite info */ uint8_t svid[SAT_INFO_MAX_SATELLITES]; /**< Space vehicle ID [1..255], see scheme below */ uint8_t used[SAT_INFO_MAX_SATELLITES]; /**< 0: Satellite not used, 1: used for navigation */ -- cgit v1.2.3 From d3875eabf2ce0ac538954b0d5747f1155cf2ef33 Mon Sep 17 00:00:00 2001 From: Denis Yeldandi Date: Thu, 9 Oct 2014 11:37:22 +0400 Subject: Removed VTG message parsing --- src/drivers/gps/ashtech.cpp | 36 ------------------------------------ 1 file changed, 36 deletions(-) diff --git a/src/drivers/gps/ashtech.cpp b/src/drivers/gps/ashtech.cpp index 7342d01d1..077a6ddf4 100644 --- a/src/drivers/gps/ashtech.cpp +++ b/src/drivers/gps/ashtech.cpp @@ -419,42 +419,6 @@ int ASHTECH::handle_message(int len) _gps_position->s_variance_m_s = 0; _gps_position->timestamp_variance = hrt_absolute_time(); - } else if ((memcmp(_rx_buffer + 3, "VTG,", 3) == 0) && (uiCalcComma == 9)) { - /* - Track made good and speed over ground - An example of the VTG message string is: - - $GPVTG,054.7,T,034.4,M,005.5,N,010.2,K*48 - - VTG message fields - Field Meaning - 0 Message ID $GPVTG - 1 Track made good (degrees true) - 2 T: track made good is relative to true north - 3 Track made good (degrees magnetic) - 4 M: track made good is relative to magnetic north - 5 Speed, in knots - 6 N: speed is measured in knots - 7 Speed over ground in kilometers/hour (kph) - 8 K: speed over ground is measured in kph - 9 The checksum data, always begins with * - */ - /*float64_t track_true = 0.0, speed = 0.0, track_magnetic = 0.0, ground_speed = 0.0; - char true_north = '?', magnetic_north = '?', speed_in_knots = '?', speed_in_kph = '?'; - - if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&track_true); - if(bufptr && *(++bufptr) != ',') true_north = *(bufptr++); - if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&track_magnetic); - if(bufptr && *(++bufptr) != ',') magnetic_north = *(bufptr++); - if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&speed); - if(bufptr && *(++bufptr) != ',') speed_in_knots = *(bufptr++); - if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&ground_speed); - if(bufptr && *(++bufptr) != ',') speed_in_kph = *(bufptr++); - - const float64_t dPI = 3.14159265; - float64_t tan_C = tan(track_true * dPI / 180.0); - float64_t lat_ = sqrt(ground_speed*ground_speed/ (1+tan_C)); //km/hour - float64_t lon_ = lat_*tan_C; // //km/hour*/ } else if ((memcmp(_rx_buffer + 3, "GSV,", 3) == 0)) { /* The GSV message string identifies the number of SVs in view, the PRN numbers, elevations, azimuths, and SNR values. An example of the GSV message string is: -- cgit v1.2.3 From e37b25fd5868d00a3a6b39c1112f30bfe37a8a47 Mon Sep 17 00:00:00 2001 From: Denis Yeldandi Date: Thu, 9 Oct 2014 11:47:20 +0400 Subject: Non-ascii characters cleanup --- src/drivers/gps/ashtech.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/drivers/gps/ashtech.cpp b/src/drivers/gps/ashtech.cpp index 077a6ddf4..71ddb28cc 100644 --- a/src/drivers/gps/ashtech.cpp +++ b/src/drivers/gps/ashtech.cpp @@ -124,7 +124,7 @@ int ASHTECH::handle_message(int len) 2 Day, ranging between 01 and 31 3 Month, ranging between 01 and 12 4 Year - 5 Local time zone offset from GMT, ranging from 00 through �13 hours + 5 Local time zone offset from GMT, ranging from 00 through 13 hours 6 Local time zone offset from GMT, ranging from 00 through 59 minutes 7 The checksum data, always begins with * Fields 5 and 6 together yield the total offset. For example, if field 5 is -5 and field 6 is +15, local time is 5 hours and 15 minutes earlier than GMT. @@ -432,8 +432,8 @@ int ASHTECH::handle_message(int len) 2 Message number 3 Total number of SVs visible 4 SV PRN number - 5 Elevation, in degrees, 90� maximum - 6 Azimuth, degrees from True North, 000� through 359� + 5 Elevation, in degrees, 90 maximum + 6 Azimuth, degrees from True North, 000 through 359 7 SNR, 00 through 99 dB (null when not tracking) 8-11 Information about second SV, same format as fields 4 through 7 12-15 Information about third SV, same format as fields 4 through 7 -- cgit v1.2.3 From c4e934c13311fac2eb85e5e3f1a161af78be7118 Mon Sep 17 00:00:00 2001 From: Denis Yeldandi Date: Thu, 9 Oct 2014 12:01:09 +0400 Subject: Multiple fixes: - Fixed boad - board typo - Ashtech initialization string is const char* now - Using standard M_PI constant instead of locally defined one - Removed float32_t and float64_t in favor of standard float and double --- src/drivers/gps/ashtech.cpp | 39 +++++++++++++++++---------------------- 1 file changed, 17 insertions(+), 22 deletions(-) diff --git a/src/drivers/gps/ashtech.cpp b/src/drivers/gps/ashtech.cpp index 71ddb28cc..503e56dae 100644 --- a/src/drivers/gps/ashtech.cpp +++ b/src/drivers/gps/ashtech.cpp @@ -15,9 +15,6 @@ #include #include -typedef double float64_t; -typedef float float32_t; - char *str_scanDec(const char *pos, int8_t sign, int8_t n_max_digit, int32_t *result) { int8_t n = 0; @@ -47,9 +44,9 @@ char *str_scanDec(const char *pos, int8_t sign, int8_t n_max_digit, int32_t *re } char *scanFloat64(const char *pos, int8_t sign, int8_t n_max_int, int8_t n_max_frac, - float64_t *result) + double *result) { - float64_t f = 0.0, div = 1.0; + double f = 0.0, div = 1.0; int32_t d_int; int8_t n = 0, isneg = 0; @@ -61,7 +58,7 @@ char *scanFloat64(const char *pos, int8_t sign, int8_t n_max_int, int8_t n_max_ pos++; while (*pos >= '0' && *pos <= '9') { - f = f * (10.0) + (float64_t)(*(pos++) - '0'); + f = f * (10.0) + (double)(*(pos++) - '0'); div *= (0.1); n++; @@ -70,9 +67,9 @@ char *scanFloat64(const char *pos, int8_t sign, int8_t n_max_int, int8_t n_max_ } else if (n_max_frac > 0) { return (NULL); } - if (isneg) { *result = (float64_t)d_int - f * div; } + if (isneg) { *result = (double)d_int - f * div; } - else { *result = (float64_t)d_int + f * div; } + else { *result = (double)d_int + f * div; } return ((char *)pos); } @@ -129,7 +126,7 @@ int ASHTECH::handle_message(int len) 7 The checksum data, always begins with * Fields 5 and 6 together yield the total offset. For example, if field 5 is -5 and field 6 is +15, local time is 5 hours and 15 minutes earlier than GMT. */ - float64_t ashtech_time = 0.0; + double ashtech_time = 0.0; int day = 0, month = 0, year = 0, local_time_off_hour = 0, local_time_off_min = 0; if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &ashtech_time); } @@ -147,7 +144,7 @@ int ASHTECH::handle_message(int len) int ashtech_hour = ashtech_time / 10000; int ashtech_minute = (ashtech_time - ashtech_hour * 10000) / 100; - float64_t ashtech_sec = ashtech_time - ashtech_hour * 10000 - ashtech_minute * 100; + double ashtech_sec = ashtech_time - ashtech_hour * 10000 - ashtech_minute * 100; /* * convert to unix timestamp */ @@ -203,9 +200,9 @@ int ASHTECH::handle_message(int len) The checksum data, always begins with * Note - If a user-defined geoid model, or an inclined */ - float64_t ashtech_time = 0.0, lat = 0.0, lon = 0.0, alt = 0.0; + double ashtech_time = 0.0, lat = 0.0, lon = 0.0, alt = 0.0; int num_of_sv = 0, fix_quality = 0; - float64_t hdop = 99.9; + double hdop = 99.9; char ns = '?', ew = '?'; if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &ashtech_time); } @@ -290,10 +287,10 @@ int ASHTECH::handle_message(int len) *cc Checksum */ bufptr = (char *)(_rx_buffer + 10); - float64_t ashtech_time = 0.0, lat = 0.0, lon = 0.0, alt = 0.0; + double ashtech_time = 0.0, lat = 0.0, lon = 0.0, alt = 0.0; int num_of_sv = 0, fix_quality = 0; - float64_t track_true = 0.0, ground_speed = 0.0 , age_of_corr = 0.0; - float64_t hdop = 99.9, vdop = 99.9, pdop = 99.9, tdop = 99.9, vertic_vel = 0.0; + double track_true = 0.0, ground_speed = 0.0 , age_of_corr = 0.0; + double hdop = 99.9, vdop = 99.9, pdop = 99.9, tdop = 99.9, vertic_vel = 0.0; char ns = '?', ew = '?'; if (bufptr && *(++bufptr) != ',') { bufptr = str_scanDec(bufptr, 0, 9, &fix_quality); } @@ -350,9 +347,7 @@ int ASHTECH::handle_message(int len) _gps_position->timestamp_position = hrt_absolute_time(); - const float64_t m_pi = 3.14159265; - - double track_rad = track_true * m_pi / 180.0; + double track_rad = track_true * M_PI / 180.0; double velocity_ms = ground_speed / 1.9438445; /** knots to m/s */ double velocity_north = velocity_ms * cos(track_rad); @@ -394,8 +389,8 @@ int ASHTECH::handle_message(int len) 8 Height 1 sigma error, in meters 9 The checksum data, always begins with * */ - float64_t ashtech_time = 0.0, lat_err = 0.0, lon_err = 0.0, alt_err = 0.0; - float64_t min_err = 0.0, maj_err = 0.0, deg_from_north = 0.0, rms_err = 0.0; + double ashtech_time = 0.0, lat_err = 0.0, lon_err = 0.0, alt_err = 0.0; + double min_err = 0.0, maj_err = 0.0, deg_from_north = 0.0, rms_err = 0.0; if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &ashtech_time); } @@ -645,10 +640,10 @@ void ASHTECH::decode_init(void) } /* - * ashtech boad configuration script + * ashtech board configuration script */ -char comm[] = "$PASHS,POP,20\r\n"\ +const char comm[] = "$PASHS,POP,20\r\n"\ "$PASHS,NME,ZDA,B,ON,3\r\n"\ "$PASHS,NME,GGA,B,OFF\r\n"\ "$PASHS,NME,GST,B,ON,3\r\n"\ -- cgit v1.2.3 From 676cb91a1d973313e15597280121c52cc2d84e43 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 9 Oct 2014 10:55:48 +0200 Subject: Hotfix for PX4IO comms: Raise timeout to 10 ms. --- nuttx-configs/px4fmu-v1/nsh/defconfig | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nuttx-configs/px4fmu-v1/nsh/defconfig b/nuttx-configs/px4fmu-v1/nsh/defconfig index cd9c05b13..edf49b26e 100644 --- a/nuttx-configs/px4fmu-v1/nsh/defconfig +++ b/nuttx-configs/px4fmu-v1/nsh/defconfig @@ -287,7 +287,7 @@ CONFIG_STM32_USART_SINGLEWIRE=y # # CONFIG_STM32_I2C_DYNTIMEO is not set CONFIG_STM32_I2CTIMEOSEC=0 -CONFIG_STM32_I2CTIMEOMS=1 +CONFIG_STM32_I2CTIMEOMS=10 # CONFIG_STM32_I2C_DUTY16_9 is not set # -- cgit v1.2.3 From bc5d1648fe298102eb90b0a6f4ce7cedf364b165 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 9 Oct 2014 11:13:34 +0200 Subject: Aerocore: Add missing declarations --- src/drivers/boards/aerocore/aerocore_init.c | 13 +++++++++++++ src/drivers/boards/aerocore/board_config.h | 19 +++++++++++++++++++ 2 files changed, 32 insertions(+) diff --git a/src/drivers/boards/aerocore/aerocore_init.c b/src/drivers/boards/aerocore/aerocore_init.c index 4e3ba2d7e..1ce235da8 100644 --- a/src/drivers/boards/aerocore/aerocore_init.c +++ b/src/drivers/boards/aerocore/aerocore_init.c @@ -93,6 +93,19 @@ # endif #endif +/* + * Ideally we'd be able to get these from up_internal.h, + * but since we want to be able to disable the NuttX use + * of leds for system indication at will and there is no + * separate switch, we need to build independent of the + * CONFIG_ARCH_LEDS configuration switch. + */ +__BEGIN_DECLS +extern void led_init(void); +extern void led_on(int led); +extern void led_off(int led); +__END_DECLS + /**************************************************************************** * Protected Functions ****************************************************************************/ diff --git a/src/drivers/boards/aerocore/board_config.h b/src/drivers/boards/aerocore/board_config.h index 70142a314..776a2071e 100644 --- a/src/drivers/boards/aerocore/board_config.h +++ b/src/drivers/boards/aerocore/board_config.h @@ -171,6 +171,25 @@ __BEGIN_DECLS extern void stm32_spiinitialize(void); +/**************************************************************************** + * Name: nsh_archinitialize + * + * Description: + * Perform architecture specific initialization for NSH. + * + * CONFIG_NSH_ARCHINIT=y : + * Called from the NSH library + * + * CONFIG_BOARD_INITIALIZE=y, CONFIG_NSH_LIBRARY=y, && + * CONFIG_NSH_ARCHINIT=n : + * Called from board_initialize(). + * + ****************************************************************************/ + +#ifdef CONFIG_NSH_LIBRARY +int nsh_archinitialize(void); +#endif + #endif /* __ASSEMBLY__ */ __END_DECLS -- cgit v1.2.3 From c3e25377d7d86277b338020d7efd986770a2429c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 9 Oct 2014 11:13:55 +0200 Subject: FMUv1: Add missing declarations. --- src/drivers/boards/px4fmu-v1/board_config.h | 21 +++++++++++++++++++++ 1 file changed, 21 insertions(+) diff --git a/src/drivers/boards/px4fmu-v1/board_config.h b/src/drivers/boards/px4fmu-v1/board_config.h index a70a6240c..188885be2 100644 --- a/src/drivers/boards/px4fmu-v1/board_config.h +++ b/src/drivers/boards/px4fmu-v1/board_config.h @@ -209,6 +209,27 @@ __BEGIN_DECLS extern void stm32_spiinitialize(void); +extern void stm32_usbinitialize(void); + +/**************************************************************************** + * Name: nsh_archinitialize + * + * Description: + * Perform architecture specific initialization for NSH. + * + * CONFIG_NSH_ARCHINIT=y : + * Called from the NSH library + * + * CONFIG_BOARD_INITIALIZE=y, CONFIG_NSH_LIBRARY=y, && + * CONFIG_NSH_ARCHINIT=n : + * Called from board_initialize(). + * + ****************************************************************************/ + +#ifdef CONFIG_NSH_LIBRARY +int nsh_archinitialize(void); +#endif + #endif /* __ASSEMBLY__ */ __END_DECLS -- cgit v1.2.3 From 4bfa30bfd6151c63c4cd8e2ece51e5568f4f3d16 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 9 Oct 2014 11:14:13 +0200 Subject: FMUv2: Add missing declarations. --- src/drivers/boards/px4fmu-v2/board_config.h | 21 +++++++++++++++++++++ src/drivers/boards/px4fmu-v2/px4fmu2_init.c | 13 +++++++++++++ 2 files changed, 34 insertions(+) diff --git a/src/drivers/boards/px4fmu-v2/board_config.h b/src/drivers/boards/px4fmu-v2/board_config.h index 0190a5b5b..ee365e47c 100644 --- a/src/drivers/boards/px4fmu-v2/board_config.h +++ b/src/drivers/boards/px4fmu-v2/board_config.h @@ -229,6 +229,27 @@ __BEGIN_DECLS extern void stm32_spiinitialize(void); +extern void stm32_usbinitialize(void); + +/**************************************************************************** + * Name: nsh_archinitialize + * + * Description: + * Perform architecture specific initialization for NSH. + * + * CONFIG_NSH_ARCHINIT=y : + * Called from the NSH library + * + * CONFIG_BOARD_INITIALIZE=y, CONFIG_NSH_LIBRARY=y, && + * CONFIG_NSH_ARCHINIT=n : + * Called from board_initialize(). + * + ****************************************************************************/ + +#ifdef CONFIG_NSH_LIBRARY +int nsh_archinitialize(void); +#endif + #endif /* __ASSEMBLY__ */ __END_DECLS diff --git a/src/drivers/boards/px4fmu-v2/px4fmu2_init.c b/src/drivers/boards/px4fmu-v2/px4fmu2_init.c index bf41bb1fe..9b25c574a 100644 --- a/src/drivers/boards/px4fmu-v2/px4fmu2_init.c +++ b/src/drivers/boards/px4fmu-v2/px4fmu2_init.c @@ -94,6 +94,19 @@ # endif #endif +/* + * Ideally we'd be able to get these from up_internal.h, + * but since we want to be able to disable the NuttX use + * of leds for system indication at will and there is no + * separate switch, we need to build independent of the + * CONFIG_ARCH_LEDS configuration switch. + */ +__BEGIN_DECLS +extern void led_init(void); +extern void led_on(int led); +extern void led_off(int led); +__END_DECLS + /**************************************************************************** * Protected Functions ****************************************************************************/ -- cgit v1.2.3 From 5bbca777961e0e0b109d954772ff0176c65277f5 Mon Sep 17 00:00:00 2001 From: Denis Yeldandi Date: Thu, 9 Oct 2014 14:11:18 +0400 Subject: Got rid of str_scanDec and scanFloat64. - Replaced str_scanDec and scanFloat64 with strtol and strtod. - Added __attribute__ ((unused)) to yet unused variables - Added initialization for a few variables --- src/drivers/gps/ashtech.cpp | 170 +++++++++++++++----------------------------- 1 file changed, 56 insertions(+), 114 deletions(-) diff --git a/src/drivers/gps/ashtech.cpp b/src/drivers/gps/ashtech.cpp index 503e56dae..1b706b5b2 100644 --- a/src/drivers/gps/ashtech.cpp +++ b/src/drivers/gps/ashtech.cpp @@ -1,6 +1,7 @@ #include "ashtech.h" #include +#include #include #include #include @@ -15,67 +16,6 @@ #include #include -char *str_scanDec(const char *pos, int8_t sign, int8_t n_max_digit, int32_t *result) -{ - int8_t n = 0; - int32_t d = 0; - int8_t neg = 0; - - if (*pos == '-') { neg = 1; pos++; } - - else if (*pos == '+') { pos++; } - - else if (sign) { return (NULL); } - - while (*pos >= '0' && *pos <= '9') { - d = d * 10 + (*(pos++) - '0'); - n++; - - if (n_max_digit > 0 && n == n_max_digit) { break; } - } - - if (n == 0 || n > 10) { return (NULL); } - - if (neg) { *result = -d; } - - else { *result = d; } - - return ((char *)pos); -} - -char *scanFloat64(const char *pos, int8_t sign, int8_t n_max_int, int8_t n_max_frac, - double *result) -{ - double f = 0.0, div = 1.0; - int32_t d_int; - int8_t n = 0, isneg = 0; - - if (*pos == '-') { isneg = 1; } - - if ((pos = str_scanDec(pos, sign, n_max_int, &d_int)) == NULL) { return (NULL); } - - if (*(pos) == '.') { - pos++; - - while (*pos >= '0' && *pos <= '9') { - f = f * (10.0) + (double)(*(pos++) - '0'); - div *= (0.1); - n++; - - if (n_max_frac > 0 && n == n_max_frac) { break; } - } - - } else if (n_max_frac > 0) { return (NULL); } - - if (isneg) { *result = (double)d_int - f * div; } - - else { *result = (double)d_int + f * div; } - - return ((char *)pos); -} - - - ASHTECH::ASHTECH(const int &fd, struct vehicle_gps_position_s *gps_position, struct satellite_info_s *satellite_info): _fd(fd), _satellite_info(satellite_info), @@ -97,6 +37,8 @@ ASHTECH::~ASHTECH() int ASHTECH::handle_message(int len) { + char * endp; + if (len < 7) { return 0; } int uiCalcComma = 0; @@ -127,19 +69,19 @@ int ASHTECH::handle_message(int len) Fields 5 and 6 together yield the total offset. For example, if field 5 is -5 and field 6 is +15, local time is 5 hours and 15 minutes earlier than GMT. */ double ashtech_time = 0.0; - int day = 0, month = 0, year = 0, local_time_off_hour = 0, local_time_off_min = 0; + int day = 0, month = 0, year = 0, local_time_off_hour __attribute__((unused)) = 0, local_time_off_min __attribute__((unused)) = 0; - if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &ashtech_time); } + if (bufptr && *(++bufptr) != ',') { ashtech_time = strtod(bufptr, &endp); bufptr = endp; } - if (bufptr && *(++bufptr) != ',') { bufptr = str_scanDec(bufptr, 0, 9, &day); } + if (bufptr && *(++bufptr) != ',') { day = strtol(bufptr, &endp, 10); bufptr = endp; } - if (bufptr && *(++bufptr) != ',') { bufptr = str_scanDec(bufptr, 0, 9, &month); } + if (bufptr && *(++bufptr) != ',') { month = strtol(bufptr, &endp, 10); bufptr = endp; } - if (bufptr && *(++bufptr) != ',') { bufptr = str_scanDec(bufptr, 0, 9, &year); } + if (bufptr && *(++bufptr) != ',') { year = strtol(bufptr, &endp, 10); bufptr = endp; } - if (bufptr && *(++bufptr) != ',') { bufptr = str_scanDec(bufptr, 0, 9, &local_time_off_hour); } + if (bufptr && *(++bufptr) != ',') { local_time_off_hour = strtol(bufptr, &endp, 10); bufptr = endp; } - if (bufptr && *(++bufptr) != ',') { bufptr = str_scanDec(bufptr, 0, 9, &local_time_off_min); } + if (bufptr && *(++bufptr) != ',') { local_time_off_min = strtol(bufptr, &endp, 10); bufptr = endp; } int ashtech_hour = ashtech_time / 10000; @@ -200,28 +142,28 @@ int ASHTECH::handle_message(int len) The checksum data, always begins with * Note - If a user-defined geoid model, or an inclined */ - double ashtech_time = 0.0, lat = 0.0, lon = 0.0, alt = 0.0; - int num_of_sv = 0, fix_quality = 0; - double hdop = 99.9; + double ashtech_time __attribute__((unused)) = 0.0, lat = 0.0, lon = 0.0, alt = 0.0; + int num_of_sv __attribute__((unused)) = 0, fix_quality = 0; + double hdop __attribute__((unused)) = 99.9; char ns = '?', ew = '?'; - if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &ashtech_time); } + if (bufptr && *(++bufptr) != ',') { ashtech_time = strtod(bufptr, &endp); bufptr = endp; } - if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &lat); } + if (bufptr && *(++bufptr) != ',') { lat = strtod(bufptr, &endp); bufptr = endp; } if (bufptr && *(++bufptr) != ',') { ns = *(bufptr++); } - if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &lon); } + if (bufptr && *(++bufptr) != ',') { lon = strtod(bufptr, &endp); bufptr = endp; } if (bufptr && *(++bufptr) != ',') { ew = *(bufptr++); } - if (bufptr && *(++bufptr) != ',') { bufptr = str_scanDec(bufptr, 0, 9, &fix_quality); } + if (bufptr && *(++bufptr) != ',') { fix_quality = strtol(bufptr, &endp, 10); bufptr = endp; } - if (bufptr && *(++bufptr) != ',') { bufptr = str_scanDec(bufptr, 0, 9, &num_of_sv); } + if (bufptr && *(++bufptr) != ',') { num_of_sv = strtol(bufptr, &endp, 10); bufptr = endp; } - if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &hdop); } + if (bufptr && *(++bufptr) != ',') { hdop = strtod(bufptr, &endp); bufptr = endp; } - if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &alt); } + if (bufptr && *(++bufptr) != ',') { alt = strtod(bufptr, &endp); bufptr = endp; } if (ns == 'S') { lat = -lat; @@ -287,43 +229,43 @@ int ASHTECH::handle_message(int len) *cc Checksum */ bufptr = (char *)(_rx_buffer + 10); - double ashtech_time = 0.0, lat = 0.0, lon = 0.0, alt = 0.0; - int num_of_sv = 0, fix_quality = 0; - double track_true = 0.0, ground_speed = 0.0 , age_of_corr = 0.0; - double hdop = 99.9, vdop = 99.9, pdop = 99.9, tdop = 99.9, vertic_vel = 0.0; + double ashtech_time __attribute__((unused)) = 0.0, lat = 0.0, lon = 0.0, alt = 0.0; + int num_of_sv __attribute__((unused)) = 0, fix_quality = 0; + double track_true = 0.0, ground_speed = 0.0 , age_of_corr __attribute__((unused)) = 0.0; + double hdop __attribute__((unused)) = 99.9, vdop __attribute__((unused)) = 99.9, pdop __attribute__((unused)) = 99.9, tdop __attribute__((unused)) = 99.9, vertic_vel = 0.0; char ns = '?', ew = '?'; - if (bufptr && *(++bufptr) != ',') { bufptr = str_scanDec(bufptr, 0, 9, &fix_quality); } + if (bufptr && *(++bufptr) != ',') { fix_quality = strtol(bufptr, &endp, 10); bufptr = endp; } - if (bufptr && *(++bufptr) != ',') { bufptr = str_scanDec(bufptr, 0, 9, &num_of_sv); } + if (bufptr && *(++bufptr) != ',') { num_of_sv = strtol(bufptr, &endp, 10); bufptr = endp; } - if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &ashtech_time); } + if (bufptr && *(++bufptr) != ',') { ashtech_time = strtod(bufptr, &endp); bufptr = endp; } - if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &lat); } + if (bufptr && *(++bufptr) != ',') { lat = strtod(bufptr, &endp); bufptr = endp; } if (bufptr && *(++bufptr) != ',') { ns = *(bufptr++); } - if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &lon); } + if (bufptr && *(++bufptr) != ',') { lon = strtod(bufptr, &endp); bufptr = endp; } if (bufptr && *(++bufptr) != ',') { ew = *(bufptr++); } - if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &alt); } + if (bufptr && *(++bufptr) != ',') { alt = strtod(bufptr, &endp); bufptr = endp; } - if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &age_of_corr); } + if (bufptr && *(++bufptr) != ',') { age_of_corr = strtod(bufptr, &endp); bufptr = endp; } - if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &track_true); } + if (bufptr && *(++bufptr) != ',') { track_true = strtod(bufptr, &endp); bufptr = endp; } - if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &ground_speed); } + if (bufptr && *(++bufptr) != ',') { ground_speed = strtod(bufptr, &endp); bufptr = endp; } - if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &vertic_vel); } + if (bufptr && *(++bufptr) != ',') { vertic_vel = strtod(bufptr, &endp); bufptr = endp; } - if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &pdop); } + if (bufptr && *(++bufptr) != ',') { pdop = strtod(bufptr, &endp); bufptr = endp; } - if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &hdop); } + if (bufptr && *(++bufptr) != ',') { hdop = strtod(bufptr, &endp); bufptr = endp; } - if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &vdop); } + if (bufptr && *(++bufptr) != ',') { vdop = strtod(bufptr, &endp); bufptr = endp; } - if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &tdop); } + if (bufptr && *(++bufptr) != ',') { tdop = strtod(bufptr, &endp); bufptr = endp; } if (ns == 'S') { lat = -lat; @@ -389,24 +331,24 @@ int ASHTECH::handle_message(int len) 8 Height 1 sigma error, in meters 9 The checksum data, always begins with * */ - double ashtech_time = 0.0, lat_err = 0.0, lon_err = 0.0, alt_err = 0.0; - double min_err = 0.0, maj_err = 0.0, deg_from_north = 0.0, rms_err = 0.0; + double ashtech_time __attribute__((unused)) = 0.0, lat_err = 0.0, lon_err = 0.0, alt_err = 0.0; + double min_err __attribute__((unused)) = 0.0, maj_err __attribute__((unused)) = 0.0, deg_from_north __attribute__((unused)) = 0.0, rms_err __attribute__((unused)) = 0.0; - if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &ashtech_time); } + if (bufptr && *(++bufptr) != ',') { ashtech_time = strtod(bufptr, &endp); bufptr = endp; } - if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &rms_err); } + if (bufptr && *(++bufptr) != ',') { rms_err = strtod(bufptr, &endp); bufptr = endp; } - if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &maj_err); } + if (bufptr && *(++bufptr) != ',') { maj_err = strtod(bufptr, &endp); bufptr = endp; } - if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &min_err); } + if (bufptr && *(++bufptr) != ',') { min_err = strtod(bufptr, &endp); bufptr = endp; } - if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, °_from_north); } + if (bufptr && *(++bufptr) != ',') { deg_from_north = strtod(bufptr, &endp); bufptr = endp; } - if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &lat_err); } + if (bufptr && *(++bufptr) != ',') { lat_err = strtod(bufptr, &endp); bufptr = endp; } - if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &lon_err); } + if (bufptr && *(++bufptr) != ',') { lon_err = strtod(bufptr, &endp); bufptr = endp; } - if (bufptr && *(++bufptr) != ',') { bufptr = scanFloat64(bufptr, 0, 9, 9, &alt_err); } + if (bufptr && *(++bufptr) != ',') { alt_err = strtod(bufptr, &endp); bufptr = endp; } _gps_position->eph = sqrt(lat_err * lat_err + lon_err * lon_err); _gps_position->epv = alt_err; @@ -448,7 +390,7 @@ int ASHTECH::handle_message(int len) bGPS = true; } - int all_msg_num, this_msg_num, tot_sv_visible; + int all_msg_num = 0, this_msg_num = 0, tot_sv_visible = 0; struct gsv_sat { int svid; int elevation; @@ -457,11 +399,11 @@ int ASHTECH::handle_message(int len) } sat[4]; memset(sat, 0, sizeof(sat)); - if (bufptr && *(++bufptr) != ',') { bufptr = str_scanDec(bufptr, 0, 9, &all_msg_num); } + if (bufptr && *(++bufptr) != ',') { all_msg_num = strtol(bufptr, &endp, 10); bufptr = endp; } - if (bufptr && *(++bufptr) != ',') { bufptr = str_scanDec(bufptr, 0, 9, &this_msg_num); } + if (bufptr && *(++bufptr) != ',') { this_msg_num = strtol(bufptr, &endp, 10); bufptr = endp; } - if (bufptr && *(++bufptr) != ',') { bufptr = str_scanDec(bufptr, 0, 9, &tot_sv_visible); } + if (bufptr && *(++bufptr) != ',') { tot_sv_visible = strtol(bufptr, &endp, 10); bufptr = endp; } if ((this_msg_num < 1) || (this_msg_num > all_msg_num)) { return 0; @@ -485,13 +427,13 @@ int ASHTECH::handle_message(int len) } for (int y = 0 ; y < end ; y++) { - if (bufptr && *(++bufptr) != ',') { bufptr = str_scanDec(bufptr, 0, 9, &sat[y].svid); } + if (bufptr && *(++bufptr) != ',') { sat[y].svid = strtol(bufptr, &endp, 10); bufptr = endp; } - if (bufptr && *(++bufptr) != ',') { bufptr = str_scanDec(bufptr, 0, 9, &sat[y].elevation); } + if (bufptr && *(++bufptr) != ',') { sat[y].elevation = strtol(bufptr, &endp, 10); bufptr = endp; } - if (bufptr && *(++bufptr) != ',') { bufptr = str_scanDec(bufptr, 0, 9, &sat[y].azimuth); } + if (bufptr && *(++bufptr) != ',') { sat[y].azimuth = strtol(bufptr, &endp, 10); bufptr = endp; } - if (bufptr && *(++bufptr) != ',') { bufptr = str_scanDec(bufptr, 0, 9, &sat[y].snr); } + if (bufptr && *(++bufptr) != ',') { sat[y].snr = strtol(bufptr, &endp, 10); bufptr = endp; } _satellite_info->svid[y + (this_msg_num - 1) * 4] = sat[y].svid; _satellite_info->used[y + (this_msg_num - 1) * 4] = ((sat[y].snr > 0) ? true : false); -- cgit v1.2.3 From fb6a68af70b280c7e5c2db9949c88cadcd1921ba Mon Sep 17 00:00:00 2001 From: Denis Yeldandi Date: Fri, 10 Oct 2014 17:51:18 +0400 Subject: Fixed issue #1382 - Ashtech driver is no longer checking fix status from comparing coordinates to 0.0;0.0;0.0, instead it's checking fix type in GGA or checking coordinate exsistance in POS. This removes compiler warning about float euqality comparison. - Fixed compiler warning about comparison between signed and unsigned int - Fixed compiler warning about class property masking --- src/drivers/gps/ashtech.cpp | 52 +++++++++++++++++++++++++++++++++++---------- src/drivers/gps/ashtech.h | 2 +- 2 files changed, 42 insertions(+), 12 deletions(-) diff --git a/src/drivers/gps/ashtech.cpp b/src/drivers/gps/ashtech.cpp index 1b706b5b2..a61c7196b 100644 --- a/src/drivers/gps/ashtech.cpp +++ b/src/drivers/gps/ashtech.cpp @@ -178,11 +178,20 @@ int ASHTECH::handle_message(int len) _gps_position->alt = alt * 1000; _rate_count_lat_lon++; - if ((lat == 0.0) && (lon == 0.0) && (alt == 0.0)) { + if (fix_quality == 0) { _gps_position->fix_type = 0; } else { - _gps_position->fix_type = 3 + fix_quality; + /* + * in this NMEA message float integers (value 5) mode has higher value than fixed integers (value 4), whereas it provides lower quality, + * and since value 3 is not being used, I "moved" value 5 to 3 to add it to _gps_position->fix_type + */ + if (fix_quality == 5) { fix_quality = 3; } + + /* + * fix quality 1 means just a normal 3D fix, so I'm subtracting 1 here. This way we'll have 3 for auto, 4 for DGPS, 5 for floats, 6 for fixed. + */ + _gps_position->fix_type = 3 + fix_quality - 1; } _gps_position->timestamp_position = hrt_absolute_time(); @@ -229,6 +238,11 @@ int ASHTECH::handle_message(int len) *cc Checksum */ bufptr = (char *)(_rx_buffer + 10); + + /* + * Ashtech would return empty space as coordinate (lat, lon or alt) if it doesn't have a fix yet + */ + int coordinatesFound = 0; double ashtech_time __attribute__((unused)) = 0.0, lat = 0.0, lon = 0.0, alt = 0.0; int num_of_sv __attribute__((unused)) = 0, fix_quality = 0; double track_true = 0.0, ground_speed = 0.0 , age_of_corr __attribute__((unused)) = 0.0; @@ -241,15 +255,31 @@ int ASHTECH::handle_message(int len) if (bufptr && *(++bufptr) != ',') { ashtech_time = strtod(bufptr, &endp); bufptr = endp; } - if (bufptr && *(++bufptr) != ',') { lat = strtod(bufptr, &endp); bufptr = endp; } + if (bufptr && *(++bufptr) != ',') { + /* + * if a coordinate is skipped (i.e. no fix), it either won't get into this block (two commas in a row) + * or strtod won't find anything and endp will point exactly where bufptr is. The same is for lon and alt. + */ + lat = strtod(bufptr, &endp); + if (bufptr != endp) {coordinatesFound++;} + bufptr = endp; + } if (bufptr && *(++bufptr) != ',') { ns = *(bufptr++); } - if (bufptr && *(++bufptr) != ',') { lon = strtod(bufptr, &endp); bufptr = endp; } + if (bufptr && *(++bufptr) != ',') { + lon = strtod(bufptr, &endp); + if (bufptr != endp) {coordinatesFound++;} + bufptr = endp; + } if (bufptr && *(++bufptr) != ',') { ew = *(bufptr++); } - if (bufptr && *(++bufptr) != ',') { alt = strtod(bufptr, &endp); bufptr = endp; } + if (bufptr && *(++bufptr) != ',') { + alt = strtod(bufptr, &endp); + if (bufptr != endp) {coordinatesFound++;} + bufptr = endp; + } if (bufptr && *(++bufptr) != ',') { age_of_corr = strtod(bufptr, &endp); bufptr = endp; } @@ -280,7 +310,7 @@ int ASHTECH::handle_message(int len) _gps_position->alt = alt * 1000; _rate_count_lat_lon++; - if ((lat == 0.0) && (lon == 0.0) && (alt == 0.0)) { + if (coordinatesFound < 3) { _gps_position->fix_type = 0; } else { @@ -461,12 +491,12 @@ int ASHTECH::receive(unsigned timeout) uint64_t time_started = hrt_absolute_time(); int j = 0; - ssize_t count = 0; + ssize_t bytes_count = 0; while (true) { /* pass received bytes to the packet decoder */ - while (j < count) { + while (j < bytes_count) { int l = 0; if ((l = parse_char(buf[j])) > 0) { @@ -486,7 +516,7 @@ int ASHTECH::receive(unsigned timeout) } /* everything is read */ - j = count = 0; + j = bytes_count = 0; /* then poll for new data */ int ret = ::poll(fds, sizeof(fds) / sizeof(fds[0]), timeout * 2); @@ -507,7 +537,7 @@ int ASHTECH::receive(unsigned timeout) * won't block even on a blocking device. If more bytes are * available, we'll go back to poll() again... */ - count = ::read(_fd, buf, sizeof(buf)); + bytes_count = ::read(_fd, buf, sizeof(buf)); } } } @@ -600,7 +630,7 @@ int ASHTECH::configure(unsigned &baudrate) const unsigned baudrates_to_try[] = {9600, 38400, 19200, 57600, 115200}; - for (int baud_i = 0; baud_i < sizeof(baudrates_to_try) / sizeof(baudrates_to_try[0]); baud_i++) { + for (unsigned int baud_i = 0; baud_i < sizeof(baudrates_to_try) / sizeof(baudrates_to_try[0]); baud_i++) { baudrate = baudrates_to_try[baud_i]; set_baudrate(_fd, baudrate); write(_fd, (uint8_t *)comm, sizeof(comm)); diff --git a/src/drivers/gps/ashtech.h b/src/drivers/gps/ashtech.h index 6e65d0b2b..6ba522b9c 100644 --- a/src/drivers/gps/ashtech.h +++ b/src/drivers/gps/ashtech.h @@ -68,7 +68,7 @@ class ASHTECH : public GPS_Helper char *_parse_pos; /** parse position */ bool _gsv_in_progress; /**< Indicates that gsv data parsing is in progress */ - /* int _satellites_count; /**< Number of satellites info parsed. */ + /* int _satellites_count; **< Number of satellites info parsed. */ uint8_t count; /**< Number of satellites in satellite info */ uint8_t svid[SAT_INFO_MAX_SATELLITES]; /**< Space vehicle ID [1..255], see scheme below */ uint8_t used[SAT_INFO_MAX_SATELLITES]; /**< 0: Satellite not used, 1: used for navigation */ -- cgit v1.2.3 From 686d3f4c7989f9b883b54fc26bb1974d10df98d3 Mon Sep 17 00:00:00 2001 From: Denis Yeldandi Date: Fri, 10 Oct 2014 18:42:24 +0400 Subject: Checking if fix status is less or equal to 0 rather than just equal --- src/drivers/gps/ashtech.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/gps/ashtech.cpp b/src/drivers/gps/ashtech.cpp index a61c7196b..a2e292de2 100644 --- a/src/drivers/gps/ashtech.cpp +++ b/src/drivers/gps/ashtech.cpp @@ -178,7 +178,7 @@ int ASHTECH::handle_message(int len) _gps_position->alt = alt * 1000; _rate_count_lat_lon++; - if (fix_quality == 0) { + if (fix_quality <= 0) { _gps_position->fix_type = 0; } else { -- cgit v1.2.3 From 73e9137865942b1c5970838ade92f9abb965f15d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 11 Oct 2014 00:28:14 +0200 Subject: Fix unsigned comparison --- src/drivers/sf0x/sf0x.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/sf0x/sf0x.cpp b/src/drivers/sf0x/sf0x.cpp index 801bcf40a..fdd524189 100644 --- a/src/drivers/sf0x/sf0x.cpp +++ b/src/drivers/sf0x/sf0x.cpp @@ -546,7 +546,7 @@ SF0X::collect() float si_units; bool valid = false; - for (unsigned i = 0; i < ret; i++) { + for (int i = 0; i < ret; i++) { if (OK == sf0x_parser(readbuf[i], _linebuf, &_linebuf_index, &_parse_state, &si_units)) { valid = true; } -- cgit v1.2.3 From b7b48047910d0e25d3349b7dade0b8d34649fdc3 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 11 Oct 2014 00:28:48 +0200 Subject: GEO: fix compile warnings --- src/lib/geo/geo.c | 17 ++++++++++------- 1 file changed, 10 insertions(+), 7 deletions(-) diff --git a/src/lib/geo/geo.c b/src/lib/geo/geo.c index 1c8d2a2a7..f595467b3 100644 --- a/src/lib/geo/geo.c +++ b/src/lib/geo/geo.c @@ -362,8 +362,12 @@ __EXPORT int get_distance_to_line(struct crosstrack_error_s *crosstrack_error, d crosstrack_error->distance = 0.0f; crosstrack_error->bearing = 0.0f; + dist_to_end = get_distance_to_next_waypoint(lat_now, lon_now, lat_end, lon_end); + // Return error if arguments are bad - if (lat_now == 0.0 || lon_now == 0.0 || lat_start == 0.0 || lon_start == 0.0 || lat_end == 0.0d || lon_end == 0.0d) { return return_value; } + if (dist_to_end < 0.1f) { + return ERROR; + } bearing_end = get_bearing_to_next_waypoint(lat_now, lon_now, lat_end, lon_end); bearing_track = get_bearing_to_next_waypoint(lat_start, lon_start, lat_end, lon_end); @@ -377,7 +381,6 @@ __EXPORT int get_distance_to_line(struct crosstrack_error_s *crosstrack_error, d return return_value; } - dist_to_end = get_distance_to_next_waypoint(lat_now, lon_now, lat_end, lon_end); crosstrack_error->distance = (dist_to_end) * sinf(bearing_diff); if (sin(bearing_diff) >= 0) { @@ -414,10 +417,10 @@ __EXPORT int get_distance_to_arc(struct crosstrack_error_s *crosstrack_error, do crosstrack_error->bearing = 0.0f; // Return error if arguments are bad - if (lat_now == 0.0 || lon_now == 0.0 || lat_center == 0.0 || lon_center == 0.0 || radius == 0.0f) { return return_value; } + if (radius < 0.1f) { return return_value; } - if (arc_sweep >= 0) { + if (arc_sweep >= 0.0f) { bearing_sector_start = arc_start_bearing; bearing_sector_end = arc_start_bearing + arc_sweep; @@ -463,8 +466,8 @@ __EXPORT int get_distance_to_arc(struct crosstrack_error_s *crosstrack_error, do double start_disp_x = (double)radius * sin(arc_start_bearing); double start_disp_y = (double)radius * cos(arc_start_bearing); - double end_disp_x = (double)radius * sin(_wrapPI((double)(arc_start_bearing + arc_sweep))); - double end_disp_y = (double)radius * cos(_wrapPI((double)(arc_start_bearing + arc_sweep))); + double end_disp_x = (double)radius * sin(_wrap_pi((double)(arc_start_bearing + arc_sweep))); + double end_disp_y = (double)radius * cos(_wrap_pi((double)(arc_start_bearing + arc_sweep))); double lon_start = lon_now + start_disp_x / 111111.0; double lat_start = lat_now + start_disp_y * cos(lat_now) / 111111.0; double lon_end = lon_now + end_disp_x / 111111.0; @@ -484,7 +487,7 @@ __EXPORT int get_distance_to_arc(struct crosstrack_error_s *crosstrack_error, do } - crosstrack_error->bearing = _wrapPI((double)crosstrack_error->bearing); + crosstrack_error->bearing = _wrap_pi((double)crosstrack_error->bearing); return_value = OK; return return_value; } -- cgit v1.2.3 From 835bab9115b49d90326a992917e8bd3e088e3fcd Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 12 Oct 2014 15:20:22 +0400 Subject: UAVCAN update --- uavcan | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/uavcan b/uavcan index 01d5bb242..4de033882 160000 --- a/uavcan +++ b/uavcan @@ -1 +1 @@ -Subproject commit 01d5bb242a6197e17e0ed466ed86e7ba42bd7d01 +Subproject commit 4de0338824972de4d3a8e29697ea1a2d95a926c0 -- cgit v1.2.3 From ced75deebe527581930419155fb0d73f18b7a5fe Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 12 Oct 2014 15:34:28 +0400 Subject: ESC scaling fix --- src/modules/uavcan/actuators/esc.cpp | 15 ++++++--------- src/modules/uavcan/actuators/esc.hpp | 2 +- 2 files changed, 7 insertions(+), 10 deletions(-) diff --git a/src/modules/uavcan/actuators/esc.cpp b/src/modules/uavcan/actuators/esc.cpp index 223d94731..b516536c7 100644 --- a/src/modules/uavcan/actuators/esc.cpp +++ b/src/modules/uavcan/actuators/esc.cpp @@ -93,25 +93,22 @@ void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs) */ uavcan::equipment::esc::RawCommand msg; + static const int cmd_max = uavcan::equipment::esc::RawCommand::FieldTypes::cmd::RawValueType::max(); + if (_armed) { for (unsigned i = 0; i < num_outputs; i++) { - float scaled = (outputs[i] + 1.0F) * 0.5F * uavcan::equipment::esc::RawCommand::CMD_MAX; + float scaled = (outputs[i] + 1.0F) * 0.5F * cmd_max; if (scaled < 1.0F) { scaled = 1.0F; // Since we're armed, we don't want to stop it completely } - if (scaled < uavcan::equipment::esc::RawCommand::CMD_MIN) { - scaled = uavcan::equipment::esc::RawCommand::CMD_MIN; - perf_count(_perfcnt_scaling_error); - } else if (scaled > uavcan::equipment::esc::RawCommand::CMD_MAX) { - scaled = uavcan::equipment::esc::RawCommand::CMD_MAX; + if (scaled > cmd_max) { + scaled = cmd_max; perf_count(_perfcnt_scaling_error); - } else { - ; // Correct value } - msg.cmd.push_back(static_cast(scaled)); + msg.cmd.push_back(static_cast(scaled)); } } diff --git a/src/modules/uavcan/actuators/esc.hpp b/src/modules/uavcan/actuators/esc.hpp index cf0988210..0add7e0ad 100644 --- a/src/modules/uavcan/actuators/esc.hpp +++ b/src/modules/uavcan/actuators/esc.hpp @@ -73,7 +73,7 @@ private: void orb_pub_timer_cb(const uavcan::TimerEvent &event); - static constexpr unsigned MAX_RATE_HZ = 100; ///< XXX make this configurable + static constexpr unsigned MAX_RATE_HZ = 200; ///< XXX make this configurable static constexpr unsigned ESC_STATUS_UPDATE_RATE_HZ = 5; static constexpr unsigned MAX_ESCS = uavcan::equipment::esc::RawCommand::FieldTypes::cmd::MaxSize; -- cgit v1.2.3 From e5a77a638a53aa9baec2ffe9d8ad96fb095b0966 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 12 Oct 2014 20:56:45 +0400 Subject: ESC status feedback --- src/modules/uavcan/actuators/esc.cpp | 29 ++++++++++++++++++++++++++--- src/modules/uavcan/actuators/esc.hpp | 15 +++++++-------- 2 files changed, 33 insertions(+), 11 deletions(-) diff --git a/src/modules/uavcan/actuators/esc.cpp b/src/modules/uavcan/actuators/esc.cpp index b516536c7..0601d9fa2 100644 --- a/src/modules/uavcan/actuators/esc.cpp +++ b/src/modules/uavcan/actuators/esc.cpp @@ -73,7 +73,7 @@ int UavcanEscController::init() void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs) { - if ((outputs == nullptr) || (num_outputs > MAX_ESCS)) { + if ((outputs == nullptr) || (num_outputs > uavcan::equipment::esc::RawCommand::FieldTypes::cmd::MaxSize)) { perf_count(_perfcnt_invalid_input); return; } @@ -126,10 +126,33 @@ void UavcanEscController::arm_esc(bool arm) void UavcanEscController::esc_status_sub_cb(const uavcan::ReceivedDataStructure &msg) { - // TODO save status into a local storage; publish to ORB later from orb_pub_timer_cb() + if (msg.esc_index < CONNECTED_ESC_MAX) { + _esc_status.esc_count = uavcan::max(_esc_status.esc_count, msg.esc_index + 1); + _esc_status.timestamp = msg.getMonotonicTimestamp().toUSec(); + + auto &ref = _esc_status.esc[msg.esc_index]; + + ref.esc_address = msg.getSrcNodeID().get(); + + // >0 checks allow to weed out NaNs and negative values that aren't supported. + ref.esc_voltage = (msg.voltage > 0) ? msg.voltage * 10.0F : 0; + ref.esc_current = (msg.current > 0) ? msg.current * 10.0F : 0; + ref.esc_temperature = (msg.temperature > 0) ? msg.temperature * 10.0F : 0; + + ref.esc_setpoint = msg.power_rating_pct; + ref.esc_rpm = abs(msg.rpm); + ref.esc_errorcount = msg.error_count; + } } void UavcanEscController::orb_pub_timer_cb(const uavcan::TimerEvent&) { - // TODO publish to ORB + _esc_status.counter += 1; + _esc_status.esc_connectiontype = ESC_CONNECTION_TYPE_CAN; + + if (_esc_status_pub > 0) { + (void)orb_publish(ORB_ID(esc_status), _esc_status_pub, &_esc_status); + } else { + _esc_status_pub = orb_advertise(ORB_ID(esc_status), &_esc_status); + } } diff --git a/src/modules/uavcan/actuators/esc.hpp b/src/modules/uavcan/actuators/esc.hpp index 0add7e0ad..f4a3877e6 100644 --- a/src/modules/uavcan/actuators/esc.hpp +++ b/src/modules/uavcan/actuators/esc.hpp @@ -48,6 +48,8 @@ #include #include #include +#include + class UavcanEscController { @@ -74,8 +76,7 @@ private: static constexpr unsigned MAX_RATE_HZ = 200; ///< XXX make this configurable - static constexpr unsigned ESC_STATUS_UPDATE_RATE_HZ = 5; - static constexpr unsigned MAX_ESCS = uavcan::equipment::esc::RawCommand::FieldTypes::cmd::MaxSize; + static constexpr unsigned ESC_STATUS_UPDATE_RATE_HZ = 10; typedef uavcan::MethodBinder&)> @@ -84,6 +85,10 @@ private: typedef uavcan::MethodBinder TimerCbBinder; + bool _armed = false; + esc_status_s _esc_status = {}; + orb_advert_t _esc_status_pub = -1; + /* * libuavcan related things */ @@ -93,12 +98,6 @@ private: uavcan::Subscriber _uavcan_sub_status; uavcan::TimerEventForwarder _orb_timer; - /* - * ESC states - */ - bool _armed = false; - uavcan::equipment::esc::Status _states[MAX_ESCS]; - /* * Perf counters */ -- cgit v1.2.3 From db0b892c23d56d6363ea91988d2a25e6b4d48bce Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Sun, 12 Oct 2014 11:38:34 -0700 Subject: Fix 8+ channel binding --- src/drivers/drv_pwm_output.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/drv_pwm_output.h b/src/drivers/drv_pwm_output.h index bc586f395..f66ec7c95 100644 --- a/src/drivers/drv_pwm_output.h +++ b/src/drivers/drv_pwm_output.h @@ -165,7 +165,7 @@ ORB_DECLARE(output_pwm); #define DSM2_BIND_PULSES 3 /* DSM_BIND_START ioctl parameter, pulses required to start dsm2 pairing */ #define DSMX_BIND_PULSES 7 /* DSM_BIND_START ioctl parameter, pulses required to start dsmx pairing */ -#define DSMX8_BIND_PULSES 10 /* DSM_BIND_START ioctl parameter, pulses required to start 8 or more channel dsmx pairing */ +#define DSMX8_BIND_PULSES 9 /* DSM_BIND_START ioctl parameter, pulses required to start 8 or more channel dsmx pairing */ /** power up DSM receiver */ #define DSM_BIND_POWER_UP _IOC(_PWM_SERVO_BASE, 11) -- cgit v1.2.3 From 1bf4270e3ee6f33f8adf0027c1a59f3fc0b35263 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Mon, 13 Oct 2014 17:01:34 +0400 Subject: Update ORB topic 'esc_status' --- src/drivers/hott/messages.cpp | 14 +++++++------- src/drivers/mkblctrl/mkblctrl.cpp | 6 +++--- src/modules/sdlog2/sdlog2_messages.h | 14 +++++++------- src/modules/uORB/topics/esc_status.h | 19 ++++++++++--------- src/modules/uavcan/actuators/esc.cpp | 14 ++++++-------- 5 files changed, 33 insertions(+), 34 deletions(-) diff --git a/src/drivers/hott/messages.cpp b/src/drivers/hott/messages.cpp index 086132573..f1b12b067 100644 --- a/src/drivers/hott/messages.cpp +++ b/src/drivers/hott/messages.cpp @@ -115,9 +115,9 @@ publish_gam_message(const uint8_t *buffer) esc.esc[0].esc_vendor = ESC_VENDOR_GRAUPNER_HOTT; esc.esc[0].esc_rpm = (uint16_t)((msg.rpm_H << 8) | (msg.rpm_L & 0xff)) * 10; - esc.esc[0].esc_temperature = msg.temperature1 - 20; - esc.esc[0].esc_voltage = (uint16_t)((msg.main_voltage_H << 8) | (msg.main_voltage_L & 0xff)); - esc.esc[0].esc_current = (uint16_t)((msg.current_H << 8) | (msg.current_L & 0xff)); + esc.esc[0].esc_temperature = static_cast(msg.temperature1) - 20.0F; + esc.esc[0].esc_voltage = static_cast((msg.main_voltage_H << 8) | (msg.main_voltage_L & 0xff)) * 0.1F; + esc.esc[0].esc_current = static_cast((msg.current_H << 8) | (msg.current_L & 0xff)) * 0.1F; /* announce the esc if needed, just publish else */ if (_esc_pub > 0) { @@ -186,18 +186,18 @@ build_gam_response(uint8_t *buffer, size_t *size) msg.gam_sensor_id = GAM_SENSOR_ID; msg.sensor_text_id = GAM_SENSOR_TEXT_ID; - msg.temperature1 = (uint8_t)(esc.esc[0].esc_temperature + 20); + msg.temperature1 = (uint8_t)(esc.esc[0].esc_temperature + 20.0F); msg.temperature2 = 20; // 0 deg. C. - uint16_t voltage = (uint16_t)(esc.esc[0].esc_voltage); + const uint16_t voltage = (uint16_t)(esc.esc[0].esc_voltage * 10.0F); msg.main_voltage_L = (uint8_t)voltage & 0xff; msg.main_voltage_H = (uint8_t)(voltage >> 8) & 0xff; - uint16_t current = (uint16_t)(esc.esc[0].esc_current); + const uint16_t current = (uint16_t)(esc.esc[0].esc_current * 10.0F); msg.current_L = (uint8_t)current & 0xff; msg.current_H = (uint8_t)(current >> 8) & 0xff; - uint16_t rpm = (uint16_t)(esc.esc[0].esc_rpm * 0.1f); + const uint16_t rpm = (uint16_t)(esc.esc[0].esc_rpm * 0.1f); msg.rpm_L = (uint8_t)rpm & 0xff; msg.rpm_H = (uint8_t)(rpm >> 8) & 0xff; diff --git a/src/drivers/mkblctrl/mkblctrl.cpp b/src/drivers/mkblctrl/mkblctrl.cpp index 3996b76a6..1055487cb 100644 --- a/src/drivers/mkblctrl/mkblctrl.cpp +++ b/src/drivers/mkblctrl/mkblctrl.cpp @@ -600,8 +600,8 @@ MK::task_main() esc.esc[i].esc_address = (uint8_t) BLCTRL_BASE_ADDR + i; esc.esc[i].esc_vendor = ESC_VENDOR_MIKROKOPTER; esc.esc[i].esc_version = (uint16_t) Motor[i].Version; - esc.esc[i].esc_voltage = (uint16_t) 0; - esc.esc[i].esc_current = (uint16_t) Motor[i].Current; + esc.esc[i].esc_voltage = 0.0F; + esc.esc[i].esc_current = static_cast(Motor[i].Current) * 0.1F; esc.esc[i].esc_rpm = (uint16_t) 0; esc.esc[i].esc_setpoint = (float) Motor[i].SetPoint_PX4; @@ -614,7 +614,7 @@ MK::task_main() esc.esc[i].esc_setpoint_raw = (uint16_t) Motor[i].SetPoint; } - esc.esc[i].esc_temperature = (uint16_t) Motor[i].Temperature; + esc.esc[i].esc_temperature = static_cast(Motor[i].Temperature); esc.esc[i].esc_state = (uint16_t) Motor[i].State; esc.esc[i].esc_errorcount = (uint16_t) 0; diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index d2845eb75..aeff4fd30 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -241,15 +241,15 @@ struct log_GPSP_s { #define LOG_ESC_MSG 18 struct log_ESC_s { uint16_t counter; - uint8_t esc_count; - uint8_t esc_connectiontype; - uint8_t esc_num; + uint8_t esc_count; + uint8_t esc_connectiontype; + uint8_t esc_num; uint16_t esc_address; uint16_t esc_version; - uint16_t esc_voltage; - uint16_t esc_current; + float esc_voltage; + float esc_current; uint16_t esc_rpm; - uint16_t esc_temperature; + float esc_temperature; float esc_setpoint; uint16_t esc_setpoint_raw; }; @@ -452,7 +452,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(FLOW, "hhfffBB", "RawX,RawY,CompX,CompY,Dist,Q,SensID"), LOG_FORMAT(GPOS, "LLfffffff", "Lat,Lon,Alt,VelN,VelE,VelD,EPH,EPV,TALT"), LOG_FORMAT(GPSP, "BLLffBfbf", "NavState,Lat,Lon,Alt,Yaw,Type,LoitR,LoitDir,PitMin"), - LOG_FORMAT(ESC, "HBBBHHHHHHfH", "count,nESC,Conn,N,Ver,Adr,Volt,Amp,RPM,Temp,SetP,SetPRAW"), + LOG_FORMAT(ESC, "HBBBHHffHffH", "count,nESC,Conn,N,Ver,Adr,Volt,Amp,RPM,Temp,SetP,SetPRAW"), LOG_FORMAT(GVSP, "fff", "VX,VY,VZ"), LOG_FORMAT(BATT, "ffff", "V,VFilt,C,Discharged"), LOG_FORMAT(DIST, "ffB", "Bottom,BottomRate,Flags"), diff --git a/src/modules/uORB/topics/esc_status.h b/src/modules/uORB/topics/esc_status.h index 628824efa..b82796908 100644 --- a/src/modules/uORB/topics/esc_status.h +++ b/src/modules/uORB/topics/esc_status.h @@ -78,6 +78,7 @@ enum ESC_CONNECTION_TYPE { /** * Electronic speed controller status. + * Unsupported float fields should be assigned NaN. */ struct esc_status_s { /* use of a counter and timestamp recommended (but not necessary) */ @@ -89,17 +90,17 @@ struct esc_status_s { enum ESC_CONNECTION_TYPE esc_connectiontype; /**< how ESCs connected to the system */ struct { - uint16_t esc_address; /**< Address of current ESC (in most cases 1-8 / must be set by driver) */ enum ESC_VENDOR esc_vendor; /**< Vendor of current ESC */ - uint16_t esc_version; /**< Version of current ESC - if supported */ - uint16_t esc_voltage; /**< Voltage measured from current ESC - if supported */ - uint16_t esc_current; /**< Current measured from current ESC (100mA steps) - if supported */ - uint16_t esc_rpm; /**< RPM measured from current ESC - if supported */ - uint16_t esc_temperature; /**< Temperature measured from current ESC - if supported */ - float esc_setpoint; /**< setpoint of current ESC */ + uint32_t esc_errorcount; /**< Number of reported errors by ESC - if supported */ + float esc_voltage; /**< Voltage measured from current ESC [V] - if supported */ + float esc_current; /**< Current measured from current ESC [A] - if supported */ + float esc_temperature; /**< Temperature measured from current ESC [degC] - if supported */ + float esc_setpoint; /**< setpoint of current ESC */ uint16_t esc_setpoint_raw; /**< setpoint of current ESC (Value sent to ESC) */ - uint16_t esc_state; /**< State of ESC - depend on Vendor */ - uint16_t esc_errorcount; /**< Number of reported errors by ESC - if supported */ + uint16_t esc_address; /**< Address of current ESC (in most cases 1-8 / must be set by driver) */ + uint16_t esc_version; /**< Version of current ESC - if supported */ + uint16_t esc_rpm; /**< RPM measured from current ESC [RPM] - if supported */ + uint16_t esc_state; /**< State of ESC - depend on Vendor */ } esc[CONNECTED_ESC_MAX]; }; diff --git a/src/modules/uavcan/actuators/esc.cpp b/src/modules/uavcan/actuators/esc.cpp index 0601d9fa2..249ffba07 100644 --- a/src/modules/uavcan/actuators/esc.cpp +++ b/src/modules/uavcan/actuators/esc.cpp @@ -134,14 +134,12 @@ void UavcanEscController::esc_status_sub_cb(const uavcan::ReceivedDataStructure< ref.esc_address = msg.getSrcNodeID().get(); - // >0 checks allow to weed out NaNs and negative values that aren't supported. - ref.esc_voltage = (msg.voltage > 0) ? msg.voltage * 10.0F : 0; - ref.esc_current = (msg.current > 0) ? msg.current * 10.0F : 0; - ref.esc_temperature = (msg.temperature > 0) ? msg.temperature * 10.0F : 0; - - ref.esc_setpoint = msg.power_rating_pct; - ref.esc_rpm = abs(msg.rpm); - ref.esc_errorcount = msg.error_count; + ref.esc_voltage = msg.voltage; + ref.esc_current = msg.current; + ref.esc_temperature = msg.temperature; + ref.esc_setpoint = msg.power_rating_pct; + ref.esc_rpm = abs(msg.rpm); + ref.esc_errorcount = msg.error_count; } } -- cgit v1.2.3 From ecd144b8b4c334f4eb7a0e39e47065d6958fefaf Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Mon, 13 Oct 2014 17:07:14 +0400 Subject: Publishing esc_setpoint_raw from the UAVCAN driver --- src/modules/uavcan/actuators/esc.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/uavcan/actuators/esc.cpp b/src/modules/uavcan/actuators/esc.cpp index 249ffba07..08e504947 100644 --- a/src/modules/uavcan/actuators/esc.cpp +++ b/src/modules/uavcan/actuators/esc.cpp @@ -109,6 +109,8 @@ void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs) } msg.cmd.push_back(static_cast(scaled)); + + _esc_status.esc[i].esc_setpoint_raw = abs(static_cast(scaled)); } } -- cgit v1.2.3 From 6ec338c23f0b97d998401346ed9fa35cccecf0ae Mon Sep 17 00:00:00 2001 From: James Harrison Date: Tue, 14 Oct 2014 22:18:51 +0100 Subject: Fix px_romfs_pruner.py to skip .data files The .data extension was missing but used in some places; this would cause the pruner to fail with a UnicodeDecodeError. --- Tools/px_romfs_pruner.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Tools/px_romfs_pruner.py b/Tools/px_romfs_pruner.py index fcc40b09e..aef1cc7a3 100644 --- a/Tools/px_romfs_pruner.py +++ b/Tools/px_romfs_pruner.py @@ -57,7 +57,7 @@ def main(): for (root, dirs, files) in os.walk(args.folder): for file in files: # only prune text files - if ".zip" in file or ".bin" in file or ".swp" in file: + if ".zip" in file or ".bin" in file or ".swp" in file or ".data" in file: continue file_path = os.path.join(root, file) -- cgit v1.2.3 From f9856c6228396ca2fe8eb4c86e12c60df14d6b4c Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Wed, 15 Oct 2014 13:46:16 +0400 Subject: ESC status - supporting negative RPM --- src/modules/sdlog2/sdlog2_messages.h | 4 ++-- src/modules/uORB/topics/esc_status.h | 2 +- src/modules/uavcan/actuators/esc.cpp | 2 +- 3 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index aeff4fd30..d49fc0c79 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -248,7 +248,7 @@ struct log_ESC_s { uint16_t esc_version; float esc_voltage; float esc_current; - uint16_t esc_rpm; + int32_t esc_rpm; float esc_temperature; float esc_setpoint; uint16_t esc_setpoint_raw; @@ -452,7 +452,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(FLOW, "hhfffBB", "RawX,RawY,CompX,CompY,Dist,Q,SensID"), LOG_FORMAT(GPOS, "LLfffffff", "Lat,Lon,Alt,VelN,VelE,VelD,EPH,EPV,TALT"), LOG_FORMAT(GPSP, "BLLffBfbf", "NavState,Lat,Lon,Alt,Yaw,Type,LoitR,LoitDir,PitMin"), - LOG_FORMAT(ESC, "HBBBHHffHffH", "count,nESC,Conn,N,Ver,Adr,Volt,Amp,RPM,Temp,SetP,SetPRAW"), + LOG_FORMAT(ESC, "HBBBHHffiffH", "count,nESC,Conn,N,Ver,Adr,Volt,Amp,RPM,Temp,SetP,SetPRAW"), LOG_FORMAT(GVSP, "fff", "VX,VY,VZ"), LOG_FORMAT(BATT, "ffff", "V,VFilt,C,Discharged"), LOG_FORMAT(DIST, "ffB", "Bottom,BottomRate,Flags"), diff --git a/src/modules/uORB/topics/esc_status.h b/src/modules/uORB/topics/esc_status.h index b82796908..98cdad3d5 100644 --- a/src/modules/uORB/topics/esc_status.h +++ b/src/modules/uORB/topics/esc_status.h @@ -99,7 +99,7 @@ struct esc_status_s { uint16_t esc_setpoint_raw; /**< setpoint of current ESC (Value sent to ESC) */ uint16_t esc_address; /**< Address of current ESC (in most cases 1-8 / must be set by driver) */ uint16_t esc_version; /**< Version of current ESC - if supported */ - uint16_t esc_rpm; /**< RPM measured from current ESC [RPM] - if supported */ + int32_t esc_rpm; /**< Motor RPM, negative for reverse rotation [RPM] - if supported */ uint16_t esc_state; /**< State of ESC - depend on Vendor */ } esc[CONNECTED_ESC_MAX]; diff --git a/src/modules/uavcan/actuators/esc.cpp b/src/modules/uavcan/actuators/esc.cpp index 08e504947..1990651ef 100644 --- a/src/modules/uavcan/actuators/esc.cpp +++ b/src/modules/uavcan/actuators/esc.cpp @@ -140,7 +140,7 @@ void UavcanEscController::esc_status_sub_cb(const uavcan::ReceivedDataStructure< ref.esc_current = msg.current; ref.esc_temperature = msg.temperature; ref.esc_setpoint = msg.power_rating_pct; - ref.esc_rpm = abs(msg.rpm); + ref.esc_rpm = msg.rpm; ref.esc_errorcount = msg.error_count; } } -- cgit v1.2.3 From 7fd3bd9af8ee150911a08e75a1e30fa4cb2b8e2d Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Wed, 15 Oct 2014 14:01:17 +0400 Subject: esc_status layout optimization --- src/modules/uORB/topics/esc_status.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/uORB/topics/esc_status.h b/src/modules/uORB/topics/esc_status.h index 98cdad3d5..b45c49788 100644 --- a/src/modules/uORB/topics/esc_status.h +++ b/src/modules/uORB/topics/esc_status.h @@ -92,6 +92,7 @@ struct esc_status_s { struct { enum ESC_VENDOR esc_vendor; /**< Vendor of current ESC */ uint32_t esc_errorcount; /**< Number of reported errors by ESC - if supported */ + int32_t esc_rpm; /**< Motor RPM, negative for reverse rotation [RPM] - if supported */ float esc_voltage; /**< Voltage measured from current ESC [V] - if supported */ float esc_current; /**< Current measured from current ESC [A] - if supported */ float esc_temperature; /**< Temperature measured from current ESC [degC] - if supported */ @@ -99,7 +100,6 @@ struct esc_status_s { uint16_t esc_setpoint_raw; /**< setpoint of current ESC (Value sent to ESC) */ uint16_t esc_address; /**< Address of current ESC (in most cases 1-8 / must be set by driver) */ uint16_t esc_version; /**< Version of current ESC - if supported */ - int32_t esc_rpm; /**< Motor RPM, negative for reverse rotation [RPM] - if supported */ uint16_t esc_state; /**< State of ESC - depend on Vendor */ } esc[CONNECTED_ESC_MAX]; -- cgit v1.2.3 From 0d917576d484d2e2dc0233c5545a16e36f6e2f41 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 15 Oct 2014 22:19:06 +0200 Subject: Enable flaps in manual override --- src/drivers/px4io/px4io.cpp | 9 ++++++++- src/modules/px4iofirmware/controls.c | 14 +++++++++++--- 2 files changed, 19 insertions(+), 4 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index d212be766..73160b2d9 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1265,11 +1265,18 @@ PX4IO::io_set_rc_config() if ((ichan >= 0) && (ichan < (int)_max_rc_input)) input_map[ichan - 1] = 3; - param_get(param_find("RC_MAP_MODE_SW"), &ichan); + param_get(param_find("RC_MAP_FLAPS"), &ichan); if ((ichan >= 0) && (ichan < (int)_max_rc_input)) input_map[ichan - 1] = 4; + param_get(param_find("RC_MAP_MODE_SW"), &ichan); + + if ((ichan >= 0) && (ichan < (int)_max_rc_input)) { + /* use out of normal bounds index to indicate special channel */ + input_map[ichan - 1] = 8; + } + /* * Iterate all possible RC inputs. */ diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c index 7b127759a..0b0832d55 100644 --- a/src/modules/px4iofirmware/controls.c +++ b/src/modules/px4iofirmware/controls.c @@ -60,6 +60,8 @@ static perf_counter_t c_gather_ppm; static int _dsm_fd; +static uint16_t rc_value_override = 0; + bool dsm_port_input(uint16_t *rssi, bool *dsm_updated, bool *st24_updated) { perf_begin(c_gather_dsm); @@ -313,8 +315,14 @@ controls_tick() { } } - r_rc_values[mapped] = SIGNED_TO_REG(scaled); - assigned_channels |= (1 << mapped); + /* pick out override channel, indicated by mapping 8 (9 th channel, virtual) */ + if (mapped == 8) { + rc_value_override = SIGNED_TO_REG(scaled); + } else { + /* normal channel */ + r_rc_values[mapped] = SIGNED_TO_REG(scaled); + assigned_channels |= (1 << mapped); + } } } @@ -409,7 +417,7 @@ controls_tick() { * requested override. * */ - if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) && (REG_TO_SIGNED(r_rc_values[4]) < RC_CHANNEL_LOW_THRESH)) + if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) && (REG_TO_SIGNED(rc_value_override) < RC_CHANNEL_LOW_THRESH)) override = true; if (override) { -- cgit v1.2.3 From f4903316329ca91b9e872327f701de5b6a2cc13c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 16 Oct 2014 22:57:27 +0200 Subject: Enable flaps, avoid mode switch position --- src/drivers/px4io/px4io.cpp | 2 +- src/modules/px4iofirmware/controls.c | 13 +++++-------- src/modules/px4iofirmware/mixer.cpp | 7 ------- src/modules/px4iofirmware/protocol.h | 1 + src/modules/px4iofirmware/registers.c | 3 ++- 5 files changed, 9 insertions(+), 17 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 73160b2d9..3871b4a2c 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1274,7 +1274,7 @@ PX4IO::io_set_rc_config() if ((ichan >= 0) && (ichan < (int)_max_rc_input)) { /* use out of normal bounds index to indicate special channel */ - input_map[ichan - 1] = 8; + input_map[ichan - 1] = PX4IO_P_RC_CONFIG_ASSIGNMENT_MODESWITCH; } /* diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c index 0b0832d55..ad60ee03e 100644 --- a/src/modules/px4iofirmware/controls.c +++ b/src/modules/px4iofirmware/controls.c @@ -315,15 +315,12 @@ controls_tick() { } } - /* pick out override channel, indicated by mapping 8 (9 th channel, virtual) */ - if (mapped == 8) { - rc_value_override = SIGNED_TO_REG(scaled); - } else { - /* normal channel */ - r_rc_values[mapped] = SIGNED_TO_REG(scaled); - assigned_channels |= (1 << mapped); - } + r_rc_values[mapped] = SIGNED_TO_REG(scaled); + assigned_channels |= (1 << mapped); + } else if (mapped == PX4IO_P_RC_CONFIG_ASSIGNMENT_MODESWITCH) { + /* pick out override channel, indicated by special mapping */ + rc_value_override = SIGNED_TO_REG(scaled); } } } diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index 3e19333d8..c0b8ac358 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -60,13 +60,6 @@ extern "C" { */ #define FMU_INPUT_DROP_LIMIT_US 500000 -/* XXX need to move the RC_CHANNEL_FUNCTION out of rc_channels.h and into systemlib */ -#define ROLL 0 -#define PITCH 1 -#define YAW 2 -#define THROTTLE 3 -#define OVERRIDE 4 - /* current servo arm/disarm state */ static bool mixer_servos_armed = false; static bool should_arm = false; diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h index 89a470b44..a3e8a58d3 100644 --- a/src/modules/px4iofirmware/protocol.h +++ b/src/modules/px4iofirmware/protocol.h @@ -246,6 +246,7 @@ enum { /* DSM bind states */ #define PX4IO_P_RC_CONFIG_MAX 2 /**< highest input value */ #define PX4IO_P_RC_CONFIG_DEADZONE 3 /**< band around center that is ignored */ #define PX4IO_P_RC_CONFIG_ASSIGNMENT 4 /**< mapped input value */ +#define PX4IO_P_RC_CONFIG_ASSIGNMENT_MODESWITCH 100 /**< magic value for mode switch */ #define PX4IO_P_RC_CONFIG_OPTIONS 5 /**< channel options bitmask */ #define PX4IO_P_RC_CONFIG_OPTIONS_ENABLED (1 << 0) #define PX4IO_P_RC_CONFIG_OPTIONS_REVERSE (1 << 1) diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index 7a5a5e484..7f19e983f 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -686,7 +686,8 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) if (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] == UINT8_MAX) { disabled = true; - } else if (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] >= PX4IO_RC_MAPPED_CONTROL_CHANNELS) { + } else if ((conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] >= PX4IO_RC_MAPPED_CONTROL_CHANNELS) && + (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] != PX4IO_P_RC_CONFIG_ASSIGNMENT_MODESWITCH)) { count++; } -- cgit v1.2.3 From 5bc2b34e482fe8c4b0cab8f9748bd97dc3e17291 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 17 Oct 2014 09:37:21 +0200 Subject: Reset performance counters on arming to allow better resolution during flight --- src/modules/sdlog2/sdlog2.c | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index e13593077..1282bc0ea 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -628,6 +628,9 @@ void sdlog2_start_log() perf_print_all(perf_fd); close(perf_fd); + /* reset performance counters to get in-flight min and max values in post flight log */ + perf_reset_all(); + logging_enabled = true; } -- cgit v1.2.3 From f500ad4699da9b56ecfd0e413532119577df837d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 17 Oct 2014 09:38:04 +0200 Subject: Log messages sent via MAVLink --- src/modules/mavlink/mavlink_messages.cpp | 29 +++++++++++++++++++++++++++++ 1 file changed, 29 insertions(+) diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index c182cfdb9..a4ed14914 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -341,11 +341,18 @@ private: /* do not allow top copying this class */ MavlinkStreamStatustext(MavlinkStreamStatustext &); MavlinkStreamStatustext& operator = (const MavlinkStreamStatustext &); + FILE *fp = nullptr; protected: explicit MavlinkStreamStatustext(Mavlink *mavlink) : MavlinkStream(mavlink) {} + ~MavlinkStreamStatustext() { + if (fp) { + fclose(fp); + } + } + void send(const hrt_abstime t) { if (!mavlink_logbuffer_is_empty(_mavlink->get_logbuffer())) { @@ -359,6 +366,28 @@ protected: strncpy(msg.text, logmsg.text, sizeof(msg.text)); _mavlink->send_message(MAVLINK_MSG_ID_STATUSTEXT, &msg); + + if (fp) { + fputs(msg.text, fp); + fputs("\n", fp); + fsync(fileno(fp)); + } else { + /* string to hold the path to the log */ + char log_file_name[32] = ""; + char log_file_path[64] = ""; + + timespec ts; + clock_gettime(CLOCK_REALTIME, &ts); + /* use GPS time for log file naming, e.g. /fs/microsd/2014-01-19/19_37_52.bin */ + time_t gps_time_sec = ts.tv_sec + (ts.tv_nsec / 1e9); + struct tm t; + gmtime_r(&gps_time_sec, &t); + + // XXX we do not want to interfere here with the SD log app + strftime(log_file_name, sizeof(log_file_name), "msgs_%Y_%m_%d_%H_%M_%S.txt", &t); + snprintf(log_file_path, sizeof(log_file_path), "/fs/microsd/%s", log_file_name); + fp = fopen(log_file_path, "ab"); + } } } } -- cgit v1.2.3 From a1ea16f7947bedb258ebc198cde52e5976ed0fdd Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 17 Oct 2014 09:39:50 +0200 Subject: Log text messages only in first instance --- src/modules/mavlink/mavlink_messages.cpp | 43 +++++++++++++++++--------------- 1 file changed, 23 insertions(+), 20 deletions(-) diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index a4ed14914..cccb698bf 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -367,26 +367,29 @@ protected: _mavlink->send_message(MAVLINK_MSG_ID_STATUSTEXT, &msg); - if (fp) { - fputs(msg.text, fp); - fputs("\n", fp); - fsync(fileno(fp)); - } else { - /* string to hold the path to the log */ - char log_file_name[32] = ""; - char log_file_path[64] = ""; - - timespec ts; - clock_gettime(CLOCK_REALTIME, &ts); - /* use GPS time for log file naming, e.g. /fs/microsd/2014-01-19/19_37_52.bin */ - time_t gps_time_sec = ts.tv_sec + (ts.tv_nsec / 1e9); - struct tm t; - gmtime_r(&gps_time_sec, &t); - - // XXX we do not want to interfere here with the SD log app - strftime(log_file_name, sizeof(log_file_name), "msgs_%Y_%m_%d_%H_%M_%S.txt", &t); - snprintf(log_file_path, sizeof(log_file_path), "/fs/microsd/%s", log_file_name); - fp = fopen(log_file_path, "ab"); + /* write log messages in first instance to disk */ + if (_mavlink->get_instance_id() == 0) { + if (fp) { + fputs(msg.text, fp); + fputs("\n", fp); + fsync(fileno(fp)); + } else { + /* string to hold the path to the log */ + char log_file_name[32] = ""; + char log_file_path[64] = ""; + + timespec ts; + clock_gettime(CLOCK_REALTIME, &ts); + /* use GPS time for log file naming, e.g. /fs/microsd/2014-01-19/19_37_52.bin */ + time_t gps_time_sec = ts.tv_sec + (ts.tv_nsec / 1e9); + struct tm t; + gmtime_r(&gps_time_sec, &t); + + // XXX we do not want to interfere here with the SD log app + strftime(log_file_name, sizeof(log_file_name), "msgs_%Y_%m_%d_%H_%M_%S.txt", &t); + snprintf(log_file_path, sizeof(log_file_path), "/fs/microsd/%s", log_file_name); + fp = fopen(log_file_path, "ab"); + } } } } -- cgit v1.2.3 From 99bfbb6dc3c2d8f60b356945026e586397d66170 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 14 Oct 2014 10:07:40 +1100 Subject: ll40ls: add last distance in "ll40ls info" output --- src/drivers/ll40ls/ll40ls.cpp | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/src/drivers/ll40ls/ll40ls.cpp b/src/drivers/ll40ls/ll40ls.cpp index a69e6ee55..7fd023ec4 100644 --- a/src/drivers/ll40ls/ll40ls.cpp +++ b/src/drivers/ll40ls/ll40ls.cpp @@ -132,6 +132,7 @@ private: perf_counter_t _sample_perf; perf_counter_t _comms_errors; perf_counter_t _buffer_overflows; + uint16_t _last_distance; /** * Test whether the device supported by the driver is present at a @@ -200,7 +201,8 @@ LL40LS::LL40LS(int bus, int address) : _range_finder_topic(-1), _sample_perf(perf_alloc(PC_ELAPSED, "ll40ls_read")), _comms_errors(perf_alloc(PC_COUNT, "ll40ls_comms_errors")), - _buffer_overflows(perf_alloc(PC_COUNT, "ll40ls_buffer_overflows")) + _buffer_overflows(perf_alloc(PC_COUNT, "ll40ls_buffer_overflows")), + _last_distance(0) { // up the retries since the device misses the first measure attempts I2C::_retries = 3; @@ -521,6 +523,8 @@ LL40LS::collect() float si_units = distance * 0.01f; /* cm to m */ struct range_finder_report report; + _last_distance = distance; + /* this should be fairly close to the end of the measurement, so the best approximation of the time */ report.timestamp = hrt_absolute_time(); report.error_count = perf_event_count(_comms_errors); @@ -648,6 +652,8 @@ LL40LS::print_info() perf_print_counter(_buffer_overflows); printf("poll interval: %u ticks\n", _measure_ticks); _reports->print_info("report queue"); + printf("distance: %ucm (0x%04x)\n", + (unsigned)_last_distance, (unsigned)_last_distance); } /** -- cgit v1.2.3 From c6ada17f685fc16c0882d890be089c23a49b7390 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 20 Oct 2014 07:05:58 +1100 Subject: ll40ls: support either internal or external I2C bus --- src/drivers/ll40ls/ll40ls.cpp | 189 +++++++++++++++++++++++++++++------------- 1 file changed, 132 insertions(+), 57 deletions(-) diff --git a/src/drivers/ll40ls/ll40ls.cpp b/src/drivers/ll40ls/ll40ls.cpp index 7fd023ec4..8b156f7ba 100644 --- a/src/drivers/ll40ls/ll40ls.cpp +++ b/src/drivers/ll40ls/ll40ls.cpp @@ -74,7 +74,8 @@ /* Configuration Constants */ #define LL40LS_BUS PX4_I2C_BUS_EXPANSION #define LL40LS_BASEADDR 0x42 /* 7-bit address */ -#define LL40LS_DEVICE_PATH "/dev/ll40ls" +#define LL40LS_DEVICE_PATH_INT "/dev/ll40ls_int" +#define LL40LS_DEVICE_PATH_EXT "/dev/ll40ls_ext" /* LL40LS Registers addresses */ @@ -101,7 +102,7 @@ static const int ERROR = -1; class LL40LS : public device::I2C { public: - LL40LS(int bus = LL40LS_BUS, int address = LL40LS_BASEADDR); + LL40LS(int bus, const char *path, int address = LL40LS_BASEADDR); virtual ~LL40LS(); virtual int init(); @@ -134,6 +135,9 @@ private: perf_counter_t _buffer_overflows; uint16_t _last_distance; + /**< the bus the device is connected to */ + int _bus; + /** * Test whether the device supported by the driver is present at a * specific address. @@ -189,8 +193,8 @@ private: */ extern "C" __EXPORT int ll40ls_main(int argc, char *argv[]); -LL40LS::LL40LS(int bus, int address) : - I2C("LL40LS", LL40LS_DEVICE_PATH, bus, address, 100000), +LL40LS::LL40LS(int bus, const char *path, int address) : + I2C("LL40LS", path, bus, address, 100000), _min_distance(LL40LS_MIN_DISTANCE), _max_distance(LL40LS_MAX_DISTANCE), _reports(nullptr), @@ -202,7 +206,8 @@ LL40LS::LL40LS(int bus, int address) : _sample_perf(perf_alloc(PC_ELAPSED, "ll40ls_read")), _comms_errors(perf_alloc(PC_COUNT, "ll40ls_comms_errors")), _buffer_overflows(perf_alloc(PC_COUNT, "ll40ls_buffer_overflows")), - _last_distance(0) + _last_distance(0), + _bus(bus) { // up the retries since the device misses the first measure attempts I2C::_retries = 3; @@ -668,55 +673,89 @@ namespace ll40ls #endif const int ERROR = -1; -LL40LS *g_dev; +LL40LS *g_dev_int; +LL40LS *g_dev_ext; -void start(); -void stop(); -void test(); -void reset(); -void info(); +void start(int bus); +void stop(int bus); +void test(int bus); +void reset(int bus); +void info(int bus); +void usage(); /** * Start the driver. */ void -start() +start(int bus) { - int fd; - - if (g_dev != nullptr) { - errx(1, "already started"); + /* create the driver, attempt expansion bus first */ + if (bus == -1 || bus == PX4_I2C_BUS_EXPANSION) { + if (g_dev_ext != nullptr) + errx(0, "already started external"); + g_dev_ext = new LL40LS(PX4_I2C_BUS_EXPANSION, LL40LS_DEVICE_PATH_EXT); + if (g_dev_ext != nullptr && OK != g_dev_ext->init()) { + delete g_dev_ext; + g_dev_ext = nullptr; + } } - /* create the driver */ - g_dev = new LL40LS(LL40LS_BUS); - - if (g_dev == nullptr) { - goto fail; - } +#ifdef PX4_I2C_BUS_ONBOARD + /* if this failed, attempt onboard sensor */ + if (bus == -1 || bus == PX4_I2C_BUS_ONBOARD) { + if (g_dev_int != nullptr) + errx(0, "already started internal"); + g_dev_int = new LL40LS(PX4_I2C_BUS_ONBOARD, LL40LS_DEVICE_PATH_INT); + if (g_dev_int != nullptr && OK != g_dev_int->init()) { + /* tear down the failing onboard instance */ + delete g_dev_int; + g_dev_int = nullptr; - if (OK != g_dev->init()) { - goto fail; + if (bus == PX4_I2C_BUS_ONBOARD) { + goto fail; + } + } + if (g_dev_int == nullptr && bus == PX4_I2C_BUS_ONBOARD) { + goto fail; + } } +#endif /* set the poll rate to default, starts automatic data collection */ - fd = open(LL40LS_DEVICE_PATH, O_RDONLY); - - if (fd < 0) { - goto fail; - } - - if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { - goto fail; - } + if (g_dev_int != nullptr) { + int fd = open(LL40LS_DEVICE_PATH_INT, O_RDONLY); + if (fd == -1) { + goto fail; + } + int ret = ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT); + close(fd); + if (ret < 0) { + goto fail; + } + } + + if (g_dev_ext != nullptr) { + int fd = open(LL40LS_DEVICE_PATH_EXT, O_RDONLY); + if (fd == -1) { + goto fail; + } + int ret = ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT); + close(fd); + if (ret < 0) { + goto fail; + } + } exit(0); fail: - - if (g_dev != nullptr) { - delete g_dev; - g_dev = nullptr; + if (g_dev_int != nullptr && (bus == -1 || bus == PX4_I2C_BUS_ONBOARD)) { + delete g_dev_int; + g_dev_int = nullptr; + } + if (g_dev_ext != nullptr && (bus == -1 || bus == PX4_I2C_BUS_EXPANSION)) { + delete g_dev_ext; + g_dev_ext = nullptr; } errx(1, "driver start failed"); @@ -725,11 +764,12 @@ fail: /** * Stop the driver */ -void stop() +void stop(int bus) { - if (g_dev != nullptr) { - delete g_dev; - g_dev = nullptr; + LL40LS **g_dev = (bus == PX4_I2C_BUS_ONBOARD?&g_dev_int:&g_dev_ext); + if (*g_dev != nullptr) { + delete *g_dev; + *g_dev = nullptr; } else { errx(1, "driver not running"); @@ -744,16 +784,17 @@ void stop() * and automatic modes. */ void -test() +test(int bus) { struct range_finder_report report; ssize_t sz; int ret; + const char *path = (bus==PX4_I2C_BUS_ONBOARD?LL40LS_DEVICE_PATH_INT:LL40LS_DEVICE_PATH_EXT); - int fd = open(LL40LS_DEVICE_PATH, O_RDONLY); + int fd = open(path, O_RDONLY); if (fd < 0) { - err(1, "%s open failed (try 'll40ls start' if the driver is not running", LL40LS_DEVICE_PATH); + err(1, "%s open failed (try 'll40ls start' if the driver is not running", path); } /* do a simple demand read */ @@ -809,9 +850,10 @@ test() * Reset the driver. */ void -reset() +reset(int bus) { - int fd = open(LL40LS_DEVICE_PATH, O_RDONLY); + const char *path = (bus==PX4_I2C_BUS_ONBOARD?LL40LS_DEVICE_PATH_INT:LL40LS_DEVICE_PATH_EXT); + int fd = open(path, O_RDONLY); if (fd < 0) { err(1, "failed "); @@ -832,8 +874,9 @@ reset() * Print a little info about the driver. */ void -info() +info(int bus) { + LL40LS *g_dev = (bus == PX4_I2C_BUS_ONBOARD?g_dev_int:g_dev_ext); if (g_dev == nullptr) { errx(1, "driver not running"); } @@ -844,44 +887,76 @@ info() exit(0); } +void +usage() +{ + warnx("missing command: try 'start', 'stop', 'info', 'test', 'reset', 'info'"); + warnx("options:"); + warnx(" -X only external bus"); +#ifdef PX4_I2C_BUS_ONBOARD + warnx(" -I only internal bus"); +#endif +} + } // namespace int ll40ls_main(int argc, char *argv[]) { + int ch; + int bus = -1; + + while ((ch = getopt(argc, argv, "XI")) != EOF) { + switch (ch) { +#ifdef PX4_I2C_BUS_ONBOARD + case 'I': + bus = PX4_I2C_BUS_ONBOARD; + break; +#endif + case 'X': + bus = PX4_I2C_BUS_EXPANSION; + break; + default: + ll40ls::usage(); + exit(0); + } + } + + const char *verb = argv[optind]; + /* * Start/load the driver. */ - if (!strcmp(argv[1], "start")) { - ll40ls::start(); + if (!strcmp(verb, "start")) { + ll40ls::start(bus); } /* * Stop the driver */ - if (!strcmp(argv[1], "stop")) { - ll40ls::stop(); + if (!strcmp(verb, "stop")) { + ll40ls::stop(bus); } /* * Test the driver/device. */ - if (!strcmp(argv[1], "test")) { - ll40ls::test(); + if (!strcmp(verb, "test")) { + ll40ls::test(bus); } /* * Reset the driver. */ - if (!strcmp(argv[1], "reset")) { - ll40ls::reset(); + if (!strcmp(verb, "reset")) { + ll40ls::reset(bus); } /* * Print driver information. */ - if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status")) { - ll40ls::info(); + if (!strcmp(verb, "info") || !strcmp(verb, "status")) { + ll40ls::info(bus); } errx(1, "unrecognized command, try 'start', 'test', 'reset' or 'info'"); -- cgit v1.2.3 From 1ccb56de92719f36c8b4d80e3dae144dbb8fb913 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 20 Oct 2014 15:21:53 +0200 Subject: Better error feedback --- src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index a61346c2e..430933860 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -600,7 +600,7 @@ FixedwingEstimator::check_filter_state() "stale IMU data, resetting", "got initial position lock", "excessive gyro offsets", - "GPS velocity divergence", + "velocity diverted, check accel config", "excessive covariances", "unknown condition"}; @@ -614,7 +614,7 @@ FixedwingEstimator::check_filter_state() } warnx("reset: %s", feedback[warn_index]); - mavlink_log_critical(_mavlink_fd, "[ekf] re-init: %s", feedback[warn_index]); + mavlink_log_critical(_mavlink_fd, "[ekf check] %s", feedback[warn_index]); } struct estimator_status_report rep; -- cgit v1.2.3 From 3f4516810b149d1f8054f1b113508f9b1491b786 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 20 Oct 2014 15:22:33 +0200 Subject: Improved EKF check feedback --- src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index 430933860..685f5e12f 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -597,12 +597,12 @@ FixedwingEstimator::check_filter_state() const char* const feedback[] = { 0, "NaN in states, resetting", - "stale IMU data, resetting", + "stale sensor data, resetting", "got initial position lock", "excessive gyro offsets", "velocity diverted, check accel config", "excessive covariances", - "unknown condition"}; + "unknown condition, resetting"}; // Print out error condition if (check) { -- cgit v1.2.3 From 5c77fc0012eaaad5b92e1b31cedf8bf5b035b988 Mon Sep 17 00:00:00 2001 From: philipoe Date: Mon, 20 Oct 2014 15:55:45 +0200 Subject: commander: Added time of RC-loss/gain to the mavlink_log_critical message to better inform the user --- src/modules/commander/commander.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 616b855df..23fdcb414 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1499,7 +1499,7 @@ int commander_thread_main(int argc, char *argv[]) } else { if (status.rc_signal_lost) { - mavlink_log_critical(mavlink_fd, "RC signal regained"); + mavlink_log_critical(mavlink_fd, "RC SIGNAL REGAINED (at t=%.3fs)",(double)(hrt_absolute_time())/1.0e6); status_changed = true; } } @@ -1592,7 +1592,7 @@ int commander_thread_main(int argc, char *argv[]) } else { if (!status.rc_signal_lost) { - mavlink_log_critical(mavlink_fd, "RC SIGNAL LOST"); + mavlink_log_critical(mavlink_fd, "RC SIGNAL LOST (at t=%.3fs)",(double)(hrt_absolute_time())/1.0e6); status.rc_signal_lost = true; status_changed = true; } -- cgit v1.2.3 From 4afa8645832c71ea1c35dc73d9424df80fac531a Mon Sep 17 00:00:00 2001 From: philipoe Date: Mon, 20 Oct 2014 16:25:38 +0200 Subject: commander: Added duration of rc-loss to mavlink_log_critical message --- src/modules/commander/commander.cpp | 3 ++- src/modules/uORB/topics/vehicle_status.h | 1 + 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 23fdcb414..d8ebee4b6 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1499,7 +1499,7 @@ int commander_thread_main(int argc, char *argv[]) } else { if (status.rc_signal_lost) { - mavlink_log_critical(mavlink_fd, "RC SIGNAL REGAINED (at t=%.3fs)",(double)(hrt_absolute_time())/1.0e6); + mavlink_log_critical(mavlink_fd, "RC SIGNAL REGAINED after %.3fs (at t=%.3fs)",(double)(hrt_absolute_time()-status.rc_signal_lost_timestamp)/1.0e6,(double)(hrt_absolute_time())/1.0e6); status_changed = true; } } @@ -1594,6 +1594,7 @@ int commander_thread_main(int argc, char *argv[]) if (!status.rc_signal_lost) { mavlink_log_critical(mavlink_fd, "RC SIGNAL LOST (at t=%.3fs)",(double)(hrt_absolute_time())/1.0e6); status.rc_signal_lost = true; + status.rc_signal_lost_timestamp=sp_man.timestamp; status_changed = true; } } diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index a1b2667e3..8d797e21a 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -201,6 +201,7 @@ struct vehicle_status_s { bool rc_signal_found_once; bool rc_signal_lost; /**< true if RC reception lost */ + uint64_t rc_signal_lost_timestamp; /**< Time at which the RC reception was lost */ bool rc_signal_lost_cmd; /**< true if RC lost mode is commanded */ bool rc_input_blocked; /**< set if RC input should be ignored */ -- cgit v1.2.3 From 4656db01c215c94dda70d7b579037ec368d3275e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 22 Oct 2014 17:59:53 +0200 Subject: Enable IO safety parameter --- src/drivers/px4io/px4io.cpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index d212be766..821eb52ff 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -817,6 +817,11 @@ PX4IO::init() } + /* set safety to off if circuit breaker enabled */ + if (circuit_breaker_enabled("CBRK_IO_SAFETY", CBRK_IO_SAFETY_KEY)) { + (void)io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FORCE_SAFETY_OFF, PX4IO_FORCE_SAFETY_MAGIC); + } + /* try to claim the generic PWM output device node as well - it's OK if we fail at this */ ret = register_driver(PWM_OUTPUT_DEVICE_PATH, &fops, 0666, (void *)this); -- cgit v1.2.3 From 99f839b03861a9868f366c63cd214aa5898414aa Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Thu, 23 Oct 2014 10:57:31 +0200 Subject: Add a min PWM output so motors idle on arming. --- ROMFS/px4fmu_common/init.d/10015_tbs_discovery | 1 + 1 file changed, 1 insertion(+) diff --git a/ROMFS/px4fmu_common/init.d/10015_tbs_discovery b/ROMFS/px4fmu_common/init.d/10015_tbs_discovery index c4be16943..b1aa8c00b 100644 --- a/ROMFS/px4fmu_common/init.d/10015_tbs_discovery +++ b/ROMFS/px4fmu_common/init.d/10015_tbs_discovery @@ -27,3 +27,4 @@ fi set MIXER FMU_quad_w set PWM_OUTPUTS 1234 +set PWM_MIN 1200 -- cgit v1.2.3 From f3cda1839624b22bca8b030a43b72e8c0e354ab3 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 23 Oct 2014 16:17:20 +0200 Subject: Formatting commander.cpp to simplify later rework by ensuring formatting match. NO CODE CHANGES --- src/modules/commander/commander.cpp | 247 +++++++++++++++++++++++------------- 1 file changed, 159 insertions(+), 88 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 616b855df..cb507c9ba 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -207,7 +207,9 @@ void usage(const char *reason); /** * React to commands that are sent e.g. from the mavlink module. */ -bool handle_command(struct vehicle_status_s *status, const struct safety_s *safety, struct vehicle_command_s *cmd, struct actuator_armed_s *armed, struct home_position_s *home, struct vehicle_global_position_s *global_pos, orb_advert_t *home_pub); +bool handle_command(struct vehicle_status_s *status, const struct safety_s *safety, struct vehicle_command_s *cmd, + struct actuator_armed_s *armed, struct home_position_s *home, struct vehicle_global_position_s *global_pos, + orb_advert_t *home_pub); /** * Mainloop of commander. @@ -230,7 +232,8 @@ void print_reject_arm(const char *msg); void print_status(); -transition_result_t check_navigation_state_machine(struct vehicle_status_s *status, struct vehicle_control_mode_s *control_mode, struct vehicle_local_position_s *local_pos); +transition_result_t check_navigation_state_machine(struct vehicle_status_s *status, + struct vehicle_control_mode_s *control_mode, struct vehicle_local_position_s *local_pos); transition_result_t arm_disarm(bool arm, const int mavlink_fd, const char *armedBy); @@ -393,7 +396,8 @@ transition_result_t arm_disarm(bool arm, const int mavlink_fd_local, const char // Transition the armed state. By passing mavlink_fd to arming_state_transition it will // output appropriate error messages if the state cannot transition. - arming_res = arming_state_transition(&status, &safety, arm ? ARMING_STATE_ARMED : ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */, mavlink_fd_local); + arming_res = arming_state_transition(&status, &safety, arm ? ARMING_STATE_ARMED : ARMING_STATE_STANDBY, &armed, + true /* fRunPreArmChecks */, mavlink_fd_local); if (arming_res == TRANSITION_CHANGED && mavlink_fd) { mavlink_log_info(mavlink_fd_local, "[cmd] %s by %s", arm ? "ARMED" : "DISARMED", armedBy); @@ -406,11 +410,12 @@ transition_result_t arm_disarm(bool arm, const int mavlink_fd_local, const char } bool handle_command(struct vehicle_status_s *status_local, const struct safety_s *safety_local, - struct vehicle_command_s *cmd, struct actuator_armed_s *armed_local, - struct home_position_s *home, struct vehicle_global_position_s *global_pos, orb_advert_t *home_pub) + struct vehicle_command_s *cmd, struct actuator_armed_s *armed_local, + struct home_position_s *home, struct vehicle_global_position_s *global_pos, orb_advert_t *home_pub) { /* only handle commands that are meant to be handled by this system and component */ - if (cmd->target_system != status_local->system_id || ((cmd->target_component != status_local->component_id) && (cmd->target_component != 0))) { // component_id 0: valid for all components + if (cmd->target_system != status_local->system_id || ((cmd->target_component != status_local->component_id) + && (cmd->target_component != 0))) { // component_id 0: valid for all components return false; } @@ -537,13 +542,13 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s } else { mavlink_log_critical(mavlink_fd, "REJ CMD: %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f", - (double)cmd->param1, - (double)cmd->param2, - (double)cmd->param3, - (double)cmd->param4, - (double)cmd->param5, - (double)cmd->param6, - (double)cmd->param7); + (double)cmd->param1, + (double)cmd->param2, + (double)cmd->param3, + (double)cmd->param4, + (double)cmd->param5, + (double)cmd->param6, + (double)cmd->param7); } } break; @@ -554,35 +559,43 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s //XXX update state machine? armed_local->force_failsafe = true; warnx("forcing failsafe (termination)"); + } else { armed_local->force_failsafe = false; warnx("disabling failsafe (termination)"); } + /* param2 is currently used for other failsafe modes */ status_local->engine_failure_cmd = false; status_local->data_link_lost_cmd = false; status_local->gps_failure_cmd = false; status_local->rc_signal_lost_cmd = false; + if ((int)cmd->param2 <= 0) { /* reset all commanded failure modes */ warnx("reset all non-flighttermination failsafe commands"); + } else if ((int)cmd->param2 == 1) { /* trigger engine failure mode */ status_local->engine_failure_cmd = true; warnx("engine failure mode commanded"); + } else if ((int)cmd->param2 == 2) { /* trigger data link loss mode */ status_local->data_link_lost_cmd = true; warnx("data link loss mode commanded"); + } else if ((int)cmd->param2 == 3) { /* trigger gps loss mode */ status_local->gps_failure_cmd = true; warnx("gps loss mode commanded"); + } else if ((int)cmd->param2 == 4) { /* trigger rc loss mode */ status_local->rc_signal_lost_cmd = true; warnx("rc loss mode commanded"); } + cmd_result = VEHICLE_CMD_RESULT_ACCEPTED; } break; @@ -633,21 +646,27 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s } } break; + case VEHICLE_CMD_NAV_GUIDED_ENABLE: { transition_result_t res = TRANSITION_DENIED; static main_state_t main_state_pre_offboard = MAIN_STATE_MANUAL; + if (status_local->main_state != MAIN_STATE_OFFBOARD) { main_state_pre_offboard = status_local->main_state; } + if (cmd->param1 > 0.5f) { res = main_state_transition(status_local, MAIN_STATE_OFFBOARD); + if (res == TRANSITION_DENIED) { print_reject_mode(status_local, "OFFBOARD"); status_local->offboard_control_set_by_command = false; + } else { /* Set flag that offboard was set via command, main state is not overridden by rc */ status_local->offboard_control_set_by_command = true; } + } else { /* If the mavlink command is used to enable or disable offboard control: * switch back to previous mode when disabling */ @@ -656,6 +675,7 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s } } break; + case VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN: case VEHICLE_CMD_PREFLIGHT_CALIBRATION: case VEHICLE_CMD_PREFLIGHT_SET_SENSOR_OFFSETS: @@ -802,6 +822,7 @@ int commander_thread_main(int argc, char *argv[]) /* publish initial state */ status_pub = orb_advertise(ORB_ID(vehicle_status), &status); + if (status_pub < 0) { warnx("ERROR: orb_advertise for topic vehicle_status failed (uorb app running?).\n"); warnx("exiting."); @@ -827,11 +848,14 @@ int commander_thread_main(int argc, char *argv[]) /* init mission state, do it here to allow navigator to use stored mission even if mavlink failed to start */ orb_advert_t mission_pub = -1; mission_s mission; + if (dm_read(DM_KEY_MISSION_STATE, 0, &mission, sizeof(mission_s)) == sizeof(mission_s)) { if (mission.dataman_id >= 0 && mission.dataman_id <= 1) { - warnx("loaded mission state: dataman_id=%d, count=%u, current=%d", mission.dataman_id, mission.count, mission.current_seq); + warnx("loaded mission state: dataman_id=%d, count=%u, current=%d", mission.dataman_id, mission.count, + mission.current_seq); mavlink_log_info(mavlink_fd, "[cmd] dataman_id=%d, count=%u, current=%d", - mission.dataman_id, mission.count, mission.current_seq); + mission.dataman_id, mission.count, mission.current_seq); + } else { const char *missionfail = "reading mission state failed"; warnx("%s", missionfail); @@ -1097,6 +1121,7 @@ int commander_thread_main(int argc, char *argv[]) status.offboard_control_signal_lost = false; status_changed = true; } + } else { if (!status.offboard_control_signal_lost) { status.offboard_control_signal_lost = true; @@ -1115,9 +1140,9 @@ int commander_thread_main(int argc, char *argv[]) /* perform system checks when new telemetry link connected */ if (mavlink_fd && - telemetry_last_heartbeat[i] == 0 && - telemetry.heartbeat_time > 0 && - hrt_elapsed_time(&telemetry.heartbeat_time) < datalink_loss_timeout * 1e6) { + telemetry_last_heartbeat[i] == 0 && + telemetry.heartbeat_time > 0 && + hrt_elapsed_time(&telemetry.heartbeat_time) < datalink_loss_timeout * 1e6) { (void)rc_calibration_check(mavlink_fd); } @@ -1130,6 +1155,7 @@ int commander_thread_main(int argc, char *argv[]) if (updated) { orb_copy(ORB_ID(sensor_combined), sensor_sub, &sensors); + /* Check if the barometer is healthy and issue a warning in the GCS if not so. * Because the barometer is used for calculating AMSL altitude which is used to ensure * vertical separation from other airtraffic the operator has to know when the @@ -1142,6 +1168,7 @@ int commander_thread_main(int argc, char *argv[]) status_changed = true; mavlink_log_critical(mavlink_fd, "baro healthy"); } + } else { if (!status.barometer_failure) { status.barometer_failure = true; @@ -1164,10 +1191,11 @@ int commander_thread_main(int argc, char *argv[]) if (hrt_elapsed_time(&system_power.timestamp) < 200000) { if (system_power.servo_valid && - !system_power.brick_valid && - !system_power.usb_connected) { + !system_power.brick_valid && + !system_power.usb_connected) { /* flying only on servo rail, this is unsafe */ status.condition_power_input_valid = false; + } else { status.condition_power_input_valid = true; } @@ -1187,9 +1215,11 @@ int commander_thread_main(int argc, char *argv[]) /* disarm if safety is now on and still armed */ if (status.hil_state == HIL_STATE_OFF && safety.safety_switch_available && !safety.safety_off && armed.armed) { - arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR); + arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : + ARMING_STATE_STANDBY_ERROR); - if (TRANSITION_CHANGED == arming_state_transition(&status, &safety, new_arming_state, &armed, true /* fRunPreArmChecks */, mavlink_fd)) { + if (TRANSITION_CHANGED == arming_state_transition(&status, &safety, new_arming_state, &armed, + true /* fRunPreArmChecks */, mavlink_fd)) { mavlink_log_info(mavlink_fd, "DISARMED by safety switch"); arming_state_changed = true; } @@ -1233,7 +1263,8 @@ int commander_thread_main(int argc, char *argv[]) } } - check_valid(global_position.timestamp, POSITION_TIMEOUT, eph_good, &(status.condition_global_position_valid), &status_changed); + check_valid(global_position.timestamp, POSITION_TIMEOUT, eph_good, &(status.condition_global_position_valid), + &status_changed); /* update home position */ if (!status.condition_home_position_valid && status.condition_global_position_valid && !armed.armed && @@ -1283,8 +1314,11 @@ int commander_thread_main(int argc, char *argv[]) local_eph_good = false; } } - check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.xy_valid && local_eph_good, &(status.condition_local_position_valid), &status_changed); - check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.z_valid, &(status.condition_local_altitude_valid), &status_changed); + + check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.xy_valid + && local_eph_good, &(status.condition_local_position_valid), &status_changed); + check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.z_valid, + &(status.condition_local_altitude_valid), &status_changed); if (status.condition_local_altitude_valid) { if (status.condition_landed != local_position.landed) { @@ -1315,7 +1349,8 @@ int commander_thread_main(int argc, char *argv[]) /* get throttle (if armed), as we only care about energy negative throttle also counts */ float throttle = (armed.armed) ? fabsf(actuator_controls.control[3]) : 0.0f; - status.battery_remaining = battery_remaining_estimate_voltage(battery.voltage_filtered_v, battery.discharged_mah, throttle); + status.battery_remaining = battery_remaining_estimate_voltage(battery.voltage_filtered_v, battery.discharged_mah, + throttle); } } @@ -1383,26 +1418,30 @@ int commander_thread_main(int argc, char *argv[]) status.battery_warning = VEHICLE_BATTERY_WARNING_LOW; status_changed = true; - } else if (status.condition_battery_voltage_valid && status.battery_remaining < 0.09f && !critical_battery_voltage_actions_done && low_battery_voltage_actions_done) { + } else if (status.condition_battery_voltage_valid && status.battery_remaining < 0.09f + && !critical_battery_voltage_actions_done && low_battery_voltage_actions_done) { /* critical battery voltage, this is rather an emergency, change state machine */ critical_battery_voltage_actions_done = true; mavlink_log_emergency(mavlink_fd, "CRITICAL BATTERY, LAND IMMEDIATELY"); status.battery_warning = VEHICLE_BATTERY_WARNING_CRITICAL; if (armed.armed) { - arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed, true /* fRunPreArmChecks */, mavlink_fd); + arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed, true /* fRunPreArmChecks */, + mavlink_fd); if (arming_ret == TRANSITION_CHANGED) { arming_state_changed = true; } } else { - arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed, true /* fRunPreArmChecks */, mavlink_fd); + arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed, true /* fRunPreArmChecks */, + mavlink_fd); if (arming_ret == TRANSITION_CHANGED) { arming_state_changed = true; } } + status_changed = true; } @@ -1411,7 +1450,8 @@ int commander_thread_main(int argc, char *argv[]) /* If in INIT state, try to proceed to STANDBY state */ if (status.arming_state == ARMING_STATE_INIT && low_prio_task == LOW_PRIO_TASK_NONE) { /* TODO: check for sensors */ - arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */, mavlink_fd); + arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */, + mavlink_fd); if (arming_ret == TRANSITION_CHANGED) { arming_state_changed = true; @@ -1439,23 +1479,25 @@ int commander_thread_main(int argc, char *argv[]) /* Initialize map projection if gps is valid */ if (!map_projection_global_initialized() - && (gps_position.eph < eph_threshold) - && (gps_position.epv < epv_threshold) - && hrt_elapsed_time((hrt_abstime*)&gps_position.timestamp_position) < 1e6) { + && (gps_position.eph < eph_threshold) + && (gps_position.epv < epv_threshold) + && hrt_elapsed_time((hrt_abstime *)&gps_position.timestamp_position) < 1e6) { /* set reference for global coordinates <--> local coordiantes conversion and map_projection */ - globallocalconverter_init((double)gps_position.lat * 1.0e-7, (double)gps_position.lon * 1.0e-7, (float)gps_position.alt * 1.0e-3f, hrt_absolute_time()); + globallocalconverter_init((double)gps_position.lat * 1.0e-7, (double)gps_position.lon * 1.0e-7, + (float)gps_position.alt * 1.0e-3f, hrt_absolute_time()); } /* check if GPS fix is ok */ if (status.circuit_breaker_engaged_gpsfailure_check || - (gps_position.fix_type >= 3 && - hrt_elapsed_time(&gps_position.timestamp_position) < FAILSAFE_DEFAULT_TIMEOUT)) { + (gps_position.fix_type >= 3 && + hrt_elapsed_time(&gps_position.timestamp_position) < FAILSAFE_DEFAULT_TIMEOUT)) { /* handle the case where gps was regained */ if (status.gps_failure) { status.gps_failure = false; status_changed = true; mavlink_log_critical(mavlink_fd, "gps regained"); } + } else { if (!status.gps_failure) { status.gps_failure = true; @@ -1477,12 +1519,14 @@ int commander_thread_main(int argc, char *argv[]) armed.force_failsafe = true; status_changed = true; static bool flight_termination_printed = false; + if (!flight_termination_printed) { warnx("Flight termination because of navigator request or geofence"); mavlink_log_critical(mavlink_fd, "GF violation: flight termination"); flight_termination_printed = true; } - if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0 ) { + + if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) { mavlink_log_critical(mavlink_fd, "GF violation: flight termination"); } } // no reset is done here on purpose, on geofence violation we want to stay in flighttermination @@ -1490,7 +1534,7 @@ int commander_thread_main(int argc, char *argv[]) /* RC input check */ if (!status.rc_input_blocked && sp_man.timestamp != 0 && - hrt_absolute_time() < sp_man.timestamp + (uint64_t)(rc_loss_timeout * 1e6f)) { + hrt_absolute_time() < sp_man.timestamp + (uint64_t)(rc_loss_timeout * 1e6f)) { /* handle the case where RC signal was regained */ if (!status.rc_signal_found_once) { status.rc_signal_found_once = true; @@ -1515,11 +1559,15 @@ int commander_thread_main(int argc, char *argv[]) if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { /* disarm to STANDBY if ARMED or to STANDBY_ERROR if ARMED_ERROR */ - arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR); - arming_ret = arming_state_transition(&status, &safety, new_arming_state, &armed, true /* fRunPreArmChecks */, mavlink_fd); + arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : + ARMING_STATE_STANDBY_ERROR); + arming_ret = arming_state_transition(&status, &safety, new_arming_state, &armed, true /* fRunPreArmChecks */, + mavlink_fd); + if (arming_ret == TRANSITION_CHANGED) { arming_state_changed = true; } + stick_off_counter = 0; } else { @@ -1541,8 +1589,11 @@ int commander_thread_main(int argc, char *argv[]) */ if (status.main_state != MAIN_STATE_MANUAL) { print_reject_arm("NOT ARMING: Switch to MANUAL mode first."); + } else { - arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed, true /* fRunPreArmChecks */, mavlink_fd); + arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed, true /* fRunPreArmChecks */, + mavlink_fd); + if (arming_ret == TRANSITION_CHANGED) { arming_state_changed = true; } @@ -1565,6 +1616,7 @@ int commander_thread_main(int argc, char *argv[]) } else { mavlink_log_info(mavlink_fd, "DISARMED by RC"); } + arming_state_changed = true; } else if (arming_ret == TRANSITION_DENIED) { @@ -1600,18 +1652,20 @@ int commander_thread_main(int argc, char *argv[]) /* data links check */ bool have_link = false; + for (int i = 0; i < TELEMETRY_STATUS_ORB_ID_NUM; i++) { if (telemetry_last_heartbeat[i] != 0 && - hrt_elapsed_time(&telemetry_last_heartbeat[i]) < datalink_loss_timeout * 1e6) { + hrt_elapsed_time(&telemetry_last_heartbeat[i]) < datalink_loss_timeout * 1e6) { /* handle the case where data link was regained, * accept datalink as healthy only after datalink_regain_timeout seconds * */ if (telemetry_lost[i] && - hrt_elapsed_time(&telemetry_last_dl_loss[i]) > datalink_regain_timeout * 1e6) { + hrt_elapsed_time(&telemetry_last_dl_loss[i]) > datalink_regain_timeout * 1e6) { mavlink_log_critical(mavlink_fd, "data link %i regained", i); telemetry_lost[i] = false; have_link = true; + } else if (!telemetry_lost[i]) { /* telemetry was healthy also in last iteration * we don't have to check a timeout */ @@ -1620,6 +1674,7 @@ int commander_thread_main(int argc, char *argv[]) } else { telemetry_last_dl_loss[i] = hrt_absolute_time(); + if (!telemetry_lost[i]) { mavlink_log_critical(mavlink_fd, "data link %i lost", i); telemetry_lost[i] = true; @@ -1647,24 +1702,26 @@ int commander_thread_main(int argc, char *argv[]) * only for fixed wing for now */ if (!status.circuit_breaker_engaged_enginefailure_check && - status.is_rotary_wing == false && - armed.armed && - ((actuator_controls.control[3] > ef_throttle_thres && - battery.current_a/actuator_controls.control[3] < - ef_current2throttle_thres) || - (status.engine_failure))) { + status.is_rotary_wing == false && + armed.armed && + ((actuator_controls.control[3] > ef_throttle_thres && + battery.current_a / actuator_controls.control[3] < + ef_current2throttle_thres) || + (status.engine_failure))) { /* potential failure, measure time */ if (timestamp_engine_healthy > 0 && - hrt_elapsed_time(×tamp_engine_healthy) > - ef_time_thres * 1e6 && - !status.engine_failure) { - status.engine_failure = true; - status_changed = true; - mavlink_log_critical(mavlink_fd, "Engine Failure"); + hrt_elapsed_time(×tamp_engine_healthy) > + ef_time_thres * 1e6 && + !status.engine_failure) { + status.engine_failure = true; + status_changed = true; + mavlink_log_critical(mavlink_fd, "Engine Failure"); } + } else { /* no failure reset flag */ timestamp_engine_healthy = hrt_absolute_time(); + if (status.engine_failure) { status.engine_failure = false; status_changed = true; @@ -1691,20 +1748,22 @@ int commander_thread_main(int argc, char *argv[]) * If we are not in a manual (RC stick controlled mode) * and both failed we want to terminate the flight */ if (status.main_state != MAIN_STATE_MANUAL && - status.main_state != MAIN_STATE_ACRO && - status.main_state != MAIN_STATE_ALTCTL && - status.main_state != MAIN_STATE_POSCTL && - ((status.data_link_lost && status.gps_failure) || - (status.data_link_lost_cmd && status.gps_failure_cmd))) { + status.main_state != MAIN_STATE_ACRO && + status.main_state != MAIN_STATE_ALTCTL && + status.main_state != MAIN_STATE_POSCTL && + ((status.data_link_lost && status.gps_failure) || + (status.data_link_lost_cmd && status.gps_failure_cmd))) { armed.force_failsafe = true; status_changed = true; static bool flight_termination_printed = false; + if (!flight_termination_printed) { warnx("Flight termination because of data link loss && gps failure"); mavlink_log_critical(mavlink_fd, "DL and GPS lost: flight termination"); flight_termination_printed = true; } - if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0 ) { + + if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) { mavlink_log_critical(mavlink_fd, "DL and GPS lost: flight termination"); } } @@ -1713,19 +1772,21 @@ int commander_thread_main(int argc, char *argv[]) * If we are in manual (controlled with RC): * if both failed we want to terminate the flight */ if ((status.main_state == MAIN_STATE_ACRO || - status.main_state == MAIN_STATE_MANUAL || - status.main_state == MAIN_STATE_ALTCTL || - status.main_state == MAIN_STATE_POSCTL) && - ((status.rc_signal_lost && status.gps_failure) || - (status.rc_signal_lost_cmd && status.gps_failure_cmd))) { + status.main_state == MAIN_STATE_MANUAL || + status.main_state == MAIN_STATE_ALTCTL || + status.main_state == MAIN_STATE_POSCTL) && + ((status.rc_signal_lost && status.gps_failure) || + (status.rc_signal_lost_cmd && status.gps_failure_cmd))) { armed.force_failsafe = true; status_changed = true; static bool flight_termination_printed = false; + if (!flight_termination_printed) { warnx("Flight termination because of RC signal loss && gps failure"); flight_termination_printed = true; } - if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0 ) { + + if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) { mavlink_log_critical(mavlink_fd, "RC and GPS lost: flight termination"); } } @@ -1766,6 +1827,7 @@ int commander_thread_main(int argc, char *argv[]) /* mark home position as set */ status.condition_home_position_valid = true; } + arming_state_changed = false; } @@ -1810,7 +1872,8 @@ int commander_thread_main(int argc, char *argv[]) } /* play arming and battery warning tunes */ - if (!arm_tune_played && armed.armed && (!safety.safety_switch_available || (safety.safety_switch_available && safety.safety_off))) { + if (!arm_tune_played && armed.armed && (!safety.safety_switch_available || (safety.safety_switch_available + && safety.safety_off))) { /* play tune when armed */ set_tune(TONE_ARMING_WARNING_TUNE); arm_tune_played = true; @@ -1988,6 +2051,7 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s /* offboard switch overrides main switch */ if (sp_man->offboard_switch == SWITCH_POS_ON) { res = main_state_transition(status_local, MAIN_STATE_OFFBOARD); + if (res == TRANSITION_DENIED) { print_reject_mode(status_local, "OFFBOARD"); @@ -2009,6 +2073,7 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s } else { res = main_state_transition(status_local, MAIN_STATE_MANUAL); } + // TRANSITION_DENIED is not possible here break; @@ -2023,7 +2088,7 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s print_reject_mode(status_local, "POSCTL"); } - // fallback to ALTCTL + // fallback to ALTCTL res = main_state_transition(status_local, MAIN_STATE_ALTCTL); if (res != TRANSITION_DENIED) { @@ -2047,14 +2112,14 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s break; // changed successfully or already in this state } - print_reject_mode(status_local, "AUTO_RTL"); + print_reject_mode(status_local, "AUTO_RTL"); - // fallback to LOITER if home position not set - res = main_state_transition(status_local, MAIN_STATE_AUTO_LOITER); + // fallback to LOITER if home position not set + res = main_state_transition(status_local, MAIN_STATE_AUTO_LOITER); - if (res != TRANSITION_DENIED) { - break; // changed successfully or already in this state - } + if (res != TRANSITION_DENIED) { + break; // changed successfully or already in this state + } } else if (sp_man->loiter_switch == SWITCH_POS_ON) { res = main_state_transition(status_local, MAIN_STATE_AUTO_LOITER); @@ -2063,7 +2128,7 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s break; // changed successfully or already in this state } - print_reject_mode(status_local, "AUTO_LOITER"); + print_reject_mode(status_local, "AUTO_LOITER"); } else { res = main_state_transition(status_local, MAIN_STATE_AUTO_MISSION); @@ -2072,22 +2137,22 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s break; // changed successfully or already in this state } - print_reject_mode(status_local, "AUTO_MISSION"); + print_reject_mode(status_local, "AUTO_MISSION"); - // fallback to LOITER if home position not set - res = main_state_transition(status_local, MAIN_STATE_AUTO_LOITER); + // fallback to LOITER if home position not set + res = main_state_transition(status_local, MAIN_STATE_AUTO_LOITER); - if (res != TRANSITION_DENIED) { - break; // changed successfully or already in this state - } + if (res != TRANSITION_DENIED) { + break; // changed successfully or already in this state + } } - // fallback to POSCTL - res = main_state_transition(status_local, MAIN_STATE_POSCTL); + // fallback to POSCTL + res = main_state_transition(status_local, MAIN_STATE_POSCTL); - if (res != TRANSITION_DENIED) { - break; // changed successfully or already in this state - } + if (res != TRANSITION_DENIED) { + break; // changed successfully or already in this state + } // fallback to ALTCTL res = main_state_transition(status_local, MAIN_STATE_ALTCTL); @@ -2169,6 +2234,7 @@ set_control_mode() control_mode.flag_control_position_enabled = false; control_mode.flag_control_velocity_enabled = false; break; + case OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE: control_mode.flag_control_rates_enabled = true; control_mode.flag_control_attitude_enabled = true; @@ -2177,6 +2243,7 @@ set_control_mode() control_mode.flag_control_position_enabled = false; control_mode.flag_control_velocity_enabled = false; break; + case OFFBOARD_CONTROL_MODE_DIRECT_FORCE: control_mode.flag_control_rates_enabled = true; control_mode.flag_control_attitude_enabled = false; @@ -2186,6 +2253,7 @@ set_control_mode() control_mode.flag_control_position_enabled = false; control_mode.flag_control_velocity_enabled = false; break; + case OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED: case OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_OFFSET_NED: case OFFBOARD_CONTROL_MODE_DIRECT_BODY_NED: @@ -2198,6 +2266,7 @@ set_control_mode() control_mode.flag_control_velocity_enabled = true; //XXX: the flags could depend on sp_offboard.ignore break; + default: control_mode.flag_control_rates_enabled = false; control_mode.flag_control_attitude_enabled = false; @@ -2206,6 +2275,7 @@ set_control_mode() control_mode.flag_control_position_enabled = false; control_mode.flag_control_velocity_enabled = false; } + break; case NAVIGATION_STATE_POSCTL: @@ -2415,7 +2485,8 @@ void *commander_low_prio_loop(void *arg) int calib_ret = ERROR; /* try to go to INIT/PREFLIGHT arming state */ - if (TRANSITION_DENIED == arming_state_transition(&status, &safety, ARMING_STATE_INIT, &armed, true /* fRunPreArmChecks */, mavlink_fd)) { + if (TRANSITION_DENIED == arming_state_transition(&status, &safety, ARMING_STATE_INIT, &armed, + true /* fRunPreArmChecks */, mavlink_fd)) { answer_command(cmd, VEHICLE_CMD_RESULT_DENIED); break; } -- cgit v1.2.3 From 33dcb687e82ea5e0f6bdba37419361ca923703df Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 24 Oct 2014 08:53:10 +0200 Subject: Made some space for FDs - needs proper fix, but will give hackers some relief --- src/modules/sdlog2/sdlog2.c | 23 +++++++++++++++++++---- 1 file changed, 19 insertions(+), 4 deletions(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 1282bc0ea..9bde37432 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1038,7 +1038,6 @@ int sdlog2_thread_main(int argc, char *argv[]) subs.cmd_sub = orb_subscribe(ORB_ID(vehicle_command)); subs.status_sub = orb_subscribe(ORB_ID(vehicle_status)); subs.gps_pos_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); - subs.sat_info_sub = orb_subscribe(ORB_ID(satellite_info)); subs.sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); subs.att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); subs.att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); @@ -1057,9 +1056,6 @@ int sdlog2_thread_main(int argc, char *argv[]) subs.esc_sub = orb_subscribe(ORB_ID(esc_status)); subs.global_vel_sp_sub = orb_subscribe(ORB_ID(vehicle_global_velocity_setpoint)); subs.battery_sub = orb_subscribe(ORB_ID(battery_status)); - for (int i = 0; i < TELEMETRY_STATUS_ORB_ID_NUM; i++) { - subs.telemetry_subs[i] = orb_subscribe(telemetry_status_orb_id[i]); - } subs.range_finder_sub = orb_subscribe(ORB_ID(sensor_range_finder)); subs.estimator_status_sub = orb_subscribe(ORB_ID(estimator_status)); subs.tecs_status_sub = orb_subscribe(ORB_ID(tecs_status)); @@ -1069,6 +1065,25 @@ int sdlog2_thread_main(int argc, char *argv[]) /* we need to rate-limit wind, as we do not need the full update rate */ orb_set_interval(subs.wind_sub, 90); + + /* add new topics HERE */ + + + for (int i = 0; i < TELEMETRY_STATUS_ORB_ID_NUM; i++) { + subs.telemetry_subs[i] = orb_subscribe(telemetry_status_orb_id[i]); + } + + if (_extended_logging) { + subs.sat_info_sub = orb_subscribe(ORB_ID(satellite_info)); + } + + /* close non-needed fd's */ + + /* close stdin */ + close(0); + /* close stdout */ + close(1); + thread_running = true; /* initialize thread synchronization */ -- cgit v1.2.3 From cb79ef4df3c7514fa279f3b3259fb0ddc05f3b12 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 24 Oct 2014 15:27:43 +1100 Subject: ll40ls: auto-detect ll40ls on either 0x42 or 0x62 I2C address --- src/drivers/ll40ls/ll40ls.cpp | 53 +++++++++++++++++++++++++++++++++++++++---- 1 file changed, 49 insertions(+), 4 deletions(-) diff --git a/src/drivers/ll40ls/ll40ls.cpp b/src/drivers/ll40ls/ll40ls.cpp index 8b156f7ba..ce0363dbb 100644 --- a/src/drivers/ll40ls/ll40ls.cpp +++ b/src/drivers/ll40ls/ll40ls.cpp @@ -73,15 +73,19 @@ /* Configuration Constants */ #define LL40LS_BUS PX4_I2C_BUS_EXPANSION -#define LL40LS_BASEADDR 0x42 /* 7-bit address */ +#define LL40LS_BASEADDR 0x62 /* 7-bit address */ +#define LL40LS_BASEADDR_OLD 0x42 /* previous 7-bit address */ #define LL40LS_DEVICE_PATH_INT "/dev/ll40ls_int" #define LL40LS_DEVICE_PATH_EXT "/dev/ll40ls_ext" /* LL40LS Registers addresses */ #define LL40LS_MEASURE_REG 0x00 /* Measure range register */ -#define LL40LS_MSRREG_ACQUIRE 0x04 /* Value to initiate a measurement, varies based on sensor revision */ +#define LL40LS_MSRREG_ACQUIRE 0x04 /* Value to initiate a measurement, varies based on sensor revision */ #define LL40LS_DISTHIGH_REG 0x8F /* High byte of distance register, auto increment */ +#define LL40LS_WHO_AM_I_REG 0x11 +#define LL40LS_WHO_AM_I_REG_VAL 0xCA +#define LL40LS_SIGNAL_STRENGTH_REG 0x5b /* Device limits */ #define LL40LS_MIN_DISTANCE (0.00f) @@ -117,6 +121,7 @@ public: protected: virtual int probe(); + virtual int read_reg(uint8_t reg, uint8_t &val); private: float _min_distance; @@ -210,7 +215,7 @@ LL40LS::LL40LS(int bus, const char *path, int address) : _bus(bus) { // up the retries since the device misses the first measure attempts - I2C::_retries = 3; + _retries = 3; // enable debug() calls _debug_enabled = false; @@ -277,10 +282,50 @@ out: return ret; } +int +LL40LS::read_reg(uint8_t reg, uint8_t &val) +{ + return transfer(®, 1, &val, 1); +} + int LL40LS::probe() { - return measure(); + // cope with both old and new I2C bus address + const uint8_t addresses[2] = {LL40LS_BASEADDR, LL40LS_BASEADDR_OLD}; + + // more retries for detection + _retries = 10; + + for (uint8_t i=0; i Date: Fri, 24 Oct 2014 15:53:46 +1100 Subject: ll40ls: start a measurement after a probe this ensures register 0 also works --- src/drivers/ll40ls/ll40ls.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/drivers/ll40ls/ll40ls.cpp b/src/drivers/ll40ls/ll40ls.cpp index ce0363dbb..6793acd81 100644 --- a/src/drivers/ll40ls/ll40ls.cpp +++ b/src/drivers/ll40ls/ll40ls.cpp @@ -325,7 +325,9 @@ LL40LS::probe() ok: _retries = 3; - return OK; + + // start a measurement + return measure(); } void -- cgit v1.2.3 From ae29d04ff5dc1d3d9c48b081846f04ad34e44373 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 24 Oct 2014 15:57:47 +1100 Subject: px4flow: try a 22 byte transfer in probe() this allows us to distinguish between a ll40ls and px4flow on I2C address 0x42 --- src/drivers/px4flow/px4flow.cpp | 11 +++++++++++ 1 file changed, 11 insertions(+) diff --git a/src/drivers/px4flow/px4flow.cpp b/src/drivers/px4flow/px4flow.cpp index 60ad3c1af..04aba9eae 100644 --- a/src/drivers/px4flow/px4flow.cpp +++ b/src/drivers/px4flow/px4flow.cpp @@ -249,6 +249,17 @@ out: int PX4FLOW::probe() { + uint8_t val[22]; + + // to be sure this is not a ll40ls Lidar (which can also be on + // 0x42) we check if a 22 byte transfer works from address + // 0. The ll40ls gives an error for that, whereas the flow + // happily returns some data + if (transfer(nullptr, 0, &val[0], 22) != OK) { + return -EIO; + } + + // that worked, so start a measurement cycle return measure(); } -- cgit v1.2.3 From 276dc7fbbcc9d2b8baf65376b7ceb5e362ff978b Mon Sep 17 00:00:00 2001 From: dominiho Date: Tue, 28 Oct 2014 12:35:20 +0100 Subject: added px4flow integral frame, adjusted px4flow i2c driver, adjusted postition_estimator_inav --- src/drivers/px4flow/px4flow.cpp | 599 ++++++++++----------- .../position_estimator_inav_main.c | 11 +- src/modules/uORB/topics/optical_flow.h | 23 +- 3 files changed, 313 insertions(+), 320 deletions(-) diff --git a/src/drivers/px4flow/px4flow.cpp b/src/drivers/px4flow/px4flow.cpp index 04aba9eae..f74db1b52 100644 --- a/src/drivers/px4flow/px4flow.cpp +++ b/src/drivers/px4flow/px4flow.cpp @@ -73,15 +73,15 @@ #include /* Configuration Constants */ -#define PX4FLOW_BUS PX4_I2C_BUS_EXPANSION +#define PX4FLOW_BUS PX4_I2C_BUS_ESC//PX4_I2C_BUS_EXPANSION #define I2C_FLOW_ADDRESS 0x42 //* 7-bit address. 8-bit address is 0x84 //range 0x42 - 0x49 /* PX4FLOW Registers addresses */ -#define PX4FLOW_REG 0x00 /* Measure Register */ - -#define PX4FLOW_CONVERSION_INTERVAL 8000 /* 8ms 125Hz */ +//#define PX4FLOW_REG 0x00 /* Measure Register 0*/ +#define PX4FLOW_REG 0x16 /* Measure Register 22*/ +#define PX4FLOW_CONVERSION_INTERVAL 20000 //in microseconds! 20000 = 50 Hz 100000 = 10Hz /* oddly, ERROR is not defined for c++ */ #ifdef ERROR # undef ERROR @@ -92,94 +92,103 @@ static const int ERROR = -1; # error This requires CONFIG_SCHED_WORKQUEUE. #endif -//struct i2c_frame -//{ -// uint16_t frame_count; -// int16_t pixel_flow_x_sum; -// int16_t pixel_flow_y_sum; -// int16_t flow_comp_m_x; -// int16_t flow_comp_m_y; -// int16_t qual; -// int16_t gyro_x_rate; -// int16_t gyro_y_rate; -// int16_t gyro_z_rate; -// uint8_t gyro_range; -// uint8_t sonar_timestamp; -// int16_t ground_distance; -//}; -// -//struct i2c_frame f; - -class PX4FLOW : public device::I2C -{ +struct i2c_frame { + uint16_t frame_count; + int16_t pixel_flow_x_sum; + int16_t pixel_flow_y_sum; + int16_t flow_comp_m_x; + int16_t flow_comp_m_y; + int16_t qual; + int16_t gyro_x_rate; + int16_t gyro_y_rate; + int16_t gyro_z_rate; + uint8_t gyro_range; + uint8_t sonar_timestamp; + int16_t ground_distance; +}; +struct i2c_frame f; + +typedef struct i2c_integral_frame { + uint16_t frame_count_since_last_readout; + int16_t pixel_flow_x_integral; + int16_t pixel_flow_y_integral; + int16_t gyro_x_rate_integral; + int16_t gyro_y_rate_integral; + int16_t gyro_z_rate_integral; + uint32_t integration_timespan; + uint32_t time_since_last_sonar_update; + uint16_t ground_distance; + uint8_t qual; +}__attribute__((packed)); +struct i2c_integral_frame f_integral; + + +class PX4FLOW: public device::I2C { public: PX4FLOW(int bus = PX4FLOW_BUS, int address = I2C_FLOW_ADDRESS); virtual ~PX4FLOW(); - virtual int init(); + virtual int init(); - virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); - virtual int ioctl(struct file *filp, int cmd, unsigned long arg); + virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); + virtual int ioctl(struct file *filp, int cmd, unsigned long arg); /** - * Diagnostics - print some basic information about the driver. - */ - void print_info(); + * Diagnostics - print some basic information about the driver. + */ + void print_info(); protected: - virtual int probe(); + virtual int probe(); private: - work_s _work; - RingBuffer *_reports; - bool _sensor_ok; - int _measure_ticks; - bool _collect_phase; + work_s _work; + RingBuffer *_reports;bool _sensor_ok; + int _measure_ticks;bool _collect_phase; - orb_advert_t _px4flow_topic; + orb_advert_t _px4flow_topic; - perf_counter_t _sample_perf; - perf_counter_t _comms_errors; - perf_counter_t _buffer_overflows; + perf_counter_t _sample_perf; + perf_counter_t _comms_errors; + perf_counter_t _buffer_overflows; /** - * Test whether the device supported by the driver is present at a - * specific address. - * - * @param address The I2C bus address to probe. - * @return True if the device is present. - */ - int probe_address(uint8_t address); + * Test whether the device supported by the driver is present at a + * specific address. + * + * @param address The I2C bus address to probe. + * @return True if the device is present. + */ + int probe_address(uint8_t address); /** - * Initialise the automatic measurement state machine and start it. - * - * @note This function is called at open and error time. It might make sense - * to make it more aggressive about resetting the bus in case of errors. - */ - void start(); + * Initialise the automatic measurement state machine and start it. + * + * @note This function is called at open and error time. It might make sense + * to make it more aggressive about resetting the bus in case of errors. + */ + void start(); /** - * Stop the automatic measurement state machine. - */ - void stop(); + * Stop the automatic measurement state machine. + */ + void stop(); /** - * Perform a poll cycle; collect from the previous measurement - * and start a new one. - */ - void cycle(); - int measure(); - int collect(); + * Perform a poll cycle; collect from the previous measurement + * and start a new one. + */ + void cycle(); + int measure(); + int collect(); /** - * Static trampoline from the workq context; because we don't have a - * generic workq wrapper yet. - * - * @param arg Instance pointer for the driver that is polling. - */ - static void cycle_trampoline(void *arg); - + * Static trampoline from the workq context; because we don't have a + * generic workq wrapper yet. + * + * @param arg Instance pointer for the driver that is polling. + */ + static void cycle_trampoline(void *arg); }; @@ -189,16 +198,12 @@ private: extern "C" __EXPORT int px4flow_main(int argc, char *argv[]); PX4FLOW::PX4FLOW(int bus, int address) : - I2C("PX4FLOW", PX4FLOW_DEVICE_PATH, bus, address, 400000),//400khz - _reports(nullptr), - _sensor_ok(false), - _measure_ticks(0), - _collect_phase(false), - _px4flow_topic(-1), - _sample_perf(perf_alloc(PC_ELAPSED, "px4flow_read")), - _comms_errors(perf_alloc(PC_COUNT, "px4flow_comms_errors")), - _buffer_overflows(perf_alloc(PC_COUNT, "px4flow_buffer_overflows")) -{ + I2C("PX4FLOW", PX4FLOW_DEVICE_PATH, bus, address, 400000), //400khz + _reports(nullptr), _sensor_ok(false), _measure_ticks(0), _collect_phase( + false), _px4flow_topic(-1), _sample_perf( + perf_alloc(PC_ELAPSED, "px4flow_read")), _comms_errors( + perf_alloc(PC_COUNT, "px4flow_comms_errors")), _buffer_overflows( + perf_alloc(PC_COUNT, "px4flow_buffer_overflows")) { // enable debug() calls _debug_enabled = true; @@ -206,8 +211,7 @@ PX4FLOW::PX4FLOW(int bus, int address) : memset(&_work, 0, sizeof(_work)); } -PX4FLOW::~PX4FLOW() -{ +PX4FLOW::~PX4FLOW() { /* make sure we are truly inactive */ stop(); @@ -216,9 +220,7 @@ PX4FLOW::~PX4FLOW() delete _reports; } -int -PX4FLOW::init() -{ +int PX4FLOW::init() { int ret = ERROR; /* do I2C init (and probe) first */ @@ -226,103 +228,79 @@ PX4FLOW::init() goto out; /* allocate basic report buffers */ - _reports = new RingBuffer(2, sizeof(struct optical_flow_s)); + _reports = new RingBuffer(2, sizeof(optical_flow_s)); if (_reports == nullptr) goto out; - /* get a publish handle on the px4flow topic */ - struct optical_flow_s zero_report; - memset(&zero_report, 0, sizeof(zero_report)); - _px4flow_topic = orb_advertise(ORB_ID(optical_flow), &zero_report); - - if (_px4flow_topic < 0) - debug("failed to create px4flow object. Did you start uOrb?"); - ret = OK; /* sensor is ok, but we don't really know if it is within range */ _sensor_ok = true; -out: - return ret; + out: return ret; } -int -PX4FLOW::probe() -{ - uint8_t val[22]; - - // to be sure this is not a ll40ls Lidar (which can also be on - // 0x42) we check if a 22 byte transfer works from address - // 0. The ll40ls gives an error for that, whereas the flow - // happily returns some data - if (transfer(nullptr, 0, &val[0], 22) != OK) { - return -EIO; - } - - // that worked, so start a measurement cycle +int PX4FLOW::probe() { return measure(); } -int -PX4FLOW::ioctl(struct file *filp, int cmd, unsigned long arg) -{ +int PX4FLOW::ioctl(struct file *filp, int cmd, unsigned long arg) { switch (cmd) { case SENSORIOCSPOLLRATE: { - switch (arg) { + switch (arg) { - /* switching to manual polling */ - case SENSOR_POLLRATE_MANUAL: - stop(); - _measure_ticks = 0; - return OK; + /* switching to manual polling */ + case SENSOR_POLLRATE_MANUAL: + stop(); + _measure_ticks = 0; + return OK; - /* external signalling (DRDY) not supported */ - case SENSOR_POLLRATE_EXTERNAL: + /* external signalling (DRDY) not supported */ + case SENSOR_POLLRATE_EXTERNAL: - /* zero would be bad */ - case 0: - return -EINVAL; + /* zero would be bad */ + case 0: + return -EINVAL; - /* set default/max polling rate */ - case SENSOR_POLLRATE_MAX: - case SENSOR_POLLRATE_DEFAULT: { - /* do we need to start internal polling? */ - bool want_start = (_measure_ticks == 0); + /* set default/max polling rate */ + case SENSOR_POLLRATE_MAX: + case SENSOR_POLLRATE_DEFAULT: { + /* do we need to start internal polling? */ + bool want_start = (_measure_ticks == 0); - /* set interval for next measurement to minimum legal value */ - _measure_ticks = USEC2TICK(PX4FLOW_CONVERSION_INTERVAL); + /* set interval for next measurement to minimum legal value */ + _measure_ticks = USEC2TICK(PX4FLOW_CONVERSION_INTERVAL); - /* if we need to start the poll state machine, do it */ - if (want_start) - start(); + /* if we need to start the poll state machine, do it */ + if (want_start) + start(); - return OK; - } + return OK; + } - /* adjust to a legal polling interval in Hz */ - default: { - /* do we need to start internal polling? */ - bool want_start = (_measure_ticks == 0); + /* adjust to a legal polling interval in Hz */ + default: { + /* do we need to start internal polling? */ + bool want_start = (_measure_ticks == 0); - /* convert hz to tick interval via microseconds */ - unsigned ticks = USEC2TICK(1000000 / arg); + /* convert hz to tick interval via microseconds */ + unsigned ticks = USEC2TICK(1000000 / arg); - /* check against maximum rate */ - if (ticks < USEC2TICK(PX4FLOW_CONVERSION_INTERVAL)) - return -EINVAL; + /* check against maximum rate */ + if (ticks < USEC2TICK(PX4FLOW_CONVERSION_INTERVAL)) + return -EINVAL; - /* update interval for next measurement */ - _measure_ticks = ticks; + /* update interval for next measurement */ + _measure_ticks = ticks; - /* if we need to start the poll state machine, do it */ - if (want_start) - start(); + /* if we need to start the poll state machine, do it */ + if (want_start) + start(); - return OK; - } - } + return OK; } + } + } case SENSORIOCGPOLLRATE: if (_measure_ticks == 0) @@ -358,11 +336,10 @@ PX4FLOW::ioctl(struct file *filp, int cmd, unsigned long arg) } } -ssize_t -PX4FLOW::read(struct file *filp, char *buffer, size_t buflen) -{ +ssize_t PX4FLOW::read(struct file *filp, char *buffer, size_t buflen) { unsigned count = buflen / sizeof(struct optical_flow_s); - struct optical_flow_s *rbuf = reinterpret_cast(buffer); + struct optical_flow_s *rbuf = + reinterpret_cast(buffer); int ret = 0; /* buffer must be large enough */ @@ -398,8 +375,8 @@ PX4FLOW::read(struct file *filp, char *buffer, size_t buflen) break; } - /* wait for it to complete */ - usleep(PX4FLOW_CONVERSION_INTERVAL); +// /* wait for it to complete */ +// usleep(PX4FLOW_CONVERSION_INTERVAL); /* run the collection phase */ if (OK != collect()) { @@ -407,6 +384,9 @@ PX4FLOW::read(struct file *filp, char *buffer, size_t buflen) break; } + /* wait for it to complete */ + usleep(PX4FLOW_CONVERSION_INTERVAL); + /* state machine will have generated a report, copy it out */ if (_reports->get(rbuf)) { ret = sizeof(*rbuf); @@ -417,9 +397,7 @@ PX4FLOW::read(struct file *filp, char *buffer, size_t buflen) return ret; } -int -PX4FLOW::measure() -{ +int PX4FLOW::measure() { int ret; /* @@ -428,11 +406,10 @@ PX4FLOW::measure() uint8_t cmd = PX4FLOW_REG; ret = transfer(&cmd, 1, nullptr, 0); - if (OK != ret) - { + if (OK != ret) { perf_count(_comms_errors); log("i2c::transfer returned %d", ret); - printf("i2c::transfer flow returned %d"); + printf("i2c::transfer flow returned %d"); return ret; } ret = OK; @@ -440,56 +417,90 @@ PX4FLOW::measure() return ret; } -int -PX4FLOW::collect() -{ - int ret = -EIO; +int PX4FLOW::collect() { + int ret = -EIO; /* read from the sensor */ - uint8_t val[22] = {0, 0,0, 0,0, 0,0, 0,0, 0,0, 0,0, 0,0, 0,0, 0,0, 0,0, 0}; + uint8_t val[46] = { 0 }; + perf_begin(_sample_perf); - ret = transfer(nullptr, 0, &val[0], 22); + if(PX4FLOW_REG==0x00){ + ret = transfer(nullptr, 0, &val[0], 45); //read 45 bytes (22+23 : frame1 + frame2) + } + if(PX4FLOW_REG==0x16){ + ret = transfer(nullptr, 0, &val[0], 23); //read 23 bytes (only frame2) + } - if (ret < 0) - { + if (ret < 0) { log("error reading from sensor: %d", ret); perf_count(_comms_errors); perf_end(_sample_perf); return ret; } -// f.frame_count = val[1] << 8 | val[0]; -// f.pixel_flow_x_sum= val[3] << 8 | val[2]; -// f.pixel_flow_y_sum= val[5] << 8 | val[4]; -// f.flow_comp_m_x= val[7] << 8 | val[6]; -// f.flow_comp_m_y= val[9] << 8 | val[8]; -// f.qual= val[11] << 8 | val[10]; -// f.gyro_x_rate= val[13] << 8 | val[12]; -// f.gyro_y_rate= val[15] << 8 | val[14]; -// f.gyro_z_rate= val[17] << 8 | val[16]; -// f.gyro_range= val[18]; -// f.sonar_timestamp= val[19]; -// f.ground_distance= val[21] << 8 | val[20]; - - int16_t flowcx = val[7] << 8 | val[6]; - int16_t flowcy = val[9] << 8 | val[8]; - int16_t gdist = val[21] << 8 | val[20]; + if (PX4FLOW_REG == 0) { + f.frame_count = val[1] << 8 | val[0]; + f.pixel_flow_x_sum = val[3] << 8 | val[2]; + f.pixel_flow_y_sum = val[5] << 8 | val[4]; + f.flow_comp_m_x = val[7] << 8 | val[6]; + f.flow_comp_m_y = val[9] << 8 | val[8]; + f.qual = val[11] << 8 | val[10]; + f.gyro_x_rate = val[13] << 8 | val[12]; + f.gyro_y_rate = val[15] << 8 | val[14]; + f.gyro_z_rate = val[17] << 8 | val[16]; + f.gyro_range = val[18]; + f.sonar_timestamp = val[19]; + f.ground_distance = val[21] << 8 | val[20]; + + f_integral.frame_count_since_last_readout = val[23] << 8 | val[22]; + f_integral.pixel_flow_x_integral = val[25] << 8 | val[24]; + f_integral.pixel_flow_y_integral = val[27] << 8 | val[26]; + f_integral.gyro_x_rate_integral = val[29] << 8 | val[28]; + f_integral.gyro_y_rate_integral = val[31] << 8 | val[30]; + f_integral.gyro_z_rate_integral = val[33] << 8 | val[32]; + f_integral.integration_timespan = val[37] << 24 | val[36] << 16 + | val[35] << 8 | val[34]; + f_integral.time_since_last_sonar_update = val[41] << 24 | val[40] << 16 + | val[39] << 8 | val[38]; + f_integral.ground_distance = val[43] << 8 | val[42]; + f_integral.qual = val[44]; + } + if(PX4FLOW_REG==0x16){ + f_integral.frame_count_since_last_readout = val[1] << 8 | val[0]; + f_integral.pixel_flow_x_integral =val[3] << 8 | val[2]; + f_integral.pixel_flow_y_integral =val[5] << 8 | val[4]; + f_integral.gyro_x_rate_integral =val[7] << 8 | val[6]; + f_integral.gyro_y_rate_integral =val[9] << 8 | val[8]; + f_integral.gyro_z_rate_integral =val[11] << 8 | val[10]; + f_integral.integration_timespan = val[15] <<24 |val[14] << 16 |val[13] << 8 |val[12]; + f_integral.time_since_last_sonar_update = val[19] <<24 |val[18] << 16 |val[17] << 8 |val[16]; + f_integral.ground_distance =val[21] <<8 |val[20]; + f_integral.qual =val[22]; + } + struct optical_flow_s report; - report.flow_comp_x_m = float(flowcx)/1000.0f; - report.flow_comp_y_m = float(flowcy)/1000.0f; - report.flow_raw_x= val[3] << 8 | val[2]; - report.flow_raw_y= val[5] << 8 | val[4]; - report.ground_distance_m =float(gdist)/1000.0f; - report.quality= val[10]; - report.sensor_id = 0; report.timestamp = hrt_absolute_time(); + report.pixel_flow_x_integral = float(f_integral.pixel_flow_x_integral) / 10000.0f;//convert to radians + report.pixel_flow_y_integral = float(f_integral.pixel_flow_y_integral) / 10000.0f;//convert to radians + report.frame_count_since_last_readout = f_integral.frame_count_since_last_readout; + report.ground_distance_m = float(f_integral.ground_distance) / 1000.0f;//convert to meters + report.quality = f_integral.qual;//0:bad ; 255 max quality + report.gyro_x_rate_integral= float(f_integral.gyro_x_rate_integral)/10000.0f;//convert to radians + report.gyro_y_rate_integral= float(f_integral.gyro_y_rate_integral)/10000.0f;//convert to radians + report.gyro_z_rate_integral= float(f_integral.gyro_z_rate_integral)/10000.0f;//convert to radians + report.integration_timespan= f_integral.integration_timespan;//microseconds + report.time_since_last_sonar_update = f_integral.time_since_last_sonar_update;//microseconds + report.sensor_id = 0; - - /* publish it */ - orb_publish(ORB_ID(optical_flow), _px4flow_topic, &report); + if (_px4flow_topic < 0) { + _px4flow_topic = orb_advertise(ORB_ID(optical_flow), &report); + } else { + /* publish it */ + orb_publish(ORB_ID(optical_flow), _px4flow_topic, &report); + } /* post a report to the ring */ if (_reports->force(&report)) { @@ -505,22 +516,17 @@ PX4FLOW::collect() return ret; } -void -PX4FLOW::start() -{ +void PX4FLOW::start() { /* reset the report ring and state machine */ _collect_phase = false; _reports->flush(); /* schedule a cycle to start things */ - work_queue(HPWORK, &_work, (worker_t)&PX4FLOW::cycle_trampoline, this, 1); + work_queue(HPWORK, &_work, (worker_t) & PX4FLOW::cycle_trampoline, this, 1); /* notify about state change */ - struct subsystem_info_s info = { - true, - true, - true, - SUBSYSTEM_TYPE_OPTICALFLOW}; + struct subsystem_info_s info = { true, true, true, + SUBSYSTEM_TYPE_OPTICALFLOW }; static orb_advert_t pub = -1; if (pub > 0) { @@ -530,71 +536,54 @@ PX4FLOW::start() } } -void -PX4FLOW::stop() -{ +void PX4FLOW::stop() { work_cancel(HPWORK, &_work); } -void -PX4FLOW::cycle_trampoline(void *arg) -{ - PX4FLOW *dev = (PX4FLOW *)arg; +void PX4FLOW::cycle_trampoline(void *arg) { + PX4FLOW *dev = (PX4FLOW *) arg; dev->cycle(); } -void -PX4FLOW::cycle() -{ - /* collection phase? */ - if (_collect_phase) { - - /* perform collection */ - if (OK != collect()) { - log("collection error"); - /* restart the measurement state machine */ - start(); - return; - } +void PX4FLOW::cycle() { +// /* collection phase? */ - /* next phase is measurement */ - _collect_phase = false; +// static uint64_t deltatime = hrt_absolute_time(); - /* - * Is there a collect->measure gap? - */ - if (_measure_ticks > USEC2TICK(PX4FLOW_CONVERSION_INTERVAL)) { + if (OK != measure()) + log("measure error"); - /* schedule a fresh cycle call when we are ready to measure again */ - work_queue(HPWORK, - &_work, - (worker_t)&PX4FLOW::cycle_trampoline, - this, - _measure_ticks - USEC2TICK(PX4FLOW_CONVERSION_INTERVAL)); + //usleep(PX4FLOW_CONVERSION_INTERVAL/40); - return; - } + /* perform collection */ + if (OK != collect()) { + log("collection error"); + /* restart the measurement state machine */ + start(); + return; } - /* measurement phase */ - if (OK != measure()) - log("measure error"); - /* next phase is collection */ - _collect_phase = true; +// deltatime = hrt_absolute_time()-deltatime; +// +// +// if(deltatime>PX4FLOW_CONVERSION_INTERVAL){ +// deltatime=PX4FLOW_CONVERSION_INTERVAL; +// } + + +// work_queue(HPWORK, &_work, (worker_t) & PX4FLOW::cycle_trampoline, this, +// _measure_ticks-USEC2TICK(deltatime)); + + work_queue(HPWORK, &_work, (worker_t) & PX4FLOW::cycle_trampoline, this, + _measure_ticks); + +// deltatime = hrt_absolute_time(); - /* schedule a fresh cycle call when the measurement is done */ - work_queue(HPWORK, - &_work, - (worker_t)&PX4FLOW::cycle_trampoline, - this, - USEC2TICK(PX4FLOW_CONVERSION_INTERVAL)); } -void -PX4FLOW::print_info() -{ +void PX4FLOW::print_info() { perf_print_counter(_sample_perf); perf_print_counter(_comms_errors); perf_print_counter(_buffer_overflows); @@ -605,8 +594,7 @@ PX4FLOW::print_info() /** * Local functions in support of the shell command. */ -namespace px4flow -{ +namespace px4flow { /* oddly, ERROR is not defined for c++ */ #ifdef ERROR @@ -614,20 +602,18 @@ namespace px4flow #endif const int ERROR = -1; -PX4FLOW *g_dev; +PX4FLOW *g_dev; -void start(); -void stop(); -void test(); -void reset(); -void info(); +void start(); +void stop(); +void test(); +void reset(); +void info(); /** * Start the driver. */ -void -start() -{ +void start() { int fd; if (g_dev != nullptr) @@ -653,10 +639,9 @@ start() exit(0); -fail: + fail: - if (g_dev != nullptr) - { + if (g_dev != nullptr) { delete g_dev; g_dev = nullptr; } @@ -667,15 +652,11 @@ fail: /** * Stop the driver */ -void stop() -{ - if (g_dev != nullptr) - { +void stop() { + if (g_dev != nullptr) { delete g_dev; g_dev = nullptr; - } - else - { + } else { errx(1, "driver not running"); } exit(0); @@ -686,9 +667,7 @@ void stop() * make sure we can collect data from the sensor in polled * and automatic modes. */ -void -test() -{ +void test() { struct optical_flow_s report; ssize_t sz; int ret; @@ -696,26 +675,27 @@ test() int fd = open(PX4FLOW_DEVICE_PATH, O_RDONLY); if (fd < 0) - err(1, "%s open failed (try 'px4flow start' if the driver is not running", PX4FLOW_DEVICE_PATH); + err(1, + "%s open failed (try 'px4flow start' if the driver is not running", + PX4FLOW_DEVICE_PATH); /* do a simple demand read */ sz = read(fd, &report, sizeof(report)); if (sz != sizeof(report)) - // err(1, "immediate read failed"); + // err(1, "immediate read failed"); - warnx("single read"); - warnx("flowx: %0.2f m/s", (double)report.flow_comp_x_m); - warnx("flowy: %0.2f m/s", (double)report.flow_comp_y_m); + warnx("single read"); + warnx("flowx: %0.2f m/s", (double) f.pixel_flow_x_sum); + warnx("flowy: %0.2f m/s", (double) f.pixel_flow_y_sum); warnx("time: %lld", report.timestamp); - - /* start the sensor polling at 2Hz */ - if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) - errx(1, "failed to set 2Hz poll rate"); + /* start the sensor polling at 10Hz */ + if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 10)) // 2)) + errx(1, "failed to set 10Hz poll rate"); /* read the sensor 5x and report each value */ - for (unsigned i = 0; i < 5; i++) { + for (unsigned i = 0; i < 10; i++) { struct pollfd fds; /* wait for data to be ready */ @@ -733,9 +713,22 @@ test() err(1, "periodic read failed"); warnx("periodic read %u", i); - warnx("flowx: %0.2f m/s", (double)report.flow_comp_x_m); - warnx("flowy: %0.2f m/s", (double)report.flow_comp_y_m); - warnx("time: %lld", report.timestamp); + + warnx("framecount_total: %u", f.frame_count); + warnx("framecount_integral: %u", + f_integral.frame_count_since_last_readout); + warnx("pixel_flow_x_integral: %i", f_integral.pixel_flow_x_integral); + warnx("pixel_flow_y_integral: %i", f_integral.pixel_flow_y_integral); + warnx("gyro_x_rate_integral: %i", f_integral.gyro_x_rate_integral); + warnx("gyro_y_rate_integral: %i", f_integral.gyro_y_rate_integral); + warnx("gyro_z_rate_integral: %i", f_integral.gyro_z_rate_integral); + warnx("integration_timespan [us]: %u", f_integral.integration_timespan); + warnx("ground_distance: %0.2f m", + (double) f_integral.ground_distance / 1000); + warnx("time since last sonar update [us]: %i", + f_integral.time_since_last_sonar_update); + warnx("quality integration average : %i", f_integral.qual); + warnx("quality : %i", f.qual); } @@ -746,9 +739,7 @@ test() /** * Reset the driver. */ -void -reset() -{ +void reset() { int fd = open(PX4FLOW_DEVICE_PATH, O_RDONLY); if (fd < 0) @@ -766,9 +757,7 @@ reset() /** * Print a little info about the driver. */ -void -info() -{ +void info() { if (g_dev == nullptr) errx(1, "driver not running"); @@ -780,20 +769,18 @@ info() } // namespace -int -px4flow_main(int argc, char *argv[]) -{ +int px4flow_main(int argc, char *argv[]) { /* * Start/load the driver. */ if (!strcmp(argv[1], "start")) px4flow::start(); - /* - * Stop the driver - */ - if (!strcmp(argv[1], "stop")) - px4flow::stop(); + /* + * Stop the driver + */ + if (!strcmp(argv[1], "stop")) + px4flow::stop(); /* * Test the driver/device. diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index 81bbaa658..bc24e054e 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -298,7 +298,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) float w_flow = 0.0f; float sonar_prev = 0.0f; - hrt_abstime flow_prev = 0; // time of last flow measurement + //hrt_abstime flow_prev = 0; // time of last flow measurement hrt_abstime sonar_time = 0; // time of last sonar measurement (not filtered) hrt_abstime sonar_valid_time = 0; // time of last sonar measurement used for correction (filtered) @@ -491,8 +491,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(optical_flow), optical_flow_sub, &flow); /* calculate time from previous update */ - float flow_dt = flow_prev > 0 ? (flow.flow_timestamp - flow_prev) * 1e-6f : 0.1f; - flow_prev = flow.flow_timestamp; +// float flow_dt = flow_prev > 0 ? (flow.flow_timestamp - flow_prev) * 1e-6f : 0.1f; +// flow_prev = flow.flow_timestamp; if ((flow.ground_distance_m > 0.31f) && (flow.ground_distance_m < 4.0f) && @@ -550,8 +550,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) /* convert raw flow to angular flow (rad/s) */ float flow_ang[2]; - flow_ang[0] = flow.flow_raw_x * params.flow_k / 1000.0f / flow_dt; - flow_ang[1] = flow.flow_raw_y * params.flow_k / 1000.0f / flow_dt; + //todo check direction of x und y axis + flow_ang[0] = flow.pixel_flow_x_integral/(float)flow.integration_timespan*1000000.0f;//flow.flow_raw_x * params.flow_k / 1000.0f / flow_dt; + flow_ang[1] = flow.pixel_flow_y_integral/(float)flow.integration_timespan*1000000.0f;//flow.flow_raw_y * params.flow_k / 1000.0f / flow_dt; /* flow measurements vector */ float flow_m[3]; flow_m[0] = -flow_ang[0] * flow_dist; diff --git a/src/modules/uORB/topics/optical_flow.h b/src/modules/uORB/topics/optical_flow.h index 0196ae86b..f3731d213 100644 --- a/src/modules/uORB/topics/optical_flow.h +++ b/src/modules/uORB/topics/optical_flow.h @@ -55,16 +55,21 @@ */ struct optical_flow_s { - uint64_t timestamp; /**< in microseconds since system start */ - - uint64_t flow_timestamp; /**< timestamp from flow sensor */ - int16_t flow_raw_x; /**< flow in pixels in X direction, not rotation-compensated */ - int16_t flow_raw_y; /**< flow in pixels in Y direction, not rotation-compensated */ - float flow_comp_x_m; /**< speed over ground in meters, rotation-compensated */ - float flow_comp_y_m; /**< speed over ground in meters, rotation-compensated */ - float ground_distance_m; /**< Altitude / distance to ground in meters */ - uint8_t quality; /**< Quality of the measurement, 0: bad quality, 255: maximum quality */ + uint64_t timestamp; /**< in microseconds since system start */ uint8_t sensor_id; /**< id of the sensor emitting the flow value */ + float pixel_flow_x_integral; /**< accumulated optical flow in radians around x axis */ + float pixel_flow_y_integral; /**< accumulated optical flow in radians around y axis */ + float gyro_x_rate_integral; /**< accumulated gyro value in radians around x axis */ + float gyro_y_rate_integral; /**< accumulated gyro value in radians around y axis */ + float gyro_z_rate_integral; /**< accumulated gyro value in radians around z axis */ + float ground_distance_m; /**< Altitude / distance to ground in meters */ + uint32_t integration_timespan; /** Date: Tue, 28 Oct 2014 12:53:24 +0100 Subject: Flow driver: auto-format fixes --- src/drivers/px4flow/px4flow.cpp | 172 ++++++++++++++++++++++++---------------- 1 file changed, 102 insertions(+), 70 deletions(-) diff --git a/src/drivers/px4flow/px4flow.cpp b/src/drivers/px4flow/px4flow.cpp index 04aba9eae..6c68636c6 100644 --- a/src/drivers/px4flow/px4flow.cpp +++ b/src/drivers/px4flow/px4flow.cpp @@ -76,7 +76,7 @@ #define PX4FLOW_BUS PX4_I2C_BUS_EXPANSION #define I2C_FLOW_ADDRESS 0x42 //* 7-bit address. 8-bit address is 0x84 //range 0x42 - 0x49 - + /* PX4FLOW Registers addresses */ #define PX4FLOW_REG 0x00 /* Measure Register */ @@ -212,8 +212,9 @@ PX4FLOW::~PX4FLOW() stop(); /* free any existing reports */ - if (_reports != nullptr) + if (_reports != nullptr) { delete _reports; + } } int @@ -222,22 +223,25 @@ PX4FLOW::init() int ret = ERROR; /* do I2C init (and probe) first */ - if (I2C::init() != OK) + if (I2C::init() != OK) { goto out; + } /* allocate basic report buffers */ _reports = new RingBuffer(2, sizeof(struct optical_flow_s)); - if (_reports == nullptr) + if (_reports == nullptr) { goto out; + } /* get a publish handle on the px4flow topic */ struct optical_flow_s zero_report; memset(&zero_report, 0, sizeof(zero_report)); _px4flow_topic = orb_advertise(ORB_ID(optical_flow), &zero_report); - if (_px4flow_topic < 0) + if (_px4flow_topic < 0) { debug("failed to create px4flow object. Did you start uOrb?"); + } ret = OK; /* sensor is ok, but we don't really know if it is within range */ @@ -258,7 +262,7 @@ PX4FLOW::probe() if (transfer(nullptr, 0, &val[0], 22) != OK) { return -EIO; } - + // that worked, so start a measurement cycle return measure(); } @@ -271,20 +275,20 @@ PX4FLOW::ioctl(struct file *filp, int cmd, unsigned long arg) case SENSORIOCSPOLLRATE: { switch (arg) { - /* switching to manual polling */ + /* switching to manual polling */ case SENSOR_POLLRATE_MANUAL: stop(); _measure_ticks = 0; return OK; - /* external signalling (DRDY) not supported */ + /* external signalling (DRDY) not supported */ case SENSOR_POLLRATE_EXTERNAL: - /* zero would be bad */ + /* zero would be bad */ case 0: return -EINVAL; - /* set default/max polling rate */ + /* set default/max polling rate */ case SENSOR_POLLRATE_MAX: case SENSOR_POLLRATE_DEFAULT: { /* do we need to start internal polling? */ @@ -294,13 +298,14 @@ PX4FLOW::ioctl(struct file *filp, int cmd, unsigned long arg) _measure_ticks = USEC2TICK(PX4FLOW_CONVERSION_INTERVAL); /* if we need to start the poll state machine, do it */ - if (want_start) + if (want_start) { start(); + } return OK; } - /* adjust to a legal polling interval in Hz */ + /* adjust to a legal polling interval in Hz */ default: { /* do we need to start internal polling? */ bool want_start = (_measure_ticks == 0); @@ -309,15 +314,17 @@ PX4FLOW::ioctl(struct file *filp, int cmd, unsigned long arg) unsigned ticks = USEC2TICK(1000000 / arg); /* check against maximum rate */ - if (ticks < USEC2TICK(PX4FLOW_CONVERSION_INTERVAL)) + if (ticks < USEC2TICK(PX4FLOW_CONVERSION_INTERVAL)) { return -EINVAL; + } /* update interval for next measurement */ _measure_ticks = ticks; /* if we need to start the poll state machine, do it */ - if (want_start) + if (want_start) { start(); + } return OK; } @@ -325,25 +332,29 @@ PX4FLOW::ioctl(struct file *filp, int cmd, unsigned long arg) } case SENSORIOCGPOLLRATE: - if (_measure_ticks == 0) + if (_measure_ticks == 0) { return SENSOR_POLLRATE_MANUAL; + } return (1000 / _measure_ticks); case SENSORIOCSQUEUEDEPTH: { - /* lower bound is mandatory, upper bound is a sanity check */ - if ((arg < 1) || (arg > 100)) - return -EINVAL; + /* lower bound is mandatory, upper bound is a sanity check */ + if ((arg < 1) || (arg > 100)) { + return -EINVAL; + } + + irqstate_t flags = irqsave(); + + if (!_reports->resize(arg)) { + irqrestore(flags); + return -ENOMEM; + } - irqstate_t flags = irqsave(); - if (!_reports->resize(arg)) { irqrestore(flags); - return -ENOMEM; - } - irqrestore(flags); - return OK; - } + return OK; + } case SENSORIOCGQUEUEDEPTH: return _reports->size(); @@ -366,8 +377,9 @@ PX4FLOW::read(struct file *filp, char *buffer, size_t buflen) int ret = 0; /* buffer must be large enough */ - if (count < 1) + if (count < 1) { return -ENOSPC; + } /* if automatic measurement is enabled */ if (_measure_ticks > 0) { @@ -428,13 +440,13 @@ PX4FLOW::measure() uint8_t cmd = PX4FLOW_REG; ret = transfer(&cmd, 1, nullptr, 0); - if (OK != ret) - { + if (OK != ret) { perf_count(_comms_errors); log("i2c::transfer returned %d", ret); - printf("i2c::transfer flow returned %d"); + printf("i2c::transfer flow returned %d"); return ret; } + ret = OK; return ret; @@ -446,14 +458,13 @@ PX4FLOW::collect() int ret = -EIO; /* read from the sensor */ - uint8_t val[22] = {0, 0,0, 0,0, 0,0, 0,0, 0,0, 0,0, 0,0, 0,0, 0,0, 0,0, 0}; + uint8_t val[22] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}; perf_begin(_sample_perf); ret = transfer(nullptr, 0, &val[0], 22); - if (ret < 0) - { + if (ret < 0) { log("error reading from sensor: %d", ret); perf_count(_comms_errors); perf_end(_sample_perf); @@ -478,12 +489,12 @@ PX4FLOW::collect() int16_t gdist = val[21] << 8 | val[20]; struct optical_flow_s report; - report.flow_comp_x_m = float(flowcx)/1000.0f; - report.flow_comp_y_m = float(flowcy)/1000.0f; - report.flow_raw_x= val[3] << 8 | val[2]; - report.flow_raw_y= val[5] << 8 | val[4]; - report.ground_distance_m =float(gdist)/1000.0f; - report.quality= val[10]; + report.flow_comp_x_m = float(flowcx) / 1000.0f; + report.flow_comp_y_m = float(flowcy) / 1000.0f; + report.flow_raw_x = val[3] << 8 | val[2]; + report.flow_raw_y = val[5] << 8 | val[4]; + report.ground_distance_m = float(gdist) / 1000.0f; + report.quality = val[10]; report.sensor_id = 0; report.timestamp = hrt_absolute_time(); @@ -520,11 +531,13 @@ PX4FLOW::start() true, true, true, - SUBSYSTEM_TYPE_OPTICALFLOW}; + SUBSYSTEM_TYPE_OPTICALFLOW + }; static orb_advert_t pub = -1; if (pub > 0) { orb_publish(ORB_ID(subsystem_info), pub, &info); + } else { pub = orb_advertise(ORB_ID(subsystem_info), &info); } @@ -578,8 +591,9 @@ PX4FLOW::cycle() } /* measurement phase */ - if (OK != measure()) + if (OK != measure()) { log("measure error"); + } /* next phase is collection */ _collect_phase = true; @@ -630,33 +644,37 @@ start() { int fd; - if (g_dev != nullptr) + if (g_dev != nullptr) { errx(1, "already started"); + } /* create the driver */ g_dev = new PX4FLOW(PX4FLOW_BUS); - if (g_dev == nullptr) + if (g_dev == nullptr) { goto fail; + } - if (OK != g_dev->init()) + if (OK != g_dev->init()) { goto fail; + } /* set the poll rate to default, starts automatic data collection */ fd = open(PX4FLOW_DEVICE_PATH, O_RDONLY); - if (fd < 0) + if (fd < 0) { goto fail; + } - if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MAX) < 0) + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MAX) < 0) { goto fail; + } exit(0); fail: - if (g_dev != nullptr) - { + if (g_dev != nullptr) { delete g_dev; g_dev = nullptr; } @@ -669,15 +687,14 @@ fail: */ void stop() { - if (g_dev != nullptr) - { + if (g_dev != nullptr) { delete g_dev; g_dev = nullptr; - } - else - { + + } else { errx(1, "driver not running"); } + exit(0); } @@ -695,14 +712,17 @@ test() int fd = open(PX4FLOW_DEVICE_PATH, O_RDONLY); - if (fd < 0) + if (fd < 0) { err(1, "%s open failed (try 'px4flow start' if the driver is not running", PX4FLOW_DEVICE_PATH); + } /* do a simple demand read */ sz = read(fd, &report, sizeof(report)); if (sz != sizeof(report)) - // err(1, "immediate read failed"); + { + warnx("immediate read failed"); + } warnx("single read"); warnx("flowx: %0.2f m/s", (double)report.flow_comp_x_m); @@ -711,8 +731,9 @@ test() /* start the sensor polling at 2Hz */ - if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) + if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) { errx(1, "failed to set 2Hz poll rate"); + } /* read the sensor 5x and report each value */ for (unsigned i = 0; i < 5; i++) { @@ -723,14 +744,16 @@ test() fds.events = POLLIN; ret = poll(&fds, 1, 2000); - if (ret != 1) + if (ret != 1) { errx(1, "timed out waiting for sensor data"); + } /* now go get it */ sz = read(fd, &report, sizeof(report)); - if (sz != sizeof(report)) + if (sz != sizeof(report)) { err(1, "periodic read failed"); + } warnx("periodic read %u", i); warnx("flowx: %0.2f m/s", (double)report.flow_comp_x_m); @@ -751,14 +774,17 @@ reset() { int fd = open(PX4FLOW_DEVICE_PATH, O_RDONLY); - if (fd < 0) + if (fd < 0) { err(1, "failed "); + } - if (ioctl(fd, SENSORIOCRESET, 0) < 0) + if (ioctl(fd, SENSORIOCRESET, 0) < 0) { err(1, "driver reset failed"); + } - if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { err(1, "driver poll restart failed"); + } exit(0); } @@ -769,8 +795,9 @@ reset() void info() { - if (g_dev == nullptr) + if (g_dev == nullptr) { errx(1, "driver not running"); + } printf("state @ %p\n", g_dev); g_dev->print_info(); @@ -786,32 +813,37 @@ px4flow_main(int argc, char *argv[]) /* * Start/load the driver. */ - if (!strcmp(argv[1], "start")) + if (!strcmp(argv[1], "start")) { px4flow::start(); + } - /* - * Stop the driver - */ - if (!strcmp(argv[1], "stop")) - px4flow::stop(); + /* + * Stop the driver + */ + if (!strcmp(argv[1], "stop")) { + px4flow::stop(); + } /* * Test the driver/device. */ - if (!strcmp(argv[1], "test")) + if (!strcmp(argv[1], "test")) { px4flow::test(); + } /* * Reset the driver. */ - if (!strcmp(argv[1], "reset")) + if (!strcmp(argv[1], "reset")) { px4flow::reset(); + } /* * Print driver information. */ - if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status")) + if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status")) { px4flow::info(); + } errx(1, "unrecognized command, try 'start', 'test', 'reset' or 'info'"); } -- cgit v1.2.3 From 08a52ee17eeb3e28087b21743614eeb792ed0239 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 28 Oct 2014 12:54:27 +0100 Subject: Fixed formatting of flow driver --- src/drivers/px4flow/px4flow.cpp | 323 ++++++++++++++++++++++++---------------- 1 file changed, 198 insertions(+), 125 deletions(-) diff --git a/src/drivers/px4flow/px4flow.cpp b/src/drivers/px4flow/px4flow.cpp index f74db1b52..24592a301 100644 --- a/src/drivers/px4flow/px4flow.cpp +++ b/src/drivers/px4flow/px4flow.cpp @@ -76,7 +76,7 @@ #define PX4FLOW_BUS PX4_I2C_BUS_ESC//PX4_I2C_BUS_EXPANSION #define I2C_FLOW_ADDRESS 0x42 //* 7-bit address. 8-bit address is 0x84 //range 0x42 - 0x49 - + /* PX4FLOW Registers addresses */ //#define PX4FLOW_REG 0x00 /* Measure Register 0*/ #define PX4FLOW_REG 0x16 /* Measure Register 22*/ @@ -119,11 +119,12 @@ typedef struct i2c_integral_frame { uint32_t time_since_last_sonar_update; uint16_t ground_distance; uint8_t qual; -}__attribute__((packed)); +} __attribute__((packed)); struct i2c_integral_frame f_integral; -class PX4FLOW: public device::I2C { +class PX4FLOW: public device::I2C +{ public: PX4FLOW(int bus = PX4FLOW_BUS, int address = I2C_FLOW_ADDRESS); virtual ~PX4FLOW(); @@ -144,8 +145,8 @@ protected: private: work_s _work; - RingBuffer *_reports;bool _sensor_ok; - int _measure_ticks;bool _collect_phase; + RingBuffer *_reports; bool _sensor_ok; + int _measure_ticks; bool _collect_phase; orb_advert_t _px4flow_topic; @@ -198,12 +199,13 @@ private: extern "C" __EXPORT int px4flow_main(int argc, char *argv[]); PX4FLOW::PX4FLOW(int bus, int address) : - I2C("PX4FLOW", PX4FLOW_DEVICE_PATH, bus, address, 400000), //400khz - _reports(nullptr), _sensor_ok(false), _measure_ticks(0), _collect_phase( - false), _px4flow_topic(-1), _sample_perf( - perf_alloc(PC_ELAPSED, "px4flow_read")), _comms_errors( + I2C("PX4FLOW", PX4FLOW_DEVICE_PATH, bus, address, 400000), //400khz + _reports(nullptr), _sensor_ok(false), _measure_ticks(0), _collect_phase( + false), _px4flow_topic(-1), _sample_perf( + perf_alloc(PC_ELAPSED, "px4flow_read")), _comms_errors( perf_alloc(PC_COUNT, "px4flow_comms_errors")), _buffer_overflows( - perf_alloc(PC_COUNT, "px4flow_buffer_overflows")) { + perf_alloc(PC_COUNT, "px4flow_buffer_overflows")) +{ // enable debug() calls _debug_enabled = true; @@ -211,117 +213,131 @@ PX4FLOW::PX4FLOW(int bus, int address) : memset(&_work, 0, sizeof(_work)); } -PX4FLOW::~PX4FLOW() { +PX4FLOW::~PX4FLOW() +{ /* make sure we are truly inactive */ stop(); /* free any existing reports */ - if (_reports != nullptr) + if (_reports != nullptr) { delete _reports; + } } -int PX4FLOW::init() { +int PX4FLOW::init() +{ int ret = ERROR; /* do I2C init (and probe) first */ - if (I2C::init() != OK) + if (I2C::init() != OK) { goto out; + } /* allocate basic report buffers */ _reports = new RingBuffer(2, sizeof(optical_flow_s)); - if (_reports == nullptr) + if (_reports == nullptr) { goto out; + } ret = OK; /* sensor is ok, but we don't really know if it is within range */ _sensor_ok = true; - out: return ret; +out: return ret; } -int PX4FLOW::probe() { +int PX4FLOW::probe() +{ return measure(); } -int PX4FLOW::ioctl(struct file *filp, int cmd, unsigned long arg) { +int PX4FLOW::ioctl(struct file *filp, int cmd, unsigned long arg) +{ switch (cmd) { case SENSORIOCSPOLLRATE: { - switch (arg) { + switch (arg) { - /* switching to manual polling */ - case SENSOR_POLLRATE_MANUAL: - stop(); - _measure_ticks = 0; - return OK; + /* switching to manual polling */ + case SENSOR_POLLRATE_MANUAL: + stop(); + _measure_ticks = 0; + return OK; /* external signalling (DRDY) not supported */ - case SENSOR_POLLRATE_EXTERNAL: + case SENSOR_POLLRATE_EXTERNAL: /* zero would be bad */ - case 0: - return -EINVAL; + case 0: + return -EINVAL; /* set default/max polling rate */ - case SENSOR_POLLRATE_MAX: - case SENSOR_POLLRATE_DEFAULT: { - /* do we need to start internal polling? */ - bool want_start = (_measure_ticks == 0); + case SENSOR_POLLRATE_MAX: + case SENSOR_POLLRATE_DEFAULT: { + /* do we need to start internal polling? */ + bool want_start = (_measure_ticks == 0); - /* set interval for next measurement to minimum legal value */ - _measure_ticks = USEC2TICK(PX4FLOW_CONVERSION_INTERVAL); + /* set interval for next measurement to minimum legal value */ + _measure_ticks = USEC2TICK(PX4FLOW_CONVERSION_INTERVAL); - /* if we need to start the poll state machine, do it */ - if (want_start) - start(); + /* if we need to start the poll state machine, do it */ + if (want_start) { + start(); + } - return OK; - } + return OK; + } /* adjust to a legal polling interval in Hz */ - default: { - /* do we need to start internal polling? */ - bool want_start = (_measure_ticks == 0); + default: { + /* do we need to start internal polling? */ + bool want_start = (_measure_ticks == 0); - /* convert hz to tick interval via microseconds */ - unsigned ticks = USEC2TICK(1000000 / arg); + /* convert hz to tick interval via microseconds */ + unsigned ticks = USEC2TICK(1000000 / arg); - /* check against maximum rate */ - if (ticks < USEC2TICK(PX4FLOW_CONVERSION_INTERVAL)) - return -EINVAL; + /* check against maximum rate */ + if (ticks < USEC2TICK(PX4FLOW_CONVERSION_INTERVAL)) { + return -EINVAL; + } - /* update interval for next measurement */ - _measure_ticks = ticks; + /* update interval for next measurement */ + _measure_ticks = ticks; - /* if we need to start the poll state machine, do it */ - if (want_start) - start(); + /* if we need to start the poll state machine, do it */ + if (want_start) { + start(); + } - return OK; - } + return OK; + } + } } - } case SENSORIOCGPOLLRATE: - if (_measure_ticks == 0) + if (_measure_ticks == 0) { return SENSOR_POLLRATE_MANUAL; + } return (1000 / _measure_ticks); case SENSORIOCSQUEUEDEPTH: { - /* lower bound is mandatory, upper bound is a sanity check */ - if ((arg < 1) || (arg > 100)) - return -EINVAL; + /* lower bound is mandatory, upper bound is a sanity check */ + if ((arg < 1) || (arg > 100)) { + return -EINVAL; + } + + irqstate_t flags = irqsave(); + + if (!_reports->resize(arg)) { + irqrestore(flags); + return -ENOMEM; + } - irqstate_t flags = irqsave(); - if (!_reports->resize(arg)) { irqrestore(flags); - return -ENOMEM; - } - irqrestore(flags); - return OK; - } + return OK; + } case SENSORIOCGQUEUEDEPTH: return _reports->size(); @@ -336,15 +352,17 @@ int PX4FLOW::ioctl(struct file *filp, int cmd, unsigned long arg) { } } -ssize_t PX4FLOW::read(struct file *filp, char *buffer, size_t buflen) { +ssize_t PX4FLOW::read(struct file *filp, char *buffer, size_t buflen) +{ unsigned count = buflen / sizeof(struct optical_flow_s); struct optical_flow_s *rbuf = - reinterpret_cast(buffer); + reinterpret_cast(buffer); int ret = 0; /* buffer must be large enough */ - if (count < 1) + if (count < 1) { return -ENOSPC; + } /* if automatic measurement is enabled */ if (_measure_ticks > 0) { @@ -397,7 +415,8 @@ ssize_t PX4FLOW::read(struct file *filp, char *buffer, size_t buflen) { return ret; } -int PX4FLOW::measure() { +int PX4FLOW::measure() +{ int ret; /* @@ -412,12 +431,14 @@ int PX4FLOW::measure() { printf("i2c::transfer flow returned %d"); return ret; } + ret = OK; return ret; } -int PX4FLOW::collect() { +int PX4FLOW::collect() +{ int ret = -EIO; /* read from the sensor */ @@ -426,10 +447,11 @@ int PX4FLOW::collect() { perf_begin(_sample_perf); - if(PX4FLOW_REG==0x00){ + if (PX4FLOW_REG == 0x00) { ret = transfer(nullptr, 0, &val[0], 45); //read 45 bytes (22+23 : frame1 + frame2) } - if(PX4FLOW_REG==0x16){ + + if (PX4FLOW_REG == 0x16) { ret = transfer(nullptr, 0, &val[0], 23); //read 23 bytes (only frame2) } @@ -461,42 +483,56 @@ int PX4FLOW::collect() { f_integral.gyro_y_rate_integral = val[31] << 8 | val[30]; f_integral.gyro_z_rate_integral = val[33] << 8 | val[32]; f_integral.integration_timespan = val[37] << 24 | val[36] << 16 - | val[35] << 8 | val[34]; + | val[35] << 8 | val[34]; f_integral.time_since_last_sonar_update = val[41] << 24 | val[40] << 16 | val[39] << 8 | val[38]; f_integral.ground_distance = val[43] << 8 | val[42]; f_integral.qual = val[44]; } - if(PX4FLOW_REG==0x16){ + + if (PX4FLOW_REG == 0x16) { f_integral.frame_count_since_last_readout = val[1] << 8 | val[0]; - f_integral.pixel_flow_x_integral =val[3] << 8 | val[2]; - f_integral.pixel_flow_y_integral =val[5] << 8 | val[4]; - f_integral.gyro_x_rate_integral =val[7] << 8 | val[6]; - f_integral.gyro_y_rate_integral =val[9] << 8 | val[8]; - f_integral.gyro_z_rate_integral =val[11] << 8 | val[10]; - f_integral.integration_timespan = val[15] <<24 |val[14] << 16 |val[13] << 8 |val[12]; - f_integral.time_since_last_sonar_update = val[19] <<24 |val[18] << 16 |val[17] << 8 |val[16]; - f_integral.ground_distance =val[21] <<8 |val[20]; - f_integral.qual =val[22]; + f_integral.pixel_flow_x_integral = val[3] << 8 | val[2]; + f_integral.pixel_flow_y_integral = val[5] << 8 | val[4]; + f_integral.gyro_x_rate_integral = val[7] << 8 | val[6]; + f_integral.gyro_y_rate_integral = val[9] << 8 | val[8]; + f_integral.gyro_z_rate_integral = val[11] << 8 | val[10]; + f_integral.integration_timespan = val[15] << 24 | val[14] << 16 | val[13] << 8 | val[12]; + f_integral.time_since_last_sonar_update = val[19] << 24 | val[18] << 16 | val[17] << 8 | val[16]; + f_integral.ground_distance = val[21] << 8 | val[20]; + f_integral.qual = val[22]; } struct optical_flow_s report; + report.timestamp = hrt_absolute_time(); + report.pixel_flow_x_integral = float(f_integral.pixel_flow_x_integral) / 10000.0f;//convert to radians + report.pixel_flow_y_integral = float(f_integral.pixel_flow_y_integral) / 10000.0f;//convert to radians + report.frame_count_since_last_readout = f_integral.frame_count_since_last_readout; + report.ground_distance_m = float(f_integral.ground_distance) / 1000.0f;//convert to meters + report.quality = f_integral.qual;//0:bad ; 255 max quality - report.gyro_x_rate_integral= float(f_integral.gyro_x_rate_integral)/10000.0f;//convert to radians - report.gyro_y_rate_integral= float(f_integral.gyro_y_rate_integral)/10000.0f;//convert to radians - report.gyro_z_rate_integral= float(f_integral.gyro_z_rate_integral)/10000.0f;//convert to radians - report.integration_timespan= f_integral.integration_timespan;//microseconds + + report.gyro_x_rate_integral = float(f_integral.gyro_x_rate_integral) / 10000.0f; //convert to radians + + report.gyro_y_rate_integral = float(f_integral.gyro_y_rate_integral) / 10000.0f; //convert to radians + + report.gyro_z_rate_integral = float(f_integral.gyro_z_rate_integral) / 10000.0f; //convert to radians + + report.integration_timespan = f_integral.integration_timespan; //microseconds + report.time_since_last_sonar_update = f_integral.time_since_last_sonar_update;//microseconds + report.sensor_id = 0; if (_px4flow_topic < 0) { _px4flow_topic = orb_advertise(ORB_ID(optical_flow), &report); + } else { /* publish it */ orb_publish(ORB_ID(optical_flow), _px4flow_topic, &report); @@ -516,7 +552,8 @@ int PX4FLOW::collect() { return ret; } -void PX4FLOW::start() { +void PX4FLOW::start() +{ /* reset the report ring and state machine */ _collect_phase = false; _reports->flush(); @@ -526,33 +563,39 @@ void PX4FLOW::start() { /* notify about state change */ struct subsystem_info_s info = { true, true, true, - SUBSYSTEM_TYPE_OPTICALFLOW }; + SUBSYSTEM_TYPE_OPTICALFLOW + }; static orb_advert_t pub = -1; if (pub > 0) { orb_publish(ORB_ID(subsystem_info), pub, &info); + } else { pub = orb_advertise(ORB_ID(subsystem_info), &info); } } -void PX4FLOW::stop() { +void PX4FLOW::stop() +{ work_cancel(HPWORK, &_work); } -void PX4FLOW::cycle_trampoline(void *arg) { +void PX4FLOW::cycle_trampoline(void *arg) +{ PX4FLOW *dev = (PX4FLOW *) arg; dev->cycle(); } -void PX4FLOW::cycle() { +void PX4FLOW::cycle() +{ // /* collection phase? */ // static uint64_t deltatime = hrt_absolute_time(); - if (OK != measure()) + if (OK != measure()) { log("measure error"); + } //usleep(PX4FLOW_CONVERSION_INTERVAL/40); @@ -577,13 +620,14 @@ void PX4FLOW::cycle() { // _measure_ticks-USEC2TICK(deltatime)); work_queue(HPWORK, &_work, (worker_t) & PX4FLOW::cycle_trampoline, this, - _measure_ticks); + _measure_ticks); // deltatime = hrt_absolute_time(); } -void PX4FLOW::print_info() { +void PX4FLOW::print_info() +{ perf_print_counter(_sample_perf); perf_print_counter(_comms_errors); perf_print_counter(_buffer_overflows); @@ -594,7 +638,8 @@ void PX4FLOW::print_info() { /** * Local functions in support of the shell command. */ -namespace px4flow { +namespace px4flow +{ /* oddly, ERROR is not defined for c++ */ #ifdef ERROR @@ -613,33 +658,39 @@ void info(); /** * Start the driver. */ -void start() { +void start() +{ int fd; - if (g_dev != nullptr) + if (g_dev != nullptr) { errx(1, "already started"); + } /* create the driver */ g_dev = new PX4FLOW(PX4FLOW_BUS); - if (g_dev == nullptr) + if (g_dev == nullptr) { goto fail; + } - if (OK != g_dev->init()) + if (OK != g_dev->init()) { goto fail; + } /* set the poll rate to default, starts automatic data collection */ fd = open(PX4FLOW_DEVICE_PATH, O_RDONLY); - if (fd < 0) + if (fd < 0) { goto fail; + } - if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MAX) < 0) + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MAX) < 0) { goto fail; + } exit(0); - fail: +fail: if (g_dev != nullptr) { delete g_dev; @@ -652,13 +703,16 @@ void start() { /** * Stop the driver */ -void stop() { +void stop() +{ if (g_dev != nullptr) { delete g_dev; g_dev = nullptr; + } else { errx(1, "driver not running"); } + exit(0); } @@ -667,7 +721,8 @@ void stop() { * make sure we can collect data from the sensor in polled * and automatic modes. */ -void test() { +void test() +{ struct optical_flow_s report; ssize_t sz; int ret; @@ -676,8 +731,8 @@ void test() { if (fd < 0) err(1, - "%s open failed (try 'px4flow start' if the driver is not running", - PX4FLOW_DEVICE_PATH); + "%s open failed (try 'px4flow start' if the driver is not running", + PX4FLOW_DEVICE_PATH); /* do a simple demand read */ sz = read(fd, &report, sizeof(report)); @@ -685,14 +740,18 @@ void test() { if (sz != sizeof(report)) // err(1, "immediate read failed"); + { warnx("single read"); + } + warnx("flowx: %0.2f m/s", (double) f.pixel_flow_x_sum); warnx("flowy: %0.2f m/s", (double) f.pixel_flow_y_sum); warnx("time: %lld", report.timestamp); /* start the sensor polling at 10Hz */ - if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 10)) // 2)) + if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 10)) { // 2)) errx(1, "failed to set 10Hz poll rate"); + } /* read the sensor 5x and report each value */ for (unsigned i = 0; i < 10; i++) { @@ -703,20 +762,22 @@ void test() { fds.events = POLLIN; ret = poll(&fds, 1, 2000); - if (ret != 1) + if (ret != 1) { errx(1, "timed out waiting for sensor data"); + } /* now go get it */ sz = read(fd, &report, sizeof(report)); - if (sz != sizeof(report)) + if (sz != sizeof(report)) { err(1, "periodic read failed"); + } warnx("periodic read %u", i); warnx("framecount_total: %u", f.frame_count); warnx("framecount_integral: %u", - f_integral.frame_count_since_last_readout); + f_integral.frame_count_since_last_readout); warnx("pixel_flow_x_integral: %i", f_integral.pixel_flow_x_integral); warnx("pixel_flow_y_integral: %i", f_integral.pixel_flow_y_integral); warnx("gyro_x_rate_integral: %i", f_integral.gyro_x_rate_integral); @@ -724,9 +785,9 @@ void test() { warnx("gyro_z_rate_integral: %i", f_integral.gyro_z_rate_integral); warnx("integration_timespan [us]: %u", f_integral.integration_timespan); warnx("ground_distance: %0.2f m", - (double) f_integral.ground_distance / 1000); + (double) f_integral.ground_distance / 1000); warnx("time since last sonar update [us]: %i", - f_integral.time_since_last_sonar_update); + f_integral.time_since_last_sonar_update); warnx("quality integration average : %i", f_integral.qual); warnx("quality : %i", f.qual); @@ -739,17 +800,21 @@ void test() { /** * Reset the driver. */ -void reset() { +void reset() +{ int fd = open(PX4FLOW_DEVICE_PATH, O_RDONLY); - if (fd < 0) + if (fd < 0) { err(1, "failed "); + } - if (ioctl(fd, SENSORIOCRESET, 0) < 0) + if (ioctl(fd, SENSORIOCRESET, 0) < 0) { err(1, "driver reset failed"); + } - if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { err(1, "driver poll restart failed"); + } exit(0); } @@ -757,9 +822,11 @@ void reset() { /** * Print a little info about the driver. */ -void info() { - if (g_dev == nullptr) +void info() +{ + if (g_dev == nullptr) { errx(1, "driver not running"); + } printf("state @ %p\n", g_dev); g_dev->print_info(); @@ -769,36 +836,42 @@ void info() { } // namespace -int px4flow_main(int argc, char *argv[]) { +int px4flow_main(int argc, char *argv[]) +{ /* * Start/load the driver. */ - if (!strcmp(argv[1], "start")) + if (!strcmp(argv[1], "start")) { px4flow::start(); + } /* * Stop the driver */ - if (!strcmp(argv[1], "stop")) + if (!strcmp(argv[1], "stop")) { px4flow::stop(); + } /* * Test the driver/device. */ - if (!strcmp(argv[1], "test")) + if (!strcmp(argv[1], "test")) { px4flow::test(); + } /* * Reset the driver. */ - if (!strcmp(argv[1], "reset")) + if (!strcmp(argv[1], "reset")) { px4flow::reset(); + } /* * Print driver information. */ - if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status")) + if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status")) { px4flow::info(); + } errx(1, "unrecognized command, try 'start', 'test', 'reset' or 'info'"); } -- cgit v1.2.3 From 1418254fca7ec97ea2b502ce227a77a6957424b9 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 28 Oct 2014 13:10:39 +0100 Subject: Formatting fixes --- src/drivers/px4flow/px4flow.cpp | 104 ++++++++++++++++++++++++---------------- 1 file changed, 63 insertions(+), 41 deletions(-) diff --git a/src/drivers/px4flow/px4flow.cpp b/src/drivers/px4flow/px4flow.cpp index 29f26cab9..96c0b720c 100644 --- a/src/drivers/px4flow/px4flow.cpp +++ b/src/drivers/px4flow/px4flow.cpp @@ -129,30 +129,32 @@ public: PX4FLOW(int bus = PX4FLOW_BUS, int address = I2C_FLOW_ADDRESS); virtual ~PX4FLOW(); - virtual int init(); + virtual int init(); - virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); - virtual int ioctl(struct file *filp, int cmd, unsigned long arg); + virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); + virtual int ioctl(struct file *filp, int cmd, unsigned long arg); /** * Diagnostics - print some basic information about the driver. */ - void print_info(); + void print_info(); protected: - virtual int probe(); + virtual int probe(); private: - work_s _work; - RingBuffer *_reports; bool _sensor_ok; - int _measure_ticks; bool _collect_phase; + work_s _work; + RingBuffer *_reports; + bool _sensor_ok; + int _measure_ticks; + bool _collect_phase; - orb_advert_t _px4flow_topic; + orb_advert_t _px4flow_topic; - perf_counter_t _sample_perf; - perf_counter_t _comms_errors; - perf_counter_t _buffer_overflows; + perf_counter_t _sample_perf; + perf_counter_t _comms_errors; + perf_counter_t _buffer_overflows; /** * Test whether the device supported by the driver is present at a @@ -161,7 +163,7 @@ private: * @param address The I2C bus address to probe. * @return True if the device is present. */ - int probe_address(uint8_t address); + int probe_address(uint8_t address); /** * Initialise the automatic measurement state machine and start it. @@ -169,27 +171,27 @@ private: * @note This function is called at open and error time. It might make sense * to make it more aggressive about resetting the bus in case of errors. */ - void start(); + void start(); /** * Stop the automatic measurement state machine. */ - void stop(); + void stop(); /** * Perform a poll cycle; collect from the previous measurement * and start a new one. */ - void cycle(); - int measure(); - int collect(); + void cycle(); + int measure(); + int collect(); /** * Static trampoline from the workq context; because we don't have a * generic workq wrapper yet. * * @param arg Instance pointer for the driver that is polling. */ - static void cycle_trampoline(void *arg); + static void cycle_trampoline(void *arg); }; @@ -200,11 +202,14 @@ extern "C" __EXPORT int px4flow_main(int argc, char *argv[]); PX4FLOW::PX4FLOW(int bus, int address) : I2C("PX4FLOW", PX4FLOW_DEVICE_PATH, bus, address, 400000), //400khz - _reports(nullptr), _sensor_ok(false), _measure_ticks(0), _collect_phase( - false), _px4flow_topic(-1), _sample_perf( - perf_alloc(PC_ELAPSED, "px4flow_read")), _comms_errors( - perf_alloc(PC_COUNT, "px4flow_comms_errors")), _buffer_overflows( - perf_alloc(PC_COUNT, "px4flow_buffer_overflows")) + _reports(nullptr), + _sensor_ok(false), + _measure_ticks(0), + _collect_phase(false), + _px4flow_topic(-1), + _sample_perf(perf_alloc(PC_ELAPSED, "px4flow_read")), + _comms_errors(perf_alloc(PC_COUNT, "px4flow_comms_errors")), + _buffer_overflows(perf_alloc(PC_COUNT, "px4flow_buffer_overflows")) { // enable debug() calls _debug_enabled = true; @@ -224,7 +229,8 @@ PX4FLOW::~PX4FLOW() } } -int PX4FLOW::init() +int +PX4FLOW::init() { int ret = ERROR; @@ -246,7 +252,8 @@ int PX4FLOW::init() out: return ret; } -int PX4FLOW::probe() +int +PX4FLOW::probe() { uint8_t val[22]; @@ -262,7 +269,8 @@ int PX4FLOW::probe() return measure(); } -int PX4FLOW::ioctl(struct file *filp, int cmd, unsigned long arg) +int +PX4FLOW::ioctl(struct file *filp, int cmd, unsigned long arg) { switch (cmd) { @@ -363,7 +371,8 @@ int PX4FLOW::ioctl(struct file *filp, int cmd, unsigned long arg) } } -ssize_t PX4FLOW::read(struct file *filp, char *buffer, size_t buflen) +ssize_t +PX4FLOW::read(struct file *filp, char *buffer, size_t buflen) { unsigned count = buflen / sizeof(struct optical_flow_s); struct optical_flow_s *rbuf = @@ -426,7 +435,8 @@ ssize_t PX4FLOW::read(struct file *filp, char *buffer, size_t buflen) return ret; } -int PX4FLOW::measure() +int +PX4FLOW::measure() { int ret; @@ -448,7 +458,8 @@ int PX4FLOW::measure() return ret; } -int PX4FLOW::collect() +int +PX4FLOW::collect() { int ret = -EIO; @@ -562,7 +573,8 @@ int PX4FLOW::collect() return ret; } -void PX4FLOW::start() +void +PX4FLOW::start() { /* reset the report ring and state machine */ _collect_phase = false; @@ -588,19 +600,22 @@ void PX4FLOW::start() } } -void PX4FLOW::stop() +void +PX4FLOW::stop() { work_cancel(HPWORK, &_work); } -void PX4FLOW::cycle_trampoline(void *arg) +void +PX4FLOW::cycle_trampoline(void *arg) { PX4FLOW *dev = (PX4FLOW *) arg; dev->cycle(); } -void PX4FLOW::cycle() +void +PX4FLOW::cycle() { if (OK != measure()) { log("measure error"); @@ -619,7 +634,8 @@ void PX4FLOW::cycle() } -void PX4FLOW::print_info() +void +PX4FLOW::print_info() { perf_print_counter(_sample_perf); perf_print_counter(_comms_errors); @@ -651,7 +667,8 @@ void info(); /** * Start the driver. */ -void start() +void +start() { int fd; @@ -696,7 +713,8 @@ fail: /** * Stop the driver */ -void stop() +void +stop() { if (g_dev != nullptr) { delete g_dev; @@ -714,7 +732,8 @@ void stop() * make sure we can collect data from the sensor in polled * and automatic modes. */ -void test() +void +test() { struct optical_flow_s report; ssize_t sz; @@ -792,7 +811,8 @@ void test() /** * Reset the driver. */ -void reset() +void +reset() { int fd = open(PX4FLOW_DEVICE_PATH, O_RDONLY); @@ -814,7 +834,8 @@ void reset() /** * Print a little info about the driver. */ -void info() +void +info() { if (g_dev == nullptr) { errx(1, "driver not running"); @@ -828,7 +849,8 @@ void info() } // namespace -int px4flow_main(int argc, char *argv[]) +int +px4flow_main(int argc, char *argv[]) { /* * Start/load the driver. -- cgit v1.2.3 From fd4418278e0dbd9442c33a0fe8fcf11d86804092 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 28 Oct 2014 13:14:37 +0100 Subject: More formatting and cast fixes --- src/drivers/px4flow/px4flow.cpp | 35 +++++++++++------------------------ 1 file changed, 11 insertions(+), 24 deletions(-) diff --git a/src/drivers/px4flow/px4flow.cpp b/src/drivers/px4flow/px4flow.cpp index 96c0b720c..e9b2de629 100644 --- a/src/drivers/px4flow/px4flow.cpp +++ b/src/drivers/px4flow/px4flow.cpp @@ -145,9 +145,9 @@ protected: private: work_s _work; - RingBuffer *_reports; + RingBuffer *_reports; bool _sensor_ok; - int _measure_ticks; + int _measure_ticks; bool _collect_phase; orb_advert_t _px4flow_topic; @@ -249,7 +249,8 @@ PX4FLOW::init() ret = OK; /* sensor is ok, but we don't really know if it is within range */ _sensor_ok = true; -out: return ret; +out: + return ret; } int @@ -375,8 +376,7 @@ ssize_t PX4FLOW::read(struct file *filp, char *buffer, size_t buflen) { unsigned count = buflen / sizeof(struct optical_flow_s); - struct optical_flow_s *rbuf = - reinterpret_cast(buffer); + struct optical_flow_s *rbuf = reinterpret_cast(buffer); int ret = 0; /* buffer must be large enough */ @@ -413,9 +413,6 @@ PX4FLOW::read(struct file *filp, char *buffer, size_t buflen) break; } -// /* wait for it to complete */ -// usleep(PX4FLOW_CONVERSION_INTERVAL); - /* run the collection phase */ if (OK != collect()) { ret = -EIO; @@ -528,25 +525,15 @@ PX4FLOW::collect() struct optical_flow_s report; report.timestamp = hrt_absolute_time(); - - report.pixel_flow_x_integral = float(f_integral.pixel_flow_x_integral) / 10000.0f;//convert to radians - - report.pixel_flow_y_integral = float(f_integral.pixel_flow_y_integral) / 10000.0f;//convert to radians - + report.pixel_flow_x_integral = static_cast(f_integral.pixel_flow_x_integral) / 10000.0f;//convert to radians + report.pixel_flow_y_integral = static_cast(f_integral.pixel_flow_y_integral) / 10000.0f;//convert to radians report.frame_count_since_last_readout = f_integral.frame_count_since_last_readout; - - report.ground_distance_m = float(f_integral.ground_distance) / 1000.0f;//convert to meters - + report.ground_distance_m = static_cast(f_integral.ground_distance) / 1000.0f;//convert to meters report.quality = f_integral.qual; //0:bad ; 255 max quality - - report.gyro_x_rate_integral = float(f_integral.gyro_x_rate_integral) / 10000.0f; //convert to radians - - report.gyro_y_rate_integral = float(f_integral.gyro_y_rate_integral) / 10000.0f; //convert to radians - - report.gyro_z_rate_integral = float(f_integral.gyro_z_rate_integral) / 10000.0f; //convert to radians - + report.gyro_x_rate_integral = static_cast(f_integral.gyro_x_rate_integral) / 10000.0f; //convert to radians + report.gyro_y_rate_integral = static_cast(f_integral.gyro_y_rate_integral) / 10000.0f; //convert to radians + report.gyro_z_rate_integral = static_cast(f_integral.gyro_z_rate_integral) / 10000.0f; //convert to radians report.integration_timespan = f_integral.integration_timespan; //microseconds - report.time_since_last_sonar_update = f_integral.time_since_last_sonar_update;//microseconds report.sensor_id = 0; -- cgit v1.2.3 From 16f33ee6b5b92e244bb413bad5530882fac6644d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 28 Oct 2014 13:17:00 +0100 Subject: More formatting fixes --- src/drivers/px4flow/px4flow.cpp | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/src/drivers/px4flow/px4flow.cpp b/src/drivers/px4flow/px4flow.cpp index e9b2de629..818b34a5e 100644 --- a/src/drivers/px4flow/px4flow.cpp +++ b/src/drivers/px4flow/px4flow.cpp @@ -568,7 +568,7 @@ PX4FLOW::start() _reports->flush(); /* schedule a cycle to start things */ - work_queue(HPWORK, &_work, (worker_t) & PX4FLOW::cycle_trampoline, this, 1); + work_queue(HPWORK, &_work, (worker_t)&PX4FLOW::cycle_trampoline, this, 1); /* notify about state change */ struct subsystem_info_s info = { @@ -596,7 +596,7 @@ PX4FLOW::stop() void PX4FLOW::cycle_trampoline(void *arg) { - PX4FLOW *dev = (PX4FLOW *) arg; + PX4FLOW *dev = (PX4FLOW *)arg; dev->cycle(); } @@ -616,7 +616,7 @@ PX4FLOW::cycle() return; } - work_queue(HPWORK, &_work, (worker_t) & PX4FLOW::cycle_trampoline, this, + work_queue(HPWORK, &_work, (worker_t)&PX4FLOW::cycle_trampoline, this, _measure_ticks); } @@ -643,13 +643,13 @@ namespace px4flow #endif const int ERROR = -1; -PX4FLOW *g_dev; +PX4FLOW *g_dev; -void start(); -void stop(); -void test(); -void reset(); -void info(); +void start(); +void stop(); +void test(); +void reset(); +void info(); /** * Start the driver. -- cgit v1.2.3 From 774ff3db31309c2f2aeb235d751cbdfc9e20baf8 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 28 Oct 2014 13:22:25 +0100 Subject: Scan both buses --- src/drivers/px4flow/px4flow.cpp | 18 +++++++++++++----- 1 file changed, 13 insertions(+), 5 deletions(-) diff --git a/src/drivers/px4flow/px4flow.cpp b/src/drivers/px4flow/px4flow.cpp index 818b34a5e..ed788a46e 100644 --- a/src/drivers/px4flow/px4flow.cpp +++ b/src/drivers/px4flow/px4flow.cpp @@ -73,13 +73,11 @@ #include /* Configuration Constants */ -#define PX4FLOW_BUS PX4_I2C_BUS_ESC//PX4_I2C_BUS_EXPANSION #define I2C_FLOW_ADDRESS 0x42 //* 7-bit address. 8-bit address is 0x84 //range 0x42 - 0x49 /* PX4FLOW Registers addresses */ -//#define PX4FLOW_REG 0x00 /* Measure Register 0*/ -#define PX4FLOW_REG 0x16 /* Measure Register 22*/ +#define PX4FLOW_REG 0x16 /* Measure Register 22*/ #define PX4FLOW_CONVERSION_INTERVAL 20000 //in microseconds! 20000 = 50 Hz 100000 = 10Hz /* oddly, ERROR is not defined for c++ */ @@ -664,14 +662,24 @@ start() } /* create the driver */ - g_dev = new PX4FLOW(PX4FLOW_BUS); + g_dev = new PX4FLOW(PX4_I2C_BUS_EXPANSION); if (g_dev == nullptr) { goto fail; } if (OK != g_dev->init()) { - goto fail; + delete g_dev; + /* try 2nd bus */ + g_dev = new PX4FLOW(PX4_I2C_BUS_ESC); + + if (g_dev == nullptr) { + goto fail; + } + + if (OK != g_dev->init()) { + goto fail; + } } /* set the poll rate to default, starts automatic data collection */ -- cgit v1.2.3 From f443b77ae9351b360a0c735122b4c4c1629fa870 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 28 Oct 2014 13:24:12 +0100 Subject: Kill last usleep() --- src/drivers/px4flow/px4flow.cpp | 3 --- 1 file changed, 3 deletions(-) diff --git a/src/drivers/px4flow/px4flow.cpp b/src/drivers/px4flow/px4flow.cpp index ed788a46e..3607adf9c 100644 --- a/src/drivers/px4flow/px4flow.cpp +++ b/src/drivers/px4flow/px4flow.cpp @@ -417,9 +417,6 @@ PX4FLOW::read(struct file *filp, char *buffer, size_t buflen) break; } - /* wait for it to complete */ - usleep(PX4FLOW_CONVERSION_INTERVAL); - /* state machine will have generated a report, copy it out */ if (_reports->get(rbuf)) { ret = sizeof(*rbuf); -- cgit v1.2.3 From 2e4931e3cae181ee1092c693d548437641b4e1d3 Mon Sep 17 00:00:00 2001 From: dominiho Date: Tue, 28 Oct 2014 14:04:50 +0100 Subject: deleted last PX4FLOW_BUS to enable scan on both buses --- src/drivers/px4flow/px4flow.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/px4flow/px4flow.cpp b/src/drivers/px4flow/px4flow.cpp index 3607adf9c..87bcb32ec 100644 --- a/src/drivers/px4flow/px4flow.cpp +++ b/src/drivers/px4flow/px4flow.cpp @@ -124,7 +124,7 @@ struct i2c_integral_frame f_integral; class PX4FLOW: public device::I2C { public: - PX4FLOW(int bus = PX4FLOW_BUS, int address = I2C_FLOW_ADDRESS); + PX4FLOW(int bus, int address = I2C_FLOW_ADDRESS); virtual ~PX4FLOW(); virtual int init(); -- cgit v1.2.3 From 4a3a16475e3d900cb2b41a0f5fb4bc1f46077b7d Mon Sep 17 00:00:00 2001 From: dominiho Date: Tue, 28 Oct 2014 15:05:41 +0100 Subject: scan also 3rd available bus --- src/drivers/px4flow/px4flow.cpp | 12 +++++++++++- 1 file changed, 11 insertions(+), 1 deletion(-) diff --git a/src/drivers/px4flow/px4flow.cpp b/src/drivers/px4flow/px4flow.cpp index 87bcb32ec..c3660a967 100644 --- a/src/drivers/px4flow/px4flow.cpp +++ b/src/drivers/px4flow/px4flow.cpp @@ -675,7 +675,17 @@ start() } if (OK != g_dev->init()) { - goto fail; + delete g_dev; + /* try 3rd bus */ + g_dev = new PX4FLOW(PX4_I2C_BUS_ONBOARD); + + if (g_dev == nullptr) { + goto fail; + } + + if (OK != g_dev->init()) { + goto fail; + } } } -- cgit v1.2.3 From aa7b00f8199e167b94a7c5e9e4e004bbd50b2f68 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 29 Oct 2014 09:58:44 +0100 Subject: Add define to cull flash-intense mathlib tests --- src/systemcmds/tests/tests_main.c | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/systemcmds/tests/tests_main.c b/src/systemcmds/tests/tests_main.c index e3f26924f..0f56704e6 100644 --- a/src/systemcmds/tests/tests_main.c +++ b/src/systemcmds/tests/tests_main.c @@ -110,7 +110,9 @@ const struct { {"conv", test_conv, OPT_NOJIGTEST | OPT_NOALLTEST}, {"mount", test_mount, OPT_NOJIGTEST | OPT_NOALLTEST}, {"mtd", test_mtd, 0}, +#ifndef TESTS_MATHLIB_DISABLE {"mathlib", test_mathlib, 0}, +#endif {"help", test_help, OPT_NOALLTEST | OPT_NOHELP | OPT_NOJIGTEST}, {NULL, NULL, 0} }; -- cgit v1.2.3 From a6c6ae023de74f87ee593e8c914d66e7a8be7011 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 29 Oct 2014 14:04:33 +0100 Subject: Autoformatting (reviewed) IO sbus handler. No functionality changes. --- src/modules/px4iofirmware/sbus.c | 55 ++++++++++++++++++++++++---------------- 1 file changed, 33 insertions(+), 22 deletions(-) diff --git a/src/modules/px4iofirmware/sbus.c b/src/modules/px4iofirmware/sbus.c index 0f0414148..6ead38d61 100644 --- a/src/modules/px4iofirmware/sbus.c +++ b/src/modules/px4iofirmware/sbus.c @@ -61,7 +61,7 @@ /* Measured values with Futaba FX-30/R6108SB: -+100% on TX: PCM 1.100/1.520/1.950ms -> SBus raw values: 350/1024/1700 (100% ATV) - -+140% on TX: PCM 0.930/1.520/2.112ms -> SBus raw values: 78/1024/1964 (140% ATV) + -+140% on TX: PCM 0.930/1.520/2.112ms -> SBus raw values: 78/1024/1964 (140% ATV) -+152% on TX: PCM 0.884/1.520/2.160ms -> SBus raw values: 1/1024/2047 (140% ATV plus dirty tricks) */ @@ -87,13 +87,15 @@ static unsigned partial_frame_count; unsigned sbus_frame_drops; -static bool sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_channels); +static bool sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, + bool *sbus_frame_drop, uint16_t max_channels); int sbus_init(const char *device) { - if (sbus_fd < 0) + if (sbus_fd < 0) { sbus_fd = open(device, O_RDWR | O_NONBLOCK); + } if (sbus_fd >= 0) { struct termios t; @@ -113,6 +115,7 @@ sbus_init(const char *device) } else { debug("S.Bus: open failed"); } + return sbus_fd; } @@ -167,8 +170,9 @@ sbus_input(uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sb ret = read(sbus_fd, &frame[partial_frame_count], SBUS_FRAME_SIZE - partial_frame_count); /* if the read failed for any reason, just give up here */ - if (ret < 1) + if (ret < 1) { return false; + } last_rx_time = now; @@ -180,8 +184,9 @@ sbus_input(uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sb /* * If we don't have a full frame, return */ - if (partial_frame_count < SBUS_FRAME_SIZE) + if (partial_frame_count < SBUS_FRAME_SIZE) { return false; + } /* * Great, it looks like we might have a frame. Go ahead and @@ -228,7 +233,8 @@ static const struct sbus_bit_pick sbus_decoder[SBUS_INPUT_CHANNELS][3] = { }; static bool -sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_values) +sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, + uint16_t max_values) { /* check frame boundary markers to avoid out-of-sync cases */ if ((frame[0] != 0x0f)) { @@ -237,23 +243,27 @@ sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, bool } switch (frame[24]) { - case 0x00: + case 0x00: /* this is S.BUS 1 */ break; - case 0x03: + + case 0x03: /* S.BUS 2 SLOT0: RX battery and external voltage */ break; - case 0x83: + + case 0x83: /* S.BUS 2 SLOT1 */ break; - case 0x43: - case 0xC3: - case 0x23: - case 0xA3: - case 0x63: - case 0xE3: + + case 0x43: + case 0xC3: + case 0x23: + case 0xA3: + case 0x63: + case 0xE3: break; - default: + + default: /* we expect one of the bits above, but there are some we don't know yet */ break; } @@ -283,7 +293,7 @@ sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, bool /* convert 0-2048 values to 1000-2000 ppm encoding in a not too sloppy fashion */ - values[channel] = (uint16_t)(value * SBUS_SCALE_FACTOR +.5f) + SBUS_SCALE_OFFSET; + values[channel] = (uint16_t)(value * SBUS_SCALE_FACTOR + .5f) + SBUS_SCALE_OFFSET; } /* decode switch channels if data fields are wide enough */ @@ -304,16 +314,17 @@ sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, bool /* report that we failed to read anything valid off the receiver */ *sbus_failsafe = true; *sbus_frame_drop = true; - } - else if (frame[SBUS_FLAGS_BYTE] & (1 << SBUS_FRAMELOST_BIT)) { /* a frame was lost */ + + } else if (frame[SBUS_FLAGS_BYTE] & (1 << SBUS_FRAMELOST_BIT)) { /* a frame was lost */ /* set a special warning flag - * - * Attention! This flag indicates a skipped frame only, not a total link loss! Handling this - * condition as fail-safe greatly reduces the reliability and range of the radio link, + * + * Attention! This flag indicates a skipped frame only, not a total link loss! Handling this + * condition as fail-safe greatly reduces the reliability and range of the radio link, * e.g. by prematurely issueing return-to-launch!!! */ *sbus_failsafe = false; *sbus_frame_drop = true; + } else { *sbus_failsafe = false; *sbus_frame_drop = false; -- cgit v1.2.3 From 4839ed915c68ffc60700fe170e5ba3766a6b34f1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 29 Oct 2014 17:00:00 +0100 Subject: Update MAVLink submodule --- mavlink/include/mavlink/v1.0 | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mavlink/include/mavlink/v1.0 b/mavlink/include/mavlink/v1.0 index 8587a007c..76a9dd756 160000 --- a/mavlink/include/mavlink/v1.0 +++ b/mavlink/include/mavlink/v1.0 @@ -1 +1 @@ -Subproject commit 8587a007cf89cb13f146ac46be94b61276343ce1 +Subproject commit 76a9dd75632ecf0f622b8b0af098fa51033796a7 -- cgit v1.2.3 From d5b8385a13f4d09453bc00406ec96da9345addcf Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 29 Oct 2014 17:55:53 +0100 Subject: Fix low stack space on commander - relevant in HIL --- src/modules/commander/commander.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index cb507c9ba..b72ebcc50 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -263,7 +263,7 @@ int commander_main(int argc, char *argv[]) daemon_task = task_spawn_cmd("commander", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 40, - 2950, + 3200, commander_thread_main, (argv) ? (const char **)&argv[2] : (const char **)NULL); -- cgit v1.2.3 From 2e33683630002aef5881734c96383685b7e8443f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 27 Oct 2014 13:10:58 +0100 Subject: Abort on large packets which do not fit in buffer - not just if the gap is not provided any more. --- src/modules/mavlink/mavlink_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 0d932d20a..4448f8d6f 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -763,7 +763,7 @@ Mavlink::send_message(const uint8_t msgid, const void *msg) _last_write_try_time = hrt_absolute_time(); /* check if there is space in the buffer, let it overflow else */ - if (buf_free < TX_BUFFER_GAP) { + if ((buf_free < TX_BUFFER_GAP) || (buf_free < packet_len)) { /* no enough space in buffer to send */ count_txerr(); count_txerrbytes(packet_len); -- cgit v1.2.3 From dd23d0acbcce9f4c79e120ec782a522164a25d83 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 9 Oct 2014 15:35:18 +1100 Subject: drivers: allow forcing the safety switch on This allows forcing the safety switch to the on position from software which stops the pwm outputs --- src/drivers/drv_pwm_output.h | 3 +++ src/drivers/px4fmu/fmu.cpp | 1 + src/drivers/px4io/px4io.cpp | 5 +++++ src/modules/px4iofirmware/protocol.h | 1 + src/modules/px4iofirmware/registers.c | 6 ++++++ 5 files changed, 16 insertions(+) diff --git a/src/drivers/drv_pwm_output.h b/src/drivers/drv_pwm_output.h index f66ec7c95..6873f24b6 100644 --- a/src/drivers/drv_pwm_output.h +++ b/src/drivers/drv_pwm_output.h @@ -213,6 +213,9 @@ ORB_DECLARE(output_pwm); /** make failsafe non-recoverable (termination) if it occurs */ #define PWM_SERVO_SET_TERMINATION_FAILSAFE _IOC(_PWM_SERVO_BASE, 25) +/** force safety switch on (to enable use of safety switch) */ +#define PWM_SERVO_SET_FORCE_SAFETY_ON _IOC(_PWM_SERVO_BASE, 26) + /* * * diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 122a3cd17..3d3e1b0eb 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -829,6 +829,7 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) case PWM_SERVO_SET_ARM_OK: case PWM_SERVO_CLEAR_ARM_OK: case PWM_SERVO_SET_FORCE_SAFETY_OFF: + case PWM_SERVO_SET_FORCE_SAFETY_ON: // these are no-ops, as no safety switch break; diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 3871b4a2c..fd9eb4170 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -2278,6 +2278,11 @@ PX4IO::ioctl(file * filep, int cmd, unsigned long arg) ret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FORCE_SAFETY_OFF, PX4IO_FORCE_SAFETY_MAGIC); break; + case PWM_SERVO_SET_FORCE_SAFETY_ON: + /* force safety switch on */ + ret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FORCE_SAFETY_ON, PX4IO_FORCE_SAFETY_MAGIC); + break; + case PWM_SERVO_SET_FORCE_FAILSAFE: /* force failsafe mode instantly */ if (arg == 0) { diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h index a3e8a58d3..9b2e047cb 100644 --- a/src/modules/px4iofirmware/protocol.h +++ b/src/modules/px4iofirmware/protocol.h @@ -221,6 +221,7 @@ enum { /* DSM bind states */ hence index 12 can safely be used. */ #define PX4IO_P_SETUP_RC_THR_FAILSAFE_US 13 /**< the throttle failsafe pulse length in microseconds */ +#define PX4IO_P_SETUP_FORCE_SAFETY_ON 14 /* force safety switch into 'disarmed' (PWM disabled state) */ #define PX4IO_FORCE_SAFETY_MAGIC 22027 /* required argument for force safety (random) */ /* autopilot control values, -10000..10000 */ diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index 7f19e983f..49c2a9f56 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -603,6 +603,12 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) dsm_bind(value & 0x0f, (value >> 4) & 0xF); break; + case PX4IO_P_SETUP_FORCE_SAFETY_ON: + if (value == PX4IO_FORCE_SAFETY_MAGIC) { + r_status_flags &= ~PX4IO_P_STATUS_FLAGS_SAFETY_OFF; + } + break; + case PX4IO_P_SETUP_FORCE_SAFETY_OFF: if (value == PX4IO_FORCE_SAFETY_MAGIC) { r_status_flags |= PX4IO_P_STATUS_FLAGS_SAFETY_OFF; -- cgit v1.2.3 From c396a6774626a6a993030c910697873e6b1b1195 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 16 Dec 2013 22:15:37 +1100 Subject: mpu6000: added logging of good transfers this helps tracking down a startup issue --- src/drivers/mpu6000/mpu6000.cpp | 11 +++++++++++ 1 file changed, 11 insertions(+) diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index b22bb2e07..a95e041a1 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -229,6 +229,7 @@ private: perf_counter_t _gyro_reads; perf_counter_t _sample_perf; perf_counter_t _bad_transfers; + perf_counter_t _good_transfers; math::LowPassFilter2p _accel_filter_x; math::LowPassFilter2p _accel_filter_y; @@ -404,6 +405,7 @@ MPU6000::MPU6000(int bus, const char *path_accel, const char *path_gyro, spi_dev _gyro_reads(perf_alloc(PC_COUNT, "mpu6000_gyro_read")), _sample_perf(perf_alloc(PC_ELAPSED, "mpu6000_read")), _bad_transfers(perf_alloc(PC_COUNT, "mpu6000_bad_transfers")), + _good_transfers(perf_alloc(PC_COUNT, "mpu6000_good_transfers")), _accel_filter_x(MPU6000_ACCEL_DEFAULT_RATE, MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), _accel_filter_y(MPU6000_ACCEL_DEFAULT_RATE, MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), _accel_filter_z(MPU6000_ACCEL_DEFAULT_RATE, MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), @@ -456,6 +458,7 @@ MPU6000::~MPU6000() perf_free(_accel_reads); perf_free(_gyro_reads); perf_free(_bad_transfers); + perf_free(_good_transfers); } int @@ -1279,8 +1282,14 @@ MPU6000::measure() // all zero data - probably a SPI bus error perf_count(_bad_transfers); perf_end(_sample_perf); + // note that we don't call reset() here as a reset() + // costs 20ms with interrupts disabled. That means if + // the mpu6k does go bad it would cause a FMU failure, + // regardless of whether another sensor is available, return; } + + perf_count(_good_transfers); /* @@ -1399,6 +1408,8 @@ MPU6000::print_info() perf_print_counter(_sample_perf); perf_print_counter(_accel_reads); perf_print_counter(_gyro_reads); + perf_print_counter(_bad_transfers); + perf_print_counter(_good_transfers); _accel_reports->print_info("accel queue"); _gyro_reports->print_info("gyro queue"); } -- cgit v1.2.3 From 47367aeed526e7ca72e9cb7290e745dc0e6e2061 Mon Sep 17 00:00:00 2001 From: philipoe Date: Wed, 29 Oct 2014 15:56:31 +0100 Subject: TECS: Fix bug (underspeed-condition did not have any effect on throttle) --- src/lib/external_lgpl/tecs/tecs.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/lib/external_lgpl/tecs/tecs.cpp b/src/lib/external_lgpl/tecs/tecs.cpp index 6a2a61b04..0ed210cf4 100644 --- a/src/lib/external_lgpl/tecs/tecs.cpp +++ b/src/lib/external_lgpl/tecs/tecs.cpp @@ -299,7 +299,7 @@ void TECS::_update_throttle(float throttle_cruise, const math::Matrix<3,3> &rotM // Calculate throttle demand // If underspeed condition is set, then demand full throttle if (_underspeed) { - _throttle_dem_unc = 1.0f; + _throttle_dem = 1.0f; } else { // Calculate gain scaler from specific energy error to throttle @@ -363,10 +363,10 @@ void TECS::_update_throttle(float throttle_cruise, const math::Matrix<3,3> &rotM } else { _throttle_dem = ff_throttle; } - } - // Constrain throttle demand - _throttle_dem = constrain(_throttle_dem, _THRminf, _THRmaxf); + // Constrain throttle demand + _throttle_dem = constrain(_throttle_dem, _THRminf, _THRmaxf); + } } void TECS::_detect_bad_descent(void) -- cgit v1.2.3 From a82f4881c7f0dee36860ca69fef1e63b0d863b8c Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 11 Oct 2014 06:14:16 +1100 Subject: sdlog2_dump: Fixing incorrect tabbing to allow for CSV generation --- Tools/sdlog2/sdlog2_dump.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) mode change 100644 => 100755 Tools/sdlog2/sdlog2_dump.py diff --git a/Tools/sdlog2/sdlog2_dump.py b/Tools/sdlog2/sdlog2_dump.py old mode 100644 new mode 100755 index ca2efb021..8b0ccb2d7 --- a/Tools/sdlog2/sdlog2_dump.py +++ b/Tools/sdlog2/sdlog2_dump.py @@ -154,8 +154,8 @@ class SDLog2Parser: first_data_msg = False self.__parseMsg(msg_descr) bytes_read += self.__ptr - if not self.__debug_out and self.__time_msg != None and self.__csv_updated: - self.__printCSVRow() + if not self.__debug_out and self.__time_msg != None and self.__csv_updated: + self.__printCSVRow() f.close() def __bytesLeft(self): -- cgit v1.2.3 From 02e35991d52f280d978c3ddd527269b1f9fd176d Mon Sep 17 00:00:00 2001 From: philipoe Date: Thu, 30 Oct 2014 09:54:27 +0100 Subject: TECS: Also deleted the _throttle_dem_unc variable from TECS.h --- src/lib/external_lgpl/tecs/tecs.h | 3 --- 1 file changed, 3 deletions(-) diff --git a/src/lib/external_lgpl/tecs/tecs.h b/src/lib/external_lgpl/tecs/tecs.h index 8ac31de6f..eb45237b7 100644 --- a/src/lib/external_lgpl/tecs/tecs.h +++ b/src/lib/external_lgpl/tecs/tecs.h @@ -345,9 +345,6 @@ private: // climbout mode bool _climbOutDem; - // throttle demand before limiting - float _throttle_dem_unc; - // pitch demand before limiting float _pitch_dem_unc; -- cgit v1.2.3 From d0d2c7f9c69c5cfdd3aabf757796d2d6ada32875 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 30 Oct 2014 15:10:44 +0100 Subject: Updated MAVLink revision --- mavlink/include/mavlink/v1.0 | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mavlink/include/mavlink/v1.0 b/mavlink/include/mavlink/v1.0 index 76a9dd756..90383fac8 160000 --- a/mavlink/include/mavlink/v1.0 +++ b/mavlink/include/mavlink/v1.0 @@ -1 +1 @@ -Subproject commit 76a9dd75632ecf0f622b8b0af098fa51033796a7 +Subproject commit 90383fac84d031aef17989a1497c2473dfa64340 -- cgit v1.2.3 From 2f715b74dd4866945e7ebe6aab735cd1e58fe01d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 30 Oct 2014 15:36:16 +0100 Subject: Fix build breakage on mavlink update --- src/modules/mavlink/mavlink_receiver.cpp | 10 ++++------ 1 file changed, 4 insertions(+), 6 deletions(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index e8d783847..9247f999a 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -391,11 +391,9 @@ MavlinkReceiver::handle_message_hil_optical_flow(mavlink_message_t *msg) f.timestamp = hrt_absolute_time(); f.flow_timestamp = flow.time_usec; - f.flow_raw_x = flow.flow_x; - f.flow_raw_y = flow.flow_y; - f.flow_comp_x_m = flow.flow_comp_m_x; - f.flow_comp_y_m = flow.flow_comp_m_y; - f.ground_distance_m = flow.ground_distance; + f.flow_raw_x = flow.integrated_x; + f.flow_raw_y = flow.integrated_y; + f.ground_distance_m = flow.distance; f.quality = flow.quality; f.sensor_id = flow.sensor_id; @@ -413,7 +411,7 @@ MavlinkReceiver::handle_message_hil_optical_flow(mavlink_message_t *msg) r.timestamp = hrt_absolute_time(); r.error_count = 0; r.type = RANGE_FINDER_TYPE_LASER; - r.distance = flow.ground_distance; + r.distance = flow.distance; r.minimum_distance = 0.0f; r.maximum_distance = 40.0f; // this is set to match the typical range of real sensors, could be made configurable r.valid = (r.distance > r.minimum_distance) && (r.distance < r.maximum_distance); -- cgit v1.2.3 From 9f3c3529593033d9acc94fb978a2aa6e34a3d1d9 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 30 Oct 2014 16:38:27 +0100 Subject: Leave NSH terminal enough stack space --- src/systemcmds/nshterm/module.mk | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/systemcmds/nshterm/module.mk b/src/systemcmds/nshterm/module.mk index 7d2c59f91..41706e137 100644 --- a/src/systemcmds/nshterm/module.mk +++ b/src/systemcmds/nshterm/module.mk @@ -38,6 +38,6 @@ MODULE_COMMAND = nshterm SRCS = nshterm.c -MODULE_STACKSIZE = 1400 +MODULE_STACKSIZE = 1600 MAXOPTIMIZATION = -Os -- cgit v1.2.3 From 9f64953bb9f7a51042922f30ad15a367d82fb4d5 Mon Sep 17 00:00:00 2001 From: dominiho Date: Thu, 30 Oct 2014 16:39:02 +0100 Subject: replaced optical_flow mavlink message with optical_flow_rad, added gyro_temperature, adapted sd2log for px4flow integral frame --- src/drivers/px4flow/px4flow.cpp | 14 ++++++---- src/modules/mavlink/mavlink_main.cpp | 2 +- src/modules/mavlink/mavlink_messages.cpp | 40 ++++++++++++++------------- src/modules/mavlink/mavlink_receiver.cpp | 46 ++++++++++++++++++-------------- src/modules/mavlink/mavlink_receiver.h | 2 +- src/modules/sdlog2/sdlog2.c | 13 +++++---- src/modules/sdlog2/sdlog2_messages.h | 18 ++++++++----- src/modules/uORB/topics/optical_flow.h | 1 + 8 files changed, 80 insertions(+), 56 deletions(-) diff --git a/src/drivers/px4flow/px4flow.cpp b/src/drivers/px4flow/px4flow.cpp index c3660a967..daaf02a77 100644 --- a/src/drivers/px4flow/px4flow.cpp +++ b/src/drivers/px4flow/px4flow.cpp @@ -116,6 +116,7 @@ typedef struct i2c_integral_frame { uint32_t integration_timespan; uint32_t time_since_last_sonar_update; uint16_t ground_distance; + int16_t gyro_temperature; uint8_t qual; } __attribute__((packed)); struct i2c_integral_frame f_integral; @@ -456,16 +457,16 @@ PX4FLOW::collect() int ret = -EIO; /* read from the sensor */ - uint8_t val[46] = { 0 }; + uint8_t val[47] = { 0 }; perf_begin(_sample_perf); if (PX4FLOW_REG == 0x00) { - ret = transfer(nullptr, 0, &val[0], 45); // read 45 bytes (22+23 : frame1 + frame2) + ret = transfer(nullptr, 0, &val[0], 47); // read 47 bytes (22+25 : frame1 + frame2) } if (PX4FLOW_REG == 0x16) { - ret = transfer(nullptr, 0, &val[0], 23); // read 23 bytes (only frame2) + ret = transfer(nullptr, 0, &val[0], 25); // read 25 bytes (only frame2) } if (ret < 0) { @@ -500,7 +501,8 @@ PX4FLOW::collect() f_integral.time_since_last_sonar_update = val[41] << 24 | val[40] << 16 | val[39] << 8 | val[38]; f_integral.ground_distance = val[43] << 8 | val[42]; - f_integral.qual = val[44]; + f_integral.gyro_temperature = val[45] << 8 | val[44]; + f_integral.qual = val[46]; } if (PX4FLOW_REG == 0x16) { @@ -513,7 +515,8 @@ PX4FLOW::collect() f_integral.integration_timespan = val[15] << 24 | val[14] << 16 | val[13] << 8 | val[12]; f_integral.time_since_last_sonar_update = val[19] << 24 | val[18] << 16 | val[17] << 8 | val[16]; f_integral.ground_distance = val[21] << 8 | val[20]; - f_integral.qual = val[22]; + f_integral.gyro_temperature = val[23] << 8 | val[22]; + f_integral.qual = val[24]; } @@ -530,6 +533,7 @@ PX4FLOW::collect() report.gyro_z_rate_integral = static_cast(f_integral.gyro_z_rate_integral) / 10000.0f; //convert to radians report.integration_timespan = f_integral.integration_timespan; //microseconds report.time_since_last_sonar_update = f_integral.time_since_last_sonar_update;//microseconds + report.gyro_temperature = f_integral.gyro_temperature;//Temperature * 100 in centi-degrees Celsius report.sensor_id = 0; diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 4448f8d6f..3c1d360c2 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1403,7 +1403,7 @@ Mavlink::task_main(int argc, char *argv[]) configure_stream("POSITION_TARGET_GLOBAL_INT", 3.0f); configure_stream("ATTITUDE_TARGET", 3.0f); configure_stream("DISTANCE_SENSOR", 0.5f); - configure_stream("OPTICAL_FLOW", 5.0f); + configure_stream("OPTICAL_FLOW_RAD", 5.0f); break; case MAVLINK_MODE_ONBOARD: diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index cccb698bf..d955b5f76 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -1786,33 +1786,32 @@ protected: } }; - -class MavlinkStreamOpticalFlow : public MavlinkStream +class MavlinkStreamOpticalFlowRad : public MavlinkStream { public: const char *get_name() const { - return MavlinkStreamOpticalFlow::get_name_static(); + return MavlinkStreamOpticalFlowRad::get_name_static(); } static const char *get_name_static() { - return "OPTICAL_FLOW"; + return "OPTICAL_FLOW_RAD"; } uint8_t get_id() { - return MAVLINK_MSG_ID_OPTICAL_FLOW; + return MAVLINK_MSG_ID_OPTICAL_FLOW_RAD; } static MavlinkStream *new_instance(Mavlink *mavlink) { - return new MavlinkStreamOpticalFlow(mavlink); + return new MavlinkStreamOpticalFlowRad(mavlink); } unsigned get_size() { - return _flow_sub->is_published() ? (MAVLINK_MSG_ID_OPTICAL_FLOW_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0; + return _flow_sub->is_published() ? (MAVLINK_MSG_ID_OPTICAL_FLOW_RAD_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0; } private: @@ -1820,11 +1819,11 @@ private: uint64_t _flow_time; /* do not allow top copying this class */ - MavlinkStreamOpticalFlow(MavlinkStreamOpticalFlow &); - MavlinkStreamOpticalFlow& operator = (const MavlinkStreamOpticalFlow &); + MavlinkStreamOpticalFlowRad(MavlinkStreamOpticalFlowRad &); + MavlinkStreamOpticalFlowRad& operator = (const MavlinkStreamOpticalFlowRad &); protected: - explicit MavlinkStreamOpticalFlow(Mavlink *mavlink) : MavlinkStream(mavlink), + explicit MavlinkStreamOpticalFlowRad(Mavlink *mavlink) : MavlinkStream(mavlink), _flow_sub(_mavlink->add_orb_subscription(ORB_ID(optical_flow))), _flow_time(0) {} @@ -1834,18 +1833,23 @@ protected: struct optical_flow_s flow; if (_flow_sub->update(&_flow_time, &flow)) { - mavlink_optical_flow_t msg; + mavlink_optical_flow_rad_t msg; msg.time_usec = flow.timestamp; msg.sensor_id = flow.sensor_id; - msg.flow_x = flow.flow_raw_x; - msg.flow_y = flow.flow_raw_y; - msg.flow_comp_m_x = flow.flow_comp_x_m; - msg.flow_comp_m_y = flow.flow_comp_y_m; + msg.integrated_x = flow.pixel_flow_x_integral; + msg.integrated_y = flow.pixel_flow_y_integral; + msg.integrated_xgyro = flow.gyro_x_rate_integral; + msg.integrated_ygyro = flow.gyro_y_rate_integral; + msg.integrated_zgyro = flow.gyro_z_rate_integral; + msg.distance = flow.ground_distance_m; msg.quality = flow.quality; - msg.ground_distance = flow.ground_distance_m; + msg.integration_time_us = flow.integration_timespan; + msg.sensor_id = flow.sensor_id; + msg.time_delta_distance_us = flow.time_since_last_sonar_update; + msg.temperature = flow.gyro_temperature; - _mavlink->send_message(MAVLINK_MSG_ID_OPTICAL_FLOW, &msg); + _mavlink->send_message(MAVLINK_MSG_ID_OPTICAL_FLOW_RAD, &msg); } } }; @@ -2151,7 +2155,7 @@ StreamListItem *streams_list[] = { new StreamListItem(&MavlinkStreamAttitudeTarget::new_instance, &MavlinkStreamAttitudeTarget::get_name_static), new StreamListItem(&MavlinkStreamRCChannelsRaw::new_instance, &MavlinkStreamRCChannelsRaw::get_name_static), new StreamListItem(&MavlinkStreamManualControl::new_instance, &MavlinkStreamManualControl::get_name_static), - new StreamListItem(&MavlinkStreamOpticalFlow::new_instance, &MavlinkStreamOpticalFlow::get_name_static), + new StreamListItem(&MavlinkStreamOpticalFlowRad::new_instance, &MavlinkStreamOpticalFlowRad::get_name_static), new StreamListItem(&MavlinkStreamAttitudeControls::new_instance, &MavlinkStreamAttitudeControls::get_name_static), new StreamListItem(&MavlinkStreamNamedValueFloat::new_instance, &MavlinkStreamNamedValueFloat::get_name_static), new StreamListItem(&MavlinkStreamCameraCapture::new_instance, &MavlinkStreamCameraCapture::get_name_static), diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index e8d783847..bee58f89b 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -144,8 +144,8 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) handle_message_command_int(msg); break; - case MAVLINK_MSG_ID_OPTICAL_FLOW: - handle_message_optical_flow(msg); + case MAVLINK_MSG_ID_OPTICAL_FLOW_RAD: + handle_message_optical_flow_rad(msg); break; case MAVLINK_MSG_ID_SET_MODE: @@ -352,24 +352,27 @@ MavlinkReceiver::handle_message_command_int(mavlink_message_t *msg) } void -MavlinkReceiver::handle_message_optical_flow(mavlink_message_t *msg) +MavlinkReceiver::handle_message_optical_flow_rad(mavlink_message_t *msg) { /* optical flow */ - mavlink_optical_flow_t flow; - mavlink_msg_optical_flow_decode(msg, &flow); + mavlink_optical_flow_rad_t flow; + mavlink_msg_optical_flow_rad_decode(msg, &flow); struct optical_flow_s f; memset(&f, 0, sizeof(f)); - f.timestamp = hrt_absolute_time(); - f.flow_timestamp = flow.time_usec; - f.flow_raw_x = flow.flow_x; - f.flow_raw_y = flow.flow_y; - f.flow_comp_x_m = flow.flow_comp_m_x; - f.flow_comp_y_m = flow.flow_comp_m_y; - f.ground_distance_m = flow.ground_distance; + f.timestamp = flow.time_usec; + f.integration_timespan = flow.integration_time_us; + f.pixel_flow_x_integral = flow.integrated_x; + f.pixel_flow_y_integral = flow.integrated_y; + f.gyro_x_rate_integral = flow.integrated_xgyro; + f.gyro_y_rate_integral = flow.integrated_ygyro; + f.gyro_z_rate_integral = flow.integrated_zgyro; + f.time_since_last_sonar_update = flow.time_delta_distance_us; + f.ground_distance_m = flow.distance; f.quality = flow.quality; f.sensor_id = flow.sensor_id; + f.gyro_temperature = flow.temperature; if (_flow_pub < 0) { _flow_pub = orb_advertise(ORB_ID(optical_flow), &f); @@ -389,15 +392,18 @@ MavlinkReceiver::handle_message_hil_optical_flow(mavlink_message_t *msg) struct optical_flow_s f; memset(&f, 0, sizeof(f)); - f.timestamp = hrt_absolute_time(); - f.flow_timestamp = flow.time_usec; - f.flow_raw_x = flow.flow_x; - f.flow_raw_y = flow.flow_y; - f.flow_comp_x_m = flow.flow_comp_m_x; - f.flow_comp_y_m = flow.flow_comp_m_y; - f.ground_distance_m = flow.ground_distance; + f.timestamp = flow.time_usec; + f.integration_timespan = flow.integration_time_us; + f.pixel_flow_x_integral = flow.integrated_x; + f.pixel_flow_y_integral = flow.integrated_y; + f.gyro_x_rate_integral = flow.integrated_xgyro; + f.gyro_y_rate_integral = flow.integrated_ygyro; + f.gyro_z_rate_integral = flow.integrated_zgyro; + f.time_since_last_sonar_update = flow.time_delta_distance_us; + f.ground_distance_m = flow.distance; f.quality = flow.quality; f.sensor_id = flow.sensor_id; + f.gyro_temperature = flow.temperature; if (_flow_pub < 0) { _flow_pub = orb_advertise(ORB_ID(optical_flow), &f); @@ -413,7 +419,7 @@ MavlinkReceiver::handle_message_hil_optical_flow(mavlink_message_t *msg) r.timestamp = hrt_absolute_time(); r.error_count = 0; r.type = RANGE_FINDER_TYPE_LASER; - r.distance = flow.ground_distance; + r.distance = flow.distance; r.minimum_distance = 0.0f; r.maximum_distance = 40.0f; // this is set to match the typical range of real sensors, could be made configurable r.valid = (r.distance > r.minimum_distance) && (r.distance < r.maximum_distance); diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index e5f2c6a73..a057074a7 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -112,7 +112,7 @@ private: void handle_message(mavlink_message_t *msg); void handle_message_command_long(mavlink_message_t *msg); void handle_message_command_int(mavlink_message_t *msg); - void handle_message_optical_flow(mavlink_message_t *msg); + void handle_message_optical_flow_rad(mavlink_message_t *msg); void handle_message_hil_optical_flow(mavlink_message_t *msg); void handle_message_set_mode(mavlink_message_t *msg); void handle_message_vicon_position_estimate(mavlink_message_t *msg); diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 9bde37432..54c7b0c83 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1507,11 +1507,14 @@ int sdlog2_thread_main(int argc, char *argv[]) /* --- FLOW --- */ if (copy_if_updated(ORB_ID(optical_flow), subs.flow_sub, &buf.flow)) { log_msg.msg_type = LOG_FLOW_MSG; - log_msg.body.log_FLOW.flow_raw_x = buf.flow.flow_raw_x; - log_msg.body.log_FLOW.flow_raw_y = buf.flow.flow_raw_y; - log_msg.body.log_FLOW.flow_comp_x = buf.flow.flow_comp_x_m; - log_msg.body.log_FLOW.flow_comp_y = buf.flow.flow_comp_y_m; - log_msg.body.log_FLOW.distance = buf.flow.ground_distance_m; + log_msg.body.log_FLOW.ground_distance_m = buf.flow.ground_distance_m; + log_msg.body.log_FLOW.gyro_temperature = buf.flow.gyro_temperature; + log_msg.body.log_FLOW.gyro_x_rate_integral = buf.flow.gyro_x_rate_integral; + log_msg.body.log_FLOW.gyro_y_rate_integral = buf.flow.gyro_y_rate_integral; + log_msg.body.log_FLOW.gyro_z_rate_integral = buf.flow.gyro_z_rate_integral; + log_msg.body.log_FLOW.integration_timespan = buf.flow.integration_timespan; + log_msg.body.log_FLOW.pixel_flow_x_integral = buf.flow.pixel_flow_x_integral; + log_msg.body.log_FLOW.pixel_flow_y_integral = buf.flow.pixel_flow_y_integral; log_msg.body.log_FLOW.quality = buf.flow.quality; log_msg.body.log_FLOW.sensor_id = buf.flow.sensor_id; LOGBUFFER_WRITE_AND_COUNT(FLOW); diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index d49fc0c79..5264ff1c8 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -200,13 +200,19 @@ struct log_ARSP_s { /* --- FLOW - OPTICAL FLOW --- */ #define LOG_FLOW_MSG 15 struct log_FLOW_s { - int16_t flow_raw_x; - int16_t flow_raw_y; - float flow_comp_x; - float flow_comp_y; - float distance; - uint8_t quality; + uint64_t timestamp; uint8_t sensor_id; + float pixel_flow_x_integral; + float pixel_flow_y_integral; + float gyro_x_rate_integral; + float gyro_y_rate_integral; + float gyro_z_rate_integral; + float ground_distance_m; + uint32_t integration_timespan; + uint32_t time_since_last_sonar_update; + uint16_t frame_count_since_last_readout; + int16_t gyro_temperature; + uint8_t quality; }; /* --- GPOS - GLOBAL POSITION ESTIMATE --- */ diff --git a/src/modules/uORB/topics/optical_flow.h b/src/modules/uORB/topics/optical_flow.h index f3731d213..d3dc46ee0 100644 --- a/src/modules/uORB/topics/optical_flow.h +++ b/src/modules/uORB/topics/optical_flow.h @@ -66,6 +66,7 @@ struct optical_flow_s { uint32_t integration_timespan; /** Date: Thu, 30 Oct 2014 17:51:49 +0100 Subject: removed debug printf, adjusted test routine single read --- src/drivers/px4flow/px4flow.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/drivers/px4flow/px4flow.cpp b/src/drivers/px4flow/px4flow.cpp index daaf02a77..af3a5d39c 100644 --- a/src/drivers/px4flow/px4flow.cpp +++ b/src/drivers/px4flow/px4flow.cpp @@ -442,7 +442,6 @@ PX4FLOW::measure() if (OK != ret) { perf_count(_comms_errors); log("i2c::transfer returned %d", ret); - printf("i2c::transfer flow returned %d"); return ret; } @@ -761,9 +760,10 @@ test() } warnx("single read"); - warnx("flowx: %0.2f m/s", (double) f.pixel_flow_x_sum); - warnx("flowy: %0.2f m/s", (double) f.pixel_flow_y_sum); - warnx("time: %lld", report.timestamp); + warnx("pixel_flow_x_integral: %i", f_integral.pixel_flow_x_integral); + warnx("pixel_flow_y_integral: %i", f_integral.pixel_flow_y_integral); + warnx("framecount_integral: %u", + f_integral.frame_count_since_last_readout); /* start the sensor polling at 10Hz */ if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 10)) { -- cgit v1.2.3 From c7a3a0db5230d1506c81c5f15254002b64cddf32 Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Fri, 31 Oct 2014 08:58:58 +0100 Subject: Don't go into an error state if we are temporarily powering via USB on the bench --- src/modules/commander/commander.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index b72ebcc50..46caddd46 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1407,8 +1407,8 @@ int commander_thread_main(int argc, char *argv[]) last_idle_time = system_load.tasks[0].total_runtime; /* check if board is connected via USB */ - //struct stat statbuf; - //on_usb_power = (stat("/dev/ttyACM0", &statbuf) == 0); + struct stat statbuf; + on_usb_power = (stat("/dev/ttyACM0", &statbuf) == 0); } /* if battery voltage is getting lower, warn using buzzer, etc. */ @@ -1418,7 +1418,7 @@ int commander_thread_main(int argc, char *argv[]) status.battery_warning = VEHICLE_BATTERY_WARNING_LOW; status_changed = true; - } else if (status.condition_battery_voltage_valid && status.battery_remaining < 0.09f + } else if (!on_usb_power && status.condition_battery_voltage_valid && status.battery_remaining < 0.09f && !critical_battery_voltage_actions_done && low_battery_voltage_actions_done) { /* critical battery voltage, this is rather an emergency, change state machine */ critical_battery_voltage_actions_done = true; -- cgit v1.2.3 From 078e81a5cb6a0558eea9e589595889d1aad0cbe7 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 31 Oct 2014 11:42:36 +0100 Subject: Build for ESC bus conditionally --- src/drivers/px4flow/px4flow.cpp | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/src/drivers/px4flow/px4flow.cpp b/src/drivers/px4flow/px4flow.cpp index af3a5d39c..b075f8706 100644 --- a/src/drivers/px4flow/px4flow.cpp +++ b/src/drivers/px4flow/px4flow.cpp @@ -669,6 +669,8 @@ start() } if (OK != g_dev->init()) { + + #ifdef PX4_I2C_BUS_ESC delete g_dev; /* try 2nd bus */ g_dev = new PX4FLOW(PX4_I2C_BUS_ESC); @@ -678,6 +680,8 @@ start() } if (OK != g_dev->init()) { + #endif + delete g_dev; /* try 3rd bus */ g_dev = new PX4FLOW(PX4_I2C_BUS_ONBOARD); @@ -689,7 +693,10 @@ start() if (OK != g_dev->init()) { goto fail; } + + #ifdef PX4_I2C_BUS_ESC } + #endif } /* set the poll rate to default, starts automatic data collection */ -- cgit v1.2.3 From 739c407cfd14d065b104977924875b3ee40d5e25 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 31 Oct 2014 11:42:46 +0100 Subject: Fix flow example --- src/examples/flow_position_estimator/flow_position_estimator_main.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/examples/flow_position_estimator/flow_position_estimator_main.c b/src/examples/flow_position_estimator/flow_position_estimator_main.c index c775428ef..0b8c01f79 100644 --- a/src/examples/flow_position_estimator/flow_position_estimator_main.c +++ b/src/examples/flow_position_estimator/flow_position_estimator_main.c @@ -308,8 +308,8 @@ int flow_position_estimator_thread_main(int argc, char *argv[]) if (vehicle_liftoff || params.debug) { /* copy flow */ - flow_speed[0] = flow.flow_comp_x_m; - flow_speed[1] = flow.flow_comp_y_m; + flow_speed[0] = flow.pixel_flow_x_integral / (flow.integration_timespan / 1e6f) * flow.ground_distance_m; + flow_speed[1] = flow.pixel_flow_y_integral / (flow.integration_timespan / 1e6f) * flow.ground_distance_m; flow_speed[2] = 0.0f; /* convert to bodyframe velocity */ -- cgit v1.2.3 From eeda1886f0e7f15b2e51d863020f794b1a84efc1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 1 Nov 2014 11:07:36 +0100 Subject: Simplify error messages for NSH term --- src/systemcmds/nshterm/nshterm.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/systemcmds/nshterm/nshterm.c b/src/systemcmds/nshterm/nshterm.c index fca1798e6..f06c49552 100644 --- a/src/systemcmds/nshterm/nshterm.c +++ b/src/systemcmds/nshterm/nshterm.c @@ -87,7 +87,7 @@ nshterm_main(int argc, char *argv[]) /* Back up the original uart configuration to restore it after exit */ if ((termios_state = tcgetattr(fd, &uart_config)) < 0) { - warnx("ERROR get termios config %s: %d\n", argv[1], termios_state); + warnx("ERR get config %s: %d\n", argv[1], termios_state); close(fd); return -1; } @@ -96,7 +96,7 @@ nshterm_main(int argc, char *argv[]) uart_config.c_oflag |= (ONLCR | OPOST/* | OCRNL*/); if ((termios_state = tcsetattr(fd, TCSANOW, &uart_config)) < 0) { - warnx("ERROR setting baudrate / termios config for %s (tcsetattr)\n", argv[1]); + warnx("ERR set config %s\n", argv[1]); close(fd); return -1; } -- cgit v1.2.3 From 83eb81251c8405c96aaa72fc1634d15a5b54af29 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 1 Nov 2014 11:22:18 +0100 Subject: NSH term: Only time out if no arming information is available, if arming information is available abort if unconnected on arming --- src/systemcmds/nshterm/nshterm.c | 27 +++++++++++++++++++++++++-- 1 file changed, 25 insertions(+), 2 deletions(-) diff --git a/src/systemcmds/nshterm/nshterm.c b/src/systemcmds/nshterm/nshterm.c index f06c49552..edeb5c624 100644 --- a/src/systemcmds/nshterm/nshterm.c +++ b/src/systemcmds/nshterm/nshterm.c @@ -51,6 +51,8 @@ #include #include +#include + __EXPORT int nshterm_main(int argc, char *argv[]); int @@ -62,9 +64,30 @@ nshterm_main(int argc, char *argv[]) } unsigned retries = 0; int fd = -1; + int armed_fd = orb_subscribe(ORB_ID(actuator_armed)); + struct actuator_armed_s armed; + /* we assume the system does not provide arming status feedback */ + bool armed_updated = false; + + /* try the first 30 seconds or if arming system is ready */ + while ((retries < 300) || armed_updated) { + + /* abort if an arming topic is published and system is armed */ + bool updated = false; + if (orb_check(armed_fd, &updated)) { + /* the system is now providing arming status feedback. + * instead of timing out, we resort to abort bringing + * up the terminal. + */ + armed_updated = true; + orb_copy(ORB_ID(actuator_armed), armed_fd, &armed); + + if (armed.armed) { + /* this is not an error, but we are done */ + exit(0); + } + } - /* try the first 30 seconds */ - while (retries < 300) { /* the retries are to cope with the behaviour of /dev/ttyACM0 */ /* which may not be ready immediately. */ fd = open(argv[1], O_RDWR); -- cgit v1.2.3 From c442f820dd89aad9fac394ce56c07b10cfcacf73 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 1 Nov 2014 16:54:11 +0100 Subject: Encode RC type in RSSI field for GCS --- src/modules/mavlink/mavlink_messages.cpp | 43 +++++++++++++++++++++++++++++++- 1 file changed, 42 insertions(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index cccb698bf..87858690f 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -1717,7 +1717,48 @@ protected: msg.chan16_raw = (rc.channel_count > 15) ? rc.values[15] : UINT16_MAX; msg.chan17_raw = (rc.channel_count > 16) ? rc.values[16] : UINT16_MAX; msg.chan18_raw = (rc.channel_count > 17) ? rc.values[17] : UINT16_MAX; - msg.rssi = rc.rssi; + + /* RSSI has a max value of 100, and when Spektrum or S.BUS are + * available, the RSSI field is invalid, as they do not provide + * an RSSI measurement. Use an out of band magic value to signal + * these digital ports. XXX revise MAVLink spec to address this. + * One option would be to use the top bit to toggle between RSSI + * and input source mode. + * + * Full RSSI field: 0b 1 111 1111 + * + * ^ If bit is set, RSSI encodes type + RSSI + * + * ^ These three bits encode a total of 8 + * digital RC input types. + * 0: PPM, 1: SBUS, 2: Spektrum, 2: ST24 + * ^ These four bits encode a total of + * 16 RSSI levels. 15 = full, 0 = no signal + * + */ + + /* Initialize RSSI with the special mode level flag */ + msg.rssi = (1 << 7); + + /* Set RSSI */ + msg.rssi |= (rc.rssi <= 100) ? ((rc.rssi / 7) + 1) : 15; + + switch (rc.input_source) { + case RC_INPUT_SOURCE_PX4FMU_PPM: + /* fallthrough */ + case RC_INPUT_SOURCE_PX4IO_PPM: + msg.rssi |= (0 << 4); + break; + case RC_INPUT_SOURCE_PX4IO_SPEKTRUM: + msg.rssi |= (1 << 4); + break; + case RC_INPUT_SOURCE_PX4IO_SBUS: + msg.rssi |= (2 << 4); + break; + case RC_INPUT_SOURCE_PX4IO_ST24: + msg.rssi |= (3 << 4); + break; + } _mavlink->send_message(MAVLINK_MSG_ID_RC_CHANNELS, &msg); } -- cgit v1.2.3 From 412ddde5dca055ba090ed2e18db2dc77b6af93a4 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 1 Nov 2014 16:58:12 +0100 Subject: Force RSSI to zero if RC is lost --- src/modules/mavlink/mavlink_messages.cpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 87858690f..a2b3cc62e 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -1760,6 +1760,11 @@ protected: break; } + if (rc.rc_lost) { + /* RSSI is by definition zero */ + msg.rssi = 0; + } + _mavlink->send_message(MAVLINK_MSG_ID_RC_CHANNELS, &msg); } } -- cgit v1.2.3 From 2cadd45f307750bb68c3f926d7d9830f680bd94b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 2 Nov 2014 10:13:11 +0100 Subject: Configure led ring and enable heartbeat on it --- src/drivers/boards/px4io-v2/board_config.h | 1 + src/drivers/boards/px4io-v2/px4iov2_init.c | 1 + src/modules/px4iofirmware/px4io.c | 2 ++ src/modules/px4iofirmware/px4io.h | 1 + 4 files changed, 5 insertions(+) diff --git a/src/drivers/boards/px4io-v2/board_config.h b/src/drivers/boards/px4io-v2/board_config.h index ef9bb5cad..10a93be0b 100644 --- a/src/drivers/boards/px4io-v2/board_config.h +++ b/src/drivers/boards/px4io-v2/board_config.h @@ -77,6 +77,7 @@ #define GPIO_LED1 (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN14) #define GPIO_LED2 (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN15) #define GPIO_LED3 (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN13) +#define GPIO_LED4 (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN11) /* Safety switch button *******************************************************/ diff --git a/src/drivers/boards/px4io-v2/px4iov2_init.c b/src/drivers/boards/px4io-v2/px4iov2_init.c index 9f8c0eeb2..5c3343ccc 100644 --- a/src/drivers/boards/px4io-v2/px4iov2_init.c +++ b/src/drivers/boards/px4io-v2/px4iov2_init.c @@ -108,6 +108,7 @@ __EXPORT void stm32_boardinitialize(void) stm32_configgpio(GPIO_LED1); stm32_configgpio(GPIO_LED2); stm32_configgpio(GPIO_LED3); + stm32_configgpio(GPIO_LED4); stm32_configgpio(GPIO_BTN_SAFETY); diff --git a/src/modules/px4iofirmware/px4io.c b/src/modules/px4iofirmware/px4io.c index cd134ccb4..f978ff406 100644 --- a/src/modules/px4iofirmware/px4io.c +++ b/src/modules/px4iofirmware/px4io.c @@ -124,6 +124,7 @@ heartbeat_blink(void) { static bool heartbeat = false; LED_BLUE(heartbeat = !heartbeat); + LED_RING(heartbeat); } static uint64_t reboot_time; @@ -191,6 +192,7 @@ user_start(int argc, char *argv[]) LED_AMBER(false); LED_BLUE(false); LED_SAFETY(false); + LED_RING(false); /* turn on servo power (if supported) */ #ifdef POWER_SERVO diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h index e32fcc72b..8186e4c78 100644 --- a/src/modules/px4iofirmware/px4io.h +++ b/src/modules/px4iofirmware/px4io.h @@ -140,6 +140,7 @@ extern pwm_limit_t pwm_limit; #define LED_BLUE(_s) stm32_gpiowrite(GPIO_LED1, !(_s)) #define LED_AMBER(_s) stm32_gpiowrite(GPIO_LED2, !(_s)) #define LED_SAFETY(_s) stm32_gpiowrite(GPIO_LED3, !(_s)) +#define LED_RING(_s) stm32_gpiowrite(GPIO_LED4, !(_s)) #ifdef CONFIG_ARCH_BOARD_PX4IO_V1 -- cgit v1.2.3 From 75d0ffe4e5b7659e20bbbdcc1e840e06dfe7a138 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 2 Nov 2014 12:34:33 +0100 Subject: Build fix --- src/modules/px4iofirmware/px4io.c | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/modules/px4iofirmware/px4io.c b/src/modules/px4iofirmware/px4io.c index f978ff406..30f32b38e 100644 --- a/src/modules/px4iofirmware/px4io.c +++ b/src/modules/px4iofirmware/px4io.c @@ -124,7 +124,9 @@ heartbeat_blink(void) { static bool heartbeat = false; LED_BLUE(heartbeat = !heartbeat); +#ifdef GPIO_LED4 LED_RING(heartbeat); +#endif } static uint64_t reboot_time; @@ -192,7 +194,9 @@ user_start(int argc, char *argv[]) LED_AMBER(false); LED_BLUE(false); LED_SAFETY(false); +#ifdef GPIO_LED4 LED_RING(false); +#endif /* turn on servo power (if supported) */ #ifdef POWER_SERVO -- cgit v1.2.3 From ce1ec430f89e2080ad053115459bf91dd6585d3a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 2 Nov 2014 12:58:32 +0100 Subject: Ensure MAVLink app has enough stack space --- src/modules/mavlink/mavlink_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 4448f8d6f..6b4edff78 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1634,7 +1634,7 @@ Mavlink::start(int argc, char *argv[]) task_spawn_cmd(buf, SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, - 2700, + 2900, (main_t)&Mavlink::start_helper, (const char **)argv); -- cgit v1.2.3 From acb739655d5c2ebf50449842ae2b7b9b7c76dbd1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 2 Nov 2014 17:06:35 +0100 Subject: Remove huge memory overhead in RC channels topic, was completely unnecessary --- src/modules/uORB/topics/rc_channels.h | 1 - 1 file changed, 1 deletion(-) diff --git a/src/modules/uORB/topics/rc_channels.h b/src/modules/uORB/topics/rc_channels.h index 8978de471..b14ae1edd 100644 --- a/src/modules/uORB/topics/rc_channels.h +++ b/src/modules/uORB/topics/rc_channels.h @@ -76,7 +76,6 @@ struct rc_channels_s { uint64_t timestamp_last_valid; /**< Timestamp of last valid RC signal */ float channels[RC_CHANNELS_FUNCTION_MAX]; /**< Scaled to -1..1 (throttle: 0..1) */ uint8_t channel_count; /**< Number of valid channels */ - char function_name[RC_CHANNELS_FUNCTION_MAX][20]; /**< String array to store the names of the functions */ int8_t function[RC_CHANNELS_FUNCTION_MAX]; /**< Functions mapping */ uint8_t rssi; /**< Receive signal strength index */ bool signal_lost; /**< Control signal lost, should be checked together with topic timeout */ -- cgit v1.2.3 From baba157785ee1298e70fb358a0ffe76f18fc3b54 Mon Sep 17 00:00:00 2001 From: James Goppert Date: Sat, 1 Nov 2014 18:45:40 -0400 Subject: Encoder logging support. --- src/modules/sdlog2/sdlog2.c | 16 +++++++++++++++- src/modules/sdlog2/sdlog2_messages.h | 11 +++++++++++ 2 files changed, 26 insertions(+), 1 deletion(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 9bde37432..1820fc104 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -90,6 +90,7 @@ #include #include #include +#include #include #include @@ -954,6 +955,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct servorail_status_s servorail_status; struct satellite_info_s sat_info; struct wind_estimate_s wind_estimate; + struct encoders_s encoders; } buf; memset(&buf, 0, sizeof(buf)); @@ -996,6 +998,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct log_GS1B_s log_GS1B; struct log_TECS_s log_TECS; struct log_WIND_s log_WIND; + struct log_ENCODERS_s log_ENCODERS; } body; } log_msg = { LOG_PACKET_HEADER_INIT(0) @@ -1033,6 +1036,7 @@ int sdlog2_thread_main(int argc, char *argv[]) int system_power_sub; int servorail_status_sub; int wind_sub; + int encoders_sub; } subs; subs.cmd_sub = orb_subscribe(ORB_ID(vehicle_command)); @@ -1064,7 +1068,7 @@ int sdlog2_thread_main(int argc, char *argv[]) subs.wind_sub = orb_subscribe(ORB_ID(wind_estimate)); /* we need to rate-limit wind, as we do not need the full update rate */ orb_set_interval(subs.wind_sub, 90); - + subs.encoders_sub = orb_subscribe(ORB_ID(encoders)); /* add new topics HERE */ @@ -1667,6 +1671,16 @@ int sdlog2_thread_main(int argc, char *argv[]) LOGBUFFER_WRITE_AND_COUNT(WIND); } + /* --- ENCODERS --- */ + if (copy_if_updated(ORB_ID(encoders), subs.encoders_sub, &buf.encoders)) { + log_msg.msg_type = LOG_ENCODERS_MSG; + log_msg.body.log_ENCODERS.counts_0 = buf.encoders.counts[0]; + log_msg.body.log_ENCODERS.velocity_0 = buf.encoders.velocity[0]; + log_msg.body.log_ENCODERS.counts_1 = buf.encoders.counts[1]; + log_msg.body.log_ENCODERS.velocity_1 = buf.encoders.velocity[1]; + LOGBUFFER_WRITE_AND_COUNT(ENCODERS); + } + /* signal the other thread new data, but not yet unlock */ if (logbuffer_count(&lb) > MIN_BYTES_TO_WRITE) { /* only request write if several packets can be written at once */ diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index d49fc0c79..7fd07809d 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -407,6 +407,16 @@ struct log_VISN_s { float qw; }; +/* --- ENCODERS - ENCODER DATA --- */ +#define LOG_ENCODERS_MSG 39 +struct log_ENCODERS_s { + int64_t counts_0; + float velocity_0; + int64_t counts_1; + float velocity_1; +}; + + /********** SYSTEM MESSAGES, ID > 0x80 **********/ /* --- TIME - TIME STAMP --- */ @@ -471,6 +481,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(GS1B, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"), LOG_FORMAT(TECS, "fffffffffffffB", "ASP,AF,FSP,F,FF,AsSP,AsF,AsDSP,AsD,TERSP,TER,EDRSP,EDR,M"), LOG_FORMAT(WIND, "ffff", "X,Y,CovX,CovY"), + LOG_FORMAT(ENCODERS, "qfqf", "counts_0, velocity_0, counts_1, velocity_1"), /* system-level messages, ID >= 0x80 */ /* FMT: don't write format of format message, it's useless */ -- cgit v1.2.3 From 0e5b91d16fd35b1ad78df1a772ceb23676e5fa7a Mon Sep 17 00:00:00 2001 From: James Goppert Date: Sun, 2 Nov 2014 14:22:23 -0500 Subject: Shortened encoder logging names. --- src/modules/sdlog2/sdlog2.c | 14 +++++++------- src/modules/sdlog2/sdlog2_messages.h | 14 +++++++------- 2 files changed, 14 insertions(+), 14 deletions(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 1820fc104..af580f1f7 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -998,7 +998,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct log_GS1B_s log_GS1B; struct log_TECS_s log_TECS; struct log_WIND_s log_WIND; - struct log_ENCODERS_s log_ENCODERS; + struct log_ENCD_s log_ENCD; } body; } log_msg = { LOG_PACKET_HEADER_INIT(0) @@ -1673,12 +1673,12 @@ int sdlog2_thread_main(int argc, char *argv[]) /* --- ENCODERS --- */ if (copy_if_updated(ORB_ID(encoders), subs.encoders_sub, &buf.encoders)) { - log_msg.msg_type = LOG_ENCODERS_MSG; - log_msg.body.log_ENCODERS.counts_0 = buf.encoders.counts[0]; - log_msg.body.log_ENCODERS.velocity_0 = buf.encoders.velocity[0]; - log_msg.body.log_ENCODERS.counts_1 = buf.encoders.counts[1]; - log_msg.body.log_ENCODERS.velocity_1 = buf.encoders.velocity[1]; - LOGBUFFER_WRITE_AND_COUNT(ENCODERS); + log_msg.msg_type = LOG_ENCD_MSG; + log_msg.body.log_ENCD.cnt0 = buf.encoders.counts[0]; + log_msg.body.log_ENCD.vel0 = buf.encoders.velocity[0]; + log_msg.body.log_ENCD.cnt1 = buf.encoders.counts[1]; + log_msg.body.log_ENCD.vel1 = buf.encoders.velocity[1]; + LOGBUFFER_WRITE_AND_COUNT(ENCD); } /* signal the other thread new data, but not yet unlock */ diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 7fd07809d..fa9bdacb8 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -408,12 +408,12 @@ struct log_VISN_s { }; /* --- ENCODERS - ENCODER DATA --- */ -#define LOG_ENCODERS_MSG 39 -struct log_ENCODERS_s { - int64_t counts_0; - float velocity_0; - int64_t counts_1; - float velocity_1; +#define LOG_ENCD_MSG 39 +struct log_ENCD_s { + int64_t cnt0; + float vel0; + int64_t cnt1; + float vel1; }; @@ -481,7 +481,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(GS1B, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"), LOG_FORMAT(TECS, "fffffffffffffB", "ASP,AF,FSP,F,FF,AsSP,AsF,AsDSP,AsD,TERSP,TER,EDRSP,EDR,M"), LOG_FORMAT(WIND, "ffff", "X,Y,CovX,CovY"), - LOG_FORMAT(ENCODERS, "qfqf", "counts_0, velocity_0, counts_1, velocity_1"), + LOG_FORMAT(ENCD, "qfqf", "cnt0,vel0,cnt1,vel1"), /* system-level messages, ID >= 0x80 */ /* FMT: don't write format of format message, it's useless */ -- cgit v1.2.3 From 82ebf3ca1c46dd228ed1a95fd334cae6af9dd252 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 2 Nov 2014 21:23:02 +0100 Subject: Update NuttX, print stack trace on stack overflow handler --- NuttX | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/NuttX b/NuttX index 41fffa0df..5ee4b2b2c 160000 --- a/NuttX +++ b/NuttX @@ -1 +1 @@ -Subproject commit 41fffa0df192e4a26d2325c7ac5d9b5d7ba0211c +Subproject commit 5ee4b2b2c26bbc35d1669840f0676e8aa383b984 -- cgit v1.2.3 From b51c6693445be6ff230b3d34ad400553aadddd7e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 2 Nov 2014 21:24:50 +0100 Subject: Commander: Improve calibration routines to produce more conscise and better sequenced instructions --- .../commander/accelerometer_calibration.cpp | 44 ++++++++++++++-------- src/modules/commander/airspeed_calibration.cpp | 25 ++++++++---- src/modules/commander/gyro_calibration.cpp | 5 ++- src/modules/commander/mag_calibration.cpp | 2 +- 4 files changed, 51 insertions(+), 25 deletions(-) diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index 7a4e7a766..0cb41489f 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -83,7 +83,7 @@ * | accel_T[1][i] | * [ accel_T[2][i] ] * - * b = [ accel_corr_ref[0][i] ] // One measurement per axis is enough + * b = [ accel_corr_ref[0][i] ] // One measurement per side is enough * | accel_corr_ref[2][i] | * [ accel_corr_ref[4][i] ] * @@ -162,6 +162,11 @@ int do_accel_calibration(int mavlink_fd) mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name); + mavlink_log_info(mavlink_fd, "You need to put the system on all six sides"); + sleep(3); + mavlink_log_info(mavlink_fd, "Follow the instructions on the screen"); + sleep(5); + struct accel_scale accel_scale = { 0.0f, 1.0f, @@ -258,7 +263,7 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float const int samples_num = 2500; float accel_ref[6][3]; bool data_collected[6] = { false, false, false, false, false, false }; - const char *orientation_strs[6] = { "x+", "x-", "y+", "y-", "z+", "z-" }; + const char *orientation_strs[6] = { "front", "back", "left", "right", "top", "bottom" }; int sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined)); @@ -287,29 +292,37 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float break; } - mavlink_log_info(mavlink_fd, "directions left: %s%s%s%s%s%s", - (!data_collected[0]) ? "x+ " : "", - (!data_collected[1]) ? "x- " : "", - (!data_collected[2]) ? "y+ " : "", - (!data_collected[3]) ? "y- " : "", - (!data_collected[4]) ? "z+ " : "", - (!data_collected[5]) ? "z- " : ""); + /* inform user which axes are still needed */ + mavlink_log_info(mavlink_fd, "pending: %s%s%s%s%s%s", + (!data_collected[0]) ? "front " : "", + (!data_collected[1]) ? "back " : "", + (!data_collected[2]) ? "left " : "", + (!data_collected[3]) ? "right " : "", + (!data_collected[4]) ? "up " : "", + (!data_collected[5]) ? "down " : ""); + + /* allow user enough time to read the message */ + sleep(3); int orient = detect_orientation(mavlink_fd, sensor_combined_sub); if (orient < 0) { - res = ERROR; - break; + mavlink_log_info(mavlink_fd, "invalid motion, hold still..."); + sleep(3); + continue; } + /* inform user about already handled side */ if (data_collected[orient]) { - mavlink_log_info(mavlink_fd, "%s done, rotate to a different axis", orientation_strs[orient]); + mavlink_log_info(mavlink_fd, "%s side done, rotate to a different side", orientation_strs[orient]); + sleep(4); continue; } - mavlink_log_info(mavlink_fd, "accel measurement started: %s axis", orientation_strs[orient]); + mavlink_log_info(mavlink_fd, "Hold still, starting to measure %s side", orientation_strs[orient]); + sleep(1); read_accelerometer_avg(sensor_combined_sub, &(accel_ref[orient][0]), samples_num); - mavlink_log_info(mavlink_fd, "result for %s axis: [ %.2f %.2f %.2f ]", orientation_strs[orient], + mavlink_log_info(mavlink_fd, "result for %s side: [ %.2f %.2f %.2f ]", orientation_strs[orient], (double)accel_ref[orient][0], (double)accel_ref[orient][1], (double)accel_ref[orient][2]); @@ -400,7 +413,7 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) /* is still now */ if (t_still == 0) { /* first time */ - mavlink_log_info(mavlink_fd, "detected rest position, waiting..."); + mavlink_log_info(mavlink_fd, "detected rest position, hold still..."); t_still = t; t_timeout = t + timeout; @@ -418,6 +431,7 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) /* not still, reset still start time */ if (t_still != 0) { mavlink_log_info(mavlink_fd, "detected motion, hold still..."); + sleep(3); t_still = 0; } } diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp index 339b11bbe..cae1d7684 100644 --- a/src/modules/commander/airspeed_calibration.cpp +++ b/src/modules/commander/airspeed_calibration.cpp @@ -61,6 +61,15 @@ static const int ERROR = -1; static const char *sensor_name = "dpress"; +#define HUMAN_ASPD_CAL_FAILED_MSG "Calibration failed, see http://px4.io/help/aspd" + +static void feedback_calibration_failed(int mavlink_fd) +{ + sleep(5); + mavlink_log_critical(mavlink_fd, CAL_FAILED_MSG, sensor_name); + mavlink_log_critical(mavlink_fd, HUMAN_ASPD_CAL_FAILED_MSG); +} + int do_airspeed_calibration(int mavlink_fd) { /* give directions */ @@ -99,7 +108,7 @@ int do_airspeed_calibration(int mavlink_fd) float analog_scaling = 0.0f; param_get(param_find("SENS_DPRES_ANSC"), &(analog_scaling)); if (fabsf(analog_scaling) < 0.1f) { - mavlink_log_critical(mavlink_fd, "If analog sens, retry with [SENS_DPRES_ANSC=1000]"); + mavlink_log_critical(mavlink_fd, "No airspeed sensor, see http://px4.io/help/aspd"); close(diff_pres_sub); return ERROR; } @@ -138,7 +147,7 @@ int do_airspeed_calibration(int mavlink_fd) } else if (poll_ret == 0) { /* any poll failure for 1s is a reason to abort */ - mavlink_log_critical(mavlink_fd, CAL_FAILED_MSG, sensor_name); + feedback_calibration_failed(mavlink_fd); close(diff_pres_sub); return ERROR; } @@ -175,7 +184,7 @@ int do_airspeed_calibration(int mavlink_fd) } } else { - mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name); + feedback_calibration_failed(mavlink_fd); close(diff_pres_sub); return ERROR; } @@ -207,7 +216,7 @@ int do_airspeed_calibration(int mavlink_fd) if (fabsf(diff_pres.differential_pressure_raw_pa) < 50.0f) { if (calibration_counter % 500 == 0) { - mavlink_log_info(mavlink_fd, "Create airflow! (%d, wanted: 50 Pa)", + mavlink_log_info(mavlink_fd, "Create air pressure! (got %d, wanted: 50 Pa)", (int)diff_pres.differential_pressure_raw_pa); } continue; @@ -215,9 +224,9 @@ int do_airspeed_calibration(int mavlink_fd) /* do not allow negative values */ if (diff_pres.differential_pressure_raw_pa < 0.0f) { - mavlink_log_critical(mavlink_fd, "Swap static and dynamic ports!"); mavlink_log_info(mavlink_fd, "ERROR: Negative pressure difference detected! (%d Pa)", (int)diff_pres.differential_pressure_raw_pa); + mavlink_log_critical(mavlink_fd, "Swap static and dynamic ports!"); close(diff_pres_sub); /* the user setup is wrong, wipe the calibration to force a proper re-calibration */ @@ -235,7 +244,7 @@ int do_airspeed_calibration(int mavlink_fd) close(diff_pres_sub); - mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name); + feedback_calibration_failed(mavlink_fd); return ERROR; } else { mavlink_log_info(mavlink_fd, "Positive pressure: OK (%d Pa)", @@ -245,14 +254,14 @@ int do_airspeed_calibration(int mavlink_fd) } else if (poll_ret == 0) { /* any poll failure for 1s is a reason to abort */ - mavlink_log_critical(mavlink_fd, CAL_FAILED_MSG, sensor_name); + feedback_calibration_failed(mavlink_fd); close(diff_pres_sub); return ERROR; } } if (calibration_counter == maxcount) { - mavlink_log_critical(mavlink_fd, CAL_FAILED_MSG, sensor_name); + feedback_calibration_failed(mavlink_fd); close(diff_pres_sub); return ERROR; } diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp index d89c67c2b..8ab14dd52 100644 --- a/src/modules/commander/gyro_calibration.cpp +++ b/src/modules/commander/gyro_calibration.cpp @@ -63,7 +63,10 @@ static const char *sensor_name = "gyro"; int do_gyro_calibration(int mavlink_fd) { mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name); - mavlink_log_info(mavlink_fd, "don't move system"); + mavlink_log_info(mavlink_fd, "HOLD STILL"); + + /* wait for the user to respond */ + sleep(2); struct gyro_scale gyro_scale = { 0.0f, diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp index 23900f386..7be8de9c6 100644 --- a/src/modules/commander/mag_calibration.cpp +++ b/src/modules/commander/mag_calibration.cpp @@ -155,7 +155,7 @@ int do_mag_calibration(int mavlink_fd) uint64_t calibration_deadline = hrt_absolute_time() + calibration_interval; unsigned poll_errcount = 0; - mavlink_log_info(mavlink_fd, "rotate in a figure 8 around all axis"); + mavlink_log_info(mavlink_fd, "Turn on all sides: front/back,left/right,up/down"); calibration_counter = 0; -- cgit v1.2.3 From 166580b8f7415de90f3e5c0171a75b9827158d12 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 2 Nov 2014 21:41:55 +0100 Subject: Time out on serial instead of just hanging there --- Tools/px_uploader.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/Tools/px_uploader.py b/Tools/px_uploader.py index b46db00b5..0bf2b82dd 100755 --- a/Tools/px_uploader.py +++ b/Tools/px_uploader.py @@ -178,9 +178,9 @@ class uploader(object): MAVLINK_REBOOT_ID1 = bytearray(b'\xfe\x21\x72\xff\x00\x4c\x00\x00\x80\x3f\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\xf6\x00\x01\x00\x00\x48\xf0') MAVLINK_REBOOT_ID0 = bytearray(b'\xfe\x21\x45\xff\x00\x4c\x00\x00\x80\x3f\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\xf6\x00\x00\x00\x00\xd7\xac') - def __init__(self, portname, baudrate, interCharTimeout=0.001, timeout=0.5): + def __init__(self, portname, baudrate): # open the port, keep the default timeout short so we can poll quickly - self.port = serial.Serial(portname, baudrate) + self.port = serial.Serial(portname, baudrate, timeout=0.5) self.otp = b'' self.sn = b'' -- cgit v1.2.3 From 5862f4ffe6b463934cceb7bb034e77f100f97633 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 2 Nov 2014 22:00:42 +0100 Subject: Fix error handling --- Tools/px_uploader.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Tools/px_uploader.py b/Tools/px_uploader.py index 0bf2b82dd..ea2f64639 100755 --- a/Tools/px_uploader.py +++ b/Tools/px_uploader.py @@ -195,7 +195,7 @@ class uploader(object): def __recv(self, count=1): c = self.port.read(count) if len(c) < 1: - raise RuntimeError("timeout waiting for data (%u bytes)", count) + raise RuntimeError("timeout waiting for data (%u bytes)" % count) # print("recv " + binascii.hexlify(c)) return c -- cgit v1.2.3 From 44a247363248caadecc4518048ab513d2dfd202e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 2 Nov 2014 22:03:49 +0100 Subject: Fix up reboot logic --- Tools/px_uploader.py | 2 ++ 1 file changed, 2 insertions(+) diff --git a/Tools/px_uploader.py b/Tools/px_uploader.py index ea2f64639..8a89fdf06 100755 --- a/Tools/px_uploader.py +++ b/Tools/px_uploader.py @@ -511,6 +511,8 @@ while True: up.send_reboot() # wait for the reboot, without we might run into Serial I/O Error 5 time.sleep(0.5) + # always close the port + up.close() continue try: -- cgit v1.2.3 From 72977ee9098758b5e00d9ac8df916bb221b2953f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 2 Nov 2014 22:13:59 +0100 Subject: Better error handling instructions --- Tools/px_uploader.py | 2 ++ 1 file changed, 2 insertions(+) diff --git a/Tools/px_uploader.py b/Tools/px_uploader.py index 8a89fdf06..d4e461226 100755 --- a/Tools/px_uploader.py +++ b/Tools/px_uploader.py @@ -459,6 +459,7 @@ if os.path.exists("/usr/sbin/ModemManager"): # Load the firmware file fw = firmware(args.firmware) print("Loaded firmware for %x,%x, waiting for the bootloader..." % (fw.property('board_id'), fw.property('board_revision'))) +print("If the board does not respond within 1-2 seconds, unplug and re-plug the USB connector.") # Spin waiting for a device to show up while True: @@ -508,6 +509,7 @@ while True: except Exception: # most probably a timeout talking to the port, no bootloader, try to reboot the board print("attempting reboot on %s..." % port) + print("if the board does not respond, unplug and re-plug the USB connector.") up.send_reboot() # wait for the reboot, without we might run into Serial I/O Error 5 time.sleep(0.5) -- cgit v1.2.3 From 5e2330abbae35bfc45e5ac67d3590b53de20f526 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 3 Nov 2014 08:39:16 +0100 Subject: Add USB load test from Mark Whitehorn --- Tools/usb_serialload.py | 55 +++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 55 insertions(+) create mode 100644 Tools/usb_serialload.py diff --git a/Tools/usb_serialload.py b/Tools/usb_serialload.py new file mode 100644 index 000000000..5c864532f --- /dev/null +++ b/Tools/usb_serialload.py @@ -0,0 +1,55 @@ +import serial, time + + +port = serial.Serial('/dev/ttyACM0', baudrate=57600, timeout=2) + +data = '01234567890123456789012345678901234567890123456789' +#data = 'hellohello' +outLine = 'echo %s\n' % data + +port.write('\n\n\n') +port.write('free\n') +line = port.readline(80) +while line != '': + print(line) + line = port.readline(80) + + +i = 0 +bytesOut = 0 +bytesIn = 0 + +startTime = time.time() +lastPrint = startTime +while True: + bytesOut += port.write(outLine) + line = port.readline(80) + bytesIn += len(line) + # check command line echo + if (data not in line): + print('command error %d: %s' % (i,line)) + #break + # read echo output + line = port.readline(80) + if (data not in line): + print('echo output error %d: %s' % (i,line)) + #break + bytesIn += len(line) + #print('%d: %s' % (i,line)) + #print('%d: bytesOut: %d, bytesIn: %d' % (i, bytesOut, bytesIn)) + + elapsedT = time.time() - lastPrint + if (time.time() - lastPrint >= 5): + outRate = bytesOut / elapsedT + inRate = bytesIn / elapsedT + usbRate = (bytesOut + bytesIn) / elapsedT + lastPrint = time.time() + print('elapsed time: %f' % (time.time() - startTime)) + print('data rates (bytes/sec): out: %f, in: %f, total: %f' % (outRate, inRate, usbRate)) + + bytesOut = 0 + bytesIn = 0 + + i += 1 + #if (i > 2): break + -- cgit v1.2.3 From 9ac13745f8356db60322fa92ecdff1125c76f172 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 3 Nov 2014 09:48:06 +0100 Subject: Adjust MAVLink RX prio to ensure received data can still be processed --- src/modules/mavlink/mavlink_receiver.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 9247f999a..bc092c7e9 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -1396,7 +1396,7 @@ MavlinkReceiver::receive_start(Mavlink *parent) struct sched_param param; (void)pthread_attr_getschedparam(&receiveloop_attr, ¶m); - param.sched_priority = SCHED_PRIORITY_MAX - 40; + param.sched_priority = SCHED_PRIORITY_MAX - 80; (void)pthread_attr_setschedparam(&receiveloop_attr, ¶m); pthread_attr_setstacksize(&receiveloop_attr, 2900); -- cgit v1.2.3 From 8652ee0b6474fa4fa92b0d6ab5eaec1436a03b8b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 3 Nov 2014 09:48:24 +0100 Subject: Drop NSH priority to keep system responsive --- src/systemcmds/nshterm/module.mk | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/systemcmds/nshterm/module.mk b/src/systemcmds/nshterm/module.mk index 41706e137..a12bc369e 100644 --- a/src/systemcmds/nshterm/module.mk +++ b/src/systemcmds/nshterm/module.mk @@ -41,3 +41,5 @@ SRCS = nshterm.c MODULE_STACKSIZE = 1600 MAXOPTIMIZATION = -Os + +MODULE_PRIORITY = "SCHED_PRIORITY_DEFAULT-30" -- cgit v1.2.3 From 06df0f23a32c87486815a9794c1425fe4534ac13 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 3 Nov 2014 12:56:35 +0100 Subject: L3GD20: Output gyro temperature in report --- src/drivers/l3gd20/l3gd20.cpp | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index cfae8761c..ff2871354 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -176,6 +176,7 @@ static const int ERROR = -1; #define L3G4200D_DEFAULT_RATE 800 #define L3GD20_DEFAULT_RANGE_DPS 2000 #define L3GD20_DEFAULT_FILTER_FREQ 30 +#define L3GD20_TEMP_OFFSET_CELCIUS 40 #ifndef SENSOR_BOARD_ROTATION_DEFAULT #define SENSOR_BOARD_ROTATION_DEFAULT SENSOR_BOARD_ROTATION_270_DEG @@ -856,7 +857,7 @@ L3GD20::measure() #pragma pack(push, 1) struct { uint8_t cmd; - uint8_t temp; + int8_t temp; uint8_t status; int16_t x; int16_t y; @@ -930,6 +931,8 @@ L3GD20::measure() report.z_raw = raw_report.z; + report.temperature_raw = raw_report.temp; + report.x = ((report.x_raw * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale; report.y = ((report.y_raw * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale; report.z = ((report.z_raw * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale; @@ -938,6 +941,8 @@ L3GD20::measure() report.y = _gyro_filter_y.apply(report.y); report.z = _gyro_filter_z.apply(report.z); + report.temperature = L3GD20_TEMP_OFFSET_CELCIUS - raw_report.temp; + // apply user specified rotation rotate_3f(_rotation, report.x, report.y, report.z); @@ -1091,9 +1096,11 @@ test() warnx("gyro x: \t% 9.5f\trad/s", (double)g_report.x); warnx("gyro y: \t% 9.5f\trad/s", (double)g_report.y); warnx("gyro z: \t% 9.5f\trad/s", (double)g_report.z); + warnx("temp: \t%d\tC", (int)g_report.temperature); warnx("gyro x: \t%d\traw", (int)g_report.x_raw); warnx("gyro y: \t%d\traw", (int)g_report.y_raw); warnx("gyro z: \t%d\traw", (int)g_report.z_raw); + warnx("temp: \t%d\traw", (int)g_report.temperature_raw); warnx("gyro range: %8.4f rad/s (%d deg/s)", (double)g_report.range_rad_s, (int)((g_report.range_rad_s / M_PI_F) * 180.0f + 0.5f)); -- cgit v1.2.3 From c600a7fbd27c298e855d1f3e6f00f590141f995e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 3 Nov 2014 15:37:24 +0100 Subject: L3GD20: Fix typo --- src/drivers/l3gd20/l3gd20.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index ff2871354..82fa5cc6e 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -176,7 +176,7 @@ static const int ERROR = -1; #define L3G4200D_DEFAULT_RATE 800 #define L3GD20_DEFAULT_RANGE_DPS 2000 #define L3GD20_DEFAULT_FILTER_FREQ 30 -#define L3GD20_TEMP_OFFSET_CELCIUS 40 +#define L3GD20_TEMP_OFFSET_CELSIUS 40 #ifndef SENSOR_BOARD_ROTATION_DEFAULT #define SENSOR_BOARD_ROTATION_DEFAULT SENSOR_BOARD_ROTATION_270_DEG @@ -941,7 +941,7 @@ L3GD20::measure() report.y = _gyro_filter_y.apply(report.y); report.z = _gyro_filter_z.apply(report.z); - report.temperature = L3GD20_TEMP_OFFSET_CELCIUS - raw_report.temp; + report.temperature = L3GD20_TEMP_OFFSET_CELSIUS - raw_report.temp; // apply user specified rotation rotate_3f(_rotation, report.x, report.y, report.z); -- cgit v1.2.3 From 58f36714f808c9d06527da1952d348201c4f7e72 Mon Sep 17 00:00:00 2001 From: Holger Steinhaus Date: Thu, 14 Aug 2014 15:43:27 +0200 Subject: UAVCAN: allow to arm single ESCs --- src/modules/uavcan/actuators/esc.cpp | 26 +++++++++++++++++++++----- src/modules/uavcan/actuators/esc.hpp | 9 ++++++++- src/modules/uavcan/uavcan_main.cpp | 2 +- 3 files changed, 30 insertions(+), 7 deletions(-) diff --git a/src/modules/uavcan/actuators/esc.cpp b/src/modules/uavcan/actuators/esc.cpp index 1990651ef..fbd4f0bcd 100644 --- a/src/modules/uavcan/actuators/esc.cpp +++ b/src/modules/uavcan/actuators/esc.cpp @@ -40,6 +40,9 @@ #include "esc.hpp" #include + +#define MOTOR_BIT(x) (1<<(x)) + UavcanEscController::UavcanEscController(uavcan::INode &node) : _node(node), _uavcan_pub_raw_cmd(node), @@ -95,9 +98,8 @@ void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs) static const int cmd_max = uavcan::equipment::esc::RawCommand::FieldTypes::cmd::RawValueType::max(); - if (_armed) { - for (unsigned i = 0; i < num_outputs; i++) { - + for (unsigned i = 0; i < num_outputs; i++) { + if (_armed_mask & MOTOR_BIT(i)) { float scaled = (outputs[i] + 1.0F) * 0.5F * cmd_max; if (scaled < 1.0F) { scaled = 1.0F; // Since we're armed, we don't want to stop it completely @@ -112,6 +114,9 @@ void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs) _esc_status.esc[i].esc_setpoint_raw = abs(static_cast(scaled)); } + else { + msg.cmd.push_back(static_cast(0)); + } } /* @@ -121,9 +126,20 @@ void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs) (void)_uavcan_pub_raw_cmd.broadcast(msg); } -void UavcanEscController::arm_esc(bool arm) +void UavcanEscController::arm_all_escs(bool arm) +{ + if (arm) + _armed_mask = -1; + else + _armed_mask = 0; +} + +void UavcanEscController::arm_single_esc(int num, bool arm) { - _armed = arm; + if (arm) + _armed_mask = MOTOR_BIT(num); + else + _armed_mask = 0; } void UavcanEscController::esc_status_sub_cb(const uavcan::ReceivedDataStructure &msg) diff --git a/src/modules/uavcan/actuators/esc.hpp b/src/modules/uavcan/actuators/esc.hpp index f4a3877e6..ff3ecfb21 100644 --- a/src/modules/uavcan/actuators/esc.hpp +++ b/src/modules/uavcan/actuators/esc.hpp @@ -61,7 +61,8 @@ public: void update_outputs(float *outputs, unsigned num_outputs); - void arm_esc(bool arm); + void arm_all_escs(bool arm); + void arm_single_esc(int num, bool arm); private: /** @@ -98,6 +99,12 @@ private: uavcan::Subscriber _uavcan_sub_status; uavcan::TimerEventForwarder _orb_timer; + /* + * ESC states + */ + uint32_t _armed_mask = 0; + uavcan::equipment::esc::Status _states[MAX_ESCS]; + /* * Perf counters */ diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index a8485ee44..59f0f4300 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -429,7 +429,7 @@ int UavcanNode::arm_actuators(bool arm) { _is_armed = arm; - _esc_controller.arm_esc(arm); + _esc_controller.arm_all_escs(arm); return OK; } -- cgit v1.2.3 From a18460183cdfa590af29c2299acb30206b418cb1 Mon Sep 17 00:00:00 2001 From: Holger Steinhaus Date: Mon, 3 Nov 2014 19:37:44 +0100 Subject: motor_test: cleanup --- src/modules/uORB/topics/test_motor.h | 2 +- src/systemcmds/motor_test/motor_test.c | 23 ++++++++++++----------- 2 files changed, 13 insertions(+), 12 deletions(-) diff --git a/src/modules/uORB/topics/test_motor.h b/src/modules/uORB/topics/test_motor.h index 491096934..c880fe099 100644 --- a/src/modules/uORB/topics/test_motor.h +++ b/src/modules/uORB/topics/test_motor.h @@ -57,7 +57,7 @@ struct test_motor_s { uint64_t timestamp; /**< output timestamp in us since system boot */ unsigned motor_number; /**< number of motor to spin */ - float value; /**< output data, in natural output units */ + float value; /**< output power, range [0..1] }; /** diff --git a/src/systemcmds/motor_test/motor_test.c b/src/systemcmds/motor_test/motor_test.c index 079f99674..a73f46626 100644 --- a/src/systemcmds/motor_test/motor_test.c +++ b/src/systemcmds/motor_test/motor_test.c @@ -54,7 +54,8 @@ #include "systemlib/err.h" -__EXPORT int motor_test_main(int argc, char *argv[]); +__EXPORT int motor_test_main(int argc, char *argv[]); + static void motor_test(unsigned channel, float value); static void usage(const char *reason); @@ -67,13 +68,13 @@ void motor_test(unsigned channel, float value) _test_motor.timestamp = hrt_absolute_time(); _test_motor.value = value; - if (_test_motor_pub > 0) { - /* publish armed state */ - orb_publish(ORB_ID(test_motor), _test_motor_pub, &_test_motor); - } else { - /* advertise and publish */ - _test_motor_pub = orb_advertise(ORB_ID(test_motor), &_test_motor); - } + if (_test_motor_pub > 0) { + /* publish test state */ + orb_publish(ORB_ID(test_motor), _test_motor_pub, &_test_motor); + } else { + /* advertise and publish */ + _test_motor_pub = orb_advertise(ORB_ID(test_motor), &_test_motor); + } } static void usage(const char *reason) @@ -102,18 +103,18 @@ int motor_test_main(int argc, char *argv[]) switch (ch) { case 'm': - /* Read in channels supplied as one int and convert to mask: 1234 -> 0xF */ + /* Read in motor number */ channel = strtoul(optarg, NULL, 0); break; case 'p': - /* Read in custom low value */ + /* Read in power value */ lval = strtoul(optarg, NULL, 0); if (lval > 100) usage("value invalid"); - value = (float)lval/100.f; + value = ((float)lval)/100.f; break; default: usage(NULL); -- cgit v1.2.3 From 0800fa4715994921b6a0d15cd2c44b9e51417117 Mon Sep 17 00:00:00 2001 From: Holger Steinhaus Date: Mon, 3 Nov 2014 18:47:07 +0100 Subject: UAVCAN: implemented motor testing --- src/modules/uavcan/actuators/esc.hpp | 1 - src/modules/uavcan/uavcan_main.cpp | 28 ++++++++++++++++++++++++---- src/modules/uavcan/uavcan_main.hpp | 5 +++++ 3 files changed, 29 insertions(+), 5 deletions(-) diff --git a/src/modules/uavcan/actuators/esc.hpp b/src/modules/uavcan/actuators/esc.hpp index ff3ecfb21..12c035542 100644 --- a/src/modules/uavcan/actuators/esc.hpp +++ b/src/modules/uavcan/actuators/esc.hpp @@ -103,7 +103,6 @@ private: * ESC states */ uint32_t _armed_mask = 0; - uavcan::equipment::esc::Status _states[MAX_ESCS]; /* * Perf counters diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index 59f0f4300..653d4f98c 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -279,6 +279,7 @@ int UavcanNode::run() _output_count = 2; _armed_sub = orb_subscribe(ORB_ID(actuator_armed)); + _test_motor_sub = orb_subscribe(ORB_ID(test_motor)); actuator_outputs_s outputs; memset(&outputs, 0, sizeof(outputs)); @@ -344,7 +345,14 @@ int UavcanNode::run() } // can we mix? - if (controls_updated && (_mixers != nullptr)) { + if (_test_in_progress) { + float outputs[NUM_ACTUATOR_OUTPUTS] = {}; + outputs[_test_motor.motor_number] = _test_motor.value*2.0f-1.0f; + + // Output to the bus + _esc_controller.update_outputs(outputs, NUM_ACTUATOR_OUTPUTS); + } + else if (controls_updated && (_mixers != nullptr)) { // XXX one output group has 8 outputs max, // but this driver could well serve multiple groups. @@ -384,15 +392,27 @@ int UavcanNode::run() } } - // Check arming state + // Check motor test state bool updated = false; + orb_check(_test_motor_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(test_motor), _test_motor_sub, &_test_motor); + + // Update the test status and check that we're not locked down + _test_in_progress = (_test_motor.value > 0); + _esc_controller.arm_single_esc(_test_motor.motor_number, _test_in_progress); + } + + // Check arming state orb_check(_armed_sub, &updated); if (updated) { orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed); - // Update the armed status and check that we're not locked down - bool set_armed = _armed.armed && !_armed.lockdown; + // Update the armed status and check that we're not locked down and motor + // test is not running + bool set_armed = _armed.armed && !_armed.lockdown && !_test_in_progress; arm_actuators(set_armed); } diff --git a/src/modules/uavcan/uavcan_main.hpp b/src/modules/uavcan/uavcan_main.hpp index be7db9741..274321f0d 100644 --- a/src/modules/uavcan/uavcan_main.hpp +++ b/src/modules/uavcan/uavcan_main.hpp @@ -41,6 +41,7 @@ #include #include #include +#include #include "actuators/esc.hpp" #include "sensors/sensor_bridge.hpp" @@ -103,6 +104,10 @@ private: actuator_armed_s _armed; ///< the arming request of the system bool _is_armed = false; ///< the arming status of the actuators on the bus + int _test_motor_sub = -1; ///< uORB subscription of the test_motor status + test_motor_s _test_motor; + bool _test_in_progress = false; + unsigned _output_count = 0; ///< number of actuators currently available static UavcanNode *_instance; ///< singleton pointer -- cgit v1.2.3 From f5dbfe63b3d7fbfcac524ba03028de962a8716e2 Mon Sep 17 00:00:00 2001 From: tumbili Date: Tue, 4 Nov 2014 20:52:29 +0100 Subject: added new sensor board rotation option (roll 270, yaw 270) --- src/lib/conversion/rotation.h | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/lib/conversion/rotation.h b/src/lib/conversion/rotation.h index 5187b448f..739c3a960 100644 --- a/src/lib/conversion/rotation.h +++ b/src/lib/conversion/rotation.h @@ -74,6 +74,7 @@ enum Rotation { ROTATION_ROLL_270_YAW_135 = 23, ROTATION_PITCH_90 = 24, ROTATION_PITCH_270 = 25, + ROTATION_ROLL_270_YAW_270 = 26, ROTATION_MAX }; @@ -109,7 +110,8 @@ const rot_lookup_t rot_lookup[] = { {270, 0, 90 }, {270, 0, 135 }, { 0, 90, 0 }, - { 0, 270, 0 } + { 0, 270, 0 }, + {270, 0, 270 } }; /** -- cgit v1.2.3 From cbd20f48d670749fcbfe49c0f5135572bdf7ee44 Mon Sep 17 00:00:00 2001 From: tumbili Date: Tue, 4 Nov 2014 20:57:09 +0100 Subject: fixed style --- src/lib/conversion/rotation.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/conversion/rotation.h b/src/lib/conversion/rotation.h index 739c3a960..917c7f830 100644 --- a/src/lib/conversion/rotation.h +++ b/src/lib/conversion/rotation.h @@ -74,7 +74,7 @@ enum Rotation { ROTATION_ROLL_270_YAW_135 = 23, ROTATION_PITCH_90 = 24, ROTATION_PITCH_270 = 25, - ROTATION_ROLL_270_YAW_270 = 26, + ROTATION_ROLL_270_YAW_270 = 26, ROTATION_MAX }; -- cgit v1.2.3 From 182c1c1d52ef370a42b99c825e3f45a03ecbcdc9 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 5 Nov 2014 17:17:34 +0100 Subject: Fix RC mapping indices - 0 index induces issues right now --- src/drivers/px4io/px4io.cpp | 15 ++++++++++----- 1 file changed, 10 insertions(+), 5 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index fd9eb4170..c6543982e 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1247,28 +1247,33 @@ PX4IO::io_set_rc_config() */ param_get(param_find("RC_MAP_ROLL"), &ichan); + /* subtract one from 1-based index - this might be + * a negative number now + */ + ichan -= 1; + if ((ichan >= 0) && (ichan < (int)_max_rc_input)) - input_map[ichan - 1] = 0; + input_map[ichan] = 0; param_get(param_find("RC_MAP_PITCH"), &ichan); if ((ichan >= 0) && (ichan < (int)_max_rc_input)) - input_map[ichan - 1] = 1; + input_map[ichan] = 1; param_get(param_find("RC_MAP_YAW"), &ichan); if ((ichan >= 0) && (ichan < (int)_max_rc_input)) - input_map[ichan - 1] = 2; + input_map[ichan] = 2; param_get(param_find("RC_MAP_THROTTLE"), &ichan); if ((ichan >= 0) && (ichan < (int)_max_rc_input)) - input_map[ichan - 1] = 3; + input_map[ichan] = 3; param_get(param_find("RC_MAP_FLAPS"), &ichan); if ((ichan >= 0) && (ichan < (int)_max_rc_input)) - input_map[ichan - 1] = 4; + input_map[ichan] = 4; param_get(param_find("RC_MAP_MODE_SW"), &ichan); -- cgit v1.2.3 From 23a33e31cd91d074f2d16d04b0cd7f2c867d9d4b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 5 Nov 2014 19:26:10 +0100 Subject: Add missing mode switch channel --- src/drivers/px4io/px4io.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index c6543982e..6d68d9f60 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1279,7 +1279,7 @@ PX4IO::io_set_rc_config() if ((ichan >= 0) && (ichan < (int)_max_rc_input)) { /* use out of normal bounds index to indicate special channel */ - input_map[ichan - 1] = PX4IO_P_RC_CONFIG_ASSIGNMENT_MODESWITCH; + input_map[ichan] = PX4IO_P_RC_CONFIG_ASSIGNMENT_MODESWITCH; } /* -- cgit v1.2.3 From d907b030ee618b581e31964a34a61648e88793a3 Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Wed, 5 Nov 2014 13:15:52 -0800 Subject: Initialize RSSI so it doesn't remain uninitialized MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Spektrum doesn’t support rssi so it is not set by st24_decode. --- src/modules/px4iofirmware/controls.c | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c index ad60ee03e..3fd73fc60 100644 --- a/src/modules/px4iofirmware/controls.c +++ b/src/modules/px4iofirmware/controls.c @@ -41,6 +41,7 @@ #include #include +#include #include #include #include @@ -91,6 +92,7 @@ bool dsm_port_input(uint16_t *rssi, bool *dsm_updated, bool *st24_updated) for (unsigned i = 0; i < n_bytes; i++) { /* set updated flag if one complete packet was parsed */ + st24_rssi = RC_INPUT_RSSI_MAX; *st24_updated |= (OK == st24_decode(bytes[i], &st24_rssi, &rx_count, &st24_channel_count, r_raw_rc_values, PX4IO_RC_INPUT_CHANNELS)); } -- cgit v1.2.3 From f414eef042be7ebf641638009b3676523bf4bed6 Mon Sep 17 00:00:00 2001 From: philipoe Date: Tue, 4 Nov 2014 11:10:14 +0100 Subject: TECS: Modify absolute-value limiting of throttle demand --- src/lib/external_lgpl/tecs/tecs.cpp | 14 ++++++-------- 1 file changed, 6 insertions(+), 8 deletions(-) diff --git a/src/lib/external_lgpl/tecs/tecs.cpp b/src/lib/external_lgpl/tecs/tecs.cpp index 0ed210cf4..cfcc48b62 100644 --- a/src/lib/external_lgpl/tecs/tecs.cpp +++ b/src/lib/external_lgpl/tecs/tecs.cpp @@ -321,31 +321,29 @@ void TECS::_update_throttle(float throttle_cruise, const math::Matrix<3,3> &rotM ff_throttle = nomThr - STEdot_dem / _STEdot_min * nomThr; } - // Calculate PD + FF throttle + // Calculate PD + FF throttle and constrain to avoid blow-up of the integrator later _throttle_dem = (_STE_error + STEdot_error * _thrDamp) * K_STE2Thr + ff_throttle; + _throttle_dem = constrain(_throttle_dem, _THRminf, _THRmaxf); // Rate limit PD + FF throttle // Calculate the throttle increment from the specified slew time if (fabsf(_throttle_slewrate) > 0.01f) { float thrRateIncr = _DT * (_THRmaxf - _THRminf) * _throttle_slewrate; - _throttle_dem = constrain(_throttle_dem, - _last_throttle_dem - thrRateIncr, - _last_throttle_dem + thrRateIncr); + _last_throttle_dem - thrRateIncr, + _last_throttle_dem + thrRateIncr); } // Ensure _last_throttle_dem is always initialized properly - // Also: The throttle_slewrate limit is only applied to throttle_dem, but does not limit the integrator!! _last_throttle_dem = _throttle_dem; - // Calculate integrator state upper and lower limits - // Set to a value thqat will allow 0.1 (10%) throttle saturation to allow for noise on the demand + // Set to a value that will allow 0.1 (10%) throttle saturation to allow for noise on the demand float integ_max = (_THRmaxf - _throttle_dem + 0.1f); float integ_min = (_THRminf - _throttle_dem - 0.1f); // Calculate integrator state, constraining state - // Set integrator to a max throttle value dduring climbout + // Set integrator to a max throttle value during climbout _integ6_state = _integ6_state + (_STE_error * _integGain) * _DT * K_STE2Thr; if (_climbOutDem) { -- cgit v1.2.3 From 3f3353f2c4ad9f7b8263379c8b31bd9a437b0174 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 5 Nov 2014 21:19:04 +1100 Subject: mixer: fixed stream handle leakage --- src/modules/systemlib/mixer/mixer_load.c | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/systemlib/mixer/mixer_load.c b/src/modules/systemlib/mixer/mixer_load.c index bf3428a50..0d629d610 100644 --- a/src/modules/systemlib/mixer/mixer_load.c +++ b/src/modules/systemlib/mixer/mixer_load.c @@ -91,6 +91,7 @@ int load_mixer_file(const char *fname, char *buf, unsigned maxlen) /* if the line is too long to fit in the buffer, bail */ if ((strlen(line) + strlen(buf) + 1) >= maxlen) { warnx("line too long"); + fclose(fp); return -1; } @@ -98,6 +99,7 @@ int load_mixer_file(const char *fname, char *buf, unsigned maxlen) strcat(buf, line); } + fclose(fp); return 0; } -- cgit v1.2.3 From 43418a674903d242271028c4d4eb473f95a24be6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 7 Nov 2014 13:31:33 +0100 Subject: application layer only, no drivers affected: Fix overflow in RC input topic - this topic is deprecated and will be removed, has been superseded by input_rc and manual_control --- src/modules/uORB/topics/rc_channels.h | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/src/modules/uORB/topics/rc_channels.h b/src/modules/uORB/topics/rc_channels.h index b14ae1edd..16916cc4d 100644 --- a/src/modules/uORB/topics/rc_channels.h +++ b/src/modules/uORB/topics/rc_channels.h @@ -34,6 +34,8 @@ /** * @file rc_channels.h * Definition of the rc_channels uORB topic. + * + * @deprecated DO NOT USE FOR NEW CODE */ #ifndef RC_CHANNELS_H_ @@ -63,10 +65,13 @@ enum RC_CHANNELS_FUNCTION { AUX_2, AUX_3, AUX_4, - AUX_5, - RC_CHANNELS_FUNCTION_MAX /**< Indicates the number of functions. There can be more functions than RC channels. */ + AUX_5 }; +// MAXIMUM FUNCTIONS IS != MAXIMUM RC INPUT CHANNELS + +#define RC_CHANNELS_FUNCTION_MAX 18 + /** * @addtogroup topics * @{ -- cgit v1.2.3 From 02d31522cde1de13ab397f4fbd77c0cf9b7f712d Mon Sep 17 00:00:00 2001 From: Daniel Shiels Date: Wed, 29 Oct 2014 22:00:30 +1100 Subject: First attempt at sbus1 output. --- src/modules/px4iofirmware/sbus.c | 37 ++++++++++++++++++++++++++++++++++--- 1 file changed, 34 insertions(+), 3 deletions(-) diff --git a/src/modules/px4iofirmware/sbus.c b/src/modules/px4iofirmware/sbus.c index 6ead38d61..23dae7898 100644 --- a/src/modules/px4iofirmware/sbus.c +++ b/src/modules/px4iofirmware/sbus.c @@ -80,8 +80,10 @@ static int sbus_fd = -1; static hrt_abstime last_rx_time; static hrt_abstime last_frame_time; +static hrt_abstime last_txframe_time=0; static uint8_t frame[SBUS_FRAME_SIZE]; +static uint8_t oframe[SBUS_FRAME_SIZE]; static unsigned partial_frame_count; @@ -122,10 +124,39 @@ sbus_init(const char *device) void sbus1_output(uint16_t *values, uint16_t num_values) { - char a = 'A'; - write(sbus_fd, &a, 1); -} + //int. first byte of data is offset 1 in sbus + uint8_t byteindex=1; + uint8_t offset=0; + uint16_t value; + hrt_abstime now; + //oframe[0]=0xf0; + oframe[0]=0x0f; + now = hrt_absolute_time(); + if ((now - last_txframe_time) > 14000) { + last_txframe_time = now; + + for (uint16_t i=1;i=8) { + ++byteindex; + offset-=8; + } + oframe[byteindex] |= (value<<(offset))&0xff; + oframe[byteindex+1]|=(value>>(8-offset))&0xff; + oframe[byteindex+2]|=(value>>(16-offset))&0xff; + offset+=11; + } + write(sbus_fd, oframe, SBUS_FRAME_SIZE); + } +} void sbus2_output(uint16_t *values, uint16_t num_values) { -- cgit v1.2.3 From 60ecd8868db3539e552bad5f51473d1baf5d6ecd Mon Sep 17 00:00:00 2001 From: Daniel Shiels Date: Sat, 8 Nov 2014 22:36:45 +1100 Subject: sbus1 output. Cleaned up. Safer bounds checking. --- src/modules/px4iofirmware/sbus.c | 45 +++++++++++++++++++++++----------------- 1 file changed, 26 insertions(+), 19 deletions(-) diff --git a/src/modules/px4iofirmware/sbus.c b/src/modules/px4iofirmware/sbus.c index 23dae7898..2f234234e 100644 --- a/src/modules/px4iofirmware/sbus.c +++ b/src/modules/px4iofirmware/sbus.c @@ -57,6 +57,7 @@ #define SBUS_FLAGS_BYTE 23 #define SBUS_FAILSAFE_BIT 3 #define SBUS_FRAMELOST_BIT 2 +#define SBUS1_FRAME_DELAY 14000 /* Measured values with Futaba FX-30/R6108SB: @@ -80,7 +81,7 @@ static int sbus_fd = -1; static hrt_abstime last_rx_time; static hrt_abstime last_frame_time; -static hrt_abstime last_txframe_time=0; +static hrt_abstime last_txframe_time = 0; static uint8_t frame[SBUS_FRAME_SIZE]; static uint8_t oframe[SBUS_FRAME_SIZE]; @@ -125,35 +126,41 @@ void sbus1_output(uint16_t *values, uint16_t num_values) { //int. first byte of data is offset 1 in sbus - uint8_t byteindex=1; - uint8_t offset=0; + uint8_t byteindex = 1; + uint8_t offset = 0; uint16_t value; hrt_abstime now; - //oframe[0]=0xf0; - oframe[0]=0x0f; + oframe[0] = 0x0f; - now = hrt_absolute_time(); - if ((now - last_txframe_time) > 14000) { + now = hrt_absolute_time(); + + if ((now - last_txframe_time) > SBUS1_FRAME_DELAY) { last_txframe_time = now; - for (uint16_t i=1;i=8) { + if (value > 0x07ff ) { + value = 0x07ff; + } + + while (offset >= 8) { ++byteindex; - offset-=8; + offset -= 8; } - oframe[byteindex] |= (value<<(offset))&0xff; - oframe[byteindex+1]|=(value>>(8-offset))&0xff; - oframe[byteindex+2]|=(value>>(16-offset))&0xff; - offset+=11; - } + + oframe[byteindex] |= (value << (offset)) & 0xff; + oframe[byteindex + 1] |= (value >> (8 - offset)) & 0xff; + oframe[byteindex + 2] |= (value >> (16 - offset)) & 0xff; + offset += 11; + } + write(sbus_fd, oframe, SBUS_FRAME_SIZE); } } -- cgit v1.2.3 From 8599994ebb3e4ce63b5fe77cb1f2935d27604745 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 8 Nov 2014 20:38:57 +0100 Subject: Add generic uploader tool --- Tools/upload.sh | 28 ++++++++++++++++++++++++++++ 1 file changed, 28 insertions(+) create mode 100755 Tools/upload.sh diff --git a/Tools/upload.sh b/Tools/upload.sh new file mode 100755 index 000000000..17d87fe61 --- /dev/null +++ b/Tools/upload.sh @@ -0,0 +1,28 @@ +#!/bin/bash + +EXEDIR=`pwd` +BASEDIR=$(dirname $0) + +SYSTYPE=`uname -s` + +# +# Serial port defaults. +# +# XXX The uploader should be smarter than this. +# +if [ $SYSTYPE=Darwin ]; +then +SERIAL_PORTS="/dev/tty.usbmodemPX*,/dev/tty.usbmodem*" +fi + +if [ $SYSTYPE=Linux ]; +then +SERIAL_PORTS="/dev/serial/by-id/usb-3D_Robotics*" +fi + +if [ $SYSTYPE="" ]; +then +SERIAL_PORTS="COM32,COM31,COM30,COM29,COM28,COM27,COM26,COM25,COM24,COM23,COM22,COM21,COM20,COM19,COM18,COM17,COM16,COM15,COM14,COM13,COM12,COM11,COM10,COM9,COM8,COM7,COM6,COM5,COM4,COM3,COM2,COM1,COM0" +fi + +python $BASEDIR/px_uploader.py --port $SERIAL_PORTS $1 \ No newline at end of file -- cgit v1.2.3 From e87a179fc1b9b10bfd22e741229e9cbd75b6ef73 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 9 Nov 2014 15:16:05 +0100 Subject: MAVLink update --- mavlink/include/mavlink/v1.0 | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mavlink/include/mavlink/v1.0 b/mavlink/include/mavlink/v1.0 index 90383fac8..ad5e5a645 160000 --- a/mavlink/include/mavlink/v1.0 +++ b/mavlink/include/mavlink/v1.0 @@ -1 +1 @@ -Subproject commit 90383fac84d031aef17989a1497c2473dfa64340 +Subproject commit ad5e5a645dec152419264ad32221f7c113ea5c30 -- cgit v1.2.3 From 62db84fa758ecc30081e30a18464fe3060d2c2d8 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 9 Nov 2014 18:20:53 +0100 Subject: MAVLink update compile fixes --- src/modules/mavlink/mavlink.c | 6 +----- src/modules/mavlink/mavlink_main.cpp | 12 +++++++----- src/modules/mavlink/mavlink_main.h | 4 ++++ src/modules/mavlink/mavlink_messages.cpp | 12 +++++++----- 4 files changed, 19 insertions(+), 15 deletions(-) diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c index bec706bad..9afe74af1 100644 --- a/src/modules/mavlink/mavlink.c +++ b/src/modules/mavlink/mavlink.c @@ -78,11 +78,7 @@ PARAM_DEFINE_INT32(MAV_FWDEXTSP, 1); mavlink_system_t mavlink_system = { 100, - 50, - MAV_TYPE_FIXED_WING, - 0, - 0, - 0 + 50 }; // System ID, 1-255, Component/Subsystem ID, 1-255 /* diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 6b4edff78..c251dd3b2 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -167,8 +167,10 @@ Mavlink::Mavlink() : _param_initialized(false), _param_system_id(0), _param_component_id(0), - _param_system_type(0), + _param_system_type(MAV_TYPE_FIXED_WING), _param_use_hil_gps(0), + _param_forward_externalsp(0), + _system_type(0), /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "mavlink_el")), @@ -524,7 +526,7 @@ void Mavlink::mavlink_update_system(void) param_get(_param_system_type, &system_type); if (system_type >= 0 && system_type < MAV_TYPE_ENUM_END) { - mavlink_system.type = system_type; + _system_type = system_type; } int32_t use_hil_gps; @@ -755,7 +757,7 @@ Mavlink::send_message(const uint8_t msgid, const void *msg) pthread_mutex_lock(&_send_mutex); - int buf_free = get_free_tx_buf(); + unsigned buf_free = get_free_tx_buf(); uint8_t payload_len = mavlink_message_lengths[msgid]; unsigned packet_len = payload_len + MAVLINK_NUM_NON_PAYLOAD_BYTES; @@ -820,14 +822,14 @@ Mavlink::resend_message(mavlink_message_t *msg) pthread_mutex_lock(&_send_mutex); - int buf_free = get_free_tx_buf(); + unsigned buf_free = get_free_tx_buf(); _last_write_try_time = hrt_absolute_time(); unsigned packet_len = msg->len + MAVLINK_NUM_NON_PAYLOAD_BYTES; /* check if there is space in the buffer, let it overflow else */ - if (buf_free < TX_BUFFER_GAP) { + if ((buf_free < TX_BUFFER_GAP) || (buf_free < packet_len)) { /* no enough space in buffer to send */ count_txerr(); count_txerrbytes(packet_len); diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 1f96e586b..ad5e5001b 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -265,6 +265,8 @@ public: struct mavlink_logbuffer *get_logbuffer() { return &_logbuffer; } + unsigned get_system_type() { return _system_type; } + protected: Mavlink *next; @@ -354,6 +356,8 @@ private: param_t _param_use_hil_gps; param_t _param_forward_externalsp; + unsigned _system_type; + perf_counter_t _loop_perf; /**< loop performance counter */ perf_counter_t _txerr_perf; /**< TX error counter */ diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index cccb698bf..b4911427f 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -302,7 +302,7 @@ protected: msg.base_mode = 0; msg.custom_mode = 0; get_mavlink_mode_state(&status, &pos_sp_triplet, &msg.system_status, &msg.base_mode, &msg.custom_mode); - msg.type = mavlink_system.type; + msg.type = _mavlink->get_system_type(); msg.autopilot = MAV_AUTOPILOT_PX4; msg.mavlink_version = 3; @@ -1353,15 +1353,17 @@ protected: const float pwm_center = (PWM_HIGHEST_MAX + PWM_LOWEST_MIN) / 2; + unsigned system_type = _mavlink->get_system_type(); + /* scale outputs depending on system type */ - if (mavlink_system.type == MAV_TYPE_QUADROTOR || - mavlink_system.type == MAV_TYPE_HEXAROTOR || - mavlink_system.type == MAV_TYPE_OCTOROTOR) { + if (system_type == MAV_TYPE_QUADROTOR || + system_type == MAV_TYPE_HEXAROTOR || + system_type == MAV_TYPE_OCTOROTOR) { /* multirotors: set number of rotor outputs depending on type */ unsigned n; - switch (mavlink_system.type) { + switch (system_type) { case MAV_TYPE_QUADROTOR: n = 4; break; -- cgit v1.2.3 From 34e9d9efce10ce487b4c5af0cf93a442ab3b8594 Mon Sep 17 00:00:00 2001 From: Daniel Shiels Date: Mon, 10 Nov 2014 15:54:28 +1100 Subject: Code style cleanup. --- src/modules/px4iofirmware/sbus.c | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) diff --git a/src/modules/px4iofirmware/sbus.c b/src/modules/px4iofirmware/sbus.c index 2f234234e..90c0e0503 100644 --- a/src/modules/px4iofirmware/sbus.c +++ b/src/modules/px4iofirmware/sbus.c @@ -125,8 +125,7 @@ sbus_init(const char *device) void sbus1_output(uint16_t *values, uint16_t num_values) { - //int. first byte of data is offset 1 in sbus - uint8_t byteindex = 1; + uint8_t byteindex = 1; /*Data starts one byte into the sbus frame. */ uint8_t offset = 0; uint16_t value; hrt_abstime now; @@ -141,11 +140,13 @@ sbus1_output(uint16_t *values, uint16_t num_values) oframe[i] = 0; } - // 16 is sbus number of servos/channels minus 2 single bit channels. - // currently ignoring single bit channels. - for (uint16_t i = 0; (i < num_values) && (i < 16); ++i) { + /* 16 is sbus number of servos/channels minus 2 single bit channels. + * currently ignoring single bit channels. */ + + for (unsigned i = 0; (i < num_values) && (i < 16); ++i) { value = (uint16_t)(((values[i] - SBUS_SCALE_OFFSET) / SBUS_SCALE_FACTOR) + .5f); - //protect from out of bounds values and limit to 11 bits; + + /*protect from out of bounds values and limit to 11 bits*/ if (value > 0x07ff ) { value = 0x07ff; } -- cgit v1.2.3 From 3eeec2cce05c8f32a5c2d70b956786d41114df3c Mon Sep 17 00:00:00 2001 From: Daniel Shiels Date: Mon, 10 Nov 2014 16:34:28 +1100 Subject: Cleaned up sbus output frame initialisation. --- src/modules/px4iofirmware/sbus.c | 9 ++------- 1 file changed, 2 insertions(+), 7 deletions(-) diff --git a/src/modules/px4iofirmware/sbus.c b/src/modules/px4iofirmware/sbus.c index 90c0e0503..d76ec55f0 100644 --- a/src/modules/px4iofirmware/sbus.c +++ b/src/modules/px4iofirmware/sbus.c @@ -84,7 +84,6 @@ static hrt_abstime last_frame_time; static hrt_abstime last_txframe_time = 0; static uint8_t frame[SBUS_FRAME_SIZE]; -static uint8_t oframe[SBUS_FRAME_SIZE]; static unsigned partial_frame_count; @@ -128,17 +127,13 @@ sbus1_output(uint16_t *values, uint16_t num_values) uint8_t byteindex = 1; /*Data starts one byte into the sbus frame. */ uint8_t offset = 0; uint16_t value; - hrt_abstime now; - oframe[0] = 0x0f; + hrt_abstime now; now = hrt_absolute_time(); if ((now - last_txframe_time) > SBUS1_FRAME_DELAY) { last_txframe_time = now; - - for (uint16_t i = 1; i < SBUS_FRAME_SIZE; ++i) { - oframe[i] = 0; - } + uint8_t oframe[SBUS_FRAME_SIZE] = { 0x0f }; /* 16 is sbus number of servos/channels minus 2 single bit channels. * currently ignoring single bit channels. */ -- cgit v1.2.3 From ea0a59f80650c7bd7c72889f1c7bb72bfd3fdbd5 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 10 Nov 2014 09:26:11 +0100 Subject: HMC5883: Reduce output, add indices to make back-tracking lines easier, remove output completely for an OK-operation (the driver is NOT the right place to talk to the user!) --- src/drivers/hmc5883/hmc5883.cpp | 49 +++++++++++++---------------------------- 1 file changed, 15 insertions(+), 34 deletions(-) diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index e4ecfa6b5..81f767965 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -1049,11 +1049,9 @@ int HMC5883::calibrate(struct file *filp, unsigned enable) * LSM/Ga, giving 1.16 and 1.08 */ float expected_cal[3] = { 1.16f, 1.08f, 1.08f }; - warnx("starting mag scale calibration"); - /* start the sensor polling at 50 Hz */ if (OK != ioctl(filp, SENSORIOCSPOLLRATE, 50)) { - warn("failed to set 2Hz poll rate"); + warn("FAILED: SENSORIOCSPOLLRATE 2Hz"); ret = 1; goto out; } @@ -1061,25 +1059,25 @@ int HMC5883::calibrate(struct file *filp, unsigned enable) /* Set to 2.5 Gauss. We ask for 3 to get the right part of * the chained if statement above. */ if (OK != ioctl(filp, MAGIOCSRANGE, 3)) { - warnx("failed to set 2.5 Ga range"); + warnx("FAILED: MAGIOCSRANGE 3.3 Ga"); ret = 1; goto out; } if (OK != ioctl(filp, MAGIOCEXSTRAP, 1)) { - warnx("failed to enable sensor calibration mode"); + warnx("FAILED: MAGIOCEXSTRAP 1"); ret = 1; goto out; } if (OK != ioctl(filp, MAGIOCGSCALE, (long unsigned int)&mscale_previous)) { - warn("WARNING: failed to get scale / offsets for mag"); + warn("FAILED: MAGIOCGSCALE 1"); ret = 1; goto out; } if (OK != ioctl(filp, MAGIOCSSCALE, (long unsigned int)&mscale_null)) { - warn("WARNING: failed to set null scale / offsets for mag"); + warn("FAILED: MAGIOCSSCALE 1"); ret = 1; goto out; } @@ -1094,7 +1092,7 @@ int HMC5883::calibrate(struct file *filp, unsigned enable) ret = ::poll(&fds, 1, 2000); if (ret != 1) { - warn("timed out waiting for sensor data"); + warn("ERROR: TIMEOUT 1"); goto out; } @@ -1102,7 +1100,7 @@ int HMC5883::calibrate(struct file *filp, unsigned enable) sz = ::read(fd, &report, sizeof(report)); if (sz != sizeof(report)) { - warn("periodic read failed"); + warn("ERROR: READ 1"); ret = -EIO; goto out; } @@ -1118,7 +1116,7 @@ int HMC5883::calibrate(struct file *filp, unsigned enable) ret = ::poll(&fds, 1, 2000); if (ret != 1) { - warn("timed out waiting for sensor data"); + warn("ERROR: TIMEOUT 2"); goto out; } @@ -1126,7 +1124,7 @@ int HMC5883::calibrate(struct file *filp, unsigned enable) sz = ::read(fd, &report, sizeof(report)); if (sz != sizeof(report)) { - warn("periodic read failed"); + warn("ERROR: READ 2"); ret = -EIO; goto out; } @@ -1142,33 +1140,19 @@ int HMC5883::calibrate(struct file *filp, unsigned enable) sum_excited[1] += cal[1]; sum_excited[2] += cal[2]; } - - //warnx("periodic read %u", i); - //warnx("measurement: %.6f %.6f %.6f", (double)report.x, (double)report.y, (double)report.z); - //warnx("cal: %.6f %.6f %.6f", (double)cal[0], (double)cal[1], (double)cal[2]); } if (good_count < 5) { - warn("failed calibration"); ret = -EIO; goto out; } -#if 0 - warnx("measurement avg: %.6f %.6f %.6f", - (double)sum_excited[0]/good_count, - (double)sum_excited[1]/good_count, - (double)sum_excited[2]/good_count); -#endif - float scaling[3]; scaling[0] = sum_excited[0] / good_count; scaling[1] = sum_excited[1] / good_count; scaling[2] = sum_excited[2] / good_count; - warnx("axes scaling: %.6f %.6f %.6f", (double)scaling[0], (double)scaling[1], (double)scaling[2]); - /* set scaling in device */ mscale_previous.x_scale = scaling[0]; mscale_previous.y_scale = scaling[1]; @@ -1179,29 +1163,26 @@ int HMC5883::calibrate(struct file *filp, unsigned enable) out: if (OK != ioctl(filp, MAGIOCSSCALE, (long unsigned int)&mscale_previous)) { - warn("failed to set new scale / offsets for mag"); + warn("FAILED: MAGIOCSSCALE 2"); } /* set back to normal mode */ /* Set to 1.1 Gauss */ if (OK != ::ioctl(fd, MAGIOCSRANGE, 1)) { - warnx("failed to set 1.1 Ga range"); + warnx("FAILED: MAGIOCSRANGE 1.1 Ga"); } if (OK != ::ioctl(fd, MAGIOCEXSTRAP, 0)) { - warnx("failed to disable sensor calibration mode"); + warnx("FAILED: MAGIOCEXSTRAP 0"); } if (ret == OK) { - if (!check_scale()) { - warnx("mag scale calibration successfully finished."); - } else { - warnx("mag scale calibration finished with invalid results."); + if (check_scale()) { + /* failed */ + warnx("FAILED: SCALE"); ret = ERROR; } - } else { - warnx("mag scale calibration failed."); } return ret; -- cgit v1.2.3 From d253c8ba7d9cfc408328afd7c3708892b23626c0 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 10 Nov 2014 21:38:13 +1000 Subject: commander: correct the description array --- src/modules/commander/commander.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 46caddd46..1c3b68edd 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -760,7 +760,10 @@ int commander_thread_main(int argc, char *argv[]) nav_states_str[NAVIGATION_STATE_AUTO_MISSION] = "AUTO_MISSION"; nav_states_str[NAVIGATION_STATE_AUTO_LOITER] = "AUTO_LOITER"; nav_states_str[NAVIGATION_STATE_AUTO_RTL] = "AUTO_RTL"; + nav_states_str[NAVIGATION_STATE_AUTO_RCRECOVER] = "AUTO_RCRECOVER"; nav_states_str[NAVIGATION_STATE_AUTO_RTGS] = "AUTO_RTGS"; + nav_states_str[NAVIGATION_STATE_AUTO_LANDENGFAIL] = "AUTO_LANDENGFAIL"; + nav_states_str[NAVIGATION_STATE_AUTO_LANDGPSFAIL] = "AUTO_LANDGPSFAIL"; nav_states_str[NAVIGATION_STATE_ACRO] = "ACRO"; nav_states_str[NAVIGATION_STATE_LAND] = "LAND"; nav_states_str[NAVIGATION_STATE_DESCEND] = "DESCEND"; -- cgit v1.2.3 From f7db91ac32d6db1fbef9aa0c868636561bd2282c Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 10 Nov 2014 21:38:46 +1000 Subject: commander: enhance the failsafe verbose output --- src/modules/commander/commander.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 1c3b68edd..f9ea4e914 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1851,7 +1851,11 @@ int commander_thread_main(int argc, char *argv[]) if (status.failsafe != failsafe_old) { status_changed = true; - mavlink_log_info(mavlink_fd, "[cmd] failsafe state: %i", status.failsafe); + if (status.failsafe) { + mavlink_log_info(mavlink_fd, "[cmd] failsafe on"); + } else { + mavlink_log_info(mavlink_fd, "[cmd] failsafe off"); + } failsafe_old = status.failsafe; } -- cgit v1.2.3 From f8bed3cd892157b2db147b0d6a60f26371acdeb8 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 10 Nov 2014 21:39:35 +1000 Subject: state_machine_helper: whitespace --- src/modules/commander/state_machine_helper.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 9568752ae..a49965efd 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -444,7 +444,7 @@ transition_result_t hil_state_transition(hil_state_t new_state, int status_pub, * Check failsafe and main status and set navigation status for navigator accordingly */ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_enabled, const bool mission_finished, - const bool stay_in_failsafe) + const bool stay_in_failsafe) { navigation_state_t nav_state_old = status->nav_state; -- cgit v1.2.3 From 2c577815818b579743dab13ef0055f02605699d1 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 10 Nov 2014 21:40:13 +1000 Subject: state_machine_helper: trying to clean up some failsafe logic --- src/modules/commander/state_machine_helper.cpp | 43 ++++++++++++++++++-------- 1 file changed, 30 insertions(+), 13 deletions(-) diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index a49965efd..3553dcdc3 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -497,11 +497,14 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en break; case MAIN_STATE_AUTO_MISSION: + /* go into failsafe * - if commanded to do so * - if we have an engine failure - * - if either the datalink is enabled and lost as well as RC is lost - * - if there is no datalink and the mission is finished */ + * - if the datalink is enabled and lost as well as RC is lost + * or if the datalink is disabled and lost as well as RC is lost and the mission is finished */ + + /* first look at the commands */ if (status->engine_failure_cmd) { status->nav_state = NAVIGATION_STATE_AUTO_LANDENGFAIL; } else if (status->data_link_lost_cmd) { @@ -509,14 +512,16 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en } else if (status->gps_failure_cmd) { status->nav_state = NAVIGATION_STATE_AUTO_LANDGPSFAIL; } else if (status->rc_signal_lost_cmd) { - status->nav_state = NAVIGATION_STATE_AUTO_RTGS; //XXX - /* Finished handling commands which have priority , now handle failures */ + status->nav_state = NAVIGATION_STATE_AUTO_RCRECOVER; + + /* finished handling commands which have priority, now handle failures */ } 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)) { + + /* check for RC signal and datalink lost (with datalink enabled) after mission is finished */ + } else if ((status->data_link_lost && data_link_loss_enabled) && status->rc_signal_lost) { status->failsafe = true; if (status->condition_global_position_valid && status->condition_home_position_valid) { @@ -529,7 +534,21 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en status->nav_state = NAVIGATION_STATE_TERMINATION; } - /* also go into failsafe if just datalink is lost */ + /* check if RC signal is lost (with dataink disabled) after mission is finished*/ + } else if (!data_link_loss_enabled && status->rc_signal_lost && mission_finished) { + status->failsafe = true; + + if (status->condition_global_position_valid && status->condition_home_position_valid) { + status->nav_state = NAVIGATION_STATE_AUTO_RCRECOVER; + } else if (status->condition_local_position_valid) { + status->nav_state = NAVIGATION_STATE_LAND; + } else if (status->condition_local_altitude_valid) { + status->nav_state = NAVIGATION_STATE_DESCEND; + } else { + status->nav_state = NAVIGATION_STATE_TERMINATION; + } + + /* also go into failsafe if just datalink is lost (with datalink enabled) */ } else if (status->data_link_lost && data_link_loss_enabled) { status->failsafe = true; @@ -543,13 +562,11 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en status->nav_state = NAVIGATION_STATE_TERMINATION; } - /* don't bother if RC is lost and mission is not yet finished */ - } else if (status->rc_signal_lost && !stay_in_failsafe) { + /* stay where you are */ + } else if (stay_in_failsafe){ - /* this mode is ok, we don't need RC for missions */ - status->nav_state = NAVIGATION_STATE_AUTO_MISSION; - } else if (!stay_in_failsafe){ - /* everything is perfect */ + /* everything is perfect */ + } else { status->nav_state = NAVIGATION_STATE_AUTO_MISSION; } break; -- cgit v1.2.3 From f1d56cbddc575115215f680ba42cdedde40b2626 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 10 Nov 2014 21:41:41 +1000 Subject: navigator: don't reset the finished flag, this fixes the strange problem where it toggles between MISSION and RTL --- src/modules/navigator/navigator_main.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index a867dd0da..eff3a5938 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -632,7 +632,6 @@ Navigator::publish_mission_result() } /* reset reached bool */ _mission_result.reached = false; - _mission_result.finished = false; } void -- cgit v1.2.3 From 89724c24a16bd340c33c27531435089e7bd338fb Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 10 Nov 2014 21:42:13 +1000 Subject: vehicle_status: whitespace --- src/modules/uORB/topics/vehicle_status.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index a1b2667e3..91491c148 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -108,7 +108,7 @@ typedef enum { NAVIGATION_STATE_AUTO_LANDGPSFAIL, /**< Auto land on gps failure (e.g. open loop loiter down) */ NAVIGATION_STATE_ACRO, /**< Acro mode */ NAVIGATION_STATE_LAND, /**< Land mode */ - NAVIGATION_STATE_DESCEND, /**< Descend mode (no position control) */ + NAVIGATION_STATE_DESCEND, /**< Descend mode (no position control) */ NAVIGATION_STATE_TERMINATION, /**< Termination mode */ NAVIGATION_STATE_OFFBOARD, NAVIGATION_STATE_MAX, -- cgit v1.2.3 From d15203de91cc07530809060f949374bb8918daea Mon Sep 17 00:00:00 2001 From: mazahner Date: Mon, 10 Nov 2014 17:35:48 +0100 Subject: Debug/NuttX: small changes to make it work with the current stable branch. Debug/NuttX made existing code work with python3 in gdb. Added some new features --- Debug/NuttX | 38 +++-- Debug/Nuttx.py | 433 ++++++++++++++++++++++++++++++++++++++++++++++++++------- 2 files changed, 416 insertions(+), 55 deletions(-) diff --git a/Debug/NuttX b/Debug/NuttX index d34e9f5b4..4b9f4b5a1 100644 --- a/Debug/NuttX +++ b/Debug/NuttX @@ -78,7 +78,7 @@ end ################################################################################ define showfiles - set $task = (struct _TCB *)$arg0 + set $task = (struct tcb_s *)$arg0 set $nfiles = sizeof((*(struct filelist*)0).fl_files) / sizeof(struct file) printf "%d files\n", $nfiles set $index = 0 @@ -102,7 +102,7 @@ end ################################################################################ define _showtask_oneline - set $task = (struct _TCB *)$arg0 + set $task = (struct tcb_s *)$arg0 printf " %p %.2d %.3d %s\n", $task, $task->pid, $task->sched_priority, $task->name end @@ -139,7 +139,7 @@ end # Print task registers for a NuttX v7em target with FPU enabled. # define _showtaskregs_v7em - set $task = (struct _TCB *)$arg0 + set $task = (struct tcb_s *)$arg0 set $regs = (uint32_t *)&($task->xcp.regs[0]) printf " r0: 0x%08x 0x%08x 0x%08x 0x%08x 0x%08x 0x%08x 0x%08x 0x%08x\n", $regs[27], $regs[28], $regs[29], $regs[30], $regs[2], $regs[3], $regs[4], $regs[5] @@ -162,7 +162,7 @@ end define _showsemaphore printf "count %d ", $arg0->semcount if $arg0->holder.htcb != 0 - set $_task = (struct _TCB *)$arg0->holder.htcb + set $_task = (struct tcb_s *)$arg0->holder.htcb printf "held by %s", $_task->name end printf "\n" @@ -172,7 +172,7 @@ end # Print information about a task's stack usage # define showtaskstack - set $task = (struct _TCB *)$arg0 + set $task = (struct tcb_s *)$arg0 if $task == &g_idletcb printf "can't measure idle stack\n" @@ -189,7 +189,7 @@ end # Print details of a task # define showtask - set $task = (struct _TCB *)$arg0 + set $task = (struct tcb_s *)$arg0 printf "%p %.2d ", $task, $task->pid _showtaskstate $task @@ -204,7 +204,7 @@ define showtask if $task->task_state != TSTATE_TASK_RUNNING _showtaskregs_v7em $task else - _showcurrentregs_v7em + _showtaskregs_v7em $task end # XXX print registers here @@ -247,8 +247,10 @@ define showtasks _showtasklist &g_pendingtasks printf "RUNNABLE\n" _showtasklist &g_readytorun - printf "WAITING\n" + printf "WAITING for Semaphore\n" _showtasklist &g_waitingforsemaphore + printf "WAITING for Signal\n" + _showtasklist &g_waitingforsignal printf "INACTIVE\n" _showtasklist &g_inactivetasks end @@ -257,3 +259,23 @@ document showtasks . showtasks . Print a list of all tasks in the system, separated into their respective queues. end + +define my_mem + + set $start = $arg0 + set $end = $arg1 + set $cursor = $start + + if $start < $end + while $cursor != $end + set *$cursor = 0x0000 + set $cursor = $cursor + 4 + printf "0x%x of 0x%x\n",$cursor,$end + end + else + while $cursor != $end + set *$cursor = 0x0000 + set $cursor = $cursor - 4 + end + end +end \ No newline at end of file diff --git a/Debug/Nuttx.py b/Debug/Nuttx.py index 7cc21b99f..cf86dd668 100644 --- a/Debug/Nuttx.py +++ b/Debug/Nuttx.py @@ -59,30 +59,42 @@ class NX_register_set(object): def __init__(self, xcpt_regs): if xcpt_regs is None: - self.regs['R0'] = long(gdb.parse_and_eval('$r0')) - self.regs['R1'] = long(gdb.parse_and_eval('$r1')) - self.regs['R2'] = long(gdb.parse_and_eval('$r2')) - self.regs['R3'] = long(gdb.parse_and_eval('$r3')) - self.regs['R4'] = long(gdb.parse_and_eval('$r4')) - self.regs['R5'] = long(gdb.parse_and_eval('$r5')) - self.regs['R6'] = long(gdb.parse_and_eval('$r6')) - self.regs['R7'] = long(gdb.parse_and_eval('$r7')) - self.regs['R8'] = long(gdb.parse_and_eval('$r8')) - self.regs['R9'] = long(gdb.parse_and_eval('$r9')) - self.regs['R10'] = long(gdb.parse_and_eval('$r10')) - self.regs['R11'] = long(gdb.parse_and_eval('$r11')) - self.regs['R12'] = long(gdb.parse_and_eval('$r12')) - self.regs['R13'] = long(gdb.parse_and_eval('$r13')) - self.regs['SP'] = long(gdb.parse_and_eval('$sp')) - self.regs['R14'] = long(gdb.parse_and_eval('$r14')) - self.regs['LR'] = long(gdb.parse_and_eval('$lr')) - self.regs['R15'] = long(gdb.parse_and_eval('$r15')) - self.regs['PC'] = long(gdb.parse_and_eval('$pc')) - self.regs['XPSR'] = long(gdb.parse_and_eval('$xpsr')) + self.regs['R0'] = self.mon_reg_call('r0') + self.regs['R1'] = self.mon_reg_call('r1') + self.regs['R2'] = self.mon_reg_call('r2') + self.regs['R3'] = self.mon_reg_call('r3') + self.regs['R4'] = self.mon_reg_call('r4') + self.regs['R5'] = self.mon_reg_call('r5') + self.regs['R6'] = self.mon_reg_call('r6') + self.regs['R7'] = self.mon_reg_call('r7') + self.regs['R8'] = self.mon_reg_call('r8') + self.regs['R9'] = self.mon_reg_call('r9') + self.regs['R10'] = self.mon_reg_call('r10') + self.regs['R11'] = self.mon_reg_call('r11') + self.regs['R12'] = self.mon_reg_call('r12') + self.regs['R13'] = self.mon_reg_call('r13') + self.regs['SP'] = self.mon_reg_call('sp') + self.regs['R14'] = self.mon_reg_call('r14') + self.regs['LR'] = self.mon_reg_call('lr') + self.regs['R15'] = self.mon_reg_call('r15') + self.regs['PC'] = self.mon_reg_call('pc') + self.regs['XPSR'] = self.mon_reg_call('xPSR') else: for key in self.v7em_regmap.keys(): self.regs[key] = int(xcpt_regs[self.v7em_regmap[key]]) + def mon_reg_call(self,register): + """ + register is the register as a string e.g. 'pc' + return integer containing the value of the register + """ + str_to_eval = "mon reg "+register + resp = gdb.execute(str_to_eval,to_string = True) + content = resp.split()[-1]; + try: + return int(content,16) + except: + return 0 @classmethod def with_xcpt_regs(cls, xcpt_regs): @@ -172,7 +184,7 @@ class NX_task(object): self.__dict__['stack_used'] = 0 else: stack_limit = self._tcb['adj_stack_size'] - for offset in range(0, stack_limit): + for offset in range(0, int(stack_limit)): if stack_base[offset] != 0xff: break self.__dict__['stack_used'] = stack_limit - offset @@ -187,7 +199,7 @@ class NX_task(object): def state(self): """return the name of the task's current state""" statenames = gdb.types.make_enum_dict(gdb.lookup_type('enum tstate_e')) - for name,value in statenames.iteritems(): + for name,value in statenames.items(): if value == self._tcb['task_state']: return name return 'UNKNOWN' @@ -196,16 +208,19 @@ class NX_task(object): def waiting_for(self): """return a description of what the task is waiting for, if it is waiting""" if self._state_is('TSTATE_WAIT_SEM'): - waitsem = self._tcb['waitsem'].dereference() - waitsem_holder = waitsem['holder'] - holder = NX_task.for_tcb(waitsem_holder['htcb']) - if holder is not None: - return '{}({})'.format(waitsem.address, holder.name) - else: - return '{}()'.format(waitsem.address) + try: + waitsem = self._tcb['waitsem'].dereference() + waitsem_holder = waitsem['holder'] + holder = NX_task.for_tcb(waitsem_holder['htcb']) + if holder is not None: + return '{}({})'.format(waitsem.address, holder.name) + else: + return '{}()'.format(waitsem.address) + except: + return 'EXCEPTION' if self._state_is('TSTATE_WAIT_SIG'): return 'signal' - return None + return "" @property def is_waiting(self): @@ -229,7 +244,7 @@ class NX_task(object): filearray = filelist['fl_files'] result = dict() for i in range(filearray.type.range()[0],filearray.type.range()[1]): - inode = long(filearray[i]['f_inode']) + inode = int(filearray[i]['f_inode']) if inode != 0: result[i] = inode return result @@ -253,7 +268,18 @@ class NX_task(object): def __str__(self): return "{}:{}".format(self.pid, self.name) - + + def showoff(self): + print("-------") + print(self.pid,end = ", ") + print(self.name,end = ", ") + print(self.state,end = ", ") + print(self.waiting_for,end = ", ") + print(self.stack_used,end = ", ") + print(self._tcb['adj_stack_size'],end = ", ") + print(self.file_descriptors) + print(self.registers) + def __format__(self, format_spec): return format_spec.format( pid = self.pid, @@ -265,7 +291,7 @@ class NX_task(object): file_descriptors = self.file_descriptors, registers = self.registers ) - + class NX_show_task (gdb.Command): """(NuttX) prints information about a task""" @@ -285,7 +311,7 @@ class NX_show_task (gdb.Command): my_fmt += ' R8 {registers[R8]:#010x} {registers[R9]:#010x} {registers[R10]:#010x} {registers[R11]:#010x}\n' my_fmt += ' R12 {registers[PC]:#010x}\n' my_fmt += ' SP {registers[SP]:#010x} LR {registers[LR]:#010x} PC {registers[PC]:#010x} XPSR {registers[XPSR]:#010x}\n' - print format(t, my_fmt) + print(format(t, my_fmt)) class NX_show_tasks (gdb.Command): """(NuttX) prints a list of tasks""" @@ -295,8 +321,10 @@ class NX_show_tasks (gdb.Command): def invoke(self, args, from_tty): tasks = NX_task.tasks() + print ('Number of tasks: ' + str(len(tasks))) for t in tasks: - print format(t, '{pid:<2} {name:<16} {state:<20} {stack_used:>4}/{stack_limit:<4}') + #t.showoff() + print(format(t, 'Task: {pid} {name} {state} {stack_used}/{stack_limit}')) NX_show_task() NX_show_tasks() @@ -306,15 +334,15 @@ class NX_show_heap (gdb.Command): def __init__(self): super(NX_show_heap, self).__init__('show heap', gdb.COMMAND_USER) - struct_mm_allocnode_s = gdb.lookup_type('struct mm_allocnode_s') - preceding_size = struct_mm_allocnode_s['preceding'].type.sizeof - if preceding_size == 2: + struct_mm_allocnode_s = gdb.lookup_type('struct mm_allocnode_s') + preceding_size = struct_mm_allocnode_s['preceding'].type.sizeof + if preceding_size == 2: self._allocflag = 0x8000 - elif preceding_size == 4: + elif preceding_size == 4: self._allocflag = 0x80000000 - else: - raise gdb.GdbError('invalid mm_allocnode_s.preceding size %u' % preceding_size) - self._allocnodesize = struct_mm_allocnode_s.sizeof + else: + raise gdb.GdbError('invalid mm_allocnode_s.preceding size %u' % preceding_size) + self._allocnodesize = struct_mm_allocnode_s.sizeof def _node_allocated(self, allocnode): if allocnode['preceding'] & self._allocflag: @@ -328,7 +356,7 @@ class NX_show_heap (gdb.Command): if region_start >= region_end: raise gdb.GdbError('heap region {} corrupt'.format(hex(region_start))) nodecount = region_end - region_start - print 'heap {} - {}'.format(region_start, region_end) + print ('heap {} - {}'.format(region_start, region_end)) cursor = 1 while cursor < nodecount: allocnode = region_start[cursor] @@ -336,8 +364,8 @@ class NX_show_heap (gdb.Command): state = '' else: state = '(free)' - print ' {} {} {}'.format(allocnode.address + self._allocnodesize, - self._node_size(allocnode), state) + print( ' {} {} {}'.format(allocnode.address + self._allocnodesize, + self._node_size(allocnode), state)) cursor += self._node_size(allocnode) / self._allocnodesize def invoke(self, args, from_tty): @@ -345,7 +373,7 @@ class NX_show_heap (gdb.Command): nregions = heap['mm_nregions'] region_starts = heap['mm_heapstart'] region_ends = heap['mm_heapend'] - print '{} heap(s)'.format(nregions) + print( '{} heap(s)'.format(nregions)) # walk the heaps for i in range(0, nregions): self._print_allocations(region_starts[i], region_ends[i]) @@ -370,6 +398,317 @@ class NX_show_interrupted_thread (gdb.Command): my_fmt += ' R8 {registers[R8]:#010x} {registers[R9]:#010x} {registers[R10]:#010x} {registers[R11]:#010x}\n' my_fmt += ' R12 {registers[PC]:#010x}\n' my_fmt += ' SP {registers[SP]:#010x} LR {registers[LR]:#010x} PC {registers[PC]:#010x} XPSR {registers[XPSR]:#010x}\n' - print format(registers, my_fmt) + print (format(registers, my_fmt)) NX_show_interrupted_thread() + +class NX_check_tcb(gdb.Command): + """ check the tcb of a task from a address """ + def __init__(self): + super(NX_check_tcb,self).__init__('show tcb', gdb.COMMAND_USER) + + def invoke(self,args,sth): + tasks = NX_task.tasks() + print("tcb int: ",int(args)) + print(tasks[int(args)]._tcb) + a =tasks[int(args)]._tcb['xcp']['regs'] + print("relevant registers:") + for reg in regmap: + hex_addr= hex(int(a[regmap[reg]])) + eval_string = 'info line *'+str(hex_addr) + print(reg,": ",hex_addr,) +NX_check_tcb() + +class NX_tcb(object): + def __init__(self): + pass + + def is_in(self,arg,list): + for i in list: + if arg == i: + return True; + return False + + def find_tcb_list(self,dq_entry_t): + tcb_list = [] + tcb_ptr = dq_entry_t.cast(gdb.lookup_type('struct tcb_s').pointer()) + first_tcb = tcb_ptr.dereference() + tcb_list.append(first_tcb); + next_tcb = first_tcb['flink'].dereference() + while not self.is_in(int(next_tcb['pid']),[int(t['pid']) for t in tcb_list]): + tcb_list.append(next_tcb); + old_tcb = next_tcb; + next_tcb = old_tcb['flink'].dereference() + + return [t for t in tcb_list if int(t['pid'])<2000] + + def getTCB(self): + list_of_listsnames = ['g_pendingtasks','g_readytorun','g_waitingforsemaphore','g_waitingforsignal','g_inactivetasks'] + tcb_list = []; + for l in list_of_listsnames: + li = gdb.lookup_global_symbol(l) + print(li) + cursor = li.value()['head'] + tcb_list = tcb_list + self.find_tcb_list(cursor) + +class NX_check_stack_order(gdb.Command): + """ Check the Stack order corresponding to the tasks """ + + def __init__(self): + super(NX_check_stack_order,self).__init__('show check_stack', gdb.COMMAND_USER) + + def is_in(self,arg,list): + for i in list: + if arg == i: + return True; + return False + + def find_tcb_list(self,dq_entry_t): + tcb_list = [] + tcb_ptr = dq_entry_t.cast(gdb.lookup_type('struct tcb_s').pointer()) + first_tcb = tcb_ptr.dereference() + tcb_list.append(first_tcb); + next_tcb = first_tcb['flink'].dereference() + while not self.is_in(int(next_tcb['pid']),[int(t['pid']) for t in tcb_list]): + tcb_list.append(next_tcb); + old_tcb = next_tcb; + next_tcb = old_tcb['flink'].dereference() + + return [t for t in tcb_list if int(t['pid'])<2000] + + def getTCB(self): + list_of_listsnames = ['g_pendingtasks','g_readytorun','g_waitingforsemaphore','g_waitingforsignal','g_inactivetasks'] + tcb_list = []; + for l in list_of_listsnames: + li = gdb.lookup_global_symbol(l) + cursor = li.value()['head'] + tcb_list = tcb_list + self.find_tcb_list(cursor) + return tcb_list + + def getSPfromTask(self,tcb): + regmap = NX_register_set.v7em_regmap + a =tcb['xcp']['regs'] + return int(a[regmap['SP']]) + + def find_closest(self,list,val): + tmp_list = [abs(i-val) for i in list] + tmp_min = min(tmp_list) + idx = tmp_list.index(tmp_min) + return idx,list[idx] + + def find_next_stack(self,address,_dict_in): + add_list = [] + name_list = [] + for key in _dict_in.keys(): + for i in range(3): + if _dict_in[key][i] < address: + add_list.append(_dict_in[key][i]) + if i == 2: # the last one is the processes stack pointer + name_list.append(self.check_name(key)+"_SP") + else: + name_list.append(self.check_name(key)) + + idx,new_address = self.find_closest(add_list,address) + return new_address,name_list[idx] + + def check_name(self,name): + if isinstance(name,(list)): + name = name[0]; + idx = name.find("\\") + newname = name[:idx] + + return newname + + def invoke(self,args,sth): + tcb = self.getTCB(); + stackadresses={}; + for t in tcb: + p = []; + #print(t.name,t._tcb['stack_alloc_ptr']) + p.append(int(t['stack_alloc_ptr'])) + p.append(int(t['adj_stack_ptr'])) + p.append(self.getSPfromTask(t)) + stackadresses[str(t['name'])] = p; + address = int("0x30000000",0) + print("stack address : process") + for i in range(len(stackadresses)*3): + address,name = self.find_next_stack(address,stackadresses) + print(hex(address),": ",name) + +NX_check_stack_order() + +class NX_run_debug_util(gdb.Command): + """ show the registers of a task corresponding to a tcb address""" + def __init__(self): + super(NX_run_debug_util,self).__init__('show regs', gdb.COMMAND_USER) + + def printRegisters(self,task): + regmap = NX_register_set.v7em_regmap + a =task._tcb['xcp']['regs'] + print("relevant registers in ",task.name,":") + for reg in regmap: + hex_addr= hex(int(a[regmap[reg]])) + eval_string = 'info line *'+str(hex_addr) + print(reg,": ",hex_addr,) + + def getPCfromTask(self,task): + regmap = NX_register_set.v7em_regmap + a =task._tcb['xcp']['regs'] + return hex(int(a[regmap['PC']])) + + def invoke(self,args,sth): + tasks = NX_task.tasks() + if args == '': + for t in tasks: + self.printRegisters(t) + eval_str = "list *"+str(self.getPCfromTask(t)) + print("this is the location in code where the current threads $pc is:") + gdb.execute(eval_str) + else: + tcb_nr = int(args); + print("tcb_nr = ",tcb_nr) + t = tasks[tcb_nr] + self.printRegisters(t) + eval_str = "list *"+str(self.getPCfromTask(t)) + print("this is the location in code where the current threads $pc is:") + gdb.execute(eval_str) + +NX_run_debug_util() + + +class NX_search_tcb(gdb.Command): + """ shot PID's of all running tasks """ + + def __init__(self): + super(NX_search_tcb,self).__init__('show alltcb', gdb.COMMAND_USER) + + def is_in(self,arg,list): + for i in list: + if arg == i: + return True; + return False + + def find_tcb_list(self,dq_entry_t): + tcb_list = [] + tcb_ptr = dq_entry_t.cast(gdb.lookup_type('struct tcb_s').pointer()) + first_tcb = tcb_ptr.dereference() + tcb_list.append(first_tcb); + next_tcb = first_tcb['flink'].dereference() + while not self.is_in(int(next_tcb['pid']),[int(t['pid']) for t in tcb_list]): + tcb_list.append(next_tcb); + old_tcb = next_tcb; + next_tcb = old_tcb['flink'].dereference() + + return [t for t in tcb_list if int(t['pid'])<2000] + + def invoke(self,args,sth): + list_of_listsnames = ['g_pendingtasks','g_readytorun','g_waitingforsemaphore','g_waitingforsignal','g_inactivetasks'] + tasks = []; + for l in list_of_listsnames: + li = gdb.lookup_global_symbol(l) + cursor = li.value()['head'] + tasks = tasks + self.find_tcb_list(cursor) + + # filter for tasks that are listed twice + tasks_filt = {} + for t in tasks: + pid = int(t['pid']); + if not pid in tasks_filt.keys(): + tasks_filt[pid] = t['name']; + print('{num_t} Tasks found:'.format(num_t = len(tasks_filt))) + for pid in tasks_filt.keys(): + print("PID: ",pid," ",tasks_filt[pid]) + +NX_search_tcb() + + +class NX_my_bt(gdb.Command): + """ 'fake' backtrace: backtrace the stack of a process and check every suspicious address for the list + arg: tcb_address$ + (can easily be found by typing 'showtask'). + """ + + def __init__(self): + super(NX_my_bt,self).__init__('show mybt', gdb.COMMAND_USER) + + def readmem(self,addr): + ''' + read memory at addr and return nr + ''' + str_to_eval = "x/x "+hex(addr) + resp = gdb.execute(str_to_eval,to_string = True) + idx = resp.find('\t') + return int(resp[idx:],16) + + def is_in_bounds(self,val): + lower_bound = int("08004000",16) + upper_bound = int("080ae0c0",16); + #print(lower_bound," ",val," ",upper_bound) + if val>lower_bound and val0: + name = words[3][idx+1:]; + path = words[3][:idx]; + else: + name = words[3]; + path = ""; + block = gdb.block_for_pc(addr) + func = block.function + if str(func) == "None": + func = block.superblock.function + + if valid: + print("Line: ",line," in ",path,"/",name,"in ",func) + return name,path,line,func + + + + + def invoke(self,args,sth): + addr_dec = int(args[2:],16) + _tcb = self.get_tcb_from_address(addr_dec) + print("found task with PID: ",_tcb["pid"]) + up_stack = int(_tcb['adj_stack_ptr']) + curr_sp = int(_tcb['xcp']['regs'][0]) #curr stack pointer + other_sp = int(_tcb['xcp']['regs'][8]) # other stack pointer + stacksize = int(_tcb['adj_stack_size']) # other stack pointer + + print("tasks current SP = ",hex(curr_sp),"stack max ptr is at ",hex(up_stack)) + + if curr_sp == up_stack: + sp = other_sp + else: + sp = curr_sp; + + while(sp < up_stack): + mem = self.readmem(sp) + #print(hex(sp)," : ",hex(mem)) + if self.is_in_bounds(mem): + # this is a potential instruction ptr + stack_percentage = (up_stack-sp)/stacksize + name,path,line,func = self.print_instruction_at(mem,stack_percentage) + sp = sp + 4; # jump up one word + +NX_my_bt() -- cgit v1.2.3 From 88093ebf1af5483d8b5c218adb1ea0c4798ee21e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 10 Nov 2014 18:52:18 +0100 Subject: Hotfix: Fix IO channel mapping --- src/drivers/px4io/px4io.cpp | 47 +++++++++++++++++++++++---------------------- 1 file changed, 24 insertions(+), 23 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 6d68d9f60..06faf49a4 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1245,41 +1245,42 @@ PX4IO::io_set_rc_config() * for compatibility reasons with existing * autopilots / GCS'. */ - param_get(param_find("RC_MAP_ROLL"), &ichan); - - /* subtract one from 1-based index - this might be - * a negative number now - */ - ichan -= 1; - if ((ichan >= 0) && (ichan < (int)_max_rc_input)) - input_map[ichan] = 0; + /* ROLL */ + param_get(param_find("RC_MAP_ROLL"), &ichan); + if ((ichan > 0) && (ichan <= (int)_max_rc_input)) { + input_map[ichan - 1] = 0; + } + /* PITCH */ param_get(param_find("RC_MAP_PITCH"), &ichan); + if ((ichan > 0) && (ichan <= (int)_max_rc_input)) { + input_map[ichan - 1] = 1; + } - if ((ichan >= 0) && (ichan < (int)_max_rc_input)) - input_map[ichan] = 1; - + /* YAW */ param_get(param_find("RC_MAP_YAW"), &ichan); + if ((ichan > 0) && (ichan <= (int)_max_rc_input)) { + input_map[ichan - 1] = 2; + } - if ((ichan >= 0) && (ichan < (int)_max_rc_input)) - input_map[ichan] = 2; - + /* THROTTLE */ param_get(param_find("RC_MAP_THROTTLE"), &ichan); + if ((ichan > 0) && (ichan <= (int)_max_rc_input)) { + input_map[ichan - 1] = 3; + } - if ((ichan >= 0) && (ichan < (int)_max_rc_input)) - input_map[ichan] = 3; - + /* FLAPS */ param_get(param_find("RC_MAP_FLAPS"), &ichan); + if ((ichan > 0) && (ichan <= (int)_max_rc_input)) { + input_map[ichan - 1] = 4; + } - if ((ichan >= 0) && (ichan < (int)_max_rc_input)) - input_map[ichan] = 4; - + /* MAIN MODE SWITCH */ param_get(param_find("RC_MAP_MODE_SW"), &ichan); - - if ((ichan >= 0) && (ichan < (int)_max_rc_input)) { + if ((ichan > 0) && (ichan <= (int)_max_rc_input)) { /* use out of normal bounds index to indicate special channel */ - input_map[ichan] = PX4IO_P_RC_CONFIG_ASSIGNMENT_MODESWITCH; + input_map[ichan - 1] = PX4IO_P_RC_CONFIG_ASSIGNMENT_MODESWITCH; } /* -- cgit v1.2.3 From c8a8305326f84cdfd92326b36241caee64b0c529 Mon Sep 17 00:00:00 2001 From: fludwig Date: Wed, 15 Oct 2014 23:48:40 +0200 Subject: added switch to send PWM to Brushless Controller in unarmed state --- ROMFS/px4fmu_common/init.d/3030_io_camflyer | 3 +++ 1 file changed, 3 insertions(+) diff --git a/ROMFS/px4fmu_common/init.d/3030_io_camflyer b/ROMFS/px4fmu_common/init.d/3030_io_camflyer index 83c470234..0f0cb3a89 100644 --- a/ROMFS/px4fmu_common/init.d/3030_io_camflyer +++ b/ROMFS/px4fmu_common/init.d/3030_io_camflyer @@ -3,3 +3,6 @@ sh /etc/init.d/rc.fw_defaults set MIXER FMU_Q +# Provide ESC a constant 1000 us pulse while disarmed +set PWM_OUTPUTS 4 +set PWM_DISARMED 1000 -- cgit v1.2.3 From e474600978f0fe92de79622a746d06f2c9df77a9 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 11 Nov 2014 09:18:31 +1000 Subject: commander: make the failsafe message critical --- src/modules/commander/commander.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index f9ea4e914..002a33811 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1852,9 +1852,9 @@ int commander_thread_main(int argc, char *argv[]) if (status.failsafe != failsafe_old) { status_changed = true; if (status.failsafe) { - mavlink_log_info(mavlink_fd, "[cmd] failsafe on"); + mavlink_log_critical(mavlink_fd, "failsafe mode on"); } else { - mavlink_log_info(mavlink_fd, "[cmd] failsafe off"); + mavlink_log_critical(mavlink_fd, "failsafe mode off"); } failsafe_old = status.failsafe; } -- cgit v1.2.3 From fa5f3658828280f54a7fec3c33ec3e25e4198561 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 11 Nov 2014 10:25:53 +1000 Subject: state_machine_helper: fixed comments --- src/modules/commander/state_machine_helper.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 3553dcdc3..e1a626262 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -520,7 +520,7 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en } else if (status->engine_failure) { status->nav_state = NAVIGATION_STATE_AUTO_LANDENGFAIL; - /* check for RC signal and datalink lost (with datalink enabled) after mission is finished */ + /* check for RC signal and datalink lost (with datalink enabled) */ } else if ((status->data_link_lost && data_link_loss_enabled) && status->rc_signal_lost) { status->failsafe = true; @@ -534,7 +534,7 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en status->nav_state = NAVIGATION_STATE_TERMINATION; } - /* check if RC signal is lost (with dataink disabled) after mission is finished*/ + /* check if RC signal is lost (with datalink disabled) after mission is finished */ } else if (!data_link_loss_enabled && status->rc_signal_lost && mission_finished) { status->failsafe = true; -- cgit v1.2.3 From 3c9a73f3e41a379e5e7bbf6e98d67facc3b2b49f Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 11 Nov 2014 10:28:24 +1000 Subject: fixed empty if body --- src/modules/commander/state_machine_helper.cpp | 7 ++----- 1 file changed, 2 insertions(+), 5 deletions(-) diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index e1a626262..a9a5c4051 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -562,11 +562,8 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en status->nav_state = NAVIGATION_STATE_TERMINATION; } - /* stay where you are */ - } else if (stay_in_failsafe){ - - /* everything is perfect */ - } else { + /* stay where you are if you should stay in failsafe, otherwise everything is perfect */ + } else if (!stay_in_failsafe){ status->nav_state = NAVIGATION_STATE_AUTO_MISSION; } break; -- cgit v1.2.3 From d98831c3b50addab0084f9636060cece46ff92bc Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 11 Nov 2014 10:42:26 +1000 Subject: navigator: move waypoint reached reset to a more obvious location --- src/modules/navigator/mission.cpp | 1 + src/modules/navigator/navigator_main.cpp | 2 -- 2 files changed, 1 insertion(+), 2 deletions(-) diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 7fac69a61..b8e3274fd 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -707,6 +707,7 @@ Mission::set_mission_item_reached() _navigator->get_mission_result()->reached = true; _navigator->get_mission_result()->seq_reached = _current_offboard_mission_index; _navigator->publish_mission_result(); + reset_mission_item_reached(); } void diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index eff3a5938..e933919fb 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -630,8 +630,6 @@ Navigator::publish_mission_result() /* advertise and publish */ _mission_result_pub = orb_advertise(ORB_ID(mission_result), &_mission_result); } - /* reset reached bool */ - _mission_result.reached = false; } void -- cgit v1.2.3 From 51ffb887c39ecfd0cb94879475f032ee65505321 Mon Sep 17 00:00:00 2001 From: Holger Steinhaus Date: Tue, 11 Nov 2014 11:13:08 +0100 Subject: UAVCAN: initialize device id for mag and baro to allow DEVIOCGDEVICEID ioctl to return useful data --- src/drivers/device/device.h | 3 ++- src/modules/uavcan/sensors/mag.cpp | 2 ++ src/modules/uavcan/sensors/sensor_bridge.cpp | 3 +++ src/modules/uavcan/sensors/sensor_bridge.hpp | 2 ++ 4 files changed, 9 insertions(+), 1 deletion(-) diff --git a/src/drivers/device/device.h b/src/drivers/device/device.h index 9d684e394..67aaa0aff 100644 --- a/src/drivers/device/device.h +++ b/src/drivers/device/device.h @@ -130,7 +130,8 @@ public: enum DeviceBusType { DeviceBusType_UNKNOWN = 0, DeviceBusType_I2C = 1, - DeviceBusType_SPI = 2 + DeviceBusType_SPI = 2, + DeviceBusType_UAVCAN = 3, }; /* diff --git a/src/modules/uavcan/sensors/mag.cpp b/src/modules/uavcan/sensors/mag.cpp index 8e6a9a22f..0d9ea08c5 100644 --- a/src/modules/uavcan/sensors/mag.cpp +++ b/src/modules/uavcan/sensors/mag.cpp @@ -49,6 +49,8 @@ UavcanMagnetometerBridge::UavcanMagnetometerBridge(uavcan::INode& node) : UavcanCDevSensorBridgeBase("uavcan_mag", "/dev/uavcan/mag", MAG_DEVICE_PATH, MAG_TOPICS), _sub_mag(node) { + _device_id.devid_s.devtype = DRV_MAG_DEVTYPE_HMC5883; + _scale.x_scale = 1.0F; _scale.y_scale = 1.0F; _scale.z_scale = 1.0F; diff --git a/src/modules/uavcan/sensors/sensor_bridge.cpp b/src/modules/uavcan/sensors/sensor_bridge.cpp index 9608ce680..0999938fc 100644 --- a/src/modules/uavcan/sensors/sensor_bridge.cpp +++ b/src/modules/uavcan/sensors/sensor_bridge.cpp @@ -103,6 +103,9 @@ void UavcanCDevSensorBridgeBase::publish(const int node_id, const void *report) return; } + // update device id as we now know our device node_id + _device_id.devid_s.address = static_cast(node_id); + // Ask the CDev helper which class instance we can take const int class_instance = register_class_devname(_class_devname); if (class_instance < 0 || class_instance >= int(_max_channels)) { diff --git a/src/modules/uavcan/sensors/sensor_bridge.hpp b/src/modules/uavcan/sensors/sensor_bridge.hpp index 1316f7ecc..04c59b77d 100644 --- a/src/modules/uavcan/sensors/sensor_bridge.hpp +++ b/src/modules/uavcan/sensors/sensor_bridge.hpp @@ -112,6 +112,8 @@ protected: _channels(new Channel[MaxChannels]) { memcpy(_orb_topics, orb_topics, sizeof(orb_id_t) * MaxChannels); + _device_id.devid_s.bus_type = DeviceBusType_UAVCAN; + _device_id.devid_s.bus = 1; // @TBD: insert UAVCAN bus no. here } /** -- cgit v1.2.3 From cf2baac516b6f86daf30342be47264b2dcc8abc4 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 11 Nov 2014 21:45:13 +1000 Subject: navigator: more reset reached calls --- src/modules/navigator/datalinkloss.cpp | 4 ++++ src/modules/navigator/rcloss.cpp | 2 ++ 2 files changed, 6 insertions(+) diff --git a/src/modules/navigator/datalinkloss.cpp b/src/modules/navigator/datalinkloss.cpp index 66f1c8c73..e789fd10d 100644 --- a/src/modules/navigator/datalinkloss.cpp +++ b/src/modules/navigator/datalinkloss.cpp @@ -156,6 +156,7 @@ DataLinkLoss::set_dll_item() /* Request flight termination from the commander */ _navigator->get_mission_result()->flight_termination = true; _navigator->publish_mission_result(); + reset_mission_item_reached(); warnx("not switched to manual: request flight termination"); pos_sp_triplet->previous.valid = false; pos_sp_triplet->current.valid = false; @@ -188,6 +189,7 @@ DataLinkLoss::advance_dll() mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: too many DL losses, fly to airfield home"); _navigator->get_mission_result()->stay_in_failsafe = true; _navigator->publish_mission_result(); + reset_mission_item_reached(); _dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP; } else { if (!_param_skipcommshold.get()) { @@ -208,6 +210,7 @@ DataLinkLoss::advance_dll() _dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP; _navigator->get_mission_result()->stay_in_failsafe = true; _navigator->publish_mission_result(); + reset_mission_item_reached(); break; case DLL_STATE_FLYTOAIRFIELDHOMEWP: _dll_state = DLL_STATE_TERMINATE; @@ -215,6 +218,7 @@ DataLinkLoss::advance_dll() mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: no manual control, terminating"); _navigator->get_mission_result()->stay_in_failsafe = true; _navigator->publish_mission_result(); + reset_mission_item_reached(); break; case DLL_STATE_TERMINATE: warnx("dll end"); diff --git a/src/modules/navigator/rcloss.cpp b/src/modules/navigator/rcloss.cpp index 5564a1c42..42392e739 100644 --- a/src/modules/navigator/rcloss.cpp +++ b/src/modules/navigator/rcloss.cpp @@ -163,6 +163,7 @@ RCLoss::advance_rcl() _rcl_state = RCL_STATE_TERMINATE; _navigator->get_mission_result()->stay_in_failsafe = true; _navigator->publish_mission_result(); + reset_mission_item_reached(); } break; case RCL_STATE_LOITER: @@ -171,6 +172,7 @@ RCLoss::advance_rcl() mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RC not regained, terminating"); _navigator->get_mission_result()->stay_in_failsafe = true; _navigator->publish_mission_result(); + reset_mission_item_reached(); break; case RCL_STATE_TERMINATE: warnx("rcl end"); -- cgit v1.2.3 From 1394b02c2e2158b2796a51ca622c17893f78361f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 11 Nov 2014 13:45:25 +0100 Subject: Make tools executable --- Documentation/versionfilter.sh | 0 Tools/px_generate_xml.sh | 0 Tools/px_update_wiki.sh | 0 3 files changed, 0 insertions(+), 0 deletions(-) mode change 100644 => 100755 Documentation/versionfilter.sh mode change 100644 => 100755 Tools/px_generate_xml.sh mode change 100644 => 100755 Tools/px_update_wiki.sh diff --git a/Documentation/versionfilter.sh b/Documentation/versionfilter.sh old mode 100644 new mode 100755 diff --git a/Tools/px_generate_xml.sh b/Tools/px_generate_xml.sh old mode 100644 new mode 100755 diff --git a/Tools/px_update_wiki.sh b/Tools/px_update_wiki.sh old mode 100644 new mode 100755 -- cgit v1.2.3 From 50b410664f4087bb4fd50e8bba62267c70719b71 Mon Sep 17 00:00:00 2001 From: Holger Steinhaus Date: Tue, 11 Nov 2014 17:01:20 +0100 Subject: UAVCAN: set bus number part of device_id to zero --- src/modules/uavcan/sensors/sensor_bridge.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/uavcan/sensors/sensor_bridge.hpp b/src/modules/uavcan/sensors/sensor_bridge.hpp index 04c59b77d..e31960537 100644 --- a/src/modules/uavcan/sensors/sensor_bridge.hpp +++ b/src/modules/uavcan/sensors/sensor_bridge.hpp @@ -113,7 +113,7 @@ protected: { memcpy(_orb_topics, orb_topics, sizeof(orb_id_t) * MaxChannels); _device_id.devid_s.bus_type = DeviceBusType_UAVCAN; - _device_id.devid_s.bus = 1; // @TBD: insert UAVCAN bus no. here + _device_id.devid_s.bus = 0; } /** -- cgit v1.2.3 From d63c054bb00a0df6666f498950ab797dc07817b4 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 12 Nov 2014 09:48:32 +1000 Subject: state_machine_helper: changed failsafe behaviour: always require at least one link for default, do RTGS as soon as datalink is lost if datalink loss mode is enabled --- src/modules/commander/state_machine_helper.cpp | 29 ++++++++------------------ 1 file changed, 9 insertions(+), 20 deletions(-) diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index a9a5c4051..e37019d02 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -501,8 +501,7 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en /* go into failsafe * - if commanded to do so * - if we have an engine failure - * - if the datalink is enabled and lost as well as RC is lost - * or if the datalink is disabled and lost as well as RC is lost and the mission is finished */ + * - depending on datalink, RC and if the mission is finished */ /* first look at the commands */ if (status->engine_failure_cmd) { @@ -520,8 +519,9 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en } else if (status->engine_failure) { status->nav_state = NAVIGATION_STATE_AUTO_LANDENGFAIL; - /* check for RC signal and datalink lost (with datalink enabled) */ - } else if ((status->data_link_lost && data_link_loss_enabled) && status->rc_signal_lost) { + /* datalink loss enabled: + * check for datalink lost: this should always trigger RTGS */ + } else if (data_link_loss_enabled && status->data_link_lost) { status->failsafe = true; if (status->condition_global_position_valid && status->condition_home_position_valid) { @@ -534,8 +534,11 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en status->nav_state = NAVIGATION_STATE_TERMINATION; } - /* check if RC signal is lost (with datalink disabled) after mission is finished */ - } else if (!data_link_loss_enabled && status->rc_signal_lost && mission_finished) { + /* datalink loss disabled: + * check if both, RC and datalink are lost during the mission + * or RC is lost after the mission is finished: this should always trigger RCRECOVER */ + } else if (!data_link_loss_enabled && ((status->rc_signal_lost && status->data_link_lost) || + (status->rc_signal_lost && mission_finished))) { status->failsafe = true; if (status->condition_global_position_valid && status->condition_home_position_valid) { @@ -548,20 +551,6 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en status->nav_state = NAVIGATION_STATE_TERMINATION; } - /* also go into failsafe if just datalink is lost (with datalink enabled) */ - } else if (status->data_link_lost && data_link_loss_enabled) { - status->failsafe = true; - - if (status->condition_global_position_valid && status->condition_home_position_valid) { - status->nav_state = NAVIGATION_STATE_AUTO_RTGS; - } else if (status->condition_local_position_valid) { - status->nav_state = NAVIGATION_STATE_LAND; - } else if (status->condition_local_altitude_valid) { - status->nav_state = NAVIGATION_STATE_DESCEND; - } else { - status->nav_state = NAVIGATION_STATE_TERMINATION; - } - /* stay where you are if you should stay in failsafe, otherwise everything is perfect */ } else if (!stay_in_failsafe){ status->nav_state = NAVIGATION_STATE_AUTO_MISSION; -- cgit v1.2.3 From 20703c8d2352dd137ec13be69e42fe6fdfd9f8f2 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 12 Nov 2014 11:05:06 +1000 Subject: manual_control_setpoint: whitespace --- src/modules/uORB/topics/manual_control_setpoint.h | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/src/modules/uORB/topics/manual_control_setpoint.h b/src/modules/uORB/topics/manual_control_setpoint.h index dde237adc..c0c33a629 100644 --- a/src/modules/uORB/topics/manual_control_setpoint.h +++ b/src/modules/uORB/topics/manual_control_setpoint.h @@ -47,7 +47,7 @@ * Switch position */ typedef enum { - SWITCH_POS_NONE = 0, /**< switch is not mapped */ + SWITCH_POS_NONE = 0, /**< switch is not mapped */ SWITCH_POS_ON, /**< switch activated (value = 1) */ SWITCH_POS_MIDDLE, /**< middle position (value = 0) */ SWITCH_POS_OFF /**< switch not activated (value = -1) */ @@ -93,12 +93,12 @@ struct manual_control_setpoint_s { float aux4; /**< default function: camera roll */ float aux5; /**< default function: payload drop */ - switch_pos_t mode_switch; /**< main mode 3 position switch (mandatory): _MANUAL_, ASSIST, AUTO */ - switch_pos_t return_switch; /**< return to launch 2 position switch (mandatory): _NORMAL_, RTL */ - switch_pos_t posctl_switch; /**< position control 2 position switch (optional): _ALTCTL_, POSCTL */ - switch_pos_t loiter_switch; /**< loiter 2 position switch (optional): _MISSION_, LOITER */ - switch_pos_t acro_switch; /**< acro 2 position switch (optional): _MANUAL_, ACRO */ - switch_pos_t offboard_switch; /**< offboard 2 position switch (optional): _NORMAL_, OFFBOARD */ + switch_pos_t mode_switch; /**< main mode 3 position switch (mandatory): _MANUAL_, ASSIST, AUTO */ + switch_pos_t return_switch; /**< return to launch 2 position switch (mandatory): _NORMAL_, RTL */ + switch_pos_t posctl_switch; /**< position control 2 position switch (optional): _ALTCTL_, POSCTL */ + switch_pos_t loiter_switch; /**< loiter 2 position switch (optional): _MISSION_, LOITER */ + switch_pos_t acro_switch; /**< acro 2 position switch (optional): _MANUAL_, ACRO */ + switch_pos_t offboard_switch; /**< offboard 2 position switch (optional): _NORMAL_, OFFBOARD */ }; /**< manual control inputs */ /** -- cgit v1.2.3 From b1c6692f2028b616581efaf7a320a7cf7072a02f Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 12 Nov 2014 11:06:25 +1000 Subject: commander: removed unused definition --- src/modules/commander/commander.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 46caddd46..149c0f60e 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -220,8 +220,6 @@ void control_status_leds(vehicle_status_s *status, const actuator_armed_s *actua void check_valid(hrt_abstime timestamp, hrt_abstime timeout, bool valid_in, bool *valid_out, bool *changed); -void check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_status_s *status); - transition_result_t set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoint_s *sp_man); void set_control_mode(); -- cgit v1.2.3 From f1058bdf0816cda718cee4d5a69c4ac3d53ea781 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 12 Nov 2014 11:07:27 +1000 Subject: manual_control_setpoint: wrong comment --- src/modules/uORB/topics/manual_control_setpoint.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/uORB/topics/manual_control_setpoint.h b/src/modules/uORB/topics/manual_control_setpoint.h index c0c33a629..50b7bd9e5 100644 --- a/src/modules/uORB/topics/manual_control_setpoint.h +++ b/src/modules/uORB/topics/manual_control_setpoint.h @@ -99,7 +99,7 @@ struct manual_control_setpoint_s { switch_pos_t loiter_switch; /**< loiter 2 position switch (optional): _MISSION_, LOITER */ switch_pos_t acro_switch; /**< acro 2 position switch (optional): _MANUAL_, ACRO */ switch_pos_t offboard_switch; /**< offboard 2 position switch (optional): _NORMAL_, OFFBOARD */ -}; /**< manual control inputs */ +}; /** * @} -- cgit v1.2.3 From 703fcec62d3430b75876d2c06183bbf4451f0dd6 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 12 Nov 2014 12:11:23 +1000 Subject: navigator: fix do jumps: after the repetitions are completed the current mission index is no longer invalid --- src/modules/navigator/mission.cpp | 21 ++++++++++++--------- 1 file changed, 12 insertions(+), 9 deletions(-) diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 7fac69a61..840c9ec0e 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -371,7 +371,7 @@ Mission::set_mission_items() } else { /* no mission available or mission finished, switch to loiter */ if (_mission_type != MISSION_TYPE_NONE) { - mavlink_log_critical(_navigator->get_mavlink_fd(), "mission finished, loitering"); + mavlink_log_critical(_navigator->get_mavlink_fd(), "mission finished"); /* use last setpoint for loiter */ _navigator->set_can_loiter_at_sp(true); @@ -595,13 +595,14 @@ Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s dm_item = DM_KEY_WAYPOINTS_OFFBOARD(_offboard_mission.dataman_id); } - if (*mission_index_ptr < 0 || *mission_index_ptr >= (int)mission->count) { - /* mission item index out of bounds */ - return false; - } - /* repeat several to get the mission item because we might have to follow multiple DO_JUMPS */ for (int i = 0; i < 10; i++) { + + if (*mission_index_ptr < 0 || *mission_index_ptr >= (int)mission->count) { + /* mission item index out of bounds */ + return false; + } + const ssize_t len = sizeof(struct mission_item_s); /* read mission item to temp storage first to not overwrite current mission item if data damaged */ @@ -630,7 +631,7 @@ Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s /* not supposed to happen unless the datamanager can't access the * dataman */ mavlink_log_critical(_navigator->get_mavlink_fd(), - "ERROR DO JUMP waypoint could not be written"); + "ERROR DO JUMP waypoint could not be written"); return false; } } @@ -639,8 +640,10 @@ Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s *mission_index_ptr = mission_item_tmp.do_jump_mission_index; } else { - mavlink_log_critical(_navigator->get_mavlink_fd(), - "DO JUMP repetitions completed"); + if (is_current) { + mavlink_log_critical(_navigator->get_mavlink_fd(), + "DO JUMP repetitions completed"); + } /* no more DO_JUMPS, therefore just try to continue with next mission item */ (*mission_index_ptr)++; } -- cgit v1.2.3 From 08443205e90955c13532c79aa02a283a50cce25d Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 12 Nov 2014 13:07:06 +1000 Subject: navigator: make DO_JUMPS survive power on resets --- src/modules/navigator/mission.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 840c9ec0e..cfe85c84e 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -627,7 +627,8 @@ Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s if (is_current) { (mission_item_tmp.do_jump_current_count)++; /* save repeat count */ - if (dm_write(dm_item, *mission_index_ptr, DM_PERSIST_IN_FLIGHT_RESET, &mission_item_tmp, len) != len) { + if (dm_write(dm_item, *mission_index_ptr, DM_PERSIST_POWER_ON_RESET, + &mission_item_tmp, len) != len) { /* not supposed to happen unless the datamanager can't access the * dataman */ mavlink_log_critical(_navigator->get_mavlink_fd(), -- cgit v1.2.3 From 14b5732c6eff6998d013c0816e983320906c462d Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 12 Nov 2014 13:26:10 +1000 Subject: px4flow: small verbose output fix --- src/drivers/px4flow/px4flow.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/drivers/px4flow/px4flow.cpp b/src/drivers/px4flow/px4flow.cpp index 6c68636c6..9e1ecb61e 100644 --- a/src/drivers/px4flow/px4flow.cpp +++ b/src/drivers/px4flow/px4flow.cpp @@ -76,7 +76,7 @@ #define PX4FLOW_BUS PX4_I2C_BUS_EXPANSION #define I2C_FLOW_ADDRESS 0x42 //* 7-bit address. 8-bit address is 0x84 //range 0x42 - 0x49 - + /* PX4FLOW Registers addresses */ #define PX4FLOW_REG 0x00 /* Measure Register */ @@ -443,7 +443,7 @@ PX4FLOW::measure() if (OK != ret) { perf_count(_comms_errors); log("i2c::transfer returned %d", ret); - printf("i2c::transfer flow returned %d"); + printf("i2c::transfer flow returned %d", ret); return ret; } -- cgit v1.2.3 From 7bc9a6257731a741ff309f060f6cf87c33c4a266 Mon Sep 17 00:00:00 2001 From: Holger Steinhaus Date: Wed, 12 Nov 2014 09:52:35 +0100 Subject: code style, warnings --- src/modules/uavcan/actuators/esc.cpp | 13 +++++++------ src/modules/uavcan/uavcan_main.cpp | 9 ++++----- src/systemcmds/motor_test/motor_test.c | 19 ++++++++----------- 3 files changed, 19 insertions(+), 22 deletions(-) diff --git a/src/modules/uavcan/actuators/esc.cpp b/src/modules/uavcan/actuators/esc.cpp index fbd4f0bcd..1d23099f3 100644 --- a/src/modules/uavcan/actuators/esc.cpp +++ b/src/modules/uavcan/actuators/esc.cpp @@ -113,8 +113,7 @@ void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs) msg.cmd.push_back(static_cast(scaled)); _esc_status.esc[i].esc_setpoint_raw = abs(static_cast(scaled)); - } - else { + } else { msg.cmd.push_back(static_cast(0)); } } @@ -128,18 +127,20 @@ void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs) void UavcanEscController::arm_all_escs(bool arm) { - if (arm) + if (arm) { _armed_mask = -1; - else + } else { _armed_mask = 0; + } } void UavcanEscController::arm_single_esc(int num, bool arm) { - if (arm) + if (arm) { _armed_mask = MOTOR_BIT(num); - else + } else { _armed_mask = 0; + } } void UavcanEscController::esc_status_sub_cb(const uavcan::ReceivedDataStructure &msg) diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index 653d4f98c..2c543462e 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -346,13 +346,12 @@ int UavcanNode::run() // can we mix? if (_test_in_progress) { - float outputs[NUM_ACTUATOR_OUTPUTS] = {}; - outputs[_test_motor.motor_number] = _test_motor.value*2.0f-1.0f; + float test_outputs[NUM_ACTUATOR_OUTPUTS] = {}; + test_outputs[_test_motor.motor_number] = _test_motor.value*2.0f-1.0f; // Output to the bus - _esc_controller.update_outputs(outputs, NUM_ACTUATOR_OUTPUTS); - } - else if (controls_updated && (_mixers != nullptr)) { + _esc_controller.update_outputs(test_outputs, NUM_ACTUATOR_OUTPUTS); + } else if (controls_updated && (_mixers != nullptr)) { // XXX one output group has 8 outputs max, // but this driver could well serve multiple groups. diff --git a/src/systemcmds/motor_test/motor_test.c b/src/systemcmds/motor_test/motor_test.c index 079f99674..087699b05 100644 --- a/src/systemcmds/motor_test/motor_test.c +++ b/src/systemcmds/motor_test/motor_test.c @@ -67,19 +67,15 @@ void motor_test(unsigned channel, float value) _test_motor.timestamp = hrt_absolute_time(); _test_motor.value = value; - if (_test_motor_pub > 0) { - /* publish armed state */ - orb_publish(ORB_ID(test_motor), _test_motor_pub, &_test_motor); - } else { - /* advertise and publish */ - _test_motor_pub = orb_advertise(ORB_ID(test_motor), &_test_motor); - } + _test_motor_pub = orb_advertise(ORB_ID(test_motor), &_test_motor); + orb_publish(ORB_ID(test_motor), _test_motor_pub, &_test_motor); } static void usage(const char *reason) { - if (reason != NULL) + if (reason != NULL) { warnx("%s", reason); + } errx(1, "usage:\n" @@ -90,8 +86,9 @@ static void usage(const char *reason) int motor_test_main(int argc, char *argv[]) { - unsigned long channel, lval; - float value; + unsigned long channel = 0; + unsigned long lval; + float value = 0.0f; int ch; if (argc != 5) { @@ -122,7 +119,7 @@ int motor_test_main(int argc, char *argv[]) motor_test(channel, value); - printf("motor %d set to %.2f\n", channel, value); + printf("motor %d set to %.2f\n", channel, (double)value); exit(0); } -- cgit v1.2.3 From 64de403b5f2aa4af57918510451373424c162b68 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 12 Nov 2014 20:06:49 +1000 Subject: navigator: trying to improve a comment --- src/modules/navigator/mission.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index cfe85c84e..d0611b9e5 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -595,7 +595,8 @@ Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s dm_item = DM_KEY_WAYPOINTS_OFFBOARD(_offboard_mission.dataman_id); } - /* repeat several to get the mission item because we might have to follow multiple DO_JUMPS */ + /* Repeat this several times in case there are several DO JUMPS that we need to follow along, however, after + * 10 iterations we have to assume that the DO JUMPS are probably cycling and give up. */ for (int i = 0; i < 10; i++) { if (*mission_index_ptr < 0 || *mission_index_ptr >= (int)mission->count) { -- cgit v1.2.3 From ae99a179ac0fd2b7a240d51adf7d992ff91ed598 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 12 Nov 2014 12:21:15 +0100 Subject: Include NuttX sscanf() fix --- NuttX | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/NuttX b/NuttX index 5ee4b2b2c..47a94a097 160000 --- a/NuttX +++ b/NuttX @@ -1 +1 @@ -Subproject commit 5ee4b2b2c26bbc35d1669840f0676e8aa383b984 +Subproject commit 47a94a0979fd5aee2c8416e0197d6ce5926e94ca -- cgit v1.2.3 From d7ebea6ec2f3783bfdd4becd7cda5fd24c340a85 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 12 Nov 2014 09:01:00 +0100 Subject: Fix comparison in upload script for test builds --- Tools/upload.sh | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/Tools/upload.sh b/Tools/upload.sh index 17d87fe61..c0dd65eba 100755 --- a/Tools/upload.sh +++ b/Tools/upload.sh @@ -10,17 +10,17 @@ SYSTYPE=`uname -s` # # XXX The uploader should be smarter than this. # -if [ $SYSTYPE=Darwin ]; +if [ $SYSTYPE = "Darwin" ]; then SERIAL_PORTS="/dev/tty.usbmodemPX*,/dev/tty.usbmodem*" fi -if [ $SYSTYPE=Linux ]; +if [ $SYSTYPE = "Linux" ]; then SERIAL_PORTS="/dev/serial/by-id/usb-3D_Robotics*" fi -if [ $SYSTYPE="" ]; +if [ $SYSTYPE = "" ]; then SERIAL_PORTS="COM32,COM31,COM30,COM29,COM28,COM27,COM26,COM25,COM24,COM23,COM22,COM21,COM20,COM19,COM18,COM17,COM16,COM15,COM14,COM13,COM12,COM11,COM10,COM9,COM8,COM7,COM6,COM5,COM4,COM3,COM2,COM1,COM0" fi -- cgit v1.2.3 From afcbab7fba84cf3d3d016a63fbca405ade15889a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 12 Nov 2014 13:37:58 +0100 Subject: PX4FLOW driver: Prevent sensor from spamming the system on errors. Use the perf system call to look at error counters. --- src/drivers/px4flow/px4flow.cpp | 12 +++++------- 1 file changed, 5 insertions(+), 7 deletions(-) diff --git a/src/drivers/px4flow/px4flow.cpp b/src/drivers/px4flow/px4flow.cpp index 9e1ecb61e..804027b05 100644 --- a/src/drivers/px4flow/px4flow.cpp +++ b/src/drivers/px4flow/px4flow.cpp @@ -200,7 +200,7 @@ PX4FLOW::PX4FLOW(int bus, int address) : _buffer_overflows(perf_alloc(PC_COUNT, "px4flow_buffer_overflows")) { // enable debug() calls - _debug_enabled = true; + _debug_enabled = false; // work_cancel in the dtor will explode if we don't do this... memset(&_work, 0, sizeof(_work)); @@ -240,7 +240,7 @@ PX4FLOW::init() _px4flow_topic = orb_advertise(ORB_ID(optical_flow), &zero_report); if (_px4flow_topic < 0) { - debug("failed to create px4flow object. Did you start uOrb?"); + warnx("failed to create px4flow object. Did you start uOrb?"); } ret = OK; @@ -442,8 +442,6 @@ PX4FLOW::measure() if (OK != ret) { perf_count(_comms_errors); - log("i2c::transfer returned %d", ret); - printf("i2c::transfer flow returned %d", ret); return ret; } @@ -465,7 +463,7 @@ PX4FLOW::collect() ret = transfer(nullptr, 0, &val[0], 22); if (ret < 0) { - log("error reading from sensor: %d", ret); + debug("error reading from sensor: %d", ret); perf_count(_comms_errors); perf_end(_sample_perf); return ret; @@ -565,7 +563,7 @@ PX4FLOW::cycle() /* perform collection */ if (OK != collect()) { - log("collection error"); + debug("collection error"); /* restart the measurement state machine */ start(); return; @@ -592,7 +590,7 @@ PX4FLOW::cycle() /* measurement phase */ if (OK != measure()) { - log("measure error"); + debug("measure error"); } /* next phase is collection */ -- cgit v1.2.3 From 826fd3f4cdf492be1e5b05f7da4493ff10118876 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 13 Nov 2014 09:43:51 +1000 Subject: commander: reordering navigation states to match enum --- src/modules/commander/commander.cpp | 153 +++++++++++++++++++----------------- 1 file changed, 83 insertions(+), 70 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 7917bc9f2..3307c4bfc 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -2201,18 +2201,6 @@ set_control_mode() control_mode.flag_control_termination_enabled = false; break; - case NAVIGATION_STATE_ACRO: - control_mode.flag_control_manual_enabled = true; - control_mode.flag_control_auto_enabled = false; - control_mode.flag_control_rates_enabled = true; - control_mode.flag_control_attitude_enabled = false; - control_mode.flag_control_altitude_enabled = false; - control_mode.flag_control_climb_rate_enabled = false; - control_mode.flag_control_position_enabled = false; - control_mode.flag_control_velocity_enabled = false; - control_mode.flag_control_termination_enabled = false; - break; - case NAVIGATION_STATE_ALTCTL: control_mode.flag_control_manual_enabled = true; control_mode.flag_control_auto_enabled = false; @@ -2225,64 +2213,6 @@ set_control_mode() control_mode.flag_control_termination_enabled = false; break; - case NAVIGATION_STATE_OFFBOARD: - control_mode.flag_control_manual_enabled = false; - control_mode.flag_control_auto_enabled = false; - control_mode.flag_control_offboard_enabled = true; - - switch (sp_offboard.mode) { - case OFFBOARD_CONTROL_MODE_DIRECT_RATES: - control_mode.flag_control_rates_enabled = true; - control_mode.flag_control_attitude_enabled = false; - control_mode.flag_control_altitude_enabled = false; - control_mode.flag_control_climb_rate_enabled = false; - control_mode.flag_control_position_enabled = false; - control_mode.flag_control_velocity_enabled = false; - break; - - case OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE: - control_mode.flag_control_rates_enabled = true; - control_mode.flag_control_attitude_enabled = true; - control_mode.flag_control_altitude_enabled = false; - control_mode.flag_control_climb_rate_enabled = false; - 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: - 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; - control_mode.flag_control_attitude_enabled = false; - control_mode.flag_control_altitude_enabled = false; - control_mode.flag_control_climb_rate_enabled = false; - control_mode.flag_control_position_enabled = false; - control_mode.flag_control_velocity_enabled = false; - } - - break; - case NAVIGATION_STATE_POSCTL: control_mode.flag_control_manual_enabled = true; control_mode.flag_control_auto_enabled = false; @@ -2323,6 +2253,19 @@ set_control_mode() control_mode.flag_control_termination_enabled = false; break; + case NAVIGATION_STATE_ACRO: + control_mode.flag_control_manual_enabled = true; + control_mode.flag_control_auto_enabled = false; + control_mode.flag_control_rates_enabled = true; + control_mode.flag_control_attitude_enabled = false; + control_mode.flag_control_altitude_enabled = false; + control_mode.flag_control_climb_rate_enabled = false; + control_mode.flag_control_position_enabled = false; + control_mode.flag_control_velocity_enabled = false; + control_mode.flag_control_termination_enabled = false; + break; + + case NAVIGATION_STATE_LAND: control_mode.flag_control_manual_enabled = false; control_mode.flag_control_auto_enabled = true; @@ -2336,6 +2279,19 @@ set_control_mode() control_mode.flag_control_termination_enabled = false; break; + case NAVIGATION_STATE_DESCEND: + /* TODO: check if this makes sense */ + control_mode.flag_control_manual_enabled = false; + control_mode.flag_control_auto_enabled = true; + control_mode.flag_control_rates_enabled = true; + control_mode.flag_control_attitude_enabled = true; + control_mode.flag_control_position_enabled = false; + control_mode.flag_control_velocity_enabled = false; + control_mode.flag_control_altitude_enabled = false; + control_mode.flag_control_climb_rate_enabled = true; + control_mode.flag_control_termination_enabled = false; + break; + case NAVIGATION_STATE_TERMINATION: /* disable all controllers on termination */ control_mode.flag_control_manual_enabled = false; @@ -2349,6 +2305,63 @@ set_control_mode() control_mode.flag_control_termination_enabled = true; break; + case NAVIGATION_STATE_OFFBOARD: + control_mode.flag_control_manual_enabled = false; + control_mode.flag_control_auto_enabled = false; + control_mode.flag_control_offboard_enabled = true; + + switch (sp_offboard.mode) { + case OFFBOARD_CONTROL_MODE_DIRECT_RATES: + control_mode.flag_control_rates_enabled = true; + control_mode.flag_control_attitude_enabled = false; + control_mode.flag_control_altitude_enabled = false; + control_mode.flag_control_climb_rate_enabled = false; + control_mode.flag_control_position_enabled = false; + control_mode.flag_control_velocity_enabled = false; + break; + + case OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE: + control_mode.flag_control_rates_enabled = true; + control_mode.flag_control_attitude_enabled = true; + control_mode.flag_control_altitude_enabled = false; + control_mode.flag_control_climb_rate_enabled = false; + 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: + 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; + control_mode.flag_control_attitude_enabled = false; + control_mode.flag_control_altitude_enabled = false; + control_mode.flag_control_climb_rate_enabled = false; + control_mode.flag_control_position_enabled = false; + control_mode.flag_control_velocity_enabled = false; + } + break; + default: break; } -- cgit v1.2.3 From 00cc2d55111fe6dcf03fc847aa6e65204e4a0a2c Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 13 Nov 2014 09:44:17 +1000 Subject: commander: added navstate RCRECOVER which was missing --- src/modules/commander/commander.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 3307c4bfc..5c4ad26f4 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -2225,9 +2225,11 @@ set_control_mode() control_mode.flag_control_termination_enabled = false; break; + case NAVIGATION_STATE_AUTO_MISSION: case NAVIGATION_STATE_AUTO_LOITER: case NAVIGATION_STATE_AUTO_RTL: + case NAVIGATION_STATE_AUTO_RCRECOVER: case NAVIGATION_STATE_AUTO_RTGS: case NAVIGATION_STATE_AUTO_LANDENGFAIL: control_mode.flag_control_manual_enabled = false; -- cgit v1.2.3 From db3f2fc94621ee13c7ec22d5fef1dd5ae74a0742 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 13 Nov 2014 09:46:12 +1000 Subject: commander: whitespace --- src/modules/commander/commander.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 5c4ad26f4..ec173c12b 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -2225,7 +2225,6 @@ set_control_mode() control_mode.flag_control_termination_enabled = false; break; - case NAVIGATION_STATE_AUTO_MISSION: case NAVIGATION_STATE_AUTO_LOITER: case NAVIGATION_STATE_AUTO_RTL: -- cgit v1.2.3 From 6a543b1d0154d7da9ddb4123df1479cf01aa88bc Mon Sep 17 00:00:00 2001 From: Holger Steinhaus Date: Thu, 13 Nov 2014 10:45:32 +0100 Subject: build fix --- src/modules/uORB/topics/test_motor.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/uORB/topics/test_motor.h b/src/modules/uORB/topics/test_motor.h index c880fe099..2dd90e69d 100644 --- a/src/modules/uORB/topics/test_motor.h +++ b/src/modules/uORB/topics/test_motor.h @@ -57,7 +57,7 @@ struct test_motor_s { uint64_t timestamp; /**< output timestamp in us since system boot */ unsigned motor_number; /**< number of motor to spin */ - float value; /**< output power, range [0..1] + float value; /**< output power, range [0..1] */ }; /** -- cgit v1.2.3 From 4d489ef7f4c7626a86b700b6d7144e35000ce387 Mon Sep 17 00:00:00 2001 From: Holger Steinhaus Date: Tue, 11 Nov 2014 11:19:15 +0100 Subject: UAVCAN: improve mag compatibility --- src/modules/uavcan/sensors/mag.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/modules/uavcan/sensors/mag.cpp b/src/modules/uavcan/sensors/mag.cpp index 0d9ea08c5..4f56038b5 100644 --- a/src/modules/uavcan/sensors/mag.cpp +++ b/src/modules/uavcan/sensors/mag.cpp @@ -74,6 +74,9 @@ int UavcanMagnetometerBridge::init() int UavcanMagnetometerBridge::ioctl(struct file *filp, int cmd, unsigned long arg) { switch (cmd) { + case SENSORIOCSQUEUEDEPTH: { + return OK; // Pretend that this stuff is supported to keep APM happy + } case MAGIOCSSCALE: { std::memcpy(&_scale, reinterpret_cast(arg), sizeof(_scale)); return 0; -- cgit v1.2.3 From 0fa622f22b90048ad366ac304c2c9339a5c376d3 Mon Sep 17 00:00:00 2001 From: Holger Steinhaus Date: Tue, 11 Nov 2014 11:20:03 +0100 Subject: UAVCAN: declare mag external again to allow different rotation than the internal mag --- src/modules/uavcan/sensors/mag.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/uavcan/sensors/mag.cpp b/src/modules/uavcan/sensors/mag.cpp index 4f56038b5..4d473291c 100644 --- a/src/modules/uavcan/sensors/mag.cpp +++ b/src/modules/uavcan/sensors/mag.cpp @@ -89,7 +89,7 @@ int UavcanMagnetometerBridge::ioctl(struct file *filp, int cmd, unsigned long ar return 0; // Nothing to do } case MAGIOCGEXTERNAL: { - return 0; // We don't want anyone to transform the coordinate frame, so we declare it onboard + return 1; // We don't want anyone to transform the coordinate frame, so we declare it onboard } case MAGIOCSSAMPLERATE: { return 0; // Pretend that this stuff is supported to keep the sensor app happy -- cgit v1.2.3 From 16d74e3c31b9c2cf4f2de254956a58885b5c1fbf Mon Sep 17 00:00:00 2001 From: Holger Steinhaus Date: Wed, 12 Nov 2014 13:14:05 +0100 Subject: UACVAN: add read()-style interface to mag device --- src/modules/uavcan/sensors/mag.cpp | 48 +++++++++++++++++++++++++++++--------- src/modules/uavcan/sensors/mag.hpp | 2 ++ 2 files changed, 39 insertions(+), 11 deletions(-) diff --git a/src/modules/uavcan/sensors/mag.cpp b/src/modules/uavcan/sensors/mag.cpp index 4d473291c..a529e7e75 100644 --- a/src/modules/uavcan/sensors/mag.cpp +++ b/src/modules/uavcan/sensors/mag.cpp @@ -37,6 +37,8 @@ #include "mag.hpp" +#include + static const orb_id_t MAG_TOPICS[3] = { ORB_ID(sensor_mag0), ORB_ID(sensor_mag1), @@ -71,6 +73,30 @@ int UavcanMagnetometerBridge::init() return 0; } +ssize_t UavcanMagnetometerBridge::read(struct file *filp, char *buffer, size_t buflen) +{ + static uint64_t last_read = 0; + struct mag_report *mag_buf = reinterpret_cast(buffer); + + /* buffer must be large enough */ + unsigned count = buflen / sizeof(struct mag_report); + if (count < 1) { + return -ENOSPC; + } + + if (last_read < _report.timestamp) { + /* copy report */ + lock(); + *mag_buf = _report; + last_read = _report.timestamp; + unlock(); + return sizeof(struct mag_report); + } else { + /* no new data available, warn caller */ + return -EAGAIN; + } +} + int UavcanMagnetometerBridge::ioctl(struct file *filp, int cmd, unsigned long arg) { switch (cmd) { @@ -89,7 +115,7 @@ int UavcanMagnetometerBridge::ioctl(struct file *filp, int cmd, unsigned long ar return 0; // Nothing to do } case MAGIOCGEXTERNAL: { - return 1; // We don't want anyone to transform the coordinate frame, so we declare it onboard + return 1; // declare it external rise it's priority and to allow for correct orientation compensation } case MAGIOCSSAMPLERATE: { return 0; // Pretend that this stuff is supported to keep the sensor app happy @@ -111,18 +137,18 @@ int UavcanMagnetometerBridge::ioctl(struct file *filp, int cmd, unsigned long ar void UavcanMagnetometerBridge::mag_sub_cb(const uavcan::ReceivedDataStructure &msg) { - auto report = ::mag_report(); - - report.range_ga = 1.3F; // Arbitrary number, doesn't really mean anything + lock(); + _report.range_ga = 1.3F; // Arbitrary number, doesn't really mean anything - report.timestamp = msg.getUtcTimestamp().toUSec(); - if (report.timestamp == 0) { - report.timestamp = msg.getMonotonicTimestamp().toUSec(); + _report.timestamp = msg.getUtcTimestamp().toUSec(); + if (_report.timestamp == 0) { + _report.timestamp = msg.getMonotonicTimestamp().toUSec(); } - report.x = (msg.magnetic_field[0] - _scale.x_offset) * _scale.x_scale; - report.y = (msg.magnetic_field[1] - _scale.y_offset) * _scale.y_scale; - report.z = (msg.magnetic_field[2] - _scale.z_offset) * _scale.z_scale; + _report.x = (msg.magnetic_field[0] - _scale.x_offset) * _scale.x_scale; + _report.y = (msg.magnetic_field[1] - _scale.y_offset) * _scale.y_scale; + _report.z = (msg.magnetic_field[2] - _scale.z_offset) * _scale.z_scale; + unlock(); - publish(msg.getSrcNodeID().get(), &report); + publish(msg.getSrcNodeID().get(), &_report); } diff --git a/src/modules/uavcan/sensors/mag.hpp b/src/modules/uavcan/sensors/mag.hpp index 6d413a8f7..74077d883 100644 --- a/src/modules/uavcan/sensors/mag.hpp +++ b/src/modules/uavcan/sensors/mag.hpp @@ -54,6 +54,7 @@ public: int init() override; private: + ssize_t read(struct file *filp, char *buffer, size_t buflen); int ioctl(struct file *filp, int cmd, unsigned long arg) override; void mag_sub_cb(const uavcan::ReceivedDataStructure &msg); @@ -65,4 +66,5 @@ private: uavcan::Subscriber _sub_mag; mag_scale _scale = {}; + mag_report _report = {}; }; -- cgit v1.2.3 From e026324784409bd16a561544cb608e7089d500a0 Mon Sep 17 00:00:00 2001 From: Holger Steinhaus Date: Thu, 13 Nov 2014 16:58:41 +0100 Subject: UAVCAN: fix mag report timestamp --- src/modules/uavcan/sensors/mag.cpp | 7 ++----- 1 file changed, 2 insertions(+), 5 deletions(-) diff --git a/src/modules/uavcan/sensors/mag.cpp b/src/modules/uavcan/sensors/mag.cpp index a529e7e75..5cbb96433 100644 --- a/src/modules/uavcan/sensors/mag.cpp +++ b/src/modules/uavcan/sensors/mag.cpp @@ -37,6 +37,7 @@ #include "mag.hpp" +#include #include static const orb_id_t MAG_TOPICS[3] = { @@ -139,11 +140,7 @@ void UavcanMagnetometerBridge::mag_sub_cb(const uavcan::ReceivedDataStructure Date: Sat, 15 Nov 2014 11:51:21 +0100 Subject: Updated NuttX, only comment changes --- NuttX | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/NuttX b/NuttX index 47a94a097..9d06b6457 160000 --- a/NuttX +++ b/NuttX @@ -1 +1 @@ -Subproject commit 47a94a0979fd5aee2c8416e0197d6ce5926e94ca +Subproject commit 9d06b645790e1445f14e3b19c71d40b3088f4e4f -- cgit v1.2.3 From 780451dd65db72aa28b75ddad2effbbba8db8626 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 15 Nov 2014 13:48:30 +0100 Subject: Remove unneeded file --- ROMFS/px4fmu_common/init.d/rc.jig | 10 ---------- 1 file changed, 10 deletions(-) delete mode 100644 ROMFS/px4fmu_common/init.d/rc.jig diff --git a/ROMFS/px4fmu_common/init.d/rc.jig b/ROMFS/px4fmu_common/init.d/rc.jig deleted file mode 100644 index e2b5d8f30..000000000 --- a/ROMFS/px4fmu_common/init.d/rc.jig +++ /dev/null @@ -1,10 +0,0 @@ -#!nsh -# -# Test jig startup script -# - -echo "[testing] doing production test.." - -tests jig - -echo "[testing] testing done" -- cgit v1.2.3 From 4bcb2697415009279bd481eb9d22f015846147f1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 15 Nov 2014 13:48:49 +0100 Subject: Add capability to read chip ID and revision from ver command --- src/systemcmds/ver/ver.c | 85 +++++++++++++++++++++++++++++++++++++++++++++++- 1 file changed, 84 insertions(+), 1 deletion(-) diff --git a/src/systemcmds/ver/ver.c b/src/systemcmds/ver/ver.c index 9ae080ee2..b11b593c3 100644 --- a/src/systemcmds/ver/ver.c +++ b/src/systemcmds/ver/ver.c @@ -53,13 +53,38 @@ static const char sz_ver_bdate_str[] = "bdate"; static const char sz_ver_gcc_str[] = "gcc"; static const char sz_ver_all_str[] = "all"; +#ifdef CONFIG_ARCH_CHIP_STM32 +#include + +static const char mcu_ver_str[] = "mcu"; + +#define DBGMCU_IDCODE 0xE0042000 + +#define STM32F40x_41x 0x413 +#define STM32F42x_43x 0x419 + +#define REVID_MASK 0xFFFF0000 +#define DEVID_MASK 0xFFF + +/* magic numbers from reference manual */ +enum STM32F4_REV { + STM32F4_REV_A = 0x1000, + STM32F4_REV_Z = 0x1001, + STM32F4_REV_Y = 0x1003, + STM32F4_REV_1 = 0x1007, + STM32F4_REV_3 = 0x2001 +}; +#else +#error stm32 +#endif + static void usage(const char *reason) { if (reason != NULL) { printf("%s\n", reason); } - printf("usage: ver {hw|hwcmp|git|bdate|gcc|all}\n\n"); + printf("usage: ver {hw|hwcmp|git|bdate|gcc|all|mcu}\n\n"); } __EXPORT int ver_main(int argc, char *argv[]); @@ -107,6 +132,64 @@ int ver_main(int argc, char *argv[]) printf("GCC toolchain: %s\n", __VERSION__); ret = 0; +#ifdef CONFIG_ARCH_CHIP_STM32 + } else if (!strncmp(argv[1], mcu_ver_str, sizeof(mcu_ver_str))) { + uint32_t abc = getreg32(DBGMCU_IDCODE); + + uint32_t chip_version = abc & DEVID_MASK; + enum STM32F4_REV revid = (abc & REVID_MASK) >> 16; + + printf("CHIP TYPE: "); + + switch (revid) { + case STM32F40x_41x: + printf("STM32F40x"); + break; + case STM32F42x_43x: + printf("STM32F42x"); + break; + default: + printf("STM32F???"); + break; + } + + char rev; + + switch (chip_version) { + + case STM32F4_REV_A: + rev = 'A'; + break; + case STM32F4_REV_Z: + rev = 'Z'; + break; + case STM32F4_REV_Y: + rev = 'Y'; + break; + case STM32F4_REV_1: + rev = '1'; + break; + case STM32F4_REV_3: + rev = '3'; + break; + default: + rev = '?'; + break; + } + + printf("\nHW REV: %c\n", rev); + + if (rev < STM32F4_REV_3) { + printf("\n\nWARNING WARNING WARNING!\n" + "Revision %c has a silicon errata\n" + "on USB connectivity combined with\n" + "flash bank #2. This device can only\n" + "utilize a maximum of 1MB flash safely!\n" + "http://px4.io/help/errata\n", rev); + } + ret = 0; +#endif + } else { errx(1, "unknown command.\n"); } -- cgit v1.2.3 From 8e8dd62fbd9906c980d9e88943a2fda5b90977e0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 15 Nov 2014 13:49:06 +0100 Subject: Let the uploader print the binary size --- Tools/px_uploader.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Tools/px_uploader.py b/Tools/px_uploader.py index d4e461226..3a4540ac0 100755 --- a/Tools/px_uploader.py +++ b/Tools/px_uploader.py @@ -458,7 +458,7 @@ if os.path.exists("/usr/sbin/ModemManager"): # Load the firmware file fw = firmware(args.firmware) -print("Loaded firmware for %x,%x, waiting for the bootloader..." % (fw.property('board_id'), fw.property('board_revision'))) +print("Loaded firmware for %x,%x, size: %d bytes, waiting for the bootloader..." % (fw.property('board_id'), fw.property('board_revision'), fw.property('image_size'))) print("If the board does not respond within 1-2 seconds, unplug and re-plug the USB connector.") # Spin waiting for a device to show up -- cgit v1.2.3 From a7eaf07f4f9d6df54b7602c07ce7fbdf2db700f9 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 15 Nov 2014 13:49:48 +0100 Subject: Motor test: optimize for size --- src/systemcmds/motor_test/module.mk | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/systemcmds/motor_test/module.mk b/src/systemcmds/motor_test/module.mk index eb36d2ded..261428ef9 100644 --- a/src/systemcmds/motor_test/module.mk +++ b/src/systemcmds/motor_test/module.mk @@ -39,3 +39,5 @@ MODULE_COMMAND = motor_test SRCS = motor_test.c MODULE_STACKSIZE = 4096 + +MAXOPTIMIZATION = -Os -- cgit v1.2.3 From d4a5f345aabc83506b05277b9dcf71e24700c70d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 15 Nov 2014 13:50:38 +0100 Subject: Remove unneeded apps from build --- makefiles/config_px4fmu-v2_default.mk | 17 +---------------- makefiles/config_px4fmu-v2_test.mk | 7 +++++++ 2 files changed, 8 insertions(+), 16 deletions(-) diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index d17dff5bb..3c65b19e0 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -40,14 +40,8 @@ MODULES += drivers/meas_airspeed MODULES += drivers/frsky_telemetry MODULES += modules/sensors MODULES += drivers/mkblctrl -MODULES += drivers/pca8574 MODULES += drivers/px4flow - -# Needs to be burned to the ground and re-written; for now, -# just don't build it. -#MODULES += drivers/mkblctrl - # # System commands # @@ -61,7 +55,6 @@ MODULES += systemcmds/pwm MODULES += systemcmds/esc_calib MODULES += systemcmds/reboot MODULES += systemcmds/top -MODULES += systemcmds/tests MODULES += systemcmds/config MODULES += systemcmds/nshterm MODULES += systemcmds/mtd @@ -81,10 +74,8 @@ MODULES += modules/uavcan # Estimation modules (EKF/ SO3 / other filters) # MODULES += modules/attitude_estimator_ekf -MODULES += modules/attitude_estimator_so3 MODULES += modules/ekf_att_pos_estimator MODULES += modules/position_estimator_inav -MODULES += examples/flow_position_estimator # # Vehicle Control @@ -100,12 +91,6 @@ MODULES += modules/mc_pos_control # MODULES += modules/sdlog2 -# -# Unit tests -# -#MODULES += modules/unit_test -#MODULES += modules/commander/commander_tests - # # Library modules # @@ -139,7 +124,7 @@ MODULES += modules/bottle_drop #MODULES += examples/math_demo # Tutorial code from # https://pixhawk.ethz.ch/px4/dev/hello_sky -MODULES += examples/px4_simple_app +#MODULES += examples/px4_simple_app # Tutorial code from # https://pixhawk.ethz.ch/px4/dev/daemon diff --git a/makefiles/config_px4fmu-v2_test.mk b/makefiles/config_px4fmu-v2_test.mk index 2f4d9d6a4..6f54b960c 100644 --- a/makefiles/config_px4fmu-v2_test.mk +++ b/makefiles/config_px4fmu-v2_test.mk @@ -49,6 +49,13 @@ MODULES += lib/mathlib MODULES += lib/mathlib/math/filter MODULES += lib/conversion +# +# Modules to test-build, but not useful for test environment +# +MODULES += modules/attitude_estimator_so3 +MODULES += drivers/pca8574 +MODULES += examples/flow_position_estimator + # # Libraries # -- cgit v1.2.3 From 75bc8136b1d1d773a5664a226b5b766867fe5ff3 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 15 Nov 2014 13:50:56 +0100 Subject: Build NuttX optimized for size --- nuttx-configs/px4fmu-v1/nsh/Make.defs | 2 +- nuttx-configs/px4fmu-v2/nsh/Make.defs | 2 +- nuttx-configs/px4fmu-v2/scripts/ld.script | 3 ++- nuttx-configs/px4io-v1/nsh/Make.defs | 2 +- nuttx-configs/px4io-v2/nsh/Make.defs | 6 +----- 5 files changed, 6 insertions(+), 9 deletions(-) diff --git a/nuttx-configs/px4fmu-v1/nsh/Make.defs b/nuttx-configs/px4fmu-v1/nsh/Make.defs index 7b2ea703a..e7318e519 100644 --- a/nuttx-configs/px4fmu-v1/nsh/Make.defs +++ b/nuttx-configs/px4fmu-v1/nsh/Make.defs @@ -53,7 +53,7 @@ NM = $(CROSSDEV)nm OBJCOPY = $(CROSSDEV)objcopy OBJDUMP = $(CROSSDEV)objdump -MAXOPTIMIZATION = -O3 +MAXOPTIMIZATION = -Os ARCHCPUFLAGS = -mcpu=cortex-m4 \ -mthumb \ -march=armv7e-m \ diff --git a/nuttx-configs/px4fmu-v2/nsh/Make.defs b/nuttx-configs/px4fmu-v2/nsh/Make.defs index e70320aaa..f3ce53b4a 100644 --- a/nuttx-configs/px4fmu-v2/nsh/Make.defs +++ b/nuttx-configs/px4fmu-v2/nsh/Make.defs @@ -53,7 +53,7 @@ NM = $(CROSSDEV)nm OBJCOPY = $(CROSSDEV)objcopy OBJDUMP = $(CROSSDEV)objdump -MAXOPTIMIZATION = -O3 +MAXOPTIMIZATION = -Os ARCHCPUFLAGS = -mcpu=cortex-m4 \ -mthumb \ -march=armv7e-m \ diff --git a/nuttx-configs/px4fmu-v2/scripts/ld.script b/nuttx-configs/px4fmu-v2/scripts/ld.script index 1be39fb87..bec896d1c 100644 --- a/nuttx-configs/px4fmu-v2/scripts/ld.script +++ b/nuttx-configs/px4fmu-v2/scripts/ld.script @@ -50,7 +50,8 @@ MEMORY { - flash (rx) : ORIGIN = 0x08004000, LENGTH = 2032K + /* disabled due to silicon errata flash (rx) : ORIGIN = 0x08004000, LENGTH = 2032K */ + flash (rx) : ORIGIN = 0x08004000, LENGTH = 1008K sram (rwx) : ORIGIN = 0x20000000, LENGTH = 192K ccsram (rwx) : ORIGIN = 0x10000000, LENGTH = 64K } diff --git a/nuttx-configs/px4io-v1/nsh/Make.defs b/nuttx-configs/px4io-v1/nsh/Make.defs index 712631f47..7a0792ff6 100644 --- a/nuttx-configs/px4io-v1/nsh/Make.defs +++ b/nuttx-configs/px4io-v1/nsh/Make.defs @@ -53,7 +53,7 @@ NM = $(CROSSDEV)nm OBJCOPY = $(CROSSDEV)objcopy OBJDUMP = $(CROSSDEV)objdump -MAXOPTIMIZATION = -O3 +MAXOPTIMIZATION = -Os ARCHCPUFLAGS = -mcpu=cortex-m3 \ -mthumb \ -march=armv7-m diff --git a/nuttx-configs/px4io-v2/nsh/Make.defs b/nuttx-configs/px4io-v2/nsh/Make.defs index cd2d8eba3..1717464d2 100644 --- a/nuttx-configs/px4io-v2/nsh/Make.defs +++ b/nuttx-configs/px4io-v2/nsh/Make.defs @@ -53,15 +53,11 @@ NM = $(CROSSDEV)nm OBJCOPY = $(CROSSDEV)objcopy OBJDUMP = $(CROSSDEV)objdump -MAXOPTIMIZATION = -O3 +MAXOPTIMIZATION = -Os ARCHCPUFLAGS = -mcpu=cortex-m3 \ -mthumb \ -march=armv7-m -# enable precise stack overflow tracking -#INSTRUMENTATIONDEFINES = -finstrument-functions \ -# -ffixed-r10 - # use our linker script LDSCRIPT = ld.script -- cgit v1.2.3 From 3d2a5bae51763e5a542506383c9d97f95fc7d1ef Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 15 Nov 2014 13:51:16 +0100 Subject: Board drivers: Optimize for size --- src/drivers/boards/aerocore/module.mk | 2 ++ src/drivers/boards/px4fmu-v1/module.mk | 2 ++ src/drivers/boards/px4fmu-v2/module.mk | 2 ++ src/drivers/boards/px4io-v1/module.mk | 2 ++ src/drivers/boards/px4io-v2/module.mk | 2 ++ 5 files changed, 10 insertions(+) diff --git a/src/drivers/boards/aerocore/module.mk b/src/drivers/boards/aerocore/module.mk index b53fe0a29..0a2d91009 100644 --- a/src/drivers/boards/aerocore/module.mk +++ b/src/drivers/boards/aerocore/module.mk @@ -6,3 +6,5 @@ SRCS = aerocore_init.c \ aerocore_pwm_servo.c \ aerocore_spi.c \ aerocore_led.c + +MAXOPTIMIZATION = -Os diff --git a/src/drivers/boards/px4fmu-v1/module.mk b/src/drivers/boards/px4fmu-v1/module.mk index 66b776917..5e1a27d5a 100644 --- a/src/drivers/boards/px4fmu-v1/module.mk +++ b/src/drivers/boards/px4fmu-v1/module.mk @@ -8,3 +8,5 @@ SRCS = px4fmu_can.c \ px4fmu_spi.c \ px4fmu_usb.c \ px4fmu_led.c + +MAXOPTIMIZATION = -Os diff --git a/src/drivers/boards/px4fmu-v2/module.mk b/src/drivers/boards/px4fmu-v2/module.mk index 99d37eeca..103232b0c 100644 --- a/src/drivers/boards/px4fmu-v2/module.mk +++ b/src/drivers/boards/px4fmu-v2/module.mk @@ -8,3 +8,5 @@ SRCS = px4fmu_can.c \ px4fmu_spi.c \ px4fmu_usb.c \ px4fmu2_led.c + +MAXOPTIMIZATION = -Os diff --git a/src/drivers/boards/px4io-v1/module.mk b/src/drivers/boards/px4io-v1/module.mk index 2601a1c15..a7a14dd07 100644 --- a/src/drivers/boards/px4io-v1/module.mk +++ b/src/drivers/boards/px4io-v1/module.mk @@ -4,3 +4,5 @@ SRCS = px4io_init.c \ px4io_pwm_servo.c + +MAXOPTIMIZATION = -Os diff --git a/src/drivers/boards/px4io-v2/module.mk b/src/drivers/boards/px4io-v2/module.mk index 85f94e8be..3f0e9a0b3 100644 --- a/src/drivers/boards/px4io-v2/module.mk +++ b/src/drivers/boards/px4io-v2/module.mk @@ -4,3 +4,5 @@ SRCS = px4iov2_init.c \ px4iov2_pwm_servo.c + +MAXOPTIMIZATION = -Os -- cgit v1.2.3 From c0f34dff2605381afacbec2bc1eba6b648daddd2 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 15 Nov 2014 13:51:33 +0100 Subject: STM32 drivers: Optimize for size --- src/drivers/stm32/adc/module.mk | 2 ++ src/drivers/stm32/module.mk | 2 ++ src/drivers/stm32/tone_alarm/module.mk | 2 ++ 3 files changed, 6 insertions(+) diff --git a/src/drivers/stm32/adc/module.mk b/src/drivers/stm32/adc/module.mk index 48620feea..38ea490a0 100644 --- a/src/drivers/stm32/adc/module.mk +++ b/src/drivers/stm32/adc/module.mk @@ -40,3 +40,5 @@ MODULE_COMMAND = adc SRCS = adc.cpp INCLUDE_DIRS += $(NUTTX_SRC)/arch/arm/src/stm32 $(NUTTX_SRC)/arch/arm/src/common + +MAXOPTIMIZATION = -Os diff --git a/src/drivers/stm32/module.mk b/src/drivers/stm32/module.mk index bb751c7f6..54428e270 100644 --- a/src/drivers/stm32/module.mk +++ b/src/drivers/stm32/module.mk @@ -41,3 +41,5 @@ SRCS = drv_hrt.c \ drv_pwm_servo.c INCLUDE_DIRS += $(NUTTX_SRC)/arch/arm/src/stm32 $(NUTTX_SRC)/arch/arm/src/common + +MAXOPTIMIZATION = -Os diff --git a/src/drivers/stm32/tone_alarm/module.mk b/src/drivers/stm32/tone_alarm/module.mk index 827cf30b2..25a194ef6 100644 --- a/src/drivers/stm32/tone_alarm/module.mk +++ b/src/drivers/stm32/tone_alarm/module.mk @@ -40,3 +40,5 @@ MODULE_COMMAND = tone_alarm SRCS = tone_alarm.cpp INCLUDE_DIRS += $(NUTTX_SRC)/arch/arm/src/stm32 $(NUTTX_SRC)/arch/arm/src/common + +MAXOPTIMIZATION = -Os -- cgit v1.2.3 From f7f54062439f0b1c1208d0778f992f7052e1518b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 15 Nov 2014 13:51:49 +0100 Subject: FMU driver: optimize for size --- src/drivers/px4fmu/module.mk | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/drivers/px4fmu/module.mk b/src/drivers/px4fmu/module.mk index a60f1a434..a06323a52 100644 --- a/src/drivers/px4fmu/module.mk +++ b/src/drivers/px4fmu/module.mk @@ -8,3 +8,5 @@ SRCS = fmu.cpp MODULE_STACKSIZE = 1200 EXTRACXXFLAGS = -Weffc++ + +MAXOPTIMIZATION = -Os -- cgit v1.2.3 From a12c98ba63c12ba08f1fb0f66df7b7079e444e1d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 15 Nov 2014 13:52:08 +0100 Subject: Sensors: Optimize for size --- src/modules/sensors/module.mk | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/sensors/module.mk b/src/modules/sensors/module.mk index 5b1bc5e86..dfbba92d9 100644 --- a/src/modules/sensors/module.mk +++ b/src/modules/sensors/module.mk @@ -42,3 +42,5 @@ SRCS = sensors.cpp \ sensor_params.c MODULE_STACKSIZE = 1200 + +MAXOPTIMIZATION = -Os -- cgit v1.2.3 From a869105ba23394cfabb0a0000dd3b7fa3dc9fdb1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 15 Nov 2014 13:52:34 +0100 Subject: Systemlib: Optimize for size --- src/modules/systemlib/module.mk | 1 + 1 file changed, 1 insertion(+) diff --git a/src/modules/systemlib/module.mk b/src/modules/systemlib/module.mk index 147903aa0..5c4f2ec41 100644 --- a/src/modules/systemlib/module.mk +++ b/src/modules/systemlib/module.mk @@ -55,3 +55,4 @@ SRCS = err.c \ pwm_limit/pwm_limit.c \ circuit_breaker.c +MAXOPTIMIZATION = -Os -- cgit v1.2.3 From 54f296ce9d8245845a7a0bf32f5045a44aab40c7 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 15 Nov 2014 13:54:01 +0100 Subject: Sensor drivers: Optimize for size --- src/drivers/hmc5883/module.mk | 2 ++ src/drivers/l3gd20/module.mk | 2 ++ src/drivers/lsm303d/module.mk | 2 ++ src/drivers/mpu6000/module.mk | 2 ++ 4 files changed, 8 insertions(+) diff --git a/src/drivers/hmc5883/module.mk b/src/drivers/hmc5883/module.mk index 5daa01dc5..be2ee7276 100644 --- a/src/drivers/hmc5883/module.mk +++ b/src/drivers/hmc5883/module.mk @@ -42,3 +42,5 @@ SRCS = hmc5883.cpp MODULE_STACKSIZE = 1200 EXTRACXXFLAGS = -Weffc++ + +MAXOPTIMIZATION = -Os diff --git a/src/drivers/l3gd20/module.mk b/src/drivers/l3gd20/module.mk index 5630e7aec..3d64d62be 100644 --- a/src/drivers/l3gd20/module.mk +++ b/src/drivers/l3gd20/module.mk @@ -8,3 +8,5 @@ SRCS = l3gd20.cpp MODULE_STACKSIZE = 1200 EXTRACXXFLAGS = -Weffc++ + +MAXOPTIMIZATION = -Os diff --git a/src/drivers/lsm303d/module.mk b/src/drivers/lsm303d/module.mk index b4f3974f4..0421eb113 100644 --- a/src/drivers/lsm303d/module.mk +++ b/src/drivers/lsm303d/module.mk @@ -8,3 +8,5 @@ SRCS = lsm303d.cpp MODULE_STACKSIZE = 1200 EXTRACXXFLAGS = -Weffc++ + +MAXOPTIMIZATION = -Os diff --git a/src/drivers/mpu6000/module.mk b/src/drivers/mpu6000/module.mk index 5b4893b12..da9fcc0fc 100644 --- a/src/drivers/mpu6000/module.mk +++ b/src/drivers/mpu6000/module.mk @@ -42,3 +42,5 @@ SRCS = mpu6000.cpp MODULE_STACKSIZE = 1200 EXTRACXXFLAGS = -Weffc++ + +MAXOPTIMIZATION = -Os -- cgit v1.2.3 From 32313a13dd1f98426f83b99310fc22b5adced37c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 15 Nov 2014 13:54:16 +0100 Subject: PX4IO driver: Optimize for size --- src/drivers/px4io/module.mk | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/drivers/px4io/module.mk b/src/drivers/px4io/module.mk index 5b838fb75..924283356 100644 --- a/src/drivers/px4io/module.mk +++ b/src/drivers/px4io/module.mk @@ -48,3 +48,5 @@ INCLUDE_DIRS += $(NUTTX_SRC)/arch/arm/src/stm32 $(NUTTX_SRC)/arch/arm/src/com MODULE_STACKSIZE = 1200 EXTRACXXFLAGS = -Weffc++ + +MAXOPTIMIZATION = -Os -- cgit v1.2.3 From 42575bbc3763fe2b9e2d3e5b074ec74e4fc57f5a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 15 Nov 2014 13:54:38 +0100 Subject: Bottle drop: Optimize for size --- src/modules/bottle_drop/module.mk | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/bottle_drop/module.mk b/src/modules/bottle_drop/module.mk index 6b18fff55..960228879 100644 --- a/src/modules/bottle_drop/module.mk +++ b/src/modules/bottle_drop/module.mk @@ -39,3 +39,5 @@ MODULE_COMMAND = bottle_drop SRCS = bottle_drop.cpp \ bottle_drop_params.c + +MAXOPTIMIZATION = -Os -- cgit v1.2.3 From a7bc52754de52ccbf0f958c20ce31408a8b4372e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 15 Nov 2014 13:54:54 +0100 Subject: SDLOG2: Optimize for size --- src/modules/sdlog2/module.mk | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/sdlog2/module.mk b/src/modules/sdlog2/module.mk index a28d43e72..d4a00af39 100644 --- a/src/modules/sdlog2/module.mk +++ b/src/modules/sdlog2/module.mk @@ -43,3 +43,5 @@ SRCS = sdlog2.c \ logbuffer.c MODULE_STACKSIZE = 1200 + +MAXOPTIMIZATION = -Os -- cgit v1.2.3 From 7b962e7d1bb6b60633c116b9d5395b24e6658664 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 15 Nov 2014 14:17:24 +0100 Subject: Version command optimization --- src/systemcmds/ver/ver.c | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/src/systemcmds/ver/ver.c b/src/systemcmds/ver/ver.c index b11b593c3..9d308bc3e 100644 --- a/src/systemcmds/ver/ver.c +++ b/src/systemcmds/ver/ver.c @@ -182,9 +182,7 @@ int ver_main(int argc, char *argv[]) if (rev < STM32F4_REV_3) { printf("\n\nWARNING WARNING WARNING!\n" "Revision %c has a silicon errata\n" - "on USB connectivity combined with\n" - "flash bank #2. This device can only\n" - "utilize a maximum of 1MB flash safely!\n" + "This device can only utilize a maximum of 1MB flash safely!\n" "http://px4.io/help/errata\n", rev); } ret = 0; -- cgit v1.2.3 From 2a37d274b142f49e250265eb14821d0b4f7e51cf Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 15 Nov 2014 15:51:55 +0100 Subject: Systemlib: Add MCU version command. ver commandline tool: Add support for MCU version command --- src/modules/systemlib/mcu_version.c | 109 ++++++++++++++++++++++++++++++++++++ src/modules/systemlib/mcu_version.h | 52 +++++++++++++++++ src/modules/systemlib/module.mk | 4 +- src/systemcmds/ver/ver.c | 88 ++++++----------------------- 4 files changed, 179 insertions(+), 74 deletions(-) create mode 100644 src/modules/systemlib/mcu_version.c create mode 100644 src/modules/systemlib/mcu_version.h diff --git a/src/modules/systemlib/mcu_version.c b/src/modules/systemlib/mcu_version.c new file mode 100644 index 000000000..4bcf95784 --- /dev/null +++ b/src/modules/systemlib/mcu_version.c @@ -0,0 +1,109 @@ +/**************************************************************************** + * + * 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 mcu_version.c + * + * Read out the microcontroller version from the board + * + * @author Lorenz Meier + * + */ + +#include "mcu_version.h" + +#include + +#ifdef CONFIG_ARCH_CHIP_STM32 +#include + +#define DBGMCU_IDCODE 0xE0042000 + +#define STM32F40x_41x 0x413 +#define STM32F42x_43x 0x419 + +#define REVID_MASK 0xFFFF0000 +#define DEVID_MASK 0xFFF + +#endif + + + +int mcu_version(char* rev, char** revstr) +{ +#ifdef CONFIG_ARCH_CHIP_STM32 + uint32_t abc = getreg32(DBGMCU_IDCODE); + + int32_t chip_version = abc & DEVID_MASK; + enum MCU_REV revid = (abc & REVID_MASK) >> 16; + + switch (chip_version) { + case STM32F40x_41x: + *revstr = "STM32F40x"; + break; + case STM32F42x_43x: + *revstr = "STM32F42x"; + break; + default: + *revstr = "STM32F???"; + break; + } + + switch (revid) { + + case MCU_REV_STM32F4_REV_A: + *rev = 'A'; + break; + case MCU_REV_STM32F4_REV_Z: + *rev = 'Z'; + break; + case MCU_REV_STM32F4_REV_Y: + *rev = 'Y'; + break; + case MCU_REV_STM32F4_REV_1: + *rev = '1'; + break; + case MCU_REV_STM32F4_REV_3: + *rev = '3'; + break; + default: + *rev = '?'; + revid = -1; + break; + } + + return revid; +#else + return -1; +#endif +} diff --git a/src/modules/systemlib/mcu_version.h b/src/modules/systemlib/mcu_version.h new file mode 100644 index 000000000..1b3d0aba9 --- /dev/null +++ b/src/modules/systemlib/mcu_version.h @@ -0,0 +1,52 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#pragma once + +/* magic numbers from reference manual */ +enum MCU_REV { + MCU_REV_STM32F4_REV_A = 0x1000, + MCU_REV_STM32F4_REV_Z = 0x1001, + MCU_REV_STM32F4_REV_Y = 0x1003, + MCU_REV_STM32F4_REV_1 = 0x1007, + MCU_REV_STM32F4_REV_3 = 0x2001 +}; + +/** + * Reports the microcontroller version of the main CPU. + * + * @param rev The silicon revision character + * @param revstr The full chip name string + * @return The silicon revision / version number as integer + */ +__EXPORT int mcu_version(char* rev, char** revstr); diff --git a/src/modules/systemlib/module.mk b/src/modules/systemlib/module.mk index 147903aa0..233023e25 100644 --- a/src/modules/systemlib/module.mk +++ b/src/modules/systemlib/module.mk @@ -53,5 +53,5 @@ SRCS = err.c \ otp.c \ board_serial.c \ pwm_limit/pwm_limit.c \ - circuit_breaker.c - + circuit_breaker.c \ + mcu_version.c diff --git a/src/systemcmds/ver/ver.c b/src/systemcmds/ver/ver.c index 9d308bc3e..62a7a5b92 100644 --- a/src/systemcmds/ver/ver.c +++ b/src/systemcmds/ver/ver.c @@ -44,6 +44,7 @@ #include #include #include +#include // string constants for version commands static const char sz_ver_hw_str[] = "hw"; @@ -52,32 +53,8 @@ static const char sz_ver_git_str[] = "git"; static const char sz_ver_bdate_str[] = "bdate"; static const char sz_ver_gcc_str[] = "gcc"; static const char sz_ver_all_str[] = "all"; - -#ifdef CONFIG_ARCH_CHIP_STM32 -#include - static const char mcu_ver_str[] = "mcu"; -#define DBGMCU_IDCODE 0xE0042000 - -#define STM32F40x_41x 0x413 -#define STM32F42x_43x 0x419 - -#define REVID_MASK 0xFFFF0000 -#define DEVID_MASK 0xFFF - -/* magic numbers from reference manual */ -enum STM32F4_REV { - STM32F4_REV_A = 0x1000, - STM32F4_REV_Z = 0x1001, - STM32F4_REV_Y = 0x1003, - STM32F4_REV_1 = 0x1007, - STM32F4_REV_3 = 0x2001 -}; -#else -#error stm32 -#endif - static void usage(const char *reason) { if (reason != NULL) { @@ -132,61 +109,28 @@ int ver_main(int argc, char *argv[]) printf("GCC toolchain: %s\n", __VERSION__); ret = 0; -#ifdef CONFIG_ARCH_CHIP_STM32 + } else if (!strncmp(argv[1], mcu_ver_str, sizeof(mcu_ver_str))) { - uint32_t abc = getreg32(DBGMCU_IDCODE); - - uint32_t chip_version = abc & DEVID_MASK; - enum STM32F4_REV revid = (abc & REVID_MASK) >> 16; - - printf("CHIP TYPE: "); - - switch (revid) { - case STM32F40x_41x: - printf("STM32F40x"); - break; - case STM32F42x_43x: - printf("STM32F42x"); - break; - default: - printf("STM32F???"); - break; - } char rev; + char* revstr; - switch (chip_version) { - - case STM32F4_REV_A: - rev = 'A'; - break; - case STM32F4_REV_Z: - rev = 'Z'; - break; - case STM32F4_REV_Y: - rev = 'Y'; - break; - case STM32F4_REV_1: - rev = '1'; - break; - case STM32F4_REV_3: - rev = '3'; - break; - default: - rev = '?'; - break; - } + int chip_version = mcu_version(&rev, &revstr); + + if (chip_version < 0) { + printf("UNKNOWN MCU"); + ret = 1; - printf("\nHW REV: %c\n", rev); + } else { + printf("MCU: %s, rev. %c\n", revstr, rev); - if (rev < STM32F4_REV_3) { - printf("\n\nWARNING WARNING WARNING!\n" - "Revision %c has a silicon errata\n" - "This device can only utilize a maximum of 1MB flash safely!\n" - "http://px4.io/help/errata\n", rev); + if (chip_version < MCU_REV_STM32F4_REV_3) { + printf("\n\nWARNING WARNING WARNING!\n" + "Revision %c has a silicon errata\n" + "This device can only utilize a maximum of 1MB flash safely!\n" + "http://px4.io/help/errata\n", rev); + } } - ret = 0; -#endif } else { errx(1, "unknown command.\n"); -- cgit v1.2.3 From 6d59df1a5f67a58b6881d4d8a466082b847a2e61 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 15 Nov 2014 16:19:51 +0100 Subject: Make ROMFS less chatty --- ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil | 8 ++------ ROMFS/px4fmu_common/init.d/10015_tbs_discovery | 2 +- ROMFS/px4fmu_common/init.d/10016_3dr_iris | 2 +- ROMFS/px4fmu_common/init.d/10017_steadidrone_qu4d | 3 +-- ROMFS/px4fmu_common/init.d/1001_rc_quad_x.hil | 2 +- ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil | 2 +- .../px4fmu_common/init.d/1004_rc_fw_Rascal110.hil | 4 +--- ROMFS/px4fmu_common/init.d/1005_rc_fw_Malolo1.hil | 2 +- ROMFS/px4fmu_common/init.d/11001_hexa_cox | 4 ++-- ROMFS/px4fmu_common/init.d/12001_octo_cox | 2 +- ROMFS/px4fmu_common/init.d/3031_phantom | 4 +--- ROMFS/px4fmu_common/init.d/3032_skywalker_x5 | 6 +----- ROMFS/px4fmu_common/init.d/3033_wingwing | 2 +- ROMFS/px4fmu_common/init.d/3034_fx79 | 2 +- ROMFS/px4fmu_common/init.d/3035_viper | 2 +- ROMFS/px4fmu_common/init.d/3100_tbs_caipirinha | 8 ++------ ROMFS/px4fmu_common/init.d/4001_quad_x | 2 +- ROMFS/px4fmu_common/init.d/4008_ardrone | 3 --- ROMFS/px4fmu_common/init.d/4010_dji_f330 | 4 ++-- ROMFS/px4fmu_common/init.d/4011_dji_f450 | 4 ++-- ROMFS/px4fmu_common/init.d/4012_quad_x_can | 2 +- ROMFS/px4fmu_common/init.d/4020_hk_micro_pcb | 2 +- ROMFS/px4fmu_common/init.d/5001_quad_+ | 2 +- ROMFS/px4fmu_common/init.d/6001_hexa_x | 4 ++-- ROMFS/px4fmu_common/init.d/7001_hexa_+ | 4 ++-- ROMFS/px4fmu_common/init.d/8001_octo_x | 2 +- ROMFS/px4fmu_common/init.d/9001_octo_+ | 2 +- ROMFS/px4fmu_common/init.d/rc.autostart | 5 ++--- ROMFS/px4fmu_common/init.d/rc.sensors | 7 +------ ROMFS/px4fmu_common/init.d/rcS | 24 +++------------------- 30 files changed, 39 insertions(+), 83 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil b/ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil index d114fe21a..4d6d350b8 100644 --- a/ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil +++ b/ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil @@ -1,14 +1,10 @@ #!nsh # -# HILStar / X-Plane -# -# Lorenz Meier +# HILStar +# # sh /etc/init.d/rc.fw_defaults -echo "X Plane HIL starting.." - set HIL yes - set MIXER FMU_AERT diff --git a/ROMFS/px4fmu_common/init.d/10015_tbs_discovery b/ROMFS/px4fmu_common/init.d/10015_tbs_discovery index b1aa8c00b..c8379e3a1 100644 --- a/ROMFS/px4fmu_common/init.d/10015_tbs_discovery +++ b/ROMFS/px4fmu_common/init.d/10015_tbs_discovery @@ -2,7 +2,7 @@ # # Team Blacksheep Discovery Quadcopter # -# Anton Babushkin , Simon Wilks , Thomas Gubler +# Anton Babushkin , Simon Wilks # sh /etc/init.d/rc.mc_defaults diff --git a/ROMFS/px4fmu_common/init.d/10016_3dr_iris b/ROMFS/px4fmu_common/init.d/10016_3dr_iris index 3f47390c1..0b422de7e 100644 --- a/ROMFS/px4fmu_common/init.d/10016_3dr_iris +++ b/ROMFS/px4fmu_common/init.d/10016_3dr_iris @@ -2,7 +2,7 @@ # # 3DR Iris Quadcopter # -# Anton Babushkin +# Anton Babushkin # sh /etc/init.d/rc.mc_defaults diff --git a/ROMFS/px4fmu_common/init.d/10017_steadidrone_qu4d b/ROMFS/px4fmu_common/init.d/10017_steadidrone_qu4d index 6179855f6..a621de7ce 100644 --- a/ROMFS/px4fmu_common/init.d/10017_steadidrone_qu4d +++ b/ROMFS/px4fmu_common/init.d/10017_steadidrone_qu4d @@ -2,8 +2,7 @@ # # Steadidrone QU4D # -# Thomas Gubler -# Lorenz Meier +# Thomas Gubler # sh /etc/init.d/rc.mc_defaults diff --git a/ROMFS/px4fmu_common/init.d/1001_rc_quad_x.hil b/ROMFS/px4fmu_common/init.d/1001_rc_quad_x.hil index 7a7a9542c..1c4f6803b 100644 --- a/ROMFS/px4fmu_common/init.d/1001_rc_quad_x.hil +++ b/ROMFS/px4fmu_common/init.d/1001_rc_quad_x.hil @@ -2,7 +2,7 @@ # # HIL Quadcopter X # -# Anton Babushkin +# Anton Babushkin # sh /etc/init.d/rc.mc_defaults diff --git a/ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil b/ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil index c47500c7a..0cbdd75be 100644 --- a/ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil +++ b/ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil @@ -2,7 +2,7 @@ # # HIL Quadcopter + # -# Anton Babushkin +# Anton Babushkin # sh /etc/init.d/rc.mc_defaults diff --git a/ROMFS/px4fmu_common/init.d/1004_rc_fw_Rascal110.hil b/ROMFS/px4fmu_common/init.d/1004_rc_fw_Rascal110.hil index 4e3e18326..fb440d2fc 100644 --- a/ROMFS/px4fmu_common/init.d/1004_rc_fw_Rascal110.hil +++ b/ROMFS/px4fmu_common/init.d/1004_rc_fw_Rascal110.hil @@ -2,13 +2,11 @@ # # HIL Rascal 110 (Flightgear) # -# Thomas Gubler +# Thomas Gubler # sh /etc/init.d/rc.fw_defaults -echo "HIL Rascal 110 starting.." - set HIL yes set MIXER FMU_AERT diff --git a/ROMFS/px4fmu_common/init.d/1005_rc_fw_Malolo1.hil b/ROMFS/px4fmu_common/init.d/1005_rc_fw_Malolo1.hil index 941f5664a..00b97d675 100644 --- a/ROMFS/px4fmu_common/init.d/1005_rc_fw_Malolo1.hil +++ b/ROMFS/px4fmu_common/init.d/1005_rc_fw_Malolo1.hil @@ -2,7 +2,7 @@ # # HIL Malolo 1 (Flightgear) # -# Thomas Gubler +# Thomas Gubler # sh /etc/init.d/rc.fw_defaults diff --git a/ROMFS/px4fmu_common/init.d/11001_hexa_cox b/ROMFS/px4fmu_common/init.d/11001_hexa_cox index daa04a4de..e4d96fbd5 100644 --- a/ROMFS/px4fmu_common/init.d/11001_hexa_cox +++ b/ROMFS/px4fmu_common/init.d/11001_hexa_cox @@ -2,12 +2,12 @@ # # Generic 10" Hexa coaxial geometry # -# Lorenz Meier +# Lorenz Meier # sh /etc/init.d/rc.mc_defaults set MIXER FMU_hexa_cox -# We only can run one channel group with one rate, so set all 8 channels +# Need to set all 8 channels set PWM_OUTPUTS 12345678 diff --git a/ROMFS/px4fmu_common/init.d/12001_octo_cox b/ROMFS/px4fmu_common/init.d/12001_octo_cox index 8703f5f2f..f820251ad 100644 --- a/ROMFS/px4fmu_common/init.d/12001_octo_cox +++ b/ROMFS/px4fmu_common/init.d/12001_octo_cox @@ -2,7 +2,7 @@ # # Generic 10" Octo coaxial geometry # -# Lorenz Meier +# Lorenz Meier # sh /etc/init.d/rc.mc_defaults diff --git a/ROMFS/px4fmu_common/init.d/3031_phantom b/ROMFS/px4fmu_common/init.d/3031_phantom index 53c48d8aa..a7749cba6 100644 --- a/ROMFS/px4fmu_common/init.d/3031_phantom +++ b/ROMFS/px4fmu_common/init.d/3031_phantom @@ -2,7 +2,7 @@ # # Phantom FPV Flying Wing # -# Simon Wilks , Thomas Gubler +# Simon Wilks # sh /etc/init.d/rc.fw_defaults @@ -21,8 +21,6 @@ then param set FW_PR_P 0.03 param set FW_P_LIM_MAX 50 param set FW_P_LIM_MIN -50 - param set FW_P_RMAX_NEG 0 - param set FW_P_RMAX_POS 0 param set FW_P_ROLLFF 1 param set FW_RR_FF 0.5 param set FW_RR_I 0.02 diff --git a/ROMFS/px4fmu_common/init.d/3032_skywalker_x5 b/ROMFS/px4fmu_common/init.d/3032_skywalker_x5 index 7d0dc5bff..26c7c95e6 100644 --- a/ROMFS/px4fmu_common/init.d/3032_skywalker_x5 +++ b/ROMFS/px4fmu_common/init.d/3032_skywalker_x5 @@ -2,7 +2,7 @@ # # Skywalker X5 Flying Wing # -# Thomas Gubler , Julian Oes +# Thomas Gubler , Julian Oes # sh /etc/init.d/rc.fw_defaults @@ -19,10 +19,6 @@ then param set FW_PR_I 0 param set FW_PR_IMAX 0.2 param set FW_PR_P 0.03 - param set FW_P_LIM_MAX 45 - param set FW_P_LIM_MIN -45 - param set FW_P_RMAX_NEG 0 - param set FW_P_RMAX_POS 0 param set FW_P_ROLLFF 1 param set FW_RR_FF 0.3 param set FW_RR_I 0 diff --git a/ROMFS/px4fmu_common/init.d/3033_wingwing b/ROMFS/px4fmu_common/init.d/3033_wingwing index f4dedef15..919eefb4a 100644 --- a/ROMFS/px4fmu_common/init.d/3033_wingwing +++ b/ROMFS/px4fmu_common/init.d/3033_wingwing @@ -2,7 +2,7 @@ # # Wing Wing (aka Z-84) Flying Wing # -# Simon Wilks +# Simon Wilks # sh /etc/init.d/rc.fw_defaults diff --git a/ROMFS/px4fmu_common/init.d/3034_fx79 b/ROMFS/px4fmu_common/init.d/3034_fx79 index f4bd18269..4a76ba6eb 100644 --- a/ROMFS/px4fmu_common/init.d/3034_fx79 +++ b/ROMFS/px4fmu_common/init.d/3034_fx79 @@ -2,7 +2,7 @@ # # FX-79 Buffalo Flying Wing # -# Simon Wilks +# Simon Wilks # sh /etc/init.d/rc.fw_defaults diff --git a/ROMFS/px4fmu_common/init.d/3035_viper b/ROMFS/px4fmu_common/init.d/3035_viper index f3b0e8418..0f5f5502a 100644 --- a/ROMFS/px4fmu_common/init.d/3035_viper +++ b/ROMFS/px4fmu_common/init.d/3035_viper @@ -2,7 +2,7 @@ # # Viper # -# Simon Wilks +# Simon Wilks # sh /etc/init.d/rc.fw_defaults diff --git a/ROMFS/px4fmu_common/init.d/3100_tbs_caipirinha b/ROMFS/px4fmu_common/init.d/3100_tbs_caipirinha index 9a2150403..9bfd9d9ed 100644 --- a/ROMFS/px4fmu_common/init.d/3100_tbs_caipirinha +++ b/ROMFS/px4fmu_common/init.d/3100_tbs_caipirinha @@ -1,8 +1,8 @@ #!nsh # -# TBS Caipirinha Flying Wing +# TBS Caipirinha # -# Thomas Gubler +# Thomas Gubler # sh /etc/init.d/rc.fw_defaults @@ -22,10 +22,6 @@ then param set FW_PR_I 0 param set FW_PR_IMAX 0.2 param set FW_PR_P 0.03 - param set FW_P_LIM_MAX 45 - param set FW_P_LIM_MIN -45 - param set FW_P_RMAX_NEG 0 - param set FW_P_RMAX_POS 0 param set FW_P_ROLLFF 0 param set FW_RR_FF 0.3 param set FW_RR_I 0 diff --git a/ROMFS/px4fmu_common/init.d/4001_quad_x b/ROMFS/px4fmu_common/init.d/4001_quad_x index 06c54a41d..8fe8961c5 100644 --- a/ROMFS/px4fmu_common/init.d/4001_quad_x +++ b/ROMFS/px4fmu_common/init.d/4001_quad_x @@ -2,7 +2,7 @@ # # Generic 10" Quad X geometry # -# Lorenz Meier +# Lorenz Meier # sh /etc/init.d/rc.mc_defaults diff --git a/ROMFS/px4fmu_common/init.d/4008_ardrone b/ROMFS/px4fmu_common/init.d/4008_ardrone index e6007db0e..0488e3928 100644 --- a/ROMFS/px4fmu_common/init.d/4008_ardrone +++ b/ROMFS/px4fmu_common/init.d/4008_ardrone @@ -3,9 +3,6 @@ # ARDrone # -echo "[init] 4008_ardrone: PX4FMU on PX4IOAR carrier board" - -# Just use the default multicopter settings. sh /etc/init.d/rc.mc_defaults # diff --git a/ROMFS/px4fmu_common/init.d/4010_dji_f330 b/ROMFS/px4fmu_common/init.d/4010_dji_f330 index 282ab620d..f0cc05207 100644 --- a/ROMFS/px4fmu_common/init.d/4010_dji_f330 +++ b/ROMFS/px4fmu_common/init.d/4010_dji_f330 @@ -1,8 +1,8 @@ #!nsh # -# DJI Flame Wheel F330 Quadcopter +# DJI Flame Wheel F330 # -# Anton Babushkin +# Anton Babushkin # sh /etc/init.d/4001_quad_x diff --git a/ROMFS/px4fmu_common/init.d/4011_dji_f450 b/ROMFS/px4fmu_common/init.d/4011_dji_f450 index 517b4aa86..1ca716a6b 100644 --- a/ROMFS/px4fmu_common/init.d/4011_dji_f450 +++ b/ROMFS/px4fmu_common/init.d/4011_dji_f450 @@ -1,8 +1,8 @@ #!nsh # -# DJI Flame Wheel F450 Quadcopter +# DJI Flame Wheel F450 # -# Lorenz Meier +# Lorenz Meier # sh /etc/init.d/4001_quad_x diff --git a/ROMFS/px4fmu_common/init.d/4012_quad_x_can b/ROMFS/px4fmu_common/init.d/4012_quad_x_can index 8c5a4fbf2..5c4a6497a 100644 --- a/ROMFS/px4fmu_common/init.d/4012_quad_x_can +++ b/ROMFS/px4fmu_common/init.d/4012_quad_x_can @@ -2,7 +2,7 @@ # # F450-sized quadrotor with CAN # -# Lorenz Meier +# Pavel Kirienko # sh /etc/init.d/4001_quad_x diff --git a/ROMFS/px4fmu_common/init.d/4020_hk_micro_pcb b/ROMFS/px4fmu_common/init.d/4020_hk_micro_pcb index 99ffd73a5..9fe310dde 100644 --- a/ROMFS/px4fmu_common/init.d/4020_hk_micro_pcb +++ b/ROMFS/px4fmu_common/init.d/4020_hk_micro_pcb @@ -3,7 +3,7 @@ # Hobbyking Micro Integrated PCB Quadcopter # with SimonK ESC firmware and Mystery A1510 motors # -# Thomas Gubler +# Thomas Gubler # echo "HK Micro PCB Quad" diff --git a/ROMFS/px4fmu_common/init.d/5001_quad_+ b/ROMFS/px4fmu_common/init.d/5001_quad_+ index 1fb25e5d8..5512aa738 100644 --- a/ROMFS/px4fmu_common/init.d/5001_quad_+ +++ b/ROMFS/px4fmu_common/init.d/5001_quad_+ @@ -2,7 +2,7 @@ # # Generic 10" Quad + geometry # -# Anton Babushkin +# Anton Babushkin # sh /etc/init.d/rc.mc_defaults diff --git a/ROMFS/px4fmu_common/init.d/6001_hexa_x b/ROMFS/px4fmu_common/init.d/6001_hexa_x index 34fc6523f..1043ad8ad 100644 --- a/ROMFS/px4fmu_common/init.d/6001_hexa_x +++ b/ROMFS/px4fmu_common/init.d/6001_hexa_x @@ -2,12 +2,12 @@ # # Generic 10" Hexa X geometry # -# Anton Babushkin +# Anton Babushkin # sh /etc/init.d/rc.mc_defaults set MIXER FMU_hexa_x -# We only can run one channel group with one rate, so set all 8 channels +# Need to set all 8 channels set PWM_OUTPUTS 12345678 diff --git a/ROMFS/px4fmu_common/init.d/7001_hexa_+ b/ROMFS/px4fmu_common/init.d/7001_hexa_+ index 235e376a6..84ab88883 100644 --- a/ROMFS/px4fmu_common/init.d/7001_hexa_+ +++ b/ROMFS/px4fmu_common/init.d/7001_hexa_+ @@ -2,12 +2,12 @@ # # Generic 10" Hexa + geometry # -# Anton Babushkin +# Anton Babushkin # sh /etc/init.d/rc.mc_defaults set MIXER FMU_hexa_+ -# We only can run one channel group with one rate, so set all 8 channels +# Need to set all 8 channels set PWM_OUTPUTS 12345678 diff --git a/ROMFS/px4fmu_common/init.d/8001_octo_x b/ROMFS/px4fmu_common/init.d/8001_octo_x index 769217dc7..74e304cd9 100644 --- a/ROMFS/px4fmu_common/init.d/8001_octo_x +++ b/ROMFS/px4fmu_common/init.d/8001_octo_x @@ -2,7 +2,7 @@ # # Generic 10" Octo X geometry # -# Anton Babushkin +# Anton Babushkin # sh /etc/init.d/rc.mc_defaults diff --git a/ROMFS/px4fmu_common/init.d/9001_octo_+ b/ROMFS/px4fmu_common/init.d/9001_octo_+ index 28b074a54..f7c06c6c8 100644 --- a/ROMFS/px4fmu_common/init.d/9001_octo_+ +++ b/ROMFS/px4fmu_common/init.d/9001_octo_+ @@ -2,7 +2,7 @@ # # Generic 10" Octo + geometry # -# Anton Babushkin +# Anton Babushkin # sh /etc/init.d/rc.mc_defaults diff --git a/ROMFS/px4fmu_common/init.d/rc.autostart b/ROMFS/px4fmu_common/init.d/rc.autostart index 78778d806..496a52c5f 100644 --- a/ROMFS/px4fmu_common/init.d/rc.autostart +++ b/ROMFS/px4fmu_common/init.d/rc.autostart @@ -1,5 +1,4 @@ # -# Check if auto-setup from one of the standard scripts is wanted # SYS_AUTOSTART = 0 means no autostart (default) # # AUTOSTART PARTITION: @@ -18,7 +17,7 @@ # 12000 .. 12999 Octo Cox # -# Simulation setups +# Simulation # if param compare SYS_AUTOSTART 901 @@ -53,7 +52,7 @@ then fi # -# Standard plane +# Plane # if param compare SYS_AUTOSTART 2100 100 diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors index 739df7ac0..fbac50cf7 100644 --- a/ROMFS/px4fmu_common/init.d/rc.sensors +++ b/ROMFS/px4fmu_common/init.d/rc.sensors @@ -54,7 +54,6 @@ then fi fi -# Start airspeed sensors if meas_airspeed start then echo "[init] Using MEAS airspeed sensor" @@ -68,16 +67,12 @@ else fi fi -# Check for flow sensor if px4flow start then fi # -# Start the sensor collection task. -# IMPORTANT: this also loads param offsets -# ALWAYS start this task before the -# preflight_check. +# Start sensors -> preflight_check # if sensors start then diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index ea04ece34..f9c822635 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -137,9 +137,7 @@ then # if param compare SYS_AUTOCONFIG 1 then - # We can't be sure the defaults haven't changed, so - # if someone requests a re-configuration, we do it - # cleanly from scratch (except autostart / autoconfig) + # Wipe out params param reset_nostart set DO_AUTOCONFIG yes else @@ -202,12 +200,10 @@ then if px4io checkcrc $IO_FILE then - echo "[init] PX4IO CRC OK" echo "PX4IO CRC OK" >> $LOG_FILE set IO_PRESENT yes else - echo "[init] Trying to update" echo "PX4IO Trying to update" >> $LOG_FILE tone_alarm MLL32CP8MB @@ -217,18 +213,15 @@ then usleep 500000 if px4io checkcrc $IO_FILE then - echo "[init] PX4IO CRC OK, update successful" echo "PX4IO CRC OK after updating" >> $LOG_FILE tone_alarm MLL8CDE set IO_PRESENT yes else - echo "[init] ERROR: PX4IO update failed" echo "PX4IO update failed" >> $LOG_FILE tone_alarm $TUNE_OUT_ERROR fi else - echo "[init] ERROR: PX4IO update failed" echo "PX4IO update failed" >> $LOG_FILE tone_alarm $TUNE_OUT_ERROR fi @@ -281,16 +274,12 @@ then fi fi - # - # Start the datamanager (and do not abort boot if it fails) - # + # waypoint storage if dataman start then fi - # - # Start the Commander (needs to be this early for in-air-restarts) - # + # Needs to be this early for in-air-restarts commander start # @@ -424,9 +413,6 @@ then fi fi - # - # MAVLink - # if [ $MAVLINK_FLAGS == default ] then # Normal mode, use baudrate 57600 (default) and data rate 1000 bytes/s @@ -454,10 +440,6 @@ then # Sensors, Logging, GPS # sh /etc/init.d/rc.sensors - - # - # Start logging in all modes, including HIL - # sh /etc/init.d/rc.logging if [ $GPS == yes ] -- cgit v1.2.3 From e2c0ac3f700f003061e52fd61fd4770c982605c8 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 15 Nov 2014 16:40:29 +0100 Subject: airspeed driver: Be less chatty --- src/drivers/airspeed/airspeed.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/airspeed/airspeed.cpp b/src/drivers/airspeed/airspeed.cpp index 293690d27..3a1e1b7b5 100644 --- a/src/drivers/airspeed/airspeed.cpp +++ b/src/drivers/airspeed/airspeed.cpp @@ -147,7 +147,7 @@ Airspeed::init() _airspeed_pub = orb_advertise(ORB_ID(differential_pressure), &arp); if (_airspeed_pub < 0) - warnx("failed to create airspeed sensor object. uORB started?"); + warnx("uORB started?"); } ret = OK; -- cgit v1.2.3 From 6f71173f8c7a13565ef90a8fec44187ba392f038 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 15 Nov 2014 16:40:42 +0100 Subject: ARDrone driver: be less chatty --- src/drivers/ardrone_interface/ardrone_interface.c | 36 +++++++++------------- .../ardrone_interface/ardrone_motor_control.c | 2 +- 2 files changed, 15 insertions(+), 23 deletions(-) diff --git a/src/drivers/ardrone_interface/ardrone_interface.c b/src/drivers/ardrone_interface/ardrone_interface.c index e5bb772b3..9d2c1441d 100644 --- a/src/drivers/ardrone_interface/ardrone_interface.c +++ b/src/drivers/ardrone_interface/ardrone_interface.c @@ -89,8 +89,8 @@ static void usage(const char *reason) { if (reason) - fprintf(stderr, "%s\n", reason); - fprintf(stderr, "usage: ardrone_interface {start|stop|status} [-d ]\n\n"); + warnx("%s\n", reason); + warnx("usage: {start|stop|status} [-d ]\n\n"); exit(1); } @@ -110,7 +110,7 @@ int ardrone_interface_main(int argc, char *argv[]) if (!strcmp(argv[1], "start")) { if (thread_running) { - printf("ardrone_interface already running\n"); + warnx("already running\n"); /* this is not an error */ exit(0); } @@ -132,9 +132,9 @@ int ardrone_interface_main(int argc, char *argv[]) if (!strcmp(argv[1], "status")) { if (thread_running) { - printf("\tardrone_interface is running\n"); + warnx("running"); } else { - printf("\tardrone_interface not started\n"); + warnx("not started"); } exit(0); } @@ -158,7 +158,7 @@ static int ardrone_open_uart(char *uart_name, struct termios *uart_config_origin /* Back up the original uart configuration to restore it after exit */ if ((termios_state = tcgetattr(uart, uart_config_original)) < 0) { - fprintf(stderr, "[ardrone_interface] ERROR getting baudrate / termios config for %s: %d\n", uart_name, termios_state); + warnx("ERR: TCGETATTR %s: %d", uart_name, termios_state); close(uart); return -1; } @@ -171,14 +171,14 @@ static int ardrone_open_uart(char *uart_name, struct termios *uart_config_origin /* Set baud rate */ if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) { - fprintf(stderr, "[ardrone_interface] ERROR setting baudrate / termios config for %s: %d (cfsetispeed, cfsetospeed)\n", uart_name, termios_state); + warnx("ERR: cfsetispeed %s: %d", uart_name, termios_state); close(uart); return -1; } if ((termios_state = tcsetattr(uart, TCSANOW, &uart_config)) < 0) { - fprintf(stderr, "[ardrone_interface] ERROR setting baudrate / termios config for %s (tcsetattr)\n", uart_name); + warnx("ERR: tcsetattr: %s", uart_name); close(uart); return -1; } @@ -192,9 +192,6 @@ int ardrone_interface_thread_main(int argc, char *argv[]) char *device = "/dev/ttyS1"; - /* welcome user */ - printf("[ardrone_interface] Control started, taking over motors\n"); - /* File descriptors */ int gpios; @@ -237,7 +234,7 @@ int ardrone_interface_thread_main(int argc, char *argv[]) struct termios uart_config_original; if (motor_test_mode) { - printf("[ardrone_interface] Motor test mode enabled, setting 10 %% thrust.\n"); + warnx("setting 10 %% thrust.\n"); } /* Led animation */ @@ -255,9 +252,6 @@ int ardrone_interface_thread_main(int argc, char *argv[]) int actuator_controls_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); int armed_sub = orb_subscribe(ORB_ID(actuator_armed)); - printf("[ardrone_interface] Motors initialized - ready.\n"); - fflush(stdout); - /* enable UART, writes potentially an empty buffer, but multiplexing is disabled */ ardrone_write = ardrone_open_uart(device, &uart_config_original); @@ -265,7 +259,7 @@ int ardrone_interface_thread_main(int argc, char *argv[]) gpios = ar_multiplexing_init(); if (ardrone_write < 0) { - fprintf(stderr, "[ardrone_interface] Failed opening AR.Drone UART, exiting.\n"); + warnx("No UART, exiting."); thread_running = false; exit(ERROR); } @@ -273,7 +267,7 @@ int ardrone_interface_thread_main(int argc, char *argv[]) /* initialize motors */ if (OK != ar_init_motors(ardrone_write, gpios)) { close(ardrone_write); - fprintf(stderr, "[ardrone_interface] Failed initializing AR.Drone motors, exiting.\n"); + warnx("motor init fail"); thread_running = false; exit(ERROR); } @@ -294,7 +288,7 @@ int ardrone_interface_thread_main(int argc, char *argv[]) gpios = ar_multiplexing_init(); if (ardrone_write < 0) { - fprintf(stderr, "[ardrone_interface] Failed opening AR.Drone UART, exiting.\n"); + warnx("write fail"); thread_running = false; exit(ERROR); } @@ -302,7 +296,7 @@ int ardrone_interface_thread_main(int argc, char *argv[]) /* initialize motors */ if (OK != ar_init_motors(ardrone_write, gpios)) { close(ardrone_write); - fprintf(stderr, "[ardrone_interface] Failed initializing AR.Drone motors, exiting.\n"); + warnx("motor init fail"); thread_running = false; exit(ERROR); } @@ -378,11 +372,9 @@ int ardrone_interface_thread_main(int argc, char *argv[]) int termios_state; if ((termios_state = tcsetattr(ardrone_write, TCSANOW, &uart_config_original)) < 0) { - fprintf(stderr, "[ardrone_interface] ERROR setting baudrate / termios config for (tcsetattr)\n"); + warnx("ERR: tcsetattr"); } - printf("[ardrone_interface] Restored original UART config, exiting..\n"); - /* close uarts */ close(ardrone_write); ar_multiplexing_deinit(gpios); diff --git a/src/drivers/ardrone_interface/ardrone_motor_control.c b/src/drivers/ardrone_interface/ardrone_motor_control.c index fc017dd58..4fa24275f 100644 --- a/src/drivers/ardrone_interface/ardrone_motor_control.c +++ b/src/drivers/ardrone_interface/ardrone_motor_control.c @@ -301,7 +301,7 @@ int ar_init_motors(int ardrone_uart, int gpios) ardrone_write_motor_commands(ardrone_uart, 0, 0, 0, 0); if (errcounter != 0) { - fprintf(stderr, "[ardrone_interface] init sequence incomplete, failed %d times", -errcounter); + warnx("Failed %d times", -errcounter); fflush(stdout); } return errcounter; -- cgit v1.2.3 From 8d5225b9679c2820a57ea30ed6d4c46f5d6c9baa Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 15 Nov 2014 16:41:18 +0100 Subject: FrSky: Be less chatty --- src/drivers/frsky_telemetry/frsky_telemetry.c | 9 +++------ 1 file changed, 3 insertions(+), 6 deletions(-) diff --git a/src/drivers/frsky_telemetry/frsky_telemetry.c b/src/drivers/frsky_telemetry/frsky_telemetry.c index 6e0839043..bccdf1190 100644 --- a/src/drivers/frsky_telemetry/frsky_telemetry.c +++ b/src/drivers/frsky_telemetry/frsky_telemetry.c @@ -84,7 +84,7 @@ static int frsky_open_uart(const char *uart_name, struct termios *uart_config_or /* Back up the original UART configuration to restore it after exit */ int termios_state; if ((termios_state = tcgetattr(uart, uart_config_original)) < 0) { - warnx("ERROR get termios config %s: %d\n", uart_name, termios_state); + warnx("ERR: tcgetattr%s: %d\n", uart_name, termios_state); close(uart); return -1; } @@ -100,13 +100,13 @@ static int frsky_open_uart(const char *uart_name, struct termios *uart_config_or static const speed_t speed = B9600; if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) { - warnx("ERROR setting baudrate / termios config for %s: %d (cfsetispeed, cfsetospeed)\n", uart_name, termios_state); + warnx("ERR: %s: %d (cfsetispeed, cfsetospeed)\n", uart_name, termios_state); close(uart); return -1; } if ((termios_state = tcsetattr(uart, TCSANOW, &uart_config)) < 0) { - warnx("ERROR setting baudrate / termios config for %s (tcsetattr)\n", uart_name); + warnx("ERR: %s (tcsetattr)\n", uart_name); close(uart); return -1; } @@ -151,9 +151,6 @@ static int frsky_telemetry_thread_main(int argc, char *argv[]) } } - /* Print welcome text */ - warnx("FrSky telemetry interface starting..."); - /* Open UART */ struct termios uart_config_original; const int uart = frsky_open_uart(device_name, &uart_config_original); -- cgit v1.2.3 From ef27225534df6cca0b5a98b85c6b7cf3364493c0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 15 Nov 2014 16:41:30 +0100 Subject: HIL: Be less chatty --- src/drivers/hil/hil.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/drivers/hil/hil.cpp b/src/drivers/hil/hil.cpp index f0dc0c651..9b5c8133b 100644 --- a/src/drivers/hil/hil.cpp +++ b/src/drivers/hil/hil.cpp @@ -442,8 +442,6 @@ HIL::task_main() /* make sure servos are off */ // up_pwm_servo_deinit(); - log("stopping"); - /* note - someone else is responsible for restoring the GPIO config */ /* tell the dtor that we are exiting */ -- cgit v1.2.3 From 5af710221a2948ab14f95225b4cb6c7fe5c66560 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 15 Nov 2014 16:54:34 +0100 Subject: HoTT: Less chatty --- src/drivers/hott/comms.cpp | 8 ++++---- src/drivers/hott/hott_sensors/hott_sensors.cpp | 6 +++--- src/drivers/hott/hott_telemetry/hott_telemetry.cpp | 6 +++--- 3 files changed, 10 insertions(+), 10 deletions(-) diff --git a/src/drivers/hott/comms.cpp b/src/drivers/hott/comms.cpp index cb8bbba37..60a49b559 100644 --- a/src/drivers/hott/comms.cpp +++ b/src/drivers/hott/comms.cpp @@ -55,7 +55,7 @@ open_uart(const char *device) const int uart = open(device, O_RDWR | O_NOCTTY); if (uart < 0) { - err(1, "Error opening port: %s", device); + err(1, "ERR: opening %s", device); } /* Back up the original uart configuration to restore it after exit */ @@ -63,7 +63,7 @@ open_uart(const char *device) struct termios uart_config_original; if ((termios_state = tcgetattr(uart, &uart_config_original)) < 0) { close(uart); - err(1, "Error getting baudrate / termios config for %s: %d", device, termios_state); + err(1, "ERR: %s: %d", device, termios_state); } /* Fill the struct for the new configuration */ @@ -76,13 +76,13 @@ open_uart(const char *device) /* Set baud rate */ if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) { close(uart); - err(1, "Error setting baudrate / termios config for %s: %d (cfsetispeed, cfsetospeed)", + err(1, "ERR: %s: %d (cfsetispeed, cfsetospeed)", device, termios_state); } if ((termios_state = tcsetattr(uart, TCSANOW, &uart_config)) < 0) { close(uart); - err(1, "Error setting baudrate / termios config for %s (tcsetattr)", device); + err(1, "ERR: %s (tcsetattr)", device); } /* Activate single wire mode */ diff --git a/src/drivers/hott/hott_sensors/hott_sensors.cpp b/src/drivers/hott/hott_sensors/hott_sensors.cpp index a3d3a3933..8ab9d8d55 100644 --- a/src/drivers/hott/hott_sensors/hott_sensors.cpp +++ b/src/drivers/hott/hott_sensors/hott_sensors.cpp @@ -204,7 +204,7 @@ hott_sensors_main(int argc, char *argv[]) if (!strcmp(argv[1], "start")) { if (thread_running) { - warnx("deamon already running"); + warnx("already running"); exit(0); } @@ -225,10 +225,10 @@ hott_sensors_main(int argc, char *argv[]) if (!strcmp(argv[1], "status")) { if (thread_running) { - warnx("daemon is running"); + warnx("is running"); } else { - warnx("daemon not started"); + warnx("not started"); } exit(0); diff --git a/src/drivers/hott/hott_telemetry/hott_telemetry.cpp b/src/drivers/hott/hott_telemetry/hott_telemetry.cpp index d293f9954..edbb14172 100644 --- a/src/drivers/hott/hott_telemetry/hott_telemetry.cpp +++ b/src/drivers/hott/hott_telemetry/hott_telemetry.cpp @@ -230,7 +230,7 @@ hott_telemetry_main(int argc, char *argv[]) if (!strcmp(argv[1], "start")) { if (thread_running) { - warnx("deamon already running"); + warnx("already running"); exit(0); } @@ -251,10 +251,10 @@ hott_telemetry_main(int argc, char *argv[]) if (!strcmp(argv[1], "status")) { if (thread_running) { - warnx("daemon is running"); + warnx("is running"); } else { - warnx("daemon not started"); + warnx("not started"); } exit(0); -- cgit v1.2.3 From 1fc7b588945b3bb1df70de1f779201c60a803b71 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 15 Nov 2014 16:54:47 +0100 Subject: Bottle drop: Less chatty --- src/modules/bottle_drop/bottle_drop.cpp | 19 +++++++++---------- 1 file changed, 9 insertions(+), 10 deletions(-) diff --git a/src/modules/bottle_drop/bottle_drop.cpp b/src/modules/bottle_drop/bottle_drop.cpp index 31c9157e1..6d24e5d2d 100644 --- a/src/modules/bottle_drop/bottle_drop.cpp +++ b/src/modules/bottle_drop/bottle_drop.cpp @@ -283,7 +283,6 @@ BottleDrop::drop() // 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) < 500 * 1000 && hrt_elapsed_time(&starttime) < 2000000) { @@ -723,16 +722,16 @@ BottleDrop::handle_command(struct vehicle_command_s *cmd) if (cmd->param1 > 0.5f && cmd->param2 > 0.5f) { open_bay(); drop(); - mavlink_log_info(_mavlink_fd, "#audio: drop bottle"); + mavlink_log_critical(_mavlink_fd, "drop bottle"); } else if (cmd->param1 > 0.5f) { open_bay(); - mavlink_log_info(_mavlink_fd, "#audio: opening bay"); + mavlink_log_critical(_mavlink_fd, "opening bay"); } else { lock_release(); close_bay(); - mavlink_log_info(_mavlink_fd, "#audio: closing bay"); + mavlink_log_critical(_mavlink_fd, "closing bay"); } answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); @@ -743,12 +742,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"); + mavlink_log_critical(_mavlink_fd, "got drop position, no approval"); break; case 1: _drop_approval = true; - mavlink_log_info(_mavlink_fd, "#audio: got drop position and approval"); + mavlink_log_critical(_mavlink_fd, "got drop position and approval"); break; default: @@ -818,19 +817,19 @@ BottleDrop::answer_command(struct vehicle_command_s *cmd, enum VEHICLE_CMD_RESUL break; case VEHICLE_CMD_RESULT_DENIED: - mavlink_log_critical(_mavlink_fd, "#audio: command denied: %u", cmd->command); + mavlink_log_critical(_mavlink_fd, "command denied: %u", cmd->command); break; case VEHICLE_CMD_RESULT_FAILED: - mavlink_log_critical(_mavlink_fd, "#audio: command failed: %u", cmd->command); + mavlink_log_critical(_mavlink_fd, "command failed: %u", cmd->command); break; case VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED: - mavlink_log_critical(_mavlink_fd, "#audio: command temporarily rejected: %u", cmd->command); + mavlink_log_critical(_mavlink_fd, "command temporarily rejected: %u", cmd->command); break; case VEHICLE_CMD_RESULT_UNSUPPORTED: - mavlink_log_critical(_mavlink_fd, "#audio: command unsupported: %u", cmd->command); + mavlink_log_critical(_mavlink_fd, "command unsupported: %u", cmd->command); break; default: -- cgit v1.2.3 From beee7a89ed6e7a7c98ee712ab571c58eb6f3a692 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 15 Nov 2014 16:55:04 +0100 Subject: Airspeed: less chatty --- src/drivers/meas_airspeed/meas_airspeed.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp index 1d9a463ad..ba46de379 100644 --- a/src/drivers/meas_airspeed/meas_airspeed.cpp +++ b/src/drivers/meas_airspeed/meas_airspeed.cpp @@ -519,7 +519,7 @@ test() ret = poll(&fds, 1, 2000); if (ret != 1) { - errx(1, "timed out waiting for sensor data"); + errx(1, "timed out"); } /* now go get it */ -- cgit v1.2.3 From d602c9a0c5eff39e0f533411b45f631cae63e486 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 15 Nov 2014 16:55:18 +0100 Subject: Controllib: Optimize for size --- src/modules/controllib/module.mk | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/controllib/module.mk b/src/modules/controllib/module.mk index f0139a4b7..2860d1efc 100644 --- a/src/modules/controllib/module.mk +++ b/src/modules/controllib/module.mk @@ -39,3 +39,5 @@ SRCS = test_params.c \ block/BlockParam.cpp \ uorb/blocks.cpp \ blocks.cpp + +MAXOPTIMIZATION = -Os -- cgit v1.2.3 From 9c7503ba7a3f5d8f98ab3017d221192489d0b3c9 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 15 Nov 2014 18:38:33 +0100 Subject: uORB: Save space, does not do complex operations --- src/modules/uORB/module.mk | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/uORB/module.mk b/src/modules/uORB/module.mk index 0c29101fe..9385ce253 100644 --- a/src/modules/uORB/module.mk +++ b/src/modules/uORB/module.mk @@ -44,3 +44,5 @@ SRCS = uORB.cpp \ objects_common.cpp \ Publication.cpp \ Subscription.cpp + +MAXOPTIMIZATION = -Os -- cgit v1.2.3 From 488739cc467cea08374e97f96f70317376ec58ec Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 15 Nov 2014 19:13:46 +0100 Subject: Fix up ver command handling to print MCU version on all command as well --- src/systemcmds/ver/ver.c | 58 ++++++++++++++++++++++++++++-------------------- 1 file changed, 34 insertions(+), 24 deletions(-) diff --git a/src/systemcmds/ver/ver.c b/src/systemcmds/ver/ver.c index 62a7a5b92..eebeb9289 100644 --- a/src/systemcmds/ver/ver.c +++ b/src/systemcmds/ver/ver.c @@ -46,7 +46,7 @@ #include #include -// string constants for version commands +/* string constants for version commands */ static const char sz_ver_hw_str[] = "hw"; static const char sz_ver_hwcmp_str[] = "hwcmp"; static const char sz_ver_git_str[] = "git"; @@ -68,49 +68,56 @@ __EXPORT int ver_main(int argc, char *argv[]); int ver_main(int argc, char *argv[]) { - int ret = 1; //defaults to an error + /* defaults to an error */ + int ret = 1; - // first check if there are at least 2 params + /* first check if there are at least 2 params */ if (argc >= 2) { if (argv[1] != NULL) { - if (!strncmp(argv[1], sz_ver_hw_str, sizeof(sz_ver_hw_str))) { - printf("%s\n", HW_ARCH); - ret = 0; - } else if (!strncmp(argv[1], sz_ver_hwcmp_str, sizeof(sz_ver_hwcmp_str))) { + if (!strncmp(argv[1], sz_ver_hwcmp_str, sizeof(sz_ver_hwcmp_str))) { if (argc >= 3 && argv[2] != NULL) { - // compare 3rd parameter with HW_ARCH string, in case of match, return 0 + /* compare 3rd parameter with HW_ARCH string, in case of match, return 0 */ ret = strncmp(HW_ARCH, argv[2], strlen(HW_ARCH)); if (ret == 0) { printf("ver hwcmp match: %s\n", HW_ARCH); + return ret; } } else { errx(1, "Not enough arguments, try 'ver hwcmp PX4FMU_V2'"); } + } - } else if (!strncmp(argv[1], sz_ver_git_str, sizeof(sz_ver_git_str))) { - printf("%s\n", FW_GIT); - ret = 0; + /* check if we want to show all */ + bool show_all = !strncmp(argv[1], sz_ver_all_str, sizeof(sz_ver_all_str)) - } else if (!strncmp(argv[1], sz_ver_bdate_str, sizeof(sz_ver_bdate_str))) { - printf("%s %s\n", __DATE__, __TIME__); + if (show_all || !strncmp(argv[1], sz_ver_hw_str, sizeof(sz_ver_hw_str))) { + printf("HW arch: %s\n", HW_ARCH); ret = 0; - } else if (!strncmp(argv[1], sz_ver_gcc_str, sizeof(sz_ver_gcc_str))) { - printf("%s\n", __VERSION__); + } + + if (show_all || !strncmp(argv[1], sz_ver_git_str, sizeof(sz_ver_git_str))) { + printf("FW git-hash: %s\n", FW_GIT); ret = 0; - } else if (!strncmp(argv[1], sz_ver_all_str, sizeof(sz_ver_all_str))) { - printf("HW arch: %s\n", HW_ARCH); + } + + if (show_all || !strncmp(argv[1], sz_ver_bdate_str, sizeof(sz_ver_bdate_str))) { printf("Build datetime: %s %s\n", __DATE__, __TIME__); - printf("FW git-hash: %s\n", FW_GIT); - printf("GCC toolchain: %s\n", __VERSION__); ret = 0; + } - } else if (!strncmp(argv[1], mcu_ver_str, sizeof(mcu_ver_str))) { + if (show_all || !strncmp(argv[1], sz_ver_gcc_str, sizeof(sz_ver_gcc_str))) { + printf("Toolchain: %s\n", __VERSION__); + ret = 0; + + } + + if (show_all || !strncmp(argv[1], mcu_ver_str, sizeof(mcu_ver_str))) { char rev; char* revstr; @@ -118,21 +125,24 @@ int ver_main(int argc, char *argv[]) int chip_version = mcu_version(&rev, &revstr); if (chip_version < 0) { - printf("UNKNOWN MCU"); + printf("UNKNOWN MCU\n"); ret = 1; } else { printf("MCU: %s, rev. %c\n", revstr, rev); if (chip_version < MCU_REV_STM32F4_REV_3) { - printf("\n\nWARNING WARNING WARNING!\n" + printf("\nWARNING WARNING WARNING!\n" "Revision %c has a silicon errata\n" "This device can only utilize a maximum of 1MB flash safely!\n" - "http://px4.io/help/errata\n", rev); + "http://px4.io/help/errata\n\n", rev); } } - } else { + ret = 0; + } + + if (ret = 1) { errx(1, "unknown command.\n"); } -- cgit v1.2.3 From ed0f28ed4191c296aa3592088dc7875a8246cabb Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 15 Nov 2014 19:43:11 +0100 Subject: Fix ver command --- src/systemcmds/ver/ver.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/systemcmds/ver/ver.c b/src/systemcmds/ver/ver.c index eebeb9289..55cec149f 100644 --- a/src/systemcmds/ver/ver.c +++ b/src/systemcmds/ver/ver.c @@ -91,7 +91,7 @@ int ver_main(int argc, char *argv[]) } /* check if we want to show all */ - bool show_all = !strncmp(argv[1], sz_ver_all_str, sizeof(sz_ver_all_str)) + bool show_all = !strncmp(argv[1], sz_ver_all_str, sizeof(sz_ver_all_str)); if (show_all || !strncmp(argv[1], sz_ver_hw_str, sizeof(sz_ver_hw_str))) { printf("HW arch: %s\n", HW_ARCH); -- cgit v1.2.3 From e2551d491754b752b8b835a302acaf3f4cf1f9d6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 15 Nov 2014 20:05:54 +0100 Subject: Fixed typo in ver command --- src/systemcmds/ver/ver.c | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/systemcmds/ver/ver.c b/src/systemcmds/ver/ver.c index 55cec149f..c794f5819 100644 --- a/src/systemcmds/ver/ver.c +++ b/src/systemcmds/ver/ver.c @@ -126,7 +126,6 @@ int ver_main(int argc, char *argv[]) if (chip_version < 0) { printf("UNKNOWN MCU\n"); - ret = 1; } else { printf("MCU: %s, rev. %c\n", revstr, rev); @@ -142,7 +141,7 @@ int ver_main(int argc, char *argv[]) ret = 0; } - if (ret = 1) { + if (ret == 1) { errx(1, "unknown command.\n"); } -- cgit v1.2.3 From 18dc5e342905f6171b54adba6de8a27d65d62ab2 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 15 Nov 2014 20:07:32 +0100 Subject: Fix compile warning in MAVLink app --- src/modules/mavlink/mavlink_messages.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index b4911427f..bed915bbe 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -382,11 +382,11 @@ protected: clock_gettime(CLOCK_REALTIME, &ts); /* use GPS time for log file naming, e.g. /fs/microsd/2014-01-19/19_37_52.bin */ time_t gps_time_sec = ts.tv_sec + (ts.tv_nsec / 1e9); - struct tm t; - gmtime_r(&gps_time_sec, &t); + struct tm tt; + gmtime_r(&gps_time_sec, &tt); // XXX we do not want to interfere here with the SD log app - strftime(log_file_name, sizeof(log_file_name), "msgs_%Y_%m_%d_%H_%M_%S.txt", &t); + strftime(log_file_name, sizeof(log_file_name), "msgs_%Y_%m_%d_%H_%M_%S.txt", &tt); snprintf(log_file_path, sizeof(log_file_path), "/fs/microsd/%s", log_file_name); fp = fopen(log_file_path, "ab"); } -- cgit v1.2.3 From c0f32d44a2acd0f840b4246f90816a95dc6a7527 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 16 Nov 2014 00:15:40 +0100 Subject: First stab at animation --- src/modules/px4iofirmware/px4io.c | 40 ++++++++++++++++++++++++++++++++++++++- 1 file changed, 39 insertions(+), 1 deletion(-) diff --git a/src/modules/px4iofirmware/px4io.c b/src/modules/px4iofirmware/px4io.c index 30f32b38e..51281f660 100644 --- a/src/modules/px4iofirmware/px4io.c +++ b/src/modules/px4iofirmware/px4io.c @@ -125,7 +125,45 @@ heartbeat_blink(void) static bool heartbeat = false; LED_BLUE(heartbeat = !heartbeat); #ifdef GPIO_LED4 - LED_RING(heartbeat); + + const unsigned max_brightness = 1000; + + static unsigned counter = 0; + static unsigned brightness = max_brightness; + static unsigned brightness_counter = 0; + static unsigned on_counter = 0; + + if (brightness_counter < max_brightness) { + + bool on = ((on_counter * 100) / brightness_counter) <= ((brightness * 100) / max_brightness); + + LED_RING(on); + brightness_counter++; + + if (on) { + on_counter++; + } + + } else { + + if (counter >= 62) { + counter = 0; + } + + int n; + + if (counter < 32) { + n = counter; + + } else { + n = 62 - counter; + } + + brightness = n * n;// designed to be ~1000 / (31.0f * 31.0f); + brightness_counter = 0; + on_counter = 0; + } + #endif } -- cgit v1.2.3 From 63707ef058f01e324d8d74cf6bc48635e09ab507 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 16 Nov 2014 00:57:23 +0100 Subject: Let it breathe --- src/modules/px4iofirmware/px4io.c | 19 ++++++++++++++++--- 1 file changed, 16 insertions(+), 3 deletions(-) diff --git a/src/modules/px4iofirmware/px4io.c b/src/modules/px4iofirmware/px4io.c index 51281f660..8abfb0d2e 100644 --- a/src/modules/px4iofirmware/px4io.c +++ b/src/modules/px4iofirmware/px4io.c @@ -85,6 +85,9 @@ static volatile uint8_t msg_next_out, msg_next_in; #define NUM_MSG 2 static char msg[NUM_MSG][40]; +static void heartbeat_blink(void); +static void ring_blink(void); + /* * add a debug message to be printed on the console */ @@ -124,9 +127,16 @@ heartbeat_blink(void) { static bool heartbeat = false; LED_BLUE(heartbeat = !heartbeat); +} + +static void +ring_blink(void) +{ #ifdef GPIO_LED4 - const unsigned max_brightness = 1000; + // XXX this led code does have + // intentionally a few magic numbers. + const unsigned max_brightness = 118; static unsigned counter = 0; static unsigned brightness = max_brightness; @@ -135,7 +145,7 @@ heartbeat_blink(void) if (brightness_counter < max_brightness) { - bool on = ((on_counter * 100) / brightness_counter) <= ((brightness * 100) / max_brightness); + bool on = ((on_counter * 100) / brightness_counter+1) <= ((brightness * 100) / max_brightness+1); LED_RING(on); brightness_counter++; @@ -159,9 +169,10 @@ heartbeat_blink(void) n = 62 - counter; } - brightness = n * n;// designed to be ~1000 / (31.0f * 31.0f); + brightness = (n * n) / 9; brightness_counter = 0; on_counter = 0; + counter++; } #endif @@ -338,6 +349,8 @@ user_start(int argc, char *argv[]) heartbeat_blink(); } + ring_blink(); + check_reboot(); /* check for debug activity (default: none) */ -- cgit v1.2.3 From 8583cf4dcc0e88842cdf717de26a4f0daa63729a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 16 Nov 2014 00:59:18 +0100 Subject: Let it breathe stronger --- src/modules/px4iofirmware/px4io.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/px4iofirmware/px4io.c b/src/modules/px4iofirmware/px4io.c index 8abfb0d2e..349aaac57 100644 --- a/src/modules/px4iofirmware/px4io.c +++ b/src/modules/px4iofirmware/px4io.c @@ -169,7 +169,7 @@ ring_blink(void) n = 62 - counter; } - brightness = (n * n) / 9; + brightness = (n * n) / 8; brightness_counter = 0; on_counter = 0; counter++; -- cgit v1.2.3 From 8caada2188b9c9a8fcab70fbad92ab14506c2427 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 16 Nov 2014 01:21:47 +0100 Subject: Breathe animation, led solid on arming --- src/modules/px4iofirmware/px4io.c | 14 ++++++++++++-- src/modules/px4iofirmware/px4io.h | 2 +- 2 files changed, 13 insertions(+), 3 deletions(-) diff --git a/src/modules/px4iofirmware/px4io.c b/src/modules/px4iofirmware/px4io.c index 349aaac57..14ee9cb40 100644 --- a/src/modules/px4iofirmware/px4io.c +++ b/src/modules/px4iofirmware/px4io.c @@ -134,6 +134,12 @@ ring_blink(void) { #ifdef GPIO_LED4 + if (/* IO armed */ (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) + /* and FMU is armed */ && (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)) { + LED_RING(1); + return; + } + // XXX this led code does have // intentionally a few magic numbers. const unsigned max_brightness = 118; @@ -147,7 +153,11 @@ ring_blink(void) bool on = ((on_counter * 100) / brightness_counter+1) <= ((brightness * 100) / max_brightness+1); - LED_RING(on); + // XXX once led is PWM driven, + // remove the ! in the line below + // to return to the proper breathe + // animation / pattern (currently inverted) + LED_RING(!on); brightness_counter++; if (on) { @@ -349,7 +359,7 @@ user_start(int argc, char *argv[]) heartbeat_blink(); } - ring_blink(); + ring_blink(); check_reboot(); diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h index 8186e4c78..93a33490f 100644 --- a/src/modules/px4iofirmware/px4io.h +++ b/src/modules/px4iofirmware/px4io.h @@ -140,7 +140,7 @@ extern pwm_limit_t pwm_limit; #define LED_BLUE(_s) stm32_gpiowrite(GPIO_LED1, !(_s)) #define LED_AMBER(_s) stm32_gpiowrite(GPIO_LED2, !(_s)) #define LED_SAFETY(_s) stm32_gpiowrite(GPIO_LED3, !(_s)) -#define LED_RING(_s) stm32_gpiowrite(GPIO_LED4, !(_s)) +#define LED_RING(_s) stm32_gpiowrite(GPIO_LED4, (_s)) #ifdef CONFIG_ARCH_BOARD_PX4IO_V1 -- cgit v1.2.3 From 4e8387465e01cf16e0fbbe94b5e9a3c168540b77 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sun, 16 Nov 2014 13:38:41 +1000 Subject: navigator: only update sensor_combined topic with 50Hz --- src/modules/navigator/navigator_main.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index e933919fb..ed9e999a9 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -289,8 +289,9 @@ Navigator::task_main() navigation_capabilities_update(); params_update(); - /* rate limit position updates to 50 Hz */ + /* rate limit position and sensor updates to 50 Hz */ orb_set_interval(_global_pos_sub, 20); + orb_set_interval(_sensor_combined_sub, 20); hrt_abstime mavlink_open_time = 0; const hrt_abstime mavlink_open_interval = 500000; -- cgit v1.2.3 From 4d69314a9925be8ded1b9cb723bd01c21f01a816 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 16 Nov 2014 10:55:18 +0100 Subject: Adjust navigator priority --- src/modules/navigator/navigator_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index ed9e999a9..3ca6ac2c0 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -498,7 +498,7 @@ Navigator::start() /* start the task */ _navigator_task = task_spawn_cmd("navigator", SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 5, + SCHED_PRIORITY_DEFAULT + 20, 2000, (main_t)&Navigator::task_main_trampoline, nullptr); -- cgit v1.2.3 From 88bae21ce53fc6acadaae4c8271f3d0fce18a721 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 5 Nov 2014 21:20:32 +1100 Subject: px4io: added RC_CONFIG, SET_OVERRIDE_OK and CLEAR_OVERRIDE_OK ioctls this allows for full setup of RC override without needing param_get() to PX4 specific parameters --- src/drivers/drv_pwm_output.h | 26 ++++++++++++++++++++++++++ src/drivers/px4io/px4io.cpp | 36 ++++++++++++++++++++++++++++++++++++ 2 files changed, 62 insertions(+) diff --git a/src/drivers/drv_pwm_output.h b/src/drivers/drv_pwm_output.h index 6873f24b6..b41f088eb 100644 --- a/src/drivers/drv_pwm_output.h +++ b/src/drivers/drv_pwm_output.h @@ -117,6 +117,23 @@ struct pwm_output_values { unsigned channel_count; }; + +/** + * RC config values for a channel + * + * This allows for PX4IO_PAGE_RC_CONFIG values to be set without a + * param_get() dependency + */ +struct pwm_output_rc_config { + uint8_t channel; + uint16_t rc_min; + uint16_t rc_trim; + uint16_t rc_max; + uint16_t rc_dz; + uint16_t rc_assignment; + bool rc_reverse; +}; + /* * ORB tag for PWM outputs. */ @@ -216,6 +233,15 @@ ORB_DECLARE(output_pwm); /** force safety switch on (to enable use of safety switch) */ #define PWM_SERVO_SET_FORCE_SAFETY_ON _IOC(_PWM_SERVO_BASE, 26) +/** set RC config for a channel. This takes a pointer to pwm_output_rc_config */ +#define PWM_SERVO_SET_RC_CONFIG _IOC(_PWM_SERVO_BASE, 27) + +/** set the 'OVERRIDE OK' bit, which allows for RC control on FMU loss */ +#define PWM_SERVO_SET_OVERRIDE_OK _IOC(_PWM_SERVO_BASE, 28) + +/** clear the 'OVERRIDE OK' bit, which allows for RC control on FMU loss */ +#define PWM_SERVO_CLEAR_OVERRIDE_OK _IOC(_PWM_SERVO_BASE, 29) + /* * * diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 06faf49a4..dc33f7657 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -2572,6 +2572,42 @@ PX4IO::ioctl(file * filep, int cmd, unsigned long arg) break; + case PWM_SERVO_SET_RC_CONFIG: { + /* enable setting of RC configuration without relying + on param_get() + */ + struct pwm_output_rc_config* config = (struct pwm_output_rc_config*)arg; + if (config->channel >= _max_actuators) { + /* fail with error */ + return E2BIG; + } + + /* copy values to registers in IO */ + uint16_t regs[PX4IO_P_RC_CONFIG_STRIDE]; + uint16_t offset = config->channel * PX4IO_P_RC_CONFIG_STRIDE; + regs[PX4IO_P_RC_CONFIG_MIN] = config->rc_min; + regs[PX4IO_P_RC_CONFIG_CENTER] = config->rc_trim; + regs[PX4IO_P_RC_CONFIG_MAX] = config->rc_max; + regs[PX4IO_P_RC_CONFIG_DEADZONE] = config->rc_dz; + regs[PX4IO_P_RC_CONFIG_ASSIGNMENT] = config->rc_assignment; + regs[PX4IO_P_RC_CONFIG_OPTIONS] = PX4IO_P_RC_CONFIG_OPTIONS_ENABLED; + if (config->rc_reverse) { + regs[PX4IO_P_RC_CONFIG_OPTIONS] |= PX4IO_P_RC_CONFIG_OPTIONS_REVERSE; + } + ret = io_reg_set(PX4IO_PAGE_RC_CONFIG, offset, regs, PX4IO_P_RC_CONFIG_STRIDE); + break; + } + + case PWM_SERVO_SET_OVERRIDE_OK: + /* set the 'OVERRIDE OK' bit */ + ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, 0, PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK); + break; + + case PWM_SERVO_CLEAR_OVERRIDE_OK: + /* clear the 'OVERRIDE OK' bit */ + ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK, 0); + break; + default: /* see if the parent class can make any use of it */ ret = CDev::ioctl(filep, cmd, arg); -- cgit v1.2.3 From 6406e235d6f5cb37a914442036111f616b4b3839 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 6 Nov 2014 10:32:21 +1100 Subject: px4io: publish pwm values when STATUS_FLAGS_FMU_OK is not set this allows testing of FMU failure behaviour in px4io by monitoring the reported PWM output when the vehicle code stops sending updates. Otherwise testing needs to be done with "px4io status" which is very tedious. With this change a GCS can monitor the PWM outputs from the failsafe mixer using normal mavlink messages --- src/drivers/px4io/px4io.cpp | 4 ---- 1 file changed, 4 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index dc33f7657..6a313b322 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1657,10 +1657,6 @@ PX4IO::io_publish_raw_rc() int PX4IO::io_publish_pwm_outputs() { - /* if no FMU comms(!) just don't publish */ - if (!(_status & PX4IO_P_STATUS_FLAGS_FMU_OK)) - return OK; - /* data we are going to fetch */ actuator_outputs_s outputs; outputs.timestamp = hrt_absolute_time(); -- cgit v1.2.3 From ba811254536b6a080d4b3379106bf0e648a51eb0 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 5 Nov 2014 21:20:03 +1100 Subject: px4io: added OVERRIDE_IMMEDIATE arming flag this allows the flight code to choose whether FMU failure gives immediate manual pilot control, or waits for the mode switch to go past the override threshold --- src/drivers/drv_pwm_output.h | 3 +++ src/drivers/px4io/px4io.cpp | 18 ++++++++++++++++-- src/modules/px4iofirmware/controls.c | 9 +++++++++ src/modules/px4iofirmware/protocol.h | 1 + src/modules/px4iofirmware/registers.c | 3 ++- 5 files changed, 31 insertions(+), 3 deletions(-) diff --git a/src/drivers/drv_pwm_output.h b/src/drivers/drv_pwm_output.h index b41f088eb..edb72f04e 100644 --- a/src/drivers/drv_pwm_output.h +++ b/src/drivers/drv_pwm_output.h @@ -242,6 +242,9 @@ ORB_DECLARE(output_pwm); /** clear the 'OVERRIDE OK' bit, which allows for RC control on FMU loss */ #define PWM_SERVO_CLEAR_OVERRIDE_OK _IOC(_PWM_SERVO_BASE, 29) +/** setup OVERRIDE_IMMEDIATE behaviour on FMU fail */ +#define PWM_SERVO_SET_OVERRIDE_IMMEDIATE _IOC(_PWM_SERVO_BASE, 30) + /* * * diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 6a313b322..519ba663a 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -2057,7 +2057,7 @@ PX4IO::print_status(bool extended_status) ((features & PX4IO_P_SETUP_FEATURES_ADC_RSSI) ? " RSSI_ADC" : "") ); uint16_t arming = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING); - printf("arming 0x%04x%s%s%s%s%s%s%s%s\n", + printf("arming 0x%04x%s%s%s%s%s%s%s%s%s%s\n", arming, ((arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) ? " FMU_ARMED" : " FMU_DISARMED"), ((arming & PX4IO_P_SETUP_ARMING_IO_ARM_OK) ? " IO_ARM_OK" : " IO_ARM_DENIED"), @@ -2067,7 +2067,8 @@ PX4IO::print_status(bool extended_status) ((arming & PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE) ? " ALWAYS_PWM_ENABLE" : ""), ((arming & PX4IO_P_SETUP_ARMING_LOCKDOWN) ? " LOCKDOWN" : ""), ((arming & PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE) ? " FORCE_FAILSAFE" : ""), - ((arming & PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE) ? " TERM_FAILSAFE" : "") + ((arming & PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE) ? " TERM_FAILSAFE" : ""), + ((arming & PX4IO_P_SETUP_ARMING_OVERRIDE_IMMEDIATE) ? " OVERRIDE_IMMEDIATE" : "") ); #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 printf("rates 0x%04x default %u alt %u relays 0x%04x\n", @@ -2307,6 +2308,19 @@ PX4IO::ioctl(file * filep, int cmd, unsigned long arg) } break; + case PWM_SERVO_SET_OVERRIDE_IMMEDIATE: + /* control whether override on FMU failure is + immediate or waits for override threshold on mode + switch */ + if (arg == 0) { + /* clear override immediate flag */ + ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, PX4IO_P_SETUP_ARMING_OVERRIDE_IMMEDIATE, 0); + } else { + /* set override immediate flag */ + ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, 0, PX4IO_P_SETUP_ARMING_OVERRIDE_IMMEDIATE); + } + break; + case DSM_BIND_START: /* only allow DSM2, DSM-X and DSM-X with more than 7 channels */ diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c index ad60ee03e..e3cb8d820 100644 --- a/src/modules/px4iofirmware/controls.c +++ b/src/modules/px4iofirmware/controls.c @@ -417,6 +417,15 @@ controls_tick() { if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) && (REG_TO_SIGNED(rc_value_override) < RC_CHANNEL_LOW_THRESH)) override = true; + /* + if the FMU is dead then enable override if we have a + mixer and OVERRIDE_IMMEDIATE is set + */ + if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) && + (r_setup_arming & PX4IO_P_SETUP_ARMING_OVERRIDE_IMMEDIATE) && + (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) + override = true; + if (override) { r_status_flags |= PX4IO_P_STATUS_FLAGS_OVERRIDE; diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h index 9b2e047cb..c7e9ae3eb 100644 --- a/src/modules/px4iofirmware/protocol.h +++ b/src/modules/px4iofirmware/protocol.h @@ -183,6 +183,7 @@ #define PX4IO_P_SETUP_ARMING_LOCKDOWN (1 << 7) /* If set, the system operates normally, but won't actuate any servos */ #define PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE (1 << 8) /* If set, the system will always output the failsafe values */ #define PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE (1 << 9) /* If set, the system will never return from a failsafe, but remain in failsafe once triggered. */ +#define PX4IO_P_SETUP_ARMING_OVERRIDE_IMMEDIATE (1 << 10) /* If set then on FMU failure override is immediate. Othewise it waits for the mode switch to go past the override thrshold */ #define PX4IO_P_SETUP_PWM_RATES 2 /* bitmask, 0 = low rate, 1 = high rate */ #define PX4IO_P_SETUP_PWM_DEFAULTRATE 3 /* 'low' PWM frame output rate in Hz */ diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index 49c2a9f56..fbfdd35db 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -191,7 +191,8 @@ volatile uint16_t r_page_setup[] = PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED | \ PX4IO_P_SETUP_ARMING_LOCKDOWN | \ PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE | \ - PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE) + PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE | \ + PX4IO_P_SETUP_ARMING_OVERRIDE_IMMEDIATE) #define PX4IO_P_SETUP_RATES_VALID ((1 << PX4IO_SERVO_COUNT) - 1) #define PX4IO_P_SETUP_RELAYS_VALID ((1 << PX4IO_RELAY_CHANNELS) - 1) -- cgit v1.2.3 From f533c362515e31b536c34d83342c396e54a7d652 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 5 Nov 2014 21:20:32 +1100 Subject: px4io: added RC_CONFIG, SET_OVERRIDE_OK and CLEAR_OVERRIDE_OK ioctls this allows for full setup of RC override without needing param_get() to PX4 specific parameters Conflicts: src/drivers/drv_pwm_output.h --- src/drivers/drv_pwm_output.h | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/drivers/drv_pwm_output.h b/src/drivers/drv_pwm_output.h index edb72f04e..b10c3e18a 100644 --- a/src/drivers/drv_pwm_output.h +++ b/src/drivers/drv_pwm_output.h @@ -231,16 +231,16 @@ ORB_DECLARE(output_pwm); #define PWM_SERVO_SET_TERMINATION_FAILSAFE _IOC(_PWM_SERVO_BASE, 25) /** force safety switch on (to enable use of safety switch) */ -#define PWM_SERVO_SET_FORCE_SAFETY_ON _IOC(_PWM_SERVO_BASE, 26) +#define PWM_SERVO_SET_FORCE_SAFETY_ON _IOC(_PWM_SERVO_BASE, 26) /** set RC config for a channel. This takes a pointer to pwm_output_rc_config */ -#define PWM_SERVO_SET_RC_CONFIG _IOC(_PWM_SERVO_BASE, 27) +#define PWM_SERVO_SET_RC_CONFIG _IOC(_PWM_SERVO_BASE, 27) /** set the 'OVERRIDE OK' bit, which allows for RC control on FMU loss */ -#define PWM_SERVO_SET_OVERRIDE_OK _IOC(_PWM_SERVO_BASE, 28) +#define PWM_SERVO_SET_OVERRIDE_OK _IOC(_PWM_SERVO_BASE, 28) /** clear the 'OVERRIDE OK' bit, which allows for RC control on FMU loss */ -#define PWM_SERVO_CLEAR_OVERRIDE_OK _IOC(_PWM_SERVO_BASE, 29) +#define PWM_SERVO_CLEAR_OVERRIDE_OK _IOC(_PWM_SERVO_BASE, 29) /** setup OVERRIDE_IMMEDIATE behaviour on FMU fail */ #define PWM_SERVO_SET_OVERRIDE_IMMEDIATE _IOC(_PWM_SERVO_BASE, 30) -- cgit v1.2.3 From 852fa611730be2433156b010c6162606ec857ef3 Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Sun, 16 Nov 2014 14:12:58 -0800 Subject: Correct setting of DSM and ST24 flags --- src/modules/px4iofirmware/controls.c | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c index 3fd73fc60..75d80ba5d 100644 --- a/src/modules/px4iofirmware/controls.c +++ b/src/modules/px4iofirmware/controls.c @@ -71,7 +71,6 @@ bool dsm_port_input(uint16_t *rssi, bool *dsm_updated, bool *st24_updated) uint8_t *bytes; *dsm_updated = dsm_input(r_raw_rc_values, &temp_count, &n_bytes, &bytes); if (*dsm_updated) { - r_raw_rc_flags |= PX4IO_P_STATUS_FLAGS_RC_DSM; r_raw_rc_count = temp_count & 0x7fff; if (temp_count & 0x8000) r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_RC_DSM11; @@ -172,6 +171,12 @@ controls_tick() { perf_begin(c_gather_dsm); bool dsm_updated, st24_updated; (void)dsm_port_input(&rssi, &dsm_updated, &st24_updated); + if (dsm_updated) { + r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_DSM; + } + if (st24_updated) { + r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_ST24; + } perf_end(c_gather_dsm); perf_begin(c_gather_sbus); -- cgit v1.2.3 From 52d5a7c00aa74fe0d4241f7e6636dfd6e48d0acf Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 16 Nov 2014 23:24:52 +0100 Subject: Fix PWM command --- src/systemcmds/pwm/pwm.c | 68 ++++++++++++++++++++++++------------------------ 1 file changed, 34 insertions(+), 34 deletions(-) diff --git a/src/systemcmds/pwm/pwm.c b/src/systemcmds/pwm/pwm.c index 478c2a772..eeba89fa8 100644 --- a/src/systemcmds/pwm/pwm.c +++ b/src/systemcmds/pwm/pwm.c @@ -74,34 +74,34 @@ usage(const char *reason) "usage:\n" "pwm arm|disarm|rate|failsafe|disarmed|min|max|test|info ...\n" "\n" - " arm Arm output\n" - " disarm Disarm output\n" + "arm\t\t\t\tArm output\n" + "disarm\t\t\t\tDisarm output\n" "\n" - " rate ... Configure PWM rates\n" - " [-g ] Channel group that should update at the alternate rate\n" - " [-m ] Directly supply channel mask\n" - " [-a] Configure all outputs\n" - " -r PWM rate (50 to 400 Hz)\n" + "rate ...\t\t\tConfigure PWM rates\n" + "\t[-g ]\t(e.g. 0,1,2)\n" + "\t[-m ]\t(e.g. 0xF)\n" + "\t[-a]\t\t\tConfigure all outputs\n" + "\t-r \t\tPWM rate (50 to 400 Hz)\n" "\n" - " failsafe ... Configure failsafe PWM values\n" - " disarmed ... Configure disarmed PWM values\n" - " min ... Configure minimum PWM values\n" - " max ... Configure maximum PWM values\n" - " [-c ] Supply channels (e.g. 1234)\n" - " [-m ] Directly supply channel mask (e.g. 0xF)\n" - " [-a] Configure all outputs\n" - " -p PWM value\n" + "failsafe ...\t\t\tFailsafe PWM\n" + "disarmed ...\t\t\tDisarmed PWM\n" + "min ...\t\t\t\tMinimum PWM\n" + "max ...\t\t\t\tMaximum PWM\n" + "\t[-c ]\t\t(e.g. 1234)\n" + "\t[-m ]\t(e.g. 0xF)\n" + "\t[-a]\t\t\tConfigure all outputs\n" + "\t-p \t\tPWM value\n" "\n" - " test ... Directly set PWM values\n" - " [-c ] Supply channels (e.g. 1234)\n" - " [-m ] Directly supply channel mask (e.g. 0xF)\n" - " [-a] Configure all outputs\n" - " -p PWM value\n" + "test ...\t\t\tDirectly set PWM\n" + "\t[-c ]\t\t(e.g. 1234)\n" + "\t[-m ]\t(e.g. 0xF)\n" + "\t[-a]\t\t\tConfigure all outputs\n" + "\t-p \t\tPWM value\n" "\n" - " info Print information about the PWM device\n" + "info\t\t\t\tPrint information\n" "\n" - " -v Print verbose information\n" - " -d PWM output device (defaults to " PWM_OUTPUT_DEVICE_PATH ")\n" + "\t-v\t\t\tVerbose\n" + "\t-d \t\t(default " PWM_OUTPUT_DEVICE_PATH ")\n" ); } @@ -123,7 +123,7 @@ pwm_main(int argc, char *argv[]) unsigned single_ch = 0; unsigned pwm_value = 0; - if (argc < 1) + if (argc < 2) usage(NULL); while ((ch = getopt(argc-1, &argv[1], "d:vc:g:m:ap:r:")) != EOF) { @@ -165,7 +165,7 @@ pwm_main(int argc, char *argv[]) /* Read in mask directly */ set_mask = strtoul(optarg, &ep, 0); if (*ep != '\0') - usage("bad set_mask value"); + usage("BAD set_mask VAL"); break; case 'a': @@ -176,12 +176,12 @@ pwm_main(int argc, char *argv[]) case 'p': pwm_value = strtoul(optarg, &ep, 0); if (*ep != '\0') - usage("bad PWM value provided"); + usage("BAD PWM VAL"); break; case 'r': alt_rate = strtoul(optarg, &ep, 0); if (*ep != '\0') - usage("bad alternative rate provided"); + usage("BAD rate VAL"); break; default: break; @@ -189,7 +189,7 @@ pwm_main(int argc, char *argv[]) } if (print_verbose && set_mask > 0) { - warnx("Chose channels: "); + warnx("Channels: "); printf(" "); for (unsigned i = 0; i Date: Mon, 17 Nov 2014 14:07:13 +0100 Subject: Fix output of ver hwcmd call --- src/systemcmds/ver/ver.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/systemcmds/ver/ver.c b/src/systemcmds/ver/ver.c index c794f5819..2ead3e632 100644 --- a/src/systemcmds/ver/ver.c +++ b/src/systemcmds/ver/ver.c @@ -82,8 +82,8 @@ int ver_main(int argc, char *argv[]) if (ret == 0) { printf("ver hwcmp match: %s\n", HW_ARCH); - return ret; } + return ret; } else { errx(1, "Not enough arguments, try 'ver hwcmp PX4FMU_V2'"); -- cgit v1.2.3 From f20f85f0e3c61ce754fd6d9d73745eaa925b3b04 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 17 Nov 2014 14:07:33 +0100 Subject: Do not spam filter resets in static mode --- src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index 685f5e12f..00900c995 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 @@ -613,8 +613,11 @@ FixedwingEstimator::check_filter_state() warn_index = max_warn_index; } - warnx("reset: %s", feedback[warn_index]); - mavlink_log_critical(_mavlink_fd, "[ekf check] %s", feedback[warn_index]); + // Do not warn about accel offset if we have no position updates + if (!(warn_index == 5 && _ekf->staticMode)) { + warnx("reset: %s", feedback[warn_index]); + mavlink_log_critical(_mavlink_fd, "[ekf check] %s", feedback[warn_index]); + } } struct estimator_status_report rep; -- cgit v1.2.3 From 41fe04776f362cf9f0fae6451550df8d5eaf8d99 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 17 Nov 2014 14:59:36 +0100 Subject: Fix up stack sizes --- src/modules/bottle_drop/bottle_drop.cpp | 2 +- src/modules/bottle_drop/module.mk | 2 ++ src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp | 2 +- src/modules/mavlink/mavlink_main.cpp | 2 +- src/modules/navigator/navigator_main.cpp | 2 +- 5 files changed, 6 insertions(+), 4 deletions(-) diff --git a/src/modules/bottle_drop/bottle_drop.cpp b/src/modules/bottle_drop/bottle_drop.cpp index 6d24e5d2d..e0bcbc6e9 100644 --- a/src/modules/bottle_drop/bottle_drop.cpp +++ b/src/modules/bottle_drop/bottle_drop.cpp @@ -223,7 +223,7 @@ BottleDrop::start() _main_task = task_spawn_cmd("bottle_drop", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT + 15, - 2048, + 1500, (main_t)&BottleDrop::task_main_trampoline, nullptr); diff --git a/src/modules/bottle_drop/module.mk b/src/modules/bottle_drop/module.mk index 960228879..df9f5473b 100644 --- a/src/modules/bottle_drop/module.mk +++ b/src/modules/bottle_drop/module.mk @@ -41,3 +41,5 @@ SRCS = bottle_drop.cpp \ bottle_drop_params.c MAXOPTIMIZATION = -Os + +MODULE_STACKSIZE = 1200 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 00900c995..e7805daa9 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 @@ -1560,7 +1560,7 @@ FixedwingEstimator::start() _estimator_task = task_spawn_cmd("ekf_att_pos_estimator", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 40, - 5000, + 7500, (main_t)&FixedwingEstimator::task_main_trampoline, nullptr); diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index c251dd3b2..fb9f65cf5 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1636,7 +1636,7 @@ Mavlink::start(int argc, char *argv[]) task_spawn_cmd(buf, SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, - 2900, + 2800, (main_t)&Mavlink::start_helper, (const char **)argv); diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 3ca6ac2c0..10a4ee88f 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -499,7 +499,7 @@ Navigator::start() _navigator_task = task_spawn_cmd("navigator", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT + 20, - 2000, + 1800, (main_t)&Navigator::task_main_trampoline, nullptr); -- cgit v1.2.3 From 97a1410ec99e880207e4ee6d2a03451c2e11f4cf Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 12 Nov 2014 09:03:50 +0100 Subject: Toolchain: Allow GCC 4.7 and 4.8 variants --- makefiles/toolchain_gnu-arm-eabi.mk | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/makefiles/toolchain_gnu-arm-eabi.mk b/makefiles/toolchain_gnu-arm-eabi.mk index 71c7fb49f..84e5cd08a 100644 --- a/makefiles/toolchain_gnu-arm-eabi.mk +++ b/makefiles/toolchain_gnu-arm-eabi.mk @@ -50,11 +50,11 @@ OBJDUMP = $(CROSSDEV)objdump # Check if the right version of the toolchain is available # -CROSSDEV_VER_SUPPORTED = 4.7 +CROSSDEV_VER_SUPPORTED = 4.7.4 4.7.5 4.7.6 4.8.4 CROSSDEV_VER_FOUND = $(shell $(CC) -dumpversion) -ifeq (,$(findstring $(CROSSDEV_VER_SUPPORTED),$(CROSSDEV_VER_FOUND))) -$(error Unsupported version of $(CC), found: $(CROSSDEV_VER_FOUND) instead of $(CROSSDEV_VER_SUPPORTED).x) +ifeq (,$(findstring $(CROSSDEV_VER_FOUND), $(CROSSDEV_VER_SUPPORTED))) +$(error Unsupported version of $(CC), found: $(CROSSDEV_VER_FOUND) instead of one in: $(CROSSDEV_VER_SUPPORTED)) endif -- cgit v1.2.3 From 5f6d03099e07e20abe9cec685fee1244d7c9a0d4 Mon Sep 17 00:00:00 2001 From: Steven Blass Date: Mon, 17 Nov 2014 19:58:18 -0500 Subject: fixed yaw/yawrate bit masking. fixed navigator overriding offboard setpoint --- src/modules/mavlink/mavlink_receiver.cpp | 4 ++-- src/modules/navigator/navigator_main.cpp | 7 ++++--- 2 files changed, 6 insertions(+), 5 deletions(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index bc092c7e9..c0a8d7322 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -540,10 +540,10 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t } 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; + (OFB_IGN_BIT_YAW - 10); 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; + (OFB_IGN_BIT_YAWRATE - 11); offboard_control_sp.timestamp = hrt_absolute_time(); diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 10a4ee88f..5e9806499 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -468,9 +468,10 @@ Navigator::task_main() _navigation_mode_array[i]->run(_navigation_mode == _navigation_mode_array[i]); } - /* if nothing is running, set position setpoint triplet invalid */ - if (_navigation_mode == nullptr) { - // TODO publish empty sp only once + /* if nothing is running, set position setpoint triplet invalid once */ + static bool published_once = false; + if (_navigation_mode == nullptr && !published_once) { + published_once = true; _pos_sp_triplet.previous.valid = false; _pos_sp_triplet.current.valid = false; _pos_sp_triplet.next.valid = false; -- cgit v1.2.3 From 4eb930f704018253a4cff3604cb9faee8468283b Mon Sep 17 00:00:00 2001 From: Steven Blass Date: Tue, 18 Nov 2014 08:26:29 -0500 Subject: improved readability of offboard fixes --- src/modules/mavlink/mavlink_receiver.cpp | 12 ++++++++---- src/modules/navigator/navigator.h | 1 + src/modules/navigator/navigator_main.cpp | 6 +++--- 3 files changed, 12 insertions(+), 7 deletions(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index c0a8d7322..ca00d1a67 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -538,12 +538,16 @@ 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 - 10); + if (set_position_target_local_ned.type_mask & (1 << 10)) { + offboard_control_sp.ignore |= (1 << 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 - 11); + if (set_position_target_local_ned.type_mask & (1 << 11)) { + offboard_control_sp.ignore |= (1 << OFB_IGN_BIT_YAWRATE); + } offboard_control_sp.timestamp = hrt_absolute_time(); diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index d550dcc4c..9cd609955 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -205,6 +205,7 @@ private: bool _can_loiter_at_sp; /**< flags if current position SP can be used to loiter */ bool _pos_sp_triplet_updated; /**< flags if position SP triplet needs to be published */ + bool _pos_sp_triplet_published_invalid_once; /**< flags if position SP triplet has been published once to UORB */ control::BlockParamFloat _param_loiter_radius; /**< loiter radius for fixedwing */ control::BlockParamFloat _param_acceptance_radius; /**< acceptance for takeoff */ diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 5e9806499..035b0d18c 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -137,6 +137,7 @@ Navigator::Navigator() : _gpsFailure(this, "GPSF"), _can_loiter_at_sp(false), _pos_sp_triplet_updated(false), + _pos_sp_triplet_published_invalid_once(false), _param_loiter_radius(this, "LOITER_RAD"), _param_acceptance_radius(this, "ACC_RAD"), _param_datalinkloss_obc(this, "DLL_OBC"), @@ -469,9 +470,8 @@ Navigator::task_main() } /* if nothing is running, set position setpoint triplet invalid once */ - static bool published_once = false; - if (_navigation_mode == nullptr && !published_once) { - published_once = true; + if (_navigation_mode == nullptr && !_pos_sp_triplet_published_invalid_once) { + _pos_sp_triplet_published_invalid_once = true; _pos_sp_triplet.previous.valid = false; _pos_sp_triplet.current.valid = false; _pos_sp_triplet.next.valid = false; -- cgit v1.2.3 From 69271a7251a0bcfd980c3534194b73fc45fa5f23 Mon Sep 17 00:00:00 2001 From: Steven Blass Date: Tue, 18 Nov 2014 08:40:52 -0500 Subject: made invalid setpoints publish once every time it enters an invalid state --- src/modules/navigator/navigator_main.cpp | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 035b0d18c..df620e5e7 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -428,12 +428,15 @@ Navigator::task_main() _can_loiter_at_sp = false; break; case NAVIGATION_STATE_AUTO_MISSION: + _pos_sp_triplet_published_invalid_once = false; _navigation_mode = &_mission; break; case NAVIGATION_STATE_AUTO_LOITER: + _pos_sp_triplet_published_invalid_once = false; _navigation_mode = &_loiter; break; case NAVIGATION_STATE_AUTO_RCRECOVER: + _pos_sp_triplet_published_invalid_once = false; if (_param_rcloss_obc.get() != 0) { _navigation_mode = &_rcLoss; } else { @@ -441,11 +444,13 @@ Navigator::task_main() } break; case NAVIGATION_STATE_AUTO_RTL: - _navigation_mode = &_rtl; + _pos_sp_triplet_published_invalid_once = false; + _navigation_mode = &_rtl; break; case NAVIGATION_STATE_AUTO_RTGS: /* Use complex data link loss mode only when enabled via param * otherwise use rtl */ + _pos_sp_triplet_published_invalid_once = false; if (_param_datalinkloss_obc.get() != 0) { _navigation_mode = &_dataLinkLoss; } else { @@ -453,9 +458,11 @@ Navigator::task_main() } break; case NAVIGATION_STATE_AUTO_LANDENGFAIL: + _pos_sp_triplet_published_invalid_once = false; _navigation_mode = &_engineFailure; break; case NAVIGATION_STATE_AUTO_LANDGPSFAIL: + _pos_sp_triplet_published_invalid_once = false; _navigation_mode = &_gpsFailure; break; default: -- cgit v1.2.3 From 2ce2d26d52884f868ecbcb102f38cbc853c2dbf6 Mon Sep 17 00:00:00 2001 From: Holger Steinhaus Date: Tue, 18 Nov 2014 14:58:04 +0100 Subject: UAVCAN: preserve original UAVCAN message timestamps --- src/modules/uavcan/sensors/baro.cpp | 6 +----- src/modules/uavcan/sensors/gnss.cpp | 2 +- src/modules/uavcan/sensors/gnss.hpp | 2 -- src/modules/uavcan/sensors/mag.cpp | 3 +-- 4 files changed, 3 insertions(+), 10 deletions(-) diff --git a/src/modules/uavcan/sensors/baro.cpp b/src/modules/uavcan/sensors/baro.cpp index 80c5e3828..8741ae20d 100644 --- a/src/modules/uavcan/sensors/baro.cpp +++ b/src/modules/uavcan/sensors/baro.cpp @@ -91,11 +91,7 @@ void UavcanBarometerBridge::air_data_sub_cb(const uavcan::ReceivedDataStructure< { auto report = ::baro_report(); - report.timestamp = msg.getUtcTimestamp().toUSec(); - if (report.timestamp == 0) { - report.timestamp = msg.getMonotonicTimestamp().toUSec(); - } - + report.timestamp = msg.getMonotonicTimestamp().toUSec(); report.temperature = msg.static_temperature; report.pressure = msg.static_pressure / 100.0F; // Convert to millibar diff --git a/src/modules/uavcan/sensors/gnss.cpp b/src/modules/uavcan/sensors/gnss.cpp index 24afe6aaf..a375db37f 100644 --- a/src/modules/uavcan/sensors/gnss.cpp +++ b/src/modules/uavcan/sensors/gnss.cpp @@ -92,7 +92,7 @@ void UavcanGnssBridge::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure - #include #include diff --git a/src/modules/uavcan/sensors/mag.cpp b/src/modules/uavcan/sensors/mag.cpp index 5cbb96433..35ebee542 100644 --- a/src/modules/uavcan/sensors/mag.cpp +++ b/src/modules/uavcan/sensors/mag.cpp @@ -37,7 +37,6 @@ #include "mag.hpp" -#include #include static const orb_id_t MAG_TOPICS[3] = { @@ -140,7 +139,7 @@ void UavcanMagnetometerBridge::mag_sub_cb(const uavcan::ReceivedDataStructure Date: Tue, 18 Nov 2014 17:20:50 -0800 Subject: Parameter xml metadata in .px4 --- Tools/px_mkfw.py | 5 +++++ makefiles/config_px4fmu-v1_default.mk | 3 +++ makefiles/config_px4fmu-v2_default.mk | 3 +++ makefiles/firmware.mk | 9 +++++++++ 4 files changed, 20 insertions(+) diff --git a/Tools/px_mkfw.py b/Tools/px_mkfw.py index b598a65a1..c2da8a203 100755 --- a/Tools/px_mkfw.py +++ b/Tools/px_mkfw.py @@ -73,6 +73,7 @@ parser.add_argument("--version", action="store", help="set a version string") parser.add_argument("--summary", action="store", help="set a brief description") parser.add_argument("--description", action="store", help="set a longer description") parser.add_argument("--git_identity", action="store", help="the working directory to check for git identity") +parser.add_argument("--parameter_xml", action="store", help="the parameters.xml file") parser.add_argument("--image", action="store", help="the firmware image") args = parser.parse_args() @@ -101,6 +102,10 @@ if args.git_identity != None: p = subprocess.Popen(cmd, shell=True, stdout=subprocess.PIPE).stdout desc['git_identity'] = str(p.read().strip()) p.close() +if args.parameter_xml != None: + f = open(args.parameter_xml, "rb") + bytes = f.read() + desc['parameter_xml'] = base64.b64encode(zlib.compress(bytes,9)).decode('utf-8') if args.image != None: f = open(args.image, "rb") bytes = f.read() diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk index 9fe16fbb6..4507b506c 100644 --- a/makefiles/config_px4fmu-v1_default.mk +++ b/makefiles/config_px4fmu-v1_default.mk @@ -132,6 +132,9 @@ MODULES += lib/launchdetection # Hardware test #MODULES += examples/hwtest +# Generate parameter XML file +GEN_PARAM_XML = 1 + # # Transitional support - add commands from the NuttX export archive. # diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index 3c65b19e0..d3b8ee93e 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -141,6 +141,9 @@ MODULES += modules/bottle_drop # Hardware test #MODULES += examples/hwtest +# Generate parameter XML file +GEN_PARAM_XML = 1 + # # Transitional support - add commands from the NuttX export archive. # diff --git a/makefiles/firmware.mk b/makefiles/firmware.mk index 60602e76f..21e8739aa 100644 --- a/makefiles/firmware.mk +++ b/makefiles/firmware.mk @@ -467,6 +467,7 @@ endif PRODUCT_BUNDLE = $(WORK_DIR)firmware.px4 PRODUCT_BIN = $(WORK_DIR)firmware.bin PRODUCT_ELF = $(WORK_DIR)firmware.elf +PRODUCT_PARAMXML = $(WORK_DIR)/parameters.xml .PHONY: firmware firmware: $(PRODUCT_BUNDLE) @@ -497,9 +498,17 @@ $(filter %.S.o,$(OBJS)): $(WORK_DIR)%.S.o: %.S $(GLOBAL_DEPS) $(PRODUCT_BUNDLE): $(PRODUCT_BIN) @$(ECHO) %% Generating $@ +ifdef GEN_PARAM_XML + python $(PX4_BASE)/Tools/px_process_params.py --src-path $(PX4_BASE)/src --xml $(Q) $(MKFW) --prototype $(IMAGE_DIR)/$(BOARD).prototype \ --git_identity $(PX4_BASE) \ + --parameter_xml $(PRODUCT_PARAMXML) \ --image $< > $@ +else + $(Q) $(MKFW) --prototype $(IMAGE_DIR)/$(BOARD).prototype \ + --git_identity $(PX4_BASE) \ + --image $< > $@ +endif $(PRODUCT_BIN): $(PRODUCT_ELF) $(call SYM_TO_BIN,$<,$@) -- cgit v1.2.3 From 5c34f03c4ef1d0b928c8ad32f8038bb15dba7c11 Mon Sep 17 00:00:00 2001 From: philipoe Date: Thu, 20 Nov 2014 17:25:30 +0100 Subject: commander: Change printing in RC-loss message to integers --- src/modules/commander/commander.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index d8ebee4b6..1f13aaec4 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1499,7 +1499,7 @@ int commander_thread_main(int argc, char *argv[]) } else { if (status.rc_signal_lost) { - mavlink_log_critical(mavlink_fd, "RC SIGNAL REGAINED after %.3fs (at t=%.3fs)",(double)(hrt_absolute_time()-status.rc_signal_lost_timestamp)/1.0e6,(double)(hrt_absolute_time())/1.0e6); + mavlink_log_critical(mavlink_fd, "RC SIGNAL REGAINED after %llums (at t=%llums)",(hrt_absolute_time()-status.rc_signal_lost_timestamp)/1000,hrt_absolute_time()/1000); status_changed = true; } } @@ -1592,7 +1592,7 @@ int commander_thread_main(int argc, char *argv[]) } else { if (!status.rc_signal_lost) { - mavlink_log_critical(mavlink_fd, "RC SIGNAL LOST (at t=%.3fs)",(double)(hrt_absolute_time())/1.0e6); + mavlink_log_critical(mavlink_fd, "RC SIGNAL LOST (at t=%llums)",hrt_absolute_time()/1000); status.rc_signal_lost = true; status.rc_signal_lost_timestamp=sp_man.timestamp; status_changed = true; -- cgit v1.2.3 From 9bb0ecf0ca6082355072af190018e0b5298b7e59 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 22 Nov 2014 15:06:32 +0100 Subject: Airspeed calibration feedback: Improve wording --- src/modules/commander/state_machine_helper.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index e37019d02..465f9cdc5 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -706,7 +706,7 @@ int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd) } if (fabsf(airspeed.indicated_airspeed_m_s > 6.0f)) { - mavlink_log_critical(mavlink_fd, "AIRSPEED WARNING: WIND OR CALIBRATION MISSING"); + mavlink_log_critical(mavlink_fd, "AIRSPEED WARNING: WIND OR CALIBRATION ISSUE"); // XXX do not make this fatal yet } } -- cgit v1.2.3 From 6da9063560b900c0ce13bb0e078889eeaf8c71c0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 22 Nov 2014 15:08:54 +0100 Subject: Fix FD for commander arm operation --- src/modules/commander/commander.cpp | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index ec173c12b..e081955d0 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -310,12 +310,16 @@ int commander_main(int argc, char *argv[]) } if (!strcmp(argv[1], "arm")) { - arm_disarm(true, mavlink_fd, "command line"); + int mavlink_fd_local = open(MAVLINK_LOG_DEVICE, 0); + arm_disarm(true, mavlink_fd_local, "command line"); + close(mavlink_fd_local); exit(0); } if (!strcmp(argv[1], "disarm")) { - arm_disarm(false, mavlink_fd, "command line"); + int mavlink_fd_local = open(MAVLINK_LOG_DEVICE, 0); + arm_disarm(false, mavlink_fd_local, "command line"); + close(mavlink_fd_local); exit(0); } -- cgit v1.2.3 From 366c8a9c41e22ff31a8e027c5f931b99b7569a7d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 22 Nov 2014 15:43:21 +0100 Subject: ROMFS: Do only output necessary information on boot --- ROMFS/px4fmu_common/init.d/rc.interface | 6 +----- ROMFS/px4fmu_common/init.d/rcS | 1 - 2 files changed, 1 insertion(+), 6 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.interface b/ROMFS/px4fmu_common/init.d/rc.interface index e44cd0953..41e0b149b 100644 --- a/ROMFS/px4fmu_common/init.d/rc.interface +++ b/ROMFS/px4fmu_common/init.d/rc.interface @@ -10,7 +10,7 @@ then # set MIXERSD /fs/microsd/etc/mixers/$MIXER.mix - #Use the mixer file from the SD-card if it exists + # Use the mixer file from the SD-card if it exists if [ -f $MIXERSD ] then set MIXER_FILE $MIXERSD @@ -54,7 +54,6 @@ then # if [ $PWM_RATE != none ] then - echo "[init] Set PWM rate: $PWM_RATE" pwm rate -c $PWM_OUTPUTS -r $PWM_RATE fi @@ -63,17 +62,14 @@ then # if [ $PWM_DISARMED != none ] then - echo "[init] Set PWM disarmed: $PWM_DISARMED" pwm disarmed -c $PWM_OUTPUTS -p $PWM_DISARMED fi if [ $PWM_MIN != none ] then - echo "[init] Set PWM min: $PWM_MIN" pwm min -c $PWM_OUTPUTS -p $PWM_MIN fi if [ $PWM_MAX != none ] then - echo "[init] Set PWM max: $PWM_MAX" pwm max -c $PWM_OUTPUTS -p $PWM_MAX fi fi diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index f9c822635..201b99749 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -444,7 +444,6 @@ then if [ $GPS == yes ] then - echo "[init] Start GPS" if [ $GPS_FAKE == yes ] then echo "[init] Faking GPS" -- cgit v1.2.3 From b2671c8f058fae56c468b68d3185bfc1e8626710 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 22 Nov 2014 15:46:28 +0100 Subject: GPS: be less verbose --- src/drivers/gps/gps.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index 5fb4b9ff8..47c907bd3 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -274,7 +274,6 @@ GPS::task_main_trampoline(void *arg) void GPS::task_main() { - log("starting"); /* open the serial port */ _serial_fd = ::open(_port, O_RDWR); -- cgit v1.2.3 From 7bed194f4a217835b78f89c9e8399e1c946ca579 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 22 Nov 2014 15:46:51 +0100 Subject: EKF: less verbose --- src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp | 4 ---- 1 file changed, 4 deletions(-) diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp index 35dc39ec6..e2dbc30dd 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp @@ -206,10 +206,6 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds 0, 0, 1.f }; /**< init: identity matrix */ - // print text - printf("Extended Kalman Filter Attitude Estimator initialized..\n\n"); - fflush(stdout); - int overloadcounter = 19; /* Initialize filter */ -- cgit v1.2.3 From 2fbda61521131f66cd3aab792716f3c40bc1cac8 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 22 Nov 2014 15:47:10 +0100 Subject: mc attitude controller: Less verbose --- src/modules/mc_att_control/mc_att_control_main.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index 19c10198c..81a13edb8 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -721,8 +721,6 @@ MulticopterAttitudeControl::task_main_trampoline(int argc, char *argv[]) void MulticopterAttitudeControl::task_main() { - warnx("started"); - fflush(stdout); /* * do subscriptions -- cgit v1.2.3 From df37f380cb792570ae07b1cc3e9eb61e3495fc43 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 22 Nov 2014 15:47:54 +0100 Subject: position controller main: Less verbose --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index d52138522..e3c94bc46 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -857,10 +857,8 @@ MulticopterPositionControl::control_auto(float dt) void MulticopterPositionControl::task_main() { - warnx("started"); _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); - mavlink_log_info(_mavlink_fd, "[mpc] started"); /* * do subscriptions -- cgit v1.2.3 From 90f28647539e7c8ae36ac74c7526582f3f18b3e7 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 22 Nov 2014 15:49:03 +0100 Subject: INAV: Less verbose --- .../position_estimator_inav/position_estimator_inav_main.c | 8 +++----- 1 file changed, 3 insertions(+), 5 deletions(-) diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index 81bbaa658..f82a0f989 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -161,7 +161,7 @@ int position_estimator_inav_main(int argc, char *argv[]) thread_should_exit = true; } else { - warnx("app not started"); + warnx("not started"); } exit(0); @@ -169,10 +169,10 @@ int position_estimator_inav_main(int argc, char *argv[]) if (!strcmp(argv[1], "status")) { if (thread_running) { - warnx("app is running"); + warnx("is running"); } else { - warnx("app not started"); + warnx("not started"); } exit(0); @@ -210,10 +210,8 @@ static void write_debug_log(const char *msg, float dt, float x_est[2], float y_e ****************************************************************************/ int position_estimator_inav_thread_main(int argc, char *argv[]) { - warnx("started"); int mavlink_fd; mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); - mavlink_log_info(mavlink_fd, "[inav] started"); float x_est[2] = { 0.0f, 0.0f }; // pos, vel float y_est[2] = { 0.0f, 0.0f }; // pos, vel -- cgit v1.2.3 From 1709c74f82da510fff5bf546a75c9894a19d8fd2 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 22 Nov 2014 15:49:34 +0100 Subject: dataman: less verbose, fix code style --- src/modules/dataman/dataman.c | 14 +++++--------- 1 file changed, 5 insertions(+), 9 deletions(-) diff --git a/src/modules/dataman/dataman.c b/src/modules/dataman/dataman.c index b2355d4d8..705e54be3 100644 --- a/src/modules/dataman/dataman.c +++ b/src/modules/dataman/dataman.c @@ -629,9 +629,6 @@ task_main(int argc, char *argv[]) { work_q_item_t *work; - /* inform about start */ - warnx("Initializing.."); - /* Initialize global variables */ g_key_offsets[0] = 0; @@ -694,16 +691,15 @@ task_main(int argc, char *argv[]) if (sys_restart_val == DM_INIT_REASON_POWER_ON) { warnx("Power on restart"); _restart(DM_INIT_REASON_POWER_ON); - } - else if (sys_restart_val == DM_INIT_REASON_IN_FLIGHT) { + } else if (sys_restart_val == DM_INIT_REASON_IN_FLIGHT) { warnx("In flight restart"); _restart(DM_INIT_REASON_IN_FLIGHT); - } - else + } else { warnx("Unknown restart"); - } - else + } + } else { warnx("Unknown restart"); + } /* We use two file descriptors, one for the caller context and one for the worker thread */ /* They are actually the same but we need to some way to reject caller request while the */ -- cgit v1.2.3 From b3542bec085a598f34c2e796ee5c78328d0d990c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 22 Nov 2014 16:29:08 +0100 Subject: INAV: use int for outputs --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 4 ++-- .../position_estimator_inav/position_estimator_inav_main.c | 11 ++++++----- 2 files changed, 8 insertions(+), 7 deletions(-) diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index e3c94bc46..5918d6bc5 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -535,7 +535,7 @@ MulticopterPositionControl::reset_pos_sp() - _params.vel_ff(0) * _sp_move_rate(0)) / _params.pos_p(0); _pos_sp(1) = _pos(1) + (_vel(1) - _att_sp.R_body[1][2] * _att_sp.thrust / _params.vel_p(1) - _params.vel_ff(1) * _sp_move_rate(1)) / _params.pos_p(1); - mavlink_log_info(_mavlink_fd, "[mpc] reset pos sp: %.2f, %.2f", (double)_pos_sp(0), (double)_pos_sp(1)); + mavlink_log_info(_mavlink_fd, "[mpc] reset pos sp: %d, %d", (int)_pos_sp(0), (int)_pos_sp(1)); } } @@ -545,7 +545,7 @@ MulticopterPositionControl::reset_alt_sp() if (_reset_alt_sp) { _reset_alt_sp = false; _pos_sp(2) = _pos(2) + (_vel(2) - _params.vel_ff(2) * _sp_move_rate(2)) / _params.pos_p(2); - mavlink_log_info(_mavlink_fd, "[mpc] reset alt sp: %.2f", -(double)_pos_sp(2)); + mavlink_log_info(_mavlink_fd, "[mpc] reset alt sp: %d", -(int)_pos_sp(2)); } } diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index f82a0f989..e736a86d7 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -387,8 +387,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } else { wait_baro = false; baro_offset /= (float) baro_init_cnt; - warnx("baro offs: %.2f", (double)baro_offset); - mavlink_log_info(mavlink_fd, "[inav] baro offs: %.2f", (double)baro_offset); + warnx("baro offs: %d", (int)baro_offset); + mavlink_log_info(mavlink_fd, "[inav] baro offs: %d", (int)baro_offset); local_pos.z_valid = true; local_pos.v_z_valid = true; } @@ -518,7 +518,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) sonar_valid_time = t; sonar_valid = true; local_pos.surface_bottom_timestamp = t; - mavlink_log_info(mavlink_fd, "[inav] new surface level: %.2f", (double)surface_offset); + mavlink_log_info(mavlink_fd, "[inav] new surface level: %d", (int)surface_offset); } } else { @@ -719,8 +719,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) /* initialize projection */ map_projection_init(&ref, lat, lon); - warnx("init ref: lat=%.7f, lon=%.7f, alt=%.2f", (double)lat, (double)lon, (double)alt); - mavlink_log_info(mavlink_fd, "[inav] init ref: %.7f, %.7f, %.2f", (double)lat, (double)lon, (double)alt); + // XXX replace this print + warnx("init ref: lat=%.7f, lon=%.7f, alt=%8.4f", (double)lat, (double)lon, (double)alt); + mavlink_log_info(mavlink_fd, "[inav] init ref: %.7f, %.7f, %8.4f", (double)lat, (double)lon, (double)alt); } } -- cgit v1.2.3 From 828163f2f5c6b68d9f262b1dd4f57e641c1d45a6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 22 Nov 2014 16:30:09 +0100 Subject: Update mixing handling to allow IO safety off updates --- src/modules/px4iofirmware/mixer.cpp | 20 ++++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index c0b8ac358..66f0969de 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -353,12 +353,16 @@ static unsigned mixer_text_length = 0; int mixer_handle_text(const void *buffer, size_t length) { - /* do not allow a mixer change while safety off */ - if ((r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF)) { + /* do not allow a mixer change while safety off and FMU armed */ + if ((r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) && + (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)) { return 1; } - /* abort if we're in the mixer */ + /* disable mixing, will be enabled once load is complete */ + r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_MIXER_OK); + + /* abort if we're in the mixer - the caller is expected to retry */ if (in_mixer) { return 1; } @@ -367,17 +371,16 @@ mixer_handle_text(const void *buffer, size_t length) isr_debug(2, "mix txt %u", length); - if (length < sizeof(px4io_mixdata)) + if (length < sizeof(px4io_mixdata)) { return 0; + } - unsigned text_length = length - sizeof(px4io_mixdata); + unsigned text_length = length - sizeof(px4io_mixdata); switch (msg->action) { case F2I_MIXER_ACTION_RESET: isr_debug(2, "reset"); - /* FIRST mark the mixer as invalid */ - r_status_flags &= ~PX4IO_P_STATUS_FLAGS_MIXER_OK; /* THEN actually delete it */ mixer_group.reset(); mixer_text_length = 0; @@ -386,9 +389,6 @@ mixer_handle_text(const void *buffer, size_t length) case F2I_MIXER_ACTION_APPEND: isr_debug(2, "append %d", length); - /* disable mixing during the update */ - r_status_flags &= ~PX4IO_P_STATUS_FLAGS_MIXER_OK; - /* check for overflow - this would be really fatal */ if ((mixer_text_length + text_length + 1) > sizeof(mixer_text)) { r_status_flags &= ~PX4IO_P_STATUS_FLAGS_MIXER_OK; -- cgit v1.2.3 From 300705321a0ffa79ab3d4d53e3c029fe8238086a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 22 Nov 2014 16:30:59 +0100 Subject: Allow IO safety off system handling as long as the total system is not live --- src/modules/px4iofirmware/registers.c | 21 +++++++++++++-------- 1 file changed, 13 insertions(+), 8 deletions(-) diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index fbfdd35db..a1a02965f 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -407,10 +407,11 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num /* handle text going to the mixer parser */ case PX4IO_PAGE_MIXERLOAD: - if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) || - (r_status_flags & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED)) { - return mixer_handle_text(values, num_values * sizeof(*values)); - } + /* do not change the mixer if FMU is armed and IO's safety is off + * this state defines an active system. This check is done in the + * text handling function. + */ + return mixer_handle_text(values, num_values * sizeof(*values)); break; default: @@ -583,8 +584,10 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) break; case PX4IO_P_SETUP_REBOOT_BL: - if ((r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) || - (r_status_flags & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED)) { + // do not reboot if FMU is armed and IO's safety is off + // this state defines an active system. + if ((r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) && + (r_status_flags & PX4IO_P_SETUP_ARMING_FMU_ARMED)) { // don't allow reboot while armed break; } @@ -631,9 +634,11 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) /** * do not allow a RC config change while outputs armed + * = FMU is armed and IO's safety is off + * this state defines an active system. */ - if ((r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) || - (r_status_flags & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED)) { + if ((r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) && + (r_status_flags & PX4IO_P_SETUP_ARMING_FMU_ARMED)) { break; } -- cgit v1.2.3 From a36088b9c20e52e37d48e7a08aca39d5f8b901f5 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 22 Nov 2014 16:29:08 +0100 Subject: INAV: use int for outputs --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 4 ++-- .../position_estimator_inav/position_estimator_inav_main.c | 11 ++++++----- 2 files changed, 8 insertions(+), 7 deletions(-) diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index d52138522..769f84cb6 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -535,7 +535,7 @@ MulticopterPositionControl::reset_pos_sp() - _params.vel_ff(0) * _sp_move_rate(0)) / _params.pos_p(0); _pos_sp(1) = _pos(1) + (_vel(1) - _att_sp.R_body[1][2] * _att_sp.thrust / _params.vel_p(1) - _params.vel_ff(1) * _sp_move_rate(1)) / _params.pos_p(1); - mavlink_log_info(_mavlink_fd, "[mpc] reset pos sp: %.2f, %.2f", (double)_pos_sp(0), (double)_pos_sp(1)); + mavlink_log_info(_mavlink_fd, "[mpc] reset pos sp: %d, %d", (int)_pos_sp(0), (int)_pos_sp(1)); } } @@ -545,7 +545,7 @@ MulticopterPositionControl::reset_alt_sp() if (_reset_alt_sp) { _reset_alt_sp = false; _pos_sp(2) = _pos(2) + (_vel(2) - _params.vel_ff(2) * _sp_move_rate(2)) / _params.pos_p(2); - mavlink_log_info(_mavlink_fd, "[mpc] reset alt sp: %.2f", -(double)_pos_sp(2)); + mavlink_log_info(_mavlink_fd, "[mpc] reset alt sp: %d", -(int)_pos_sp(2)); } } diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index 81bbaa658..c46d22733 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -389,8 +389,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } else { wait_baro = false; baro_offset /= (float) baro_init_cnt; - warnx("baro offs: %.2f", (double)baro_offset); - mavlink_log_info(mavlink_fd, "[inav] baro offs: %.2f", (double)baro_offset); + warnx("baro offs: %d", (int)baro_offset); + mavlink_log_info(mavlink_fd, "[inav] baro offs: %d", (int)baro_offset); local_pos.z_valid = true; local_pos.v_z_valid = true; } @@ -520,7 +520,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) sonar_valid_time = t; sonar_valid = true; local_pos.surface_bottom_timestamp = t; - mavlink_log_info(mavlink_fd, "[inav] new surface level: %.2f", (double)surface_offset); + mavlink_log_info(mavlink_fd, "[inav] new surface level: %d", (int)surface_offset); } } else { @@ -721,8 +721,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) /* initialize projection */ map_projection_init(&ref, lat, lon); - warnx("init ref: lat=%.7f, lon=%.7f, alt=%.2f", (double)lat, (double)lon, (double)alt); - mavlink_log_info(mavlink_fd, "[inav] init ref: %.7f, %.7f, %.2f", (double)lat, (double)lon, (double)alt); + // XXX replace this print + warnx("init ref: lat=%.7f, lon=%.7f, alt=%8.4f", (double)lat, (double)lon, (double)alt); + mavlink_log_info(mavlink_fd, "[inav] init ref: %.7f, %.7f, %8.4f", (double)lat, (double)lon, (double)alt); } } -- cgit v1.2.3 From 3c5c1d3c89639d3e67086386d23d10e0dc8f65b0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 22 Nov 2014 15:08:54 +0100 Subject: Fix FD for commander arm operation --- src/modules/commander/commander.cpp | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index ec173c12b..e081955d0 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -310,12 +310,16 @@ int commander_main(int argc, char *argv[]) } if (!strcmp(argv[1], "arm")) { - arm_disarm(true, mavlink_fd, "command line"); + int mavlink_fd_local = open(MAVLINK_LOG_DEVICE, 0); + arm_disarm(true, mavlink_fd_local, "command line"); + close(mavlink_fd_local); exit(0); } if (!strcmp(argv[1], "disarm")) { - arm_disarm(false, mavlink_fd, "command line"); + int mavlink_fd_local = open(MAVLINK_LOG_DEVICE, 0); + arm_disarm(false, mavlink_fd_local, "command line"); + close(mavlink_fd_local); exit(0); } -- cgit v1.2.3 From 00961b55928a949a2dd3b7022fe2cbb4df06ec22 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 22 Nov 2014 15:43:21 +0100 Subject: ROMFS: Do only output necessary information on boot --- ROMFS/px4fmu_common/init.d/rc.interface | 6 +----- ROMFS/px4fmu_common/init.d/rcS | 1 - 2 files changed, 1 insertion(+), 6 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.interface b/ROMFS/px4fmu_common/init.d/rc.interface index e44cd0953..41e0b149b 100644 --- a/ROMFS/px4fmu_common/init.d/rc.interface +++ b/ROMFS/px4fmu_common/init.d/rc.interface @@ -10,7 +10,7 @@ then # set MIXERSD /fs/microsd/etc/mixers/$MIXER.mix - #Use the mixer file from the SD-card if it exists + # Use the mixer file from the SD-card if it exists if [ -f $MIXERSD ] then set MIXER_FILE $MIXERSD @@ -54,7 +54,6 @@ then # if [ $PWM_RATE != none ] then - echo "[init] Set PWM rate: $PWM_RATE" pwm rate -c $PWM_OUTPUTS -r $PWM_RATE fi @@ -63,17 +62,14 @@ then # if [ $PWM_DISARMED != none ] then - echo "[init] Set PWM disarmed: $PWM_DISARMED" pwm disarmed -c $PWM_OUTPUTS -p $PWM_DISARMED fi if [ $PWM_MIN != none ] then - echo "[init] Set PWM min: $PWM_MIN" pwm min -c $PWM_OUTPUTS -p $PWM_MIN fi if [ $PWM_MAX != none ] then - echo "[init] Set PWM max: $PWM_MAX" pwm max -c $PWM_OUTPUTS -p $PWM_MAX fi fi diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index f9c822635..201b99749 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -444,7 +444,6 @@ then if [ $GPS == yes ] then - echo "[init] Start GPS" if [ $GPS_FAKE == yes ] then echo "[init] Faking GPS" -- cgit v1.2.3 From 54e7ed70e135f20a3470ebc71dae700e8c223102 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 22 Nov 2014 15:46:28 +0100 Subject: GPS: be less verbose --- src/drivers/gps/gps.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index 5fb4b9ff8..47c907bd3 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -274,7 +274,6 @@ GPS::task_main_trampoline(void *arg) void GPS::task_main() { - log("starting"); /* open the serial port */ _serial_fd = ::open(_port, O_RDWR); -- cgit v1.2.3 From b34b40622ba7bbd08e7ca5bc0571612ba96fc7ad Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 22 Nov 2014 15:46:51 +0100 Subject: EKF: less verbose --- src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp | 4 ---- 1 file changed, 4 deletions(-) diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp index 35dc39ec6..e2dbc30dd 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp @@ -206,10 +206,6 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds 0, 0, 1.f }; /**< init: identity matrix */ - // print text - printf("Extended Kalman Filter Attitude Estimator initialized..\n\n"); - fflush(stdout); - int overloadcounter = 19; /* Initialize filter */ -- cgit v1.2.3 From 2a76b10f7a76273ac3b16d5ac917d40ea28a277b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 22 Nov 2014 15:47:10 +0100 Subject: mc attitude controller: Less verbose --- src/modules/mc_att_control/mc_att_control_main.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index 19c10198c..81a13edb8 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -721,8 +721,6 @@ MulticopterAttitudeControl::task_main_trampoline(int argc, char *argv[]) void MulticopterAttitudeControl::task_main() { - warnx("started"); - fflush(stdout); /* * do subscriptions -- cgit v1.2.3 From 4c281030bb782f852600daf217d7a75b23489bbc Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 22 Nov 2014 15:47:54 +0100 Subject: position controller main: Less verbose --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 769f84cb6..5918d6bc5 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -857,10 +857,8 @@ MulticopterPositionControl::control_auto(float dt) void MulticopterPositionControl::task_main() { - warnx("started"); _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); - mavlink_log_info(_mavlink_fd, "[mpc] started"); /* * do subscriptions -- cgit v1.2.3 From ace6c3fe409f4b70e75bb762e0ba6e0a574a7277 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 22 Nov 2014 15:49:03 +0100 Subject: INAV: Less verbose --- .../position_estimator_inav/position_estimator_inav_main.c | 8 +++----- 1 file changed, 3 insertions(+), 5 deletions(-) diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index c46d22733..e736a86d7 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -161,7 +161,7 @@ int position_estimator_inav_main(int argc, char *argv[]) thread_should_exit = true; } else { - warnx("app not started"); + warnx("not started"); } exit(0); @@ -169,10 +169,10 @@ int position_estimator_inav_main(int argc, char *argv[]) if (!strcmp(argv[1], "status")) { if (thread_running) { - warnx("app is running"); + warnx("is running"); } else { - warnx("app not started"); + warnx("not started"); } exit(0); @@ -210,10 +210,8 @@ static void write_debug_log(const char *msg, float dt, float x_est[2], float y_e ****************************************************************************/ int position_estimator_inav_thread_main(int argc, char *argv[]) { - warnx("started"); int mavlink_fd; mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); - mavlink_log_info(mavlink_fd, "[inav] started"); float x_est[2] = { 0.0f, 0.0f }; // pos, vel float y_est[2] = { 0.0f, 0.0f }; // pos, vel -- cgit v1.2.3 From 138c25ec74f08db6f6759053d0200058a794c196 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 22 Nov 2014 15:49:34 +0100 Subject: dataman: less verbose, fix code style --- src/modules/dataman/dataman.c | 14 +++++--------- 1 file changed, 5 insertions(+), 9 deletions(-) diff --git a/src/modules/dataman/dataman.c b/src/modules/dataman/dataman.c index b2355d4d8..705e54be3 100644 --- a/src/modules/dataman/dataman.c +++ b/src/modules/dataman/dataman.c @@ -629,9 +629,6 @@ task_main(int argc, char *argv[]) { work_q_item_t *work; - /* inform about start */ - warnx("Initializing.."); - /* Initialize global variables */ g_key_offsets[0] = 0; @@ -694,16 +691,15 @@ task_main(int argc, char *argv[]) if (sys_restart_val == DM_INIT_REASON_POWER_ON) { warnx("Power on restart"); _restart(DM_INIT_REASON_POWER_ON); - } - else if (sys_restart_val == DM_INIT_REASON_IN_FLIGHT) { + } else if (sys_restart_val == DM_INIT_REASON_IN_FLIGHT) { warnx("In flight restart"); _restart(DM_INIT_REASON_IN_FLIGHT); - } - else + } else { warnx("Unknown restart"); - } - else + } + } else { warnx("Unknown restart"); + } /* We use two file descriptors, one for the caller context and one for the worker thread */ /* They are actually the same but we need to some way to reject caller request while the */ -- cgit v1.2.3 From 512779907e06f059a15d54c88d71b73aad9aced0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 23 Nov 2014 13:01:00 +0100 Subject: Update NuttX version, MD5 fix --- NuttX | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/NuttX b/NuttX index 9d06b6457..ec6b670f6 160000 --- a/NuttX +++ b/NuttX @@ -1 +1 @@ -Subproject commit 9d06b645790e1445f14e3b19c71d40b3088f4e4f +Subproject commit ec6b670f6d1e964700c0b0a50d14db12761e3097 -- cgit v1.2.3 From 2f271888d2ed934c271637c22554b503ce68e535 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 23 Nov 2014 19:22:55 +0100 Subject: Added performance counter for SD log performance of write() call --- src/modules/sdlog2/sdlog2.c | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index af580f1f7..8638a4904 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -496,6 +496,8 @@ static void *logwriter_thread(void *arg) /* set name */ prctl(PR_SET_NAME, "sdlog2_writer", 0); + perf_counter_t perf_write = perf_alloc(PC_ELAPSED, "sd write"); + int log_fd = open_log_file(); if (log_fd < 0) { @@ -553,7 +555,9 @@ static void *logwriter_thread(void *arg) n = available; } + perf_begin(perf_write); n = write(log_fd, read_ptr, n); + perf_end(perf_write); should_wait = (n == available) && !is_part; @@ -586,6 +590,9 @@ static void *logwriter_thread(void *arg) fsync(log_fd); close(log_fd); + /* free performance counter */ + perf_free(perf_write); + return NULL; } -- cgit v1.2.3 From 08d6cbe6bf0b5b04f63e42c6c60f5b1fe6167547 Mon Sep 17 00:00:00 2001 From: philipoe Date: Sun, 23 Nov 2014 21:23:01 +0100 Subject: commander: Decrease RC-signal-regained message length to stay within 50 character length limit at all times --- src/modules/commander/commander.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index deb048566..f77895efb 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1544,7 +1544,7 @@ int commander_thread_main(int argc, char *argv[]) } else { if (status.rc_signal_lost) { - mavlink_log_critical(mavlink_fd, "RC SIGNAL REGAINED after %llums (at t=%llums)",(hrt_absolute_time()-status.rc_signal_lost_timestamp)/1000,hrt_absolute_time()/1000); + mavlink_log_critical(mavlink_fd, "RC SIGNAL REGAINED after %llums",(hrt_absolute_time()-status.rc_signal_lost_timestamp)/1000); status_changed = true; } } -- cgit v1.2.3 From 2dae1bc542746ed373458b55a8e564607700d17d Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 20 Nov 2014 09:05:10 +1100 Subject: uavcan: break the link between poll fd indexes and controls this linkage was fragile and makes it harder to add new orb subscriptions to the uavcan code --- src/modules/uavcan/uavcan_main.cpp | 5 ++--- src/modules/uavcan/uavcan_main.hpp | 3 +++ 2 files changed, 5 insertions(+), 3 deletions(-) diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index 2c543462e..ccc087920 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -333,14 +333,12 @@ int UavcanNode::run() } else { // get controls for required topics bool controls_updated = false; - unsigned poll_id = 1; for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) { if (_control_subs[i] > 0) { - if (_poll_fds[poll_id].revents & POLLIN) { + if (_poll_fds[_poll_ids[i]].revents & POLLIN) { controls_updated = true; orb_copy(_control_topics[i], _control_subs[i], &_controls[i]); } - poll_id++; } } @@ -474,6 +472,7 @@ UavcanNode::subscribe() if (_control_subs[i] > 0) { _poll_fds[_poll_fds_num].fd = _control_subs[i]; _poll_fds[_poll_fds_num].events = POLLIN; + _poll_ids[i] = _poll_fds_num; _poll_fds_num++; } } diff --git a/src/modules/uavcan/uavcan_main.hpp b/src/modules/uavcan/uavcan_main.hpp index 274321f0d..b791663d4 100644 --- a/src/modules/uavcan/uavcan_main.hpp +++ b/src/modules/uavcan/uavcan_main.hpp @@ -127,4 +127,7 @@ private: orb_id_t _control_topics[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN] = {}; pollfd _poll_fds[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN + 1] = {}; ///< +1 for /dev/uavcan/busevent unsigned _poll_fds_num = 0; + + // index into _poll_fds for each _control_subs handle + uint8_t _poll_ids[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN]; }; -- cgit v1.2.3 From f6b0a3e07f27f34af6e6d209aec761374611e968 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 20 Nov 2014 09:35:58 +1100 Subject: uORB: added actuator_direct topic this topic will be used to allow direct output of actuator values for uavcan, bypassing the mixer. --- src/modules/uORB/Publication.cpp | 2 + src/modules/uORB/objects_common.cpp | 3 ++ src/modules/uORB/topics/actuator_direct.h | 69 +++++++++++++++++++++++++++++++ 3 files changed, 74 insertions(+) create mode 100644 src/modules/uORB/topics/actuator_direct.h diff --git a/src/modules/uORB/Publication.cpp b/src/modules/uORB/Publication.cpp index cd0b30dd6..71757e1f4 100644 --- a/src/modules/uORB/Publication.cpp +++ b/src/modules/uORB/Publication.cpp @@ -46,6 +46,7 @@ #include "topics/vehicle_attitude_setpoint.h" #include "topics/vehicle_rates_setpoint.h" #include "topics/actuator_outputs.h" +#include "topics/actuator_direct.h" #include "topics/encoders.h" #include "topics/tecs_status.h" @@ -76,6 +77,7 @@ template class __EXPORT Publication; template class __EXPORT Publication; template class __EXPORT Publication; template class __EXPORT Publication; +template class __EXPORT Publication; template class __EXPORT Publication; template class __EXPORT Publication; diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index b91a00c1e..49dfc7834 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -192,6 +192,9 @@ ORB_DEFINE(actuator_outputs_1, struct actuator_outputs_s); ORB_DEFINE(actuator_outputs_2, struct actuator_outputs_s); ORB_DEFINE(actuator_outputs_3, struct actuator_outputs_s); +#include "topics/actuator_direct.h" +ORB_DEFINE(actuator_direct, struct actuator_direct_s); + #include "topics/multirotor_motor_limits.h" ORB_DEFINE(multirotor_motor_limits, struct multirotor_motor_limits_s); diff --git a/src/modules/uORB/topics/actuator_direct.h b/src/modules/uORB/topics/actuator_direct.h new file mode 100644 index 000000000..5f9d0f56d --- /dev/null +++ b/src/modules/uORB/topics/actuator_direct.h @@ -0,0 +1,69 @@ +/**************************************************************************** + * + * 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 actuator_direct.h + * + * Actuator direct values. + * + * Values published to this topic are the direct actuator values which + * should be passed to actuators, bypassing mixing + */ + +#ifndef TOPIC_ACTUATOR_DIRECT_H +#define TOPIC_ACTUATOR_DIRECT_H + +#include +#include "../uORB.h" + +#define NUM_ACTUATORS_DIRECT 16 + +/** + * @addtogroup topics + * @{ + */ + +struct actuator_direct_s { + uint64_t timestamp; /**< timestamp in us since system boot */ + float values[NUM_ACTUATORS_DIRECT]; /**< actuator values, from -1 to 1 */ + unsigned nvalues; /**< number of valid values */ +}; + +/** + * @} + */ + +/* actuator direct ORB */ +ORB_DECLARE(actuator_direct); + +#endif // TOPIC_ACTUATOR_DIRECT_H -- cgit v1.2.3 From b830137ec8492dc583b61689b7d56a1b359a5c5b Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 20 Nov 2014 09:37:02 +1100 Subject: uavcan: added support for actuator_direct ORB topic this watches the actuator_direct topic and uses it to allow for direct output of actuator values, bypassing the mixer --- src/modules/uavcan/uavcan_main.cpp | 23 +++++++++++++++++++++++ src/modules/uavcan/uavcan_main.hpp | 5 +++++ 2 files changed, 28 insertions(+) diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index ccc087920..cf40f3ea8 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -280,6 +280,7 @@ int UavcanNode::run() _armed_sub = orb_subscribe(ORB_ID(actuator_armed)); _test_motor_sub = orb_subscribe(ORB_ID(test_motor)); + _actuator_direct_sub = orb_subscribe(ORB_ID(actuator_direct)); actuator_outputs_s outputs; memset(&outputs, 0, sizeof(outputs)); @@ -310,6 +311,18 @@ int UavcanNode::run() _poll_fds[_poll_fds_num].events = POLLIN; _poll_fds_num += 1; + /* + * setup poll to look for actuator direct input if we are + * subscribed to the topic + */ + if (_actuator_direct_sub != -1) { + _poll_fds[_poll_fds_num] = ::pollfd(); + _poll_fds[_poll_fds_num].fd = _actuator_direct_sub; + _poll_fds[_poll_fds_num].events = POLLIN; + _actuator_direct_poll_fd_num = _poll_fds_num; + _poll_fds_num += 1; + } + while (!_task_should_exit) { // update actuator controls subscriptions if needed if (_groups_subscribed != _groups_required) { @@ -342,6 +355,16 @@ int UavcanNode::run() } } + /* + see if we have any direct actuator updates + */ + if (_actuator_direct_sub != -1 && + (_poll_fds[_actuator_direct_poll_fd_num].revents & POLLIN) && + orb_copy(ORB_ID(actuator_direct), _actuator_direct_sub, &_actuator_direct) == OK) { + // Output to the bus + _esc_controller.update_outputs(_actuator_direct.values, _actuator_direct.nvalues); + } + // can we mix? if (_test_in_progress) { float test_outputs[NUM_ACTUATOR_OUTPUTS] = {}; diff --git a/src/modules/uavcan/uavcan_main.hpp b/src/modules/uavcan/uavcan_main.hpp index b791663d4..d7eb16764 100644 --- a/src/modules/uavcan/uavcan_main.hpp +++ b/src/modules/uavcan/uavcan_main.hpp @@ -42,6 +42,7 @@ #include #include #include +#include #include "actuators/esc.hpp" #include "sensors/sensor_bridge.hpp" @@ -128,6 +129,10 @@ private: pollfd _poll_fds[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN + 1] = {}; ///< +1 for /dev/uavcan/busevent unsigned _poll_fds_num = 0; + int _actuator_direct_sub = -1; ///< uORB subscription of the actuator_direct topic + uint8_t _actuator_direct_poll_fd_num; + actuator_direct_s _actuator_direct; + // index into _poll_fds for each _control_subs handle uint8_t _poll_ids[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN]; }; -- cgit v1.2.3 From ecc7a3cbb42f6490b43f21b3849738184934a2a0 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 20 Nov 2014 09:52:50 +1100 Subject: motor_test: prevent use of uninitialised test_motor orb handle stack variables are not initialised to zero --- src/systemcmds/motor_test/motor_test.c | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/systemcmds/motor_test/motor_test.c b/src/systemcmds/motor_test/motor_test.c index 1b7ff75f7..77dc2f0d5 100644 --- a/src/systemcmds/motor_test/motor_test.c +++ b/src/systemcmds/motor_test/motor_test.c @@ -59,9 +59,10 @@ __EXPORT int motor_test_main(int argc, char *argv[]); static void motor_test(unsigned channel, float value); static void usage(const char *reason); +static orb_advert_t _test_motor_pub; + void motor_test(unsigned channel, float value) { - orb_advert_t _test_motor_pub; struct test_motor_s _test_motor; _test_motor.motor_number = channel; -- cgit v1.2.3 From ead0458e97d6e19cc0b188124063e90962820e3a Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 20 Nov 2014 16:58:16 +1100 Subject: uavcan: don't force motors to keep spinning at zero throttle Forcing motors to keep spinning when armed should be a policy decision up at the vehicle type level, not hard coded down in the ESC driver. It isn't appropriate for fixed wing or ground vehicles for example. We could add an ioctl to enable "spin when armed" if just setting a small value in the vehicle code is inconvenient --- src/modules/uavcan/actuators/esc.cpp | 13 +++++++++---- 1 file changed, 9 insertions(+), 4 deletions(-) diff --git a/src/modules/uavcan/actuators/esc.cpp b/src/modules/uavcan/actuators/esc.cpp index 1d23099f3..7efd50511 100644 --- a/src/modules/uavcan/actuators/esc.cpp +++ b/src/modules/uavcan/actuators/esc.cpp @@ -101,10 +101,15 @@ void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs) for (unsigned i = 0; i < num_outputs; i++) { if (_armed_mask & MOTOR_BIT(i)) { float scaled = (outputs[i] + 1.0F) * 0.5F * cmd_max; - if (scaled < 1.0F) { - scaled = 1.0F; // Since we're armed, we don't want to stop it completely - } - + // trim negative values back to 0. Previously + // we set this to 0.1, which meant motors kept + // spinning when armed, but that should be a + // policy decision for a specific vehicle + // type, as it is not appropriate for all + // types of vehicles (eg. fixed wing). + if (scaled < 0.0F) { + scaled = 0.0F; + } if (scaled > cmd_max) { scaled = cmd_max; perf_count(_perfcnt_scaling_error); -- cgit v1.2.3 From 8e44ec2e3bd6852b74e4463c373555dd1bab47d3 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 20 Nov 2014 17:00:02 +1100 Subject: uavcan: prevent crash in ESC driver passing in more than 8 actuators would crash the ESC driver. We need to check again the array size of the _esc_status.esc, which is CONNECTED_ESC_MAX --- src/modules/uavcan/actuators/esc.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/modules/uavcan/actuators/esc.cpp b/src/modules/uavcan/actuators/esc.cpp index 7efd50511..9f682c7e1 100644 --- a/src/modules/uavcan/actuators/esc.cpp +++ b/src/modules/uavcan/actuators/esc.cpp @@ -76,7 +76,9 @@ int UavcanEscController::init() void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs) { - if ((outputs == nullptr) || (num_outputs > uavcan::equipment::esc::RawCommand::FieldTypes::cmd::MaxSize)) { + if ((outputs == nullptr) || + (num_outputs > uavcan::equipment::esc::RawCommand::FieldTypes::cmd::MaxSize) || + (num_outputs > CONNECTED_ESC_MAX)) { perf_count(_perfcnt_invalid_input); return; } -- cgit v1.2.3 From 724ec0ec8b0d34c631b2784236a990e535700254 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 20 Nov 2014 17:02:24 +1100 Subject: uavcan: handle all ESC output in one place moving all the ESC output handling to one place allows the limits on actuator values to apply to all types of inputs, and will make it easier to expand "uavcan status" to show actuator values --- src/modules/uavcan/uavcan_main.cpp | 80 +++++++++++++++++++++----------------- src/modules/uavcan/uavcan_main.hpp | 2 + 2 files changed, 47 insertions(+), 35 deletions(-) diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index cf40f3ea8..e4f58b310 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -282,8 +282,7 @@ int UavcanNode::run() _test_motor_sub = orb_subscribe(ORB_ID(test_motor)); _actuator_direct_sub = orb_subscribe(ORB_ID(actuator_direct)); - actuator_outputs_s outputs; - memset(&outputs, 0, sizeof(outputs)); + memset(&_outputs, 0, sizeof(_outputs)); const int busevent_fd = ::open(uavcan_stm32::BusEvent::DevName, 0); if (busevent_fd < 0) @@ -339,6 +338,8 @@ int UavcanNode::run() node_spin_once(); // Non-blocking + bool new_output = false; + // this would be bad... if (poll_ret < 0) { log("poll error %d", errno); @@ -360,18 +361,25 @@ int UavcanNode::run() */ if (_actuator_direct_sub != -1 && (_poll_fds[_actuator_direct_poll_fd_num].revents & POLLIN) && - orb_copy(ORB_ID(actuator_direct), _actuator_direct_sub, &_actuator_direct) == OK) { - // Output to the bus - _esc_controller.update_outputs(_actuator_direct.values, _actuator_direct.nvalues); + orb_copy(ORB_ID(actuator_direct), _actuator_direct_sub, &_actuator_direct) == OK && + !_test_in_progress) { + if (_actuator_direct.nvalues > NUM_ACTUATOR_OUTPUTS) { + _actuator_direct.nvalues = NUM_ACTUATOR_OUTPUTS; + } + memcpy(&_outputs.output[0], &_actuator_direct.values[0], + _actuator_direct.nvalues*sizeof(float)); + _outputs.noutputs = _actuator_direct.nvalues; + new_output = true; } // can we mix? if (_test_in_progress) { - float test_outputs[NUM_ACTUATOR_OUTPUTS] = {}; - test_outputs[_test_motor.motor_number] = _test_motor.value*2.0f-1.0f; - - // Output to the bus - _esc_controller.update_outputs(test_outputs, NUM_ACTUATOR_OUTPUTS); + memset(&_outputs, 0, sizeof(_outputs)); + if (_test_motor.motor_number < NUM_ACTUATOR_OUTPUTS) { + _outputs.output[_test_motor.motor_number] = _test_motor.value*2.0f-1.0f; + _outputs.noutputs = _test_motor.motor_number+1; + } + new_output = true; } else if (controls_updated && (_mixers != nullptr)) { // XXX one output group has 8 outputs max, @@ -379,39 +387,41 @@ int UavcanNode::run() unsigned num_outputs_max = 8; // Do mixing - outputs.noutputs = _mixers->mix(&outputs.output[0], num_outputs_max); - outputs.timestamp = hrt_absolute_time(); - - // iterate actuators - for (unsigned i = 0; i < outputs.noutputs; i++) { - // last resort: catch NaN, INF and out-of-band errors - if (!isfinite(outputs.output[i])) { - /* - * Value is NaN, INF or out of band - set to the minimum value. - * This will be clearly visible on the servo status and will limit the risk of accidentally - * spinning motors. It would be deadly in flight. - */ - outputs.output[i] = -1.0f; - } + _outputs.noutputs = _mixers->mix(&_outputs.output[0], num_outputs_max); - // limit outputs to valid range + new_output = true; + } + } - // never go below min - if (outputs.output[i] < -1.0f) { - outputs.output[i] = -1.0f; - } + if (new_output) { + // iterate actuators, checking for valid values + for (uint8_t i = 0; i < _outputs.noutputs; i++) { + // last resort: catch NaN, INF and out-of-band errors + if (!isfinite(_outputs.output[i])) { + /* + * Value is NaN, INF or out of band - set to the minimum value. + * This will be clearly visible on the servo status and will limit the risk of accidentally + * spinning motors. It would be deadly in flight. + */ + _outputs.output[i] = -1.0f; + } - // never go below max - if (outputs.output[i] > 1.0f) { - outputs.output[i] = 1.0f; - } + // never go below min + if (_outputs.output[i] < -1.0f) { + _outputs.output[i] = -1.0f; } - // Output to the bus - _esc_controller.update_outputs(outputs.output, outputs.noutputs); + // never go above max + if (_outputs.output[i] > 1.0f) { + _outputs.output[i] = 1.0f; + } } + // Output to the bus + _outputs.timestamp = hrt_absolute_time(); + _esc_controller.update_outputs(_outputs.output, _outputs.noutputs); } + // Check motor test state bool updated = false; orb_check(_test_motor_sub, &updated); diff --git a/src/modules/uavcan/uavcan_main.hpp b/src/modules/uavcan/uavcan_main.hpp index d7eb16764..5a036fcd7 100644 --- a/src/modules/uavcan/uavcan_main.hpp +++ b/src/modules/uavcan/uavcan_main.hpp @@ -133,6 +133,8 @@ private: uint8_t _actuator_direct_poll_fd_num; actuator_direct_s _actuator_direct; + actuator_outputs_s _outputs; + // index into _poll_fds for each _control_subs handle uint8_t _poll_ids[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN]; }; -- cgit v1.2.3 From a7a68c88a25c2c4cb401fb4f8894de6f4c280ba2 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 20 Nov 2014 17:03:29 +1100 Subject: uavcan: show ESC output values in uavcan status, and add arm/disarm this makes "uavcan status" show the current output values, which is useful for debugging. It also adds "uavcan arm" and "uavcan disarm" commands, which are very useful for re-arming after a motor test. --- src/modules/uavcan/uavcan_main.cpp | 20 +++++++++++++++++++- 1 file changed, 19 insertions(+), 1 deletion(-) diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index e4f58b310..8fa765633 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -604,6 +604,14 @@ UavcanNode::print_info() (unsigned)_groups_subscribed, (unsigned)_groups_required, _poll_fds_num); printf("ESC mixer: %s\n", (_mixers == nullptr) ? "NONE" : "OK"); + if (_outputs.noutputs != 0) { + printf("ESC output: "); + for (uint8_t i=0; i<_outputs.noutputs; i++) { + printf("%d ", (int)(_outputs.output[i]*1000)); + } + printf("\n"); + } + // Sensor bridges auto br = _sensor_bridges.getHead(); while (br != nullptr) { @@ -622,7 +630,7 @@ UavcanNode::print_info() static void print_usage() { warnx("usage: \n" - "\tuavcan {start|status|stop}"); + "\tuavcan {start|status|stop|arm|disarm}"); } extern "C" __EXPORT int uavcan_main(int argc, char *argv[]); @@ -669,6 +677,16 @@ int uavcan_main(int argc, char *argv[]) ::exit(0); } + if (!std::strcmp(argv[1], "arm")) { + inst->arm_actuators(true); + ::exit(0); + } + + if (!std::strcmp(argv[1], "disarm")) { + inst->arm_actuators(false); + ::exit(0); + } + if (!std::strcmp(argv[1], "stop")) { delete inst; ::exit(0); -- cgit v1.2.3 From 7ae4f6d97e398a64aeb99c52880651a2282be5b9 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 21 Nov 2014 08:17:50 +1100 Subject: uavcan: added add_poll_fd() helper function this makes the code clearer and avoids repeated code --- src/modules/uavcan/uavcan_main.cpp | 36 +++++++++++++++++++++--------------- src/modules/uavcan/uavcan_main.hpp | 7 ++++++- 2 files changed, 27 insertions(+), 16 deletions(-) diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index 8fa765633..8147a8b89 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -269,6 +269,24 @@ void UavcanNode::node_spin_once() } } +/* + add a fd to the list of polled events. This assumes you want + POLLIN for now. + */ +int UavcanNode::add_poll_fd(int fd) +{ + int ret = _poll_fds_num; + if (_poll_fds_num >= UAVCAN_NUM_POLL_FDS) { + errx(1, "uavcan: too many poll fds, exiting"); + } + _poll_fds[_poll_fds_num] = ::pollfd(); + _poll_fds[_poll_fds_num].fd = fd; + _poll_fds[_poll_fds_num].events = POLLIN; + _poll_fds_num += 1; + return ret; +} + + int UavcanNode::run() { (void)pthread_mutex_lock(&_node_mutex); @@ -304,22 +322,14 @@ int UavcanNode::run() * the value returned from poll() to detect whether actuator control has timed out or not. * Instead, all ORB events need to be checked individually (see below). */ - _poll_fds_num = 0; - _poll_fds[_poll_fds_num] = ::pollfd(); - _poll_fds[_poll_fds_num].fd = busevent_fd; - _poll_fds[_poll_fds_num].events = POLLIN; - _poll_fds_num += 1; + add_poll_fd(busevent_fd); /* * setup poll to look for actuator direct input if we are * subscribed to the topic */ if (_actuator_direct_sub != -1) { - _poll_fds[_poll_fds_num] = ::pollfd(); - _poll_fds[_poll_fds_num].fd = _actuator_direct_sub; - _poll_fds[_poll_fds_num].events = POLLIN; - _actuator_direct_poll_fd_num = _poll_fds_num; - _poll_fds_num += 1; + _actuator_direct_poll_fd_num = add_poll_fd(_actuator_direct_sub); } while (!_task_should_exit) { @@ -490,7 +500,6 @@ UavcanNode::subscribe() uint32_t sub_groups = _groups_required & ~_groups_subscribed; uint32_t unsub_groups = _groups_subscribed & ~_groups_required; // the first fd used by CAN - _poll_fds_num = 1; for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) { if (sub_groups & (1 << i)) { warnx("subscribe to actuator_controls_%d", i); @@ -503,10 +512,7 @@ UavcanNode::subscribe() } if (_control_subs[i] > 0) { - _poll_fds[_poll_fds_num].fd = _control_subs[i]; - _poll_fds[_poll_fds_num].events = POLLIN; - _poll_ids[i] = _poll_fds_num; - _poll_fds_num++; + _poll_ids[i] = add_poll_fd(_control_subs[i]); } } } diff --git a/src/modules/uavcan/uavcan_main.hpp b/src/modules/uavcan/uavcan_main.hpp index 5a036fcd7..98f2e5ad4 100644 --- a/src/modules/uavcan/uavcan_main.hpp +++ b/src/modules/uavcan/uavcan_main.hpp @@ -58,6 +58,9 @@ #define NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN 4 #define UAVCAN_DEVICE_PATH "/dev/uavcan/esc" +// we add two to allow for actuator_direct and busevent +#define UAVCAN_NUM_POLL_FDS (NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN+2) + /** * A UAVCAN node. */ @@ -98,6 +101,8 @@ private: int init(uavcan::NodeID node_id); void node_spin_once(); int run(); + int add_poll_fd(int fd); ///< add a fd to poll list, returning index into _poll_fds[] + int _task = -1; ///< handle to the OS task bool _task_should_exit = false; ///< flag to indicate to tear down the CAN driver @@ -126,7 +131,7 @@ private: int _control_subs[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN] = {}; actuator_controls_s _controls[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN] = {}; orb_id_t _control_topics[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN] = {}; - pollfd _poll_fds[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN + 1] = {}; ///< +1 for /dev/uavcan/busevent + pollfd _poll_fds[UAVCAN_NUM_POLL_FDS] = {}; unsigned _poll_fds_num = 0; int _actuator_direct_sub = -1; ///< uORB subscription of the actuator_direct topic -- cgit v1.2.3 From 809500cfcd220e65d7eb301e33533c5fe29f03d8 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 26 Nov 2014 11:37:56 +1100 Subject: px4io: fixed RC_CONFIG channel limit check number of channels is the right test, not number of actuators --- src/drivers/px4io/px4io.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 6f02ba62c..e61454b00 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -2592,7 +2592,7 @@ PX4IO::ioctl(file * filep, int cmd, unsigned long arg) on param_get() */ struct pwm_output_rc_config* config = (struct pwm_output_rc_config*)arg; - if (config->channel >= _max_actuators) { + if (config->channel >= RC_INPUT_MAX_CHANNELS) { /* fail with error */ return E2BIG; } -- cgit v1.2.3 From 29f000dc31f6f18af0130b46d5e143e983549d19 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 26 Nov 2014 11:39:17 +1100 Subject: px4io: fixed error returns to be negative follow standard conventions --- src/drivers/px4io/px4io.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index e61454b00..b31d7bbfa 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -2198,7 +2198,7 @@ PX4IO::ioctl(file * filep, int cmd, unsigned long arg) struct pwm_output_values* pwm = (struct pwm_output_values*)arg; if (pwm->channel_count > _max_actuators) /* fail with error */ - return E2BIG; + return -E2BIG; /* copy values to registers in IO */ ret = io_reg_set(PX4IO_PAGE_FAILSAFE_PWM, 0, pwm->values, pwm->channel_count); @@ -2217,7 +2217,7 @@ PX4IO::ioctl(file * filep, int cmd, unsigned long arg) struct pwm_output_values* pwm = (struct pwm_output_values*)arg; if (pwm->channel_count > _max_actuators) /* fail with error */ - return E2BIG; + return -E2BIG; /* copy values to registers in IO */ ret = io_reg_set(PX4IO_PAGE_DISARMED_PWM, 0, pwm->values, pwm->channel_count); @@ -2236,7 +2236,7 @@ PX4IO::ioctl(file * filep, int cmd, unsigned long arg) struct pwm_output_values* pwm = (struct pwm_output_values*)arg; if (pwm->channel_count > _max_actuators) /* fail with error */ - return E2BIG; + return -E2BIG; /* copy values to registers in IO */ ret = io_reg_set(PX4IO_PAGE_CONTROL_MIN_PWM, 0, pwm->values, pwm->channel_count); @@ -2255,7 +2255,7 @@ PX4IO::ioctl(file * filep, int cmd, unsigned long arg) struct pwm_output_values* pwm = (struct pwm_output_values*)arg; if (pwm->channel_count > _max_actuators) /* fail with error */ - return E2BIG; + return -E2BIG; /* copy values to registers in IO */ ret = io_reg_set(PX4IO_PAGE_CONTROL_MAX_PWM, 0, pwm->values, pwm->channel_count); @@ -2594,7 +2594,7 @@ PX4IO::ioctl(file * filep, int cmd, unsigned long arg) struct pwm_output_rc_config* config = (struct pwm_output_rc_config*)arg; if (config->channel >= RC_INPUT_MAX_CHANNELS) { /* fail with error */ - return E2BIG; + return -E2BIG; } /* copy values to registers in IO */ -- cgit v1.2.3 From c0b47d6a74197a0dc57c56efbd63803424a9835a Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 8 Nov 2014 15:48:33 +1100 Subject: px4io: only check SAFETY_OFF for allowing RC config changes and reboot If we check OUTPUTS_ARMED then we can't update trim values and scaling in flight, as there is no way to clear OUTPUTS_ARMED. If safety is on then it should be perfectly safe to update the mixer and RC config or reboot for fw update --- src/modules/px4iofirmware/registers.c | 13 +++---------- 1 file changed, 3 insertions(+), 10 deletions(-) diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index a1a02965f..f0c2cfd26 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -412,7 +412,6 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num * text handling function. */ return mixer_handle_text(values, num_values * sizeof(*values)); - break; default: /* avoid offset wrap */ @@ -584,10 +583,7 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) break; case PX4IO_P_SETUP_REBOOT_BL: - // do not reboot if FMU is armed and IO's safety is off - // this state defines an active system. - if ((r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) && - (r_status_flags & PX4IO_P_SETUP_ARMING_FMU_ARMED)) { + if (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) { // don't allow reboot while armed break; } @@ -633,12 +629,9 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) case PX4IO_PAGE_RC_CONFIG: { /** - * do not allow a RC config change while outputs armed - * = FMU is armed and IO's safety is off - * this state defines an active system. + * do not allow a RC config change while safety is off */ - if ((r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) && - (r_status_flags & PX4IO_P_SETUP_ARMING_FMU_ARMED)) { + if (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) { break; } -- cgit v1.2.3 From c906c2123822ef127026eeaf272b3aceed9f8995 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 26 Nov 2014 09:22:24 +1100 Subject: px4io: prevent use of uninitialised memory in io_set_arming_state() the vehicle may not have setup a control_mode. We need to check the return of orb_copy() to ensure we are getting initialised values --- src/drivers/px4io/px4io.cpp | 74 +++++++++++++++++++++++---------------------- 1 file changed, 38 insertions(+), 36 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index b31d7bbfa..58390ba4c 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1160,52 +1160,54 @@ PX4IO::io_set_arming_state() actuator_armed_s armed; ///< system armed state vehicle_control_mode_s control_mode; ///< vehicle_control_mode - orb_copy(ORB_ID(actuator_armed), _t_actuator_armed, &armed); - orb_copy(ORB_ID(vehicle_control_mode), _t_vehicle_control_mode, &control_mode); + int have_armed = orb_copy(ORB_ID(actuator_armed), _t_actuator_armed, &armed); + int have_control_mode = orb_copy(ORB_ID(vehicle_control_mode), _t_vehicle_control_mode, &control_mode); uint16_t set = 0; uint16_t clear = 0; - if (armed.armed) { - set |= PX4IO_P_SETUP_ARMING_FMU_ARMED; - - } else { - clear |= PX4IO_P_SETUP_ARMING_FMU_ARMED; - } - - if (armed.lockdown && !_lockdown_override) { - set |= PX4IO_P_SETUP_ARMING_LOCKDOWN; - } else { - clear |= PX4IO_P_SETUP_ARMING_LOCKDOWN; - } + if (have_armed == OK) { + if (armed.armed) { + set |= PX4IO_P_SETUP_ARMING_FMU_ARMED; + } else { + clear |= PX4IO_P_SETUP_ARMING_FMU_ARMED; + } - /* Do not set failsafe if circuit breaker is enabled */ - if (armed.force_failsafe && !_cb_flighttermination) { - set |= PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE; - } else { - clear |= PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE; - } + if (armed.lockdown && !_lockdown_override) { + set |= PX4IO_P_SETUP_ARMING_LOCKDOWN; + } else { + clear |= PX4IO_P_SETUP_ARMING_LOCKDOWN; + } - // XXX this is for future support in the commander - // but can be removed if unneeded - // if (armed.termination_failsafe) { - // set |= PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE; - // } else { - // clear |= PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE; - // } + /* Do not set failsafe if circuit breaker is enabled */ + if (armed.force_failsafe && !_cb_flighttermination) { + set |= PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE; + } else { + clear |= PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE; + } - if (armed.ready_to_arm) { - set |= PX4IO_P_SETUP_ARMING_IO_ARM_OK; + // XXX this is for future support in the commander + // but can be removed if unneeded + // if (armed.termination_failsafe) { + // set |= PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE; + // } else { + // clear |= PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE; + // } - } else { - clear |= PX4IO_P_SETUP_ARMING_IO_ARM_OK; + if (armed.ready_to_arm) { + set |= PX4IO_P_SETUP_ARMING_IO_ARM_OK; + + } else { + clear |= PX4IO_P_SETUP_ARMING_IO_ARM_OK; + } } - if (control_mode.flag_external_manual_override_ok) { - set |= PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK; - - } else { - clear |= PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK; + if (have_control_mode == OK) { + if (control_mode.flag_external_manual_override_ok) { + set |= PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK; + } else { + clear |= PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK; + } } return io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, clear, set); -- cgit v1.2.3 From 8e932cec10aa7e64e4a5cd17a571778f3ff60e9b Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 7 Nov 2014 20:00:40 +1100 Subject: systemcmds: added reflect command for USB testing --- src/systemcmds/reflect/module.mk | 41 +++++++++++++++ src/systemcmds/reflect/reflect.c | 111 +++++++++++++++++++++++++++++++++++++++ 2 files changed, 152 insertions(+) create mode 100644 src/systemcmds/reflect/module.mk create mode 100644 src/systemcmds/reflect/reflect.c diff --git a/src/systemcmds/reflect/module.mk b/src/systemcmds/reflect/module.mk new file mode 100644 index 000000000..973eb775d --- /dev/null +++ b/src/systemcmds/reflect/module.mk @@ -0,0 +1,41 @@ +############################################################################ +# +# Copyright (c) 2014 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +# +# Dump file utility +# + +MODULE_COMMAND = reflect +SRCS = reflect.c + +MAXOPTIMIZATION = -Os diff --git a/src/systemcmds/reflect/reflect.c b/src/systemcmds/reflect/reflect.c new file mode 100644 index 000000000..6bb53c71a --- /dev/null +++ b/src/systemcmds/reflect/reflect.c @@ -0,0 +1,111 @@ +/**************************************************************************** + * + * Copyright (c) 2014 Andrew Tridgell. 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 reflect.c + * + * simple data reflector for load testing terminals (especially USB) + * + * @author Andrew Tridgell + */ + +#include +#include +#include +#include +#include +#include +#include +#include + +__EXPORT int reflect_main(int argc, char *argv[]); + +// memory corruption checking +#define MAX_BLOCKS 1000 +static uint32_t nblocks; +struct block { + uint32_t v[256]; +}; +static struct block *blocks[MAX_BLOCKS]; + +#define VALUE(i) ((i*7) ^ 0xDEADBEEF) + +static void allocate_blocks(void) +{ + while (nblocks < MAX_BLOCKS) { + blocks[nblocks] = calloc(1, sizeof(struct block)); + if (blocks[nblocks] == NULL) { + break; + } + for (uint32_t i=0; iv)/sizeof(uint32_t); i++) { + blocks[nblocks]->v[i] = VALUE(i); + } + nblocks++; + } + printf("Allocated %u blocks\n", nblocks); +} + +static void check_blocks(void) +{ + for (uint32_t n=0; nv)/sizeof(uint32_t); i++) { + assert(blocks[n]->v[i] == VALUE(i)); + } + } +} + +int +reflect_main(int argc, char *argv[]) +{ + uint32_t total = 0; + printf("Starting reflector\n"); + + allocate_blocks(); + + while (true) { + char buf[128]; + ssize_t n = read(0, buf, sizeof(buf)); + if (n < 0) { + break; + } + if (n > 0) { + write(1, buf, n); + } + total += n; + if (total > 1024000) { + check_blocks(); + total = 0; + } + } + return OK; +} -- cgit v1.2.3 From 4724c050478900a0b0b760877f1613e43c0aa97c Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 3 Nov 2014 11:24:28 +1100 Subject: airspeed: use _retries=2 for I2C retries once initialised airspeed sensors often need to be on longer cables due to having to be outside the prop wash. --- src/drivers/airspeed/airspeed.cpp | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/src/drivers/airspeed/airspeed.cpp b/src/drivers/airspeed/airspeed.cpp index 3a1e1b7b5..6db6713c4 100644 --- a/src/drivers/airspeed/airspeed.cpp +++ b/src/drivers/airspeed/airspeed.cpp @@ -159,13 +159,15 @@ out: int Airspeed::probe() { - /* on initial power up the device needs more than one retry - for detection. Once it is running then retries aren't - needed + /* on initial power up the device may need more than one retry + for detection. Once it is running the number of retries can + be reduced */ _retries = 4; int ret = measure(); - _retries = 0; + + // drop back to 2 retries once initialised + _retries = 2; return ret; } -- cgit v1.2.3 From 32835757de16c8886373b8299cbcc4f5ffc19476 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 26 Nov 2014 08:24:11 +0100 Subject: Remove uncommon modules from FMU-v2 build --- makefiles/config_px4fmu-v2_default.mk | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index 1b538f52f..889e169a1 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -27,9 +27,9 @@ MODULES += drivers/l3gd20 MODULES += drivers/hmc5883 MODULES += drivers/ms5611 MODULES += drivers/mb12xx -MODULES += drivers/sf0x +#MODULES += drivers/sf0x MODULES += drivers/ll40ls -MODULES += drivers/trone +#MODULES += drivers/trone MODULES += drivers/gps MODULES += drivers/hil MODULES += drivers/hott/hott_telemetry -- cgit v1.2.3 From 2a9a649adbf64ba2c8c56d5a934aa8e09838bec6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 26 Nov 2014 18:21:54 +0100 Subject: Make boot less verbose to not hide the important status messages --- ROMFS/px4fmu_common/init.d/rcS | 5 ----- 1 file changed, 5 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 201b99749..353f44877 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -301,7 +301,6 @@ then if [ $OUTPUT_MODE == io -o $OUTPUT_MODE == uavcan_esc ] then - echo "[init] Use PX4IO PWM as primary output" if px4io start then echo "[init] PX4IO started" @@ -314,7 +313,6 @@ then if [ $OUTPUT_MODE == fmu -o $OUTPUT_MODE == ardrone ] then - echo "[init] Use FMU as primary output" if fmu mode_$FMU_MODE then echo "[init] FMU mode_$FMU_MODE started" @@ -338,7 +336,6 @@ then if [ $OUTPUT_MODE == mkblctrl ] then - echo "[init] Use MKBLCTRL as primary output" set MKBLCTRL_ARG "" if [ $MKBLCTRL_MODE == x ] then @@ -361,7 +358,6 @@ then if [ $OUTPUT_MODE == hil ] then - echo "[init] Use HIL as primary output" if hil mode_port2_pwm8 then echo "[init] HIL output started" @@ -380,7 +376,6 @@ then then if px4io start then - echo "[init] PX4IO started" sh /etc/init.d/rc.io else echo "[init] ERROR: PX4IO start failed" -- cgit v1.2.3 From 9d986f5df3c8da646451fc865c1b1e9e2dbdea9c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 26 Nov 2014 18:22:18 +0100 Subject: HMC5883: Better status reporting --- src/drivers/hmc5883/hmc5883.cpp | 39 +++++++++++++++++++++++++++++---------- 1 file changed, 29 insertions(+), 10 deletions(-) diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index 81f767965..d4dbf3778 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -1349,7 +1349,7 @@ HMC5883 *g_dev_ext = nullptr; void start(int bus, enum Rotation rotation); void test(int bus); void reset(int bus); -void info(int bus); +int info(int bus); int calibrate(int bus); void usage(); @@ -1595,17 +1595,23 @@ reset(int bus) /** * Print a little info about the driver. */ -void +int info(int bus) { - HMC5883 *g_dev = (bus == PX4_I2C_BUS_ONBOARD?g_dev_int:g_dev_ext); - if (g_dev == nullptr) - errx(1, "driver not running"); + int ret = 1; + + HMC5883 *g_dev = (bus == PX4_I2C_BUS_ONBOARD ? g_dev_int : g_dev_ext); + if (g_dev == nullptr) { + warnx("not running on bus %d", bus); + } else { - printf("state @ %p\n", g_dev); - g_dev->print_info(); + warnx("running on bus: %d (%s)\n", bus, ((PX4_I2C_BUS_ONBOARD) ? "onboard" : "offboard")); - exit(0); + g_dev->print_info(); + ret = 0; + } + + return ret; } void @@ -1685,8 +1691,21 @@ hmc5883_main(int argc, char *argv[]) /* * Print driver information. */ - if (!strcmp(verb, "info") || !strcmp(verb, "status")) - hmc5883::info(bus); + if (!strcmp(verb, "info") || !strcmp(verb, "status")) { + if (bus == -1) { + int ret = 0; + if (hmc5883::info(PX4_I2C_BUS_ONBOARD)) { + ret = 1; + } + + if (hmc5883::info(PX4_I2C_BUS_EXPANSION)) { + ret = 1; + } + exit(ret); + } else { + exit(hmc5883::info(bus)); + } + } /* * Autocalibrate the scaling -- cgit v1.2.3 From 22a247ca6760996ccb5f583b032253d4af97ed00 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 27 Nov 2014 23:19:53 +0100 Subject: Disable the BlinkM driver, code style fixes for other disabled drivers --- makefiles/config_px4fmu-v2_default.mk | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index 889e169a1..df6b6d0c5 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -27,14 +27,14 @@ MODULES += drivers/l3gd20 MODULES += drivers/hmc5883 MODULES += drivers/ms5611 MODULES += drivers/mb12xx -#MODULES += drivers/sf0x +# MODULES += drivers/sf0x MODULES += drivers/ll40ls -#MODULES += drivers/trone +# MODULES += drivers/trone MODULES += drivers/gps MODULES += drivers/hil MODULES += drivers/hott/hott_telemetry MODULES += drivers/hott/hott_sensors -MODULES += drivers/blinkm +# MODULES += drivers/blinkm MODULES += drivers/airspeed MODULES += drivers/ets_airspeed MODULES += drivers/meas_airspeed -- cgit v1.2.3 From 1da7ca7f78dfa6cd441df5df259b3c191f77fecb Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 28 Nov 2014 14:23:08 +0100 Subject: Updating NuttX version --- NuttX | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/NuttX b/NuttX index 9d06b6457..ae4b05e2c 160000 --- a/NuttX +++ b/NuttX @@ -1 +1 @@ -Subproject commit 9d06b645790e1445f14e3b19c71d40b3088f4e4f +Subproject commit ae4b05e2c51d07369b5d131052099ac346b0841c -- cgit v1.2.3 From f619dba6f96d22152c1c97216a46a27981ff2472 Mon Sep 17 00:00:00 2001 From: Holger Steinhaus Date: Fri, 28 Nov 2014 15:33:21 +0100 Subject: Corrected time_gps_usec values description. Fixes #1474 --- src/modules/uORB/topics/vehicle_gps_position.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/uORB/topics/vehicle_gps_position.h b/src/modules/uORB/topics/vehicle_gps_position.h index 31e616f4f..7888a50f4 100644 --- a/src/modules/uORB/topics/vehicle_gps_position.h +++ b/src/modules/uORB/topics/vehicle_gps_position.h @@ -79,7 +79,7 @@ struct vehicle_gps_position_s { bool vel_ned_valid; /**< Flag to indicate if NED speed is valid */ uint64_t timestamp_time; /**< Timestamp for time information */ - uint64_t time_gps_usec; /**< Timestamp (microseconds in GPS format), this is the timestamp which comes from the gps module */ + uint64_t time_gps_usec; /**< Timestamp (microseconds in GPS format), this is the timestamp which comes from the gps module. It might be unavailable right after cold start, indicated by a value of 0 */ uint8_t satellites_used; /**< Number of satellites used */ }; -- cgit v1.2.3 From e51f72000bb850f9933a5ba9c024dddb6248440c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 1 Dec 2014 11:27:59 +0100 Subject: Fix RGB led stop command --- src/drivers/rgbled/rgbled.cpp | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/src/drivers/rgbled/rgbled.cpp b/src/drivers/rgbled/rgbled.cpp index 13cbfdfa8..d35722244 100644 --- a/src/drivers/rgbled/rgbled.cpp +++ b/src/drivers/rgbled/rgbled.cpp @@ -121,7 +121,7 @@ private: /* for now, we only support one RGBLED */ namespace { -RGBLED *g_rgbled; +RGBLED *g_rgbled = nullptr; } void rgbled_usage(); @@ -680,15 +680,15 @@ rgbled_main(int argc, char *argv[]) ret = ioctl(fd, RGBLED_SET_MODE, (unsigned long)RGBLED_MODE_OFF); close(fd); + /* delete the rgbled object if stop was requested, in addition to turning off the LED. */ + if (!strcmp(verb, "stop")) { + delete g_rgbled; + g_rgbled = nullptr; + exit(0); + } exit(ret); } - if (!strcmp(verb, "stop")) { - delete g_rgbled; - g_rgbled = nullptr; - exit(0); - } - if (!strcmp(verb, "rgb")) { if (argc < 5) { errx(1, "Usage: rgbled rgb "); -- cgit v1.2.3 From 3ce7abe9d84a6c716985339c8165bec98e481847 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Tue, 2 Dec 2014 01:01:35 +0100 Subject: use consistent orientation naming in messages --- src/modules/commander/accelerometer_calibration.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index 0cb41489f..8eac0a6fd 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -263,7 +263,7 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float const int samples_num = 2500; float accel_ref[6][3]; bool data_collected[6] = { false, false, false, false, false, false }; - const char *orientation_strs[6] = { "front", "back", "left", "right", "top", "bottom" }; + const char *orientation_strs[6] = { "front", "back", "left", "right", "up", "down" }; int sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined)); -- cgit v1.2.3 From b1bd813978e48d32a66b8a49bccdda7f053ea55c Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Tue, 2 Dec 2014 01:37:54 +0100 Subject: swap fron/back > the "side" being measured is facing downwards --- src/modules/commander/accelerometer_calibration.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index 8eac0a6fd..49c653ad2 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -263,7 +263,7 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float const int samples_num = 2500; float accel_ref[6][3]; bool data_collected[6] = { false, false, false, false, false, false }; - const char *orientation_strs[6] = { "front", "back", "left", "right", "up", "down" }; + const char *orientation_strs[6] = { "back", "front", "left", "right", "up", "down" }; int sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined)); @@ -294,8 +294,8 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float /* inform user which axes are still needed */ mavlink_log_info(mavlink_fd, "pending: %s%s%s%s%s%s", - (!data_collected[0]) ? "front " : "", - (!data_collected[1]) ? "back " : "", + (!data_collected[0]) ? "back " : "", + (!data_collected[1]) ? "front " : "", (!data_collected[2]) ? "left " : "", (!data_collected[3]) ? "right " : "", (!data_collected[4]) ? "up " : "", -- cgit v1.2.3 From 45c52b51ee84bead9562a51a4d410fcbe921e798 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Tue, 2 Dec 2014 01:42:12 +0100 Subject: move natural position to the front of the pending list for QGC --- src/modules/commander/accelerometer_calibration.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index 49c653ad2..d4cd97be6 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -294,12 +294,12 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float /* inform user which axes are still needed */ mavlink_log_info(mavlink_fd, "pending: %s%s%s%s%s%s", + (!data_collected[5]) ? "down " : "", (!data_collected[0]) ? "back " : "", (!data_collected[1]) ? "front " : "", (!data_collected[2]) ? "left " : "", (!data_collected[3]) ? "right " : "", - (!data_collected[4]) ? "up " : "", - (!data_collected[5]) ? "down " : ""); + (!data_collected[4]) ? "up " : ""); /* allow user enough time to read the message */ sleep(3); -- cgit v1.2.3